diff --git a/include/moveit_grasps/grasp_candidate.h b/include/moveit_grasps/grasp_candidate.h index a67737c..b539ffa 100644 --- a/include/moveit_grasps/grasp_candidate.h +++ b/include/moveit_grasps/grasp_candidate.h @@ -73,6 +73,46 @@ struct GraspFilterCode GRASP_INVALID, // An error occured while processing the grasp LAST // Used to track last value in the base class when inheriting }; + + static bool errorCodeToString(int code, std::string& code_string) + { + if (code == NOT_FILTERED) + { + code_string = "GraspFilterCode::NOT_FILTERED"; + return true; + } + else if (code == GRASP_FILTERED_BY_IK) + { + code_string = "GraspFilterCode::GRASP_FILTERED_BY_IK"; + return true; + } + else if (code == GRASP_FILTERED_BY_CUTTING_PLANE) + { + code_string = "GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE"; + return true; + } + else if (code == GRASP_FILTERED_BY_ORIENTATION) + { + code_string = "GraspFilterCode::GRASP_FILTERED_BY_ORIENTATION"; + return true; + } + else if (code == GRASP_FILTERED_BY_IK_CLOSED) + { + code_string = "GraspFilterCode::GRASP_FILTERED_BY_IK_CLOSED"; + return true; + } + else if (code == PREGRASP_FILTERED_BY_IK) + { + code_string = "GraspFilterCode::PREGRASP_FILTERED_BY_IK"; + return true; + } + else if (code == GRASP_INVALID) + { + code_string = "GraspFilterCode::GRASP_INVALID"; + return true; + } + return false; + } }; /** diff --git a/include/moveit_grasps/grasp_filter.h b/include/moveit_grasps/grasp_filter.h index 2b7769e..4d8358e 100644 --- a/include/moveit_grasps/grasp_filter.h +++ b/include/moveit_grasps/grasp_filter.h @@ -48,6 +48,7 @@ // Grasping #include #include +#include // Rviz #include @@ -111,7 +112,8 @@ struct IkThreadStruct const planning_scene::PlanningScenePtr& planning_scene, Eigen::Isometry3d& link_transform, std::size_t grasp_id, kinematics::KinematicsBaseConstPtr kin_solver, const robot_state::RobotStatePtr& robot_state, double timeout, bool filter_pregrasp, bool visual_debug, - std::size_t thread_id, const std::string& grasp_target_object_id) + std::size_t thread_id, const std::string& grasp_target_object_id, + const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, double animation_speed) : grasp_candidates_(grasp_candidates) , planning_scene_(planning_scene::PlanningScene::clone(planning_scene)) , link_transform_(link_transform) @@ -123,6 +125,9 @@ struct IkThreadStruct , visual_debug_(visual_debug) , thread_id_(thread_id) , grasp_target_object_id_(grasp_target_object_id) + , constraint_fn_(boost::bind( + &isGraspStateValid, planning_scene_.get(), + visual_debug, animation_speed, visual_tools, _1, _2, _3)) { } @@ -144,6 +149,9 @@ struct IkThreadStruct // Used within processing function geometry_msgs::PoseStamped ik_pose_; // Set from grasp candidate std::vector ik_seed_state_; + + // Used for IK animation + const moveit::core::GroupStateValidityCallbackFn constraint_fn_; }; typedef std::shared_ptr IkThreadStructPtr; @@ -280,8 +288,7 @@ class GraspFilter * \return true on success */ bool findIKSolution(std::vector& ik_solution, const IkThreadStructPtr& ik_thread_struct, - const GraspCandidatePtr& grasp_candidate, - const moveit::core::GroupStateValidityCallbackFn& constraint_fn) const; + const GraspCandidatePtr& grasp_candidate) const; /** * \brief add a cutting plane diff --git a/include/moveit_grasps/grasp_planner.h b/include/moveit_grasps/grasp_planner.h index a7cdfae..c96c0d1 100644 --- a/include/moveit_grasps/grasp_planner.h +++ b/include/moveit_grasps/grasp_planner.h @@ -143,6 +143,9 @@ class GraspPlanner // A shared node handle ros::NodeHandle nh_; + // Name for logging + std::string name_; + // Class for publishing stuff to rviz moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; diff --git a/include/moveit_grasps/suction_grasp_candidate.h b/include/moveit_grasps/suction_grasp_candidate.h index 5e89e89..8e1943c 100644 --- a/include/moveit_grasps/suction_grasp_candidate.h +++ b/include/moveit_grasps/suction_grasp_candidate.h @@ -51,6 +51,19 @@ struct SuctionGraspFilterCode : public GraspFilterCode { GRASP_FILTERED_BY_SUCTION_VOXEL_OVERLAP = LAST + 1, // No suction voxel is in sufficient contact with the target }; + + static bool errorCodeToString(int code, std::string& code_string) + { + if (code == GRASP_FILTERED_BY_SUCTION_VOXEL_OVERLAP) + { + code_string = "SuctionGraspFilterCode::GRASP_FILTERED_BY_SUCTION_VOXEL_OVERLAP"; + return true; + } + else + { + return GraspFilterCode::errorCodeToString(code, code_string); + } + } }; /** diff --git a/include/moveit_grasps/suction_grasp_filter.h b/include/moveit_grasps/suction_grasp_filter.h index 4ef5fc4..ca6da9c 100644 --- a/include/moveit_grasps/suction_grasp_filter.h +++ b/include/moveit_grasps/suction_grasp_filter.h @@ -54,20 +54,6 @@ class SuctionGraspFilter : public GraspFilter SuctionGraspFilter(const robot_state::RobotStatePtr& robot_state, const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools); - /** - * \brief Return grasps that are kinematically feasible - * \param grasp_candidates - all possible grasps that this will test. this vector is returned modified - * \param arm_jmg - the arm to solve the IK problem on - * \param filter_pregrasp -whether to also check ik feasibility for the pregrasp position - * \param visualize - visualize IK filtering - * \return number of grasps remaining - */ - std::size_t filterGraspsHelper(std::vector& grasp_candidates, - const planning_scene::PlanningScenePtr& planning_scene_monitor, - const robot_model::JointModelGroup* arm_jmg, - const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp, bool visualize, - const std::string& target_object_id = "") override; - /** * \brief Print grasp filtering statistics */ @@ -79,10 +65,10 @@ class SuctionGraspFilter : public GraspFilter bool processCandidateGrasp(const IkThreadStructPtr& ik_thread_struct) override; /** - * \brief Filter grasps that do not have a valid suction voxel overlap - * \param grasp_candidates - all possible grasps that this will test. this vector is returned modified + * \brief Filter one grasp by checking if it has a suction voxel with a valid overlap + * \param grasp_candidate - a populated grasp candidate with a populated suction_voxel_overlap_ vector */ - bool filterGraspsBySuctionVoxelOverlapCutoff(std::vector& grasp_candidates); + bool filterGraspBySuctionVoxelOverlapCutoff(const SuctionGraspCandidatePtr& suction_grasp_candidate) const; /** * \brief For suction grippers, set the cutoff threshold used by preFilterBySuctionVoxelOverlap to @@ -121,8 +107,8 @@ class SuctionGraspFilter : public GraspFilter // Name for logging const std::string name_; - // A cutoff threshold [0,1] where at least one suction voxe must have more than this fraction overlap - // with the target object + // A cutoff threshold [0,1] where at least one suction voxel must have more than this fraction overlap + // with the target object. Set to a negative value to disable this check. Initialized to -1 double suction_voxel_overlap_cutoff_; }; // end of class diff --git a/src/demo/grasp_pipeline_demo.cpp b/src/demo/grasp_pipeline_demo.cpp index 4fad4b6..14a8bea 100644 --- a/src/demo/grasp_pipeline_demo.cpp +++ b/src/demo/grasp_pipeline_demo.cpp @@ -63,6 +63,9 @@ namespace moveit_grasps_demo { static const std::string LOGNAME = "grasp_pipeline_demo"; +// Allow an interrupt to be called that waits for user input, useful for debugging +typedef boost::function WaitForNextStepCallback; + namespace { bool isStateValid(const planning_scene::PlanningScene* planning_scene, @@ -74,11 +77,17 @@ bool isStateValid(const planning_scene::PlanningScene* planning_scene, return !planning_scene->isStateColliding(*robot_state, group->getName()); } -void waitForNextStep(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, const std::string& prompt) +void waitForNextStepBlocking(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, const std::string& prompt) { visual_tools->prompt(prompt); } +void waitForNextStepNonBlocking(const std::string& prompt) +{ + ROS_INFO_STREAM("waitForNextStepNonBlocking:" << prompt); + ros::Duration(0.5).sleep(); +} + } // end annonymous namespace class GraspPipelineDemo @@ -141,6 +150,10 @@ class GraspPipelineDemo void setupGraspPipeline() { + // --------------------------------------------------------------------------------------------- + // Set waitForNextStep callback to be non-blocking + setWaitForNextStepCallback(boost::bind(&waitForNextStepNonBlocking, _1)); + // --------------------------------------------------------------------------------------------- // Load grasp data specific to our robot grasp_data_ = @@ -183,7 +196,7 @@ class GraspPipelineDemo grasp_planner_ = std::make_shared(visual_tools_); // MoveIt Grasps allows for a manual breakpoint debugging tool to be optionally passed in - grasp_planner_->setWaitForNextStepCallback(boost::bind(&waitForNextStep, visual_tools_, _1)); + // grasp_planner_->setWaitForNextStepCallback(boost::bind(&waitForNextStepBlocking, visual_tools_, _1)); // ----------------------------------------------------- // Load the motion planning pipeline @@ -466,6 +479,17 @@ class GraspPipelineDemo return success; } + void waitForNextStep(const std::string& message) + { + if (wait_for_next_step_callback_) + wait_for_next_step_callback_(message); + } + + void setWaitForNextStepCallback(WaitForNextStepCallback callback) + { + wait_for_next_step_callback_ = callback; + } + private: // A shared node handle ros::NodeHandle nh_; @@ -501,6 +525,7 @@ class GraspPipelineDemo std::string ee_group_name_; std::string planning_group_name_; + WaitForNextStepCallback wait_for_next_step_callback_; }; // end of class } // namespace diff --git a/src/demo/suction_grasp_pipeline_demo.cpp b/src/demo/suction_grasp_pipeline_demo.cpp index 6af4afa..d61f48d 100644 --- a/src/demo/suction_grasp_pipeline_demo.cpp +++ b/src/demo/suction_grasp_pipeline_demo.cpp @@ -64,6 +64,9 @@ namespace moveit_grasps_demo { static const std::string LOGNAME = "grasp_pipeline_demo"; +// Allow an interrupt to be called that waits for user input, useful for debugging +typedef boost::function WaitForNextStepCallback; + namespace { bool isStateValid(const planning_scene::PlanningScene* planning_scene, @@ -75,11 +78,17 @@ bool isStateValid(const planning_scene::PlanningScene* planning_scene, return !planning_scene->isStateColliding(*robot_state, group->getName()); } -void waitForNextStep(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, const std::string& prompt) +void waitForNextStepBlocking(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, const std::string& prompt) { visual_tools->prompt(prompt); } +void waitForNextStepNonBlocking(const std::string& prompt) +{ + ROS_INFO_STREAM("waitForNextStepNonBlocking:" << prompt); + ros::Duration(0.5).sleep(); +} + } // end annonymous namespace class GraspPipelineDemo @@ -142,6 +151,10 @@ class GraspPipelineDemo void setupGraspPipeline() { + // --------------------------------------------------------------------------------------------- + // Set waitForNextStep callback to be non-blocking + setWaitForNextStepCallback(boost::bind(&waitForNextStepNonBlocking, _1)); + // --------------------------------------------------------------------------------------------- // Load grasp data specific to our robot grasp_data_ = @@ -173,6 +186,7 @@ class GraspPipelineDemo // Assign the grasp score weights in the grasp_generator grasp_generator_->setGraspScoreWeights(grasp_score_weights); + // --------------------------------------------------------------------------------------------- // Load grasp filter grasp_filter_ = @@ -183,7 +197,7 @@ class GraspPipelineDemo grasp_planner_ = std::make_shared(visual_tools_); // MoveIt Grasps allows for a manual breakpoint debugging tool to be optionally passed in - grasp_planner_->setWaitForNextStepCallback(boost::bind(&waitForNextStep, visual_tools_, _1)); + // grasp_planner_->setWaitForNextStepCallback(boost::bind(&waitForNextStepBlocking, visual_tools_, _1)); // ----------------------------------------------------- // Load the motion planning pipeline @@ -250,6 +264,7 @@ class GraspPipelineDemo moveit_grasps::GraspCandidatePtr selected_grasp_candidate; moveit_msgs::MotionPlanResponse pre_approach_plan; + // return false; if (!planFullGrasp(grasp_candidates, selected_grasp_candidate, pre_approach_plan, object_name)) { ROS_ERROR_STREAM_NAMED(LOGNAME, "Failed to plan grasp motions"); @@ -275,38 +290,40 @@ class GraspPipelineDemo visual_tools_->trigger(); // Get the pre and post grasp states - visual_tools_->prompt("pre_grasp"); + waitForNextStep("pre_grasp"); robot_state::RobotStatePtr pre_grasp_state = std::make_shared(*visual_tools_->getSharedRobotState()); valid_grasp_candidate->getPreGraspState(pre_grasp_state); visual_tools_->publishRobotState(pre_grasp_state, rviz_visual_tools::ORANGE); - visual_tools_->prompt("grasp"); + + waitForNextStep("grasp"); robot_state::RobotStatePtr grasp_state = std::make_shared(*visual_tools_->getSharedRobotState()); valid_grasp_candidate->getGraspStateClosed(grasp_state); visual_tools_->publishRobotState(grasp_state, rviz_visual_tools::YELLOW); - visual_tools_->prompt("lift"); - visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[1].back(), - rviz_visual_tools::BLUE); - visual_tools_->prompt("retreat"); - visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[2].back(), - rviz_visual_tools::PURPLE); - - visual_tools_->prompt("show free space approach"); - visual_tools_->hideRobot(); - visual_tools_->trigger(); - bool wait_for_animation = true; - visual_tools_->publishTrajectoryPath(pre_approach_plan.trajectory, pre_grasp_state, wait_for_animation); - ros::Duration(0.25).sleep(); - visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH], - valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation); - ros::Duration(0.25).sleep(); - visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::LIFT], - valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation); - visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::RETREAT], - valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation); - ros::Duration(0.25).sleep(); + // waitForNextStep("lift"); + // visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[1].back(), + // rviz_visual_tools::BLUE); + // waitForNextStep("retreat"); + // visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[2].back(), + // rviz_visual_tools::PURPLE); + + // waitForNextStep("show free space approach"); + // visual_tools_->hideRobot(); + // visual_tools_->trigger(); + + // bool wait_for_animation = true; + // visual_tools_->publishTrajectoryPath(pre_approach_plan.trajectory, pre_grasp_state, wait_for_animation); + // ros::Duration(0.25).sleep(); + // visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH], + // valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation); + // ros::Duration(0.25).sleep(); + // visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::LIFT], + // valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation); + // visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::RETREAT], + // valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation); + // ros::Duration(0.25).sleep(); } bool planFullGrasp(std::vector& grasp_candidates, @@ -325,20 +342,20 @@ class GraspPipelineDemo { valid_grasp_candidate = grasp_candidates.front(); valid_grasp_candidate->getPreGraspState(current_state); - if (!grasp_planner_->planApproachLiftRetreat(valid_grasp_candidate, current_state, planning_scene_monitor_, false, - object_name)) - { - ROS_INFO_NAMED(LOGNAME, "failed to plan approach lift retreat"); - continue; - } - - robot_state::RobotStatePtr pre_grasp_state = - valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH].front(); - if (!planPreApproach(*pre_grasp_state, pre_approach_plan)) - { - ROS_WARN_NAMED(LOGNAME, "failed to plan to pregrasp_state"); - continue; - } + // if (!grasp_planner_->planApproachLiftRetreat(valid_grasp_candidate, current_state, planning_scene_monitor_, false, + // object_name)) + // { + // ROS_INFO_NAMED(LOGNAME, "failed to plan approach lift retreat"); + // continue; + // } + // robot_state::RobotStatePtr pre_grasp_state = + // valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH].front(); + + // if (!planPreApproach(*pre_grasp_state, pre_approach_plan)) + // { + // ROS_WARN_NAMED(LOGNAME, "failed to plan to pregrasp_state"); + // continue; + // } success = true; break; @@ -413,20 +430,19 @@ class GraspPipelineDemo double& y_width, double& z_height) { // Generate random cuboid - double xmin = 0.125; - double xmax = 0.250; - double ymin = 0.125; - double ymax = 0.250; - double zmin = 0.200; - double zmax = 0.500; - + double xmin = 0.35; + double xmax = 0.35; + double ymin = 0.35; + double ymax = 0.35; + double zmin = 0.35; + double zmax = 0.35; double rot_tol = 0.05; double elevation_min = rot_tol; double elevation_max = rot_tol; double azimuth_min = rot_tol; double azimuth_max = rot_tol; - double angle_min = rot_tol; - double angle_max = rot_tol; + double angle_min = 0; + double angle_max = 2 * M_PI; rviz_visual_tools::RandomPoseBounds pose_bounds(xmin, xmax, ymin, ymax, zmin, zmax, elevation_min, elevation_max, azimuth_min, azimuth_max, angle_min, angle_max); @@ -452,6 +468,17 @@ class GraspPipelineDemo return success; } + void waitForNextStep(const std::string& message) + { + if (wait_for_next_step_callback_) + wait_for_next_step_callback_(message); + } + + void setWaitForNextStepCallback(WaitForNextStepCallback callback) + { + wait_for_next_step_callback_ = callback; + } + private: // A shared node handle ros::NodeHandle nh_; @@ -487,13 +514,15 @@ class GraspPipelineDemo std::string ee_group_name_; std::string planning_group_name_; + // Set a callback for pausing demo + WaitForNextStepCallback wait_for_next_step_callback_; + }; // end of class } // namespace int main(int argc, char* argv[]) { - int num_tests = 1; ros::init(argc, argv, "grasp_filter_demo"); @@ -509,8 +538,10 @@ int main(int argc, char* argv[]) start_time = ros::Time::now(); // Run Tests + int num_tests = 1; moveit_grasps_demo::GraspPipelineDemo tester; - tester.demoRandomGrasp(); + for (std::size_t ix=0; ix -#include // moveit #include @@ -103,7 +102,6 @@ bool GraspFilter::filterGrasps(std::vector& grasp_candidates, const std::string& target_object_id) { bool verbose = false; - // Error check if (grasp_candidates.empty()) { @@ -176,7 +174,6 @@ bool GraspFilter::filterGrasps(std::vector& grasp_candidates, ROS_WARN_STREAM_NAMED(name_, "No grasps remaining after filtering"); return false; } - return true; } @@ -248,17 +245,12 @@ bool GraspFilter::filterGraspByGraspIK(const GraspCandidatePtr& grasp_candidate, // Get pose ik_thread_struct->ik_pose_ = grasp_candidate->grasp_.grasp_pose; - // Create constraint_fn - moveit::core::GroupStateValidityCallbackFn constraint_fn = boost::bind( - &isGraspStateValid, ik_thread_struct->planning_scene_.get(), - collision_verbose_ || ik_thread_struct->visual_debug_, collision_verbose_speed_, visual_tools_, _1, _2, _3); - // Set gripper position (eg. how open the eef is) to the custom open position grasp_candidate->getGraspStateOpenEEOnly(ik_thread_struct->robot_state_); // Solve IK Problem for grasp posture grasp_ik_solution.resize(0); - if (!findIKSolution(grasp_ik_solution, ik_thread_struct, grasp_candidate, constraint_fn)) + if (!findIKSolution(grasp_ik_solution, ik_thread_struct, grasp_candidate)) { ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Unable to find the-grasp IK solution"); grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_IK; @@ -280,13 +272,9 @@ bool GraspFilter::filterGraspByPreGraspIK(const GraspCandidatePtr& grasp_candida const std::string& ee_parent_link_name = grasp_candidate->grasp_data_->ee_jmg_->getEndEffectorParentGroup().second; ik_thread_struct->ik_pose_ = GraspGenerator::getPreGraspPose(grasp_candidate, ee_parent_link_name); - moveit::core::GroupStateValidityCallbackFn constraint_fn = boost::bind( - &isGraspStateValid, ik_thread_struct->planning_scene_.get(), - collision_verbose_ || ik_thread_struct->visual_debug_, collision_verbose_speed_, visual_tools_, _1, _2, _3); - // Solve IK Problem for pregrasp pregrasp_ik_solution.resize(0); - if (!findIKSolution(pregrasp_ik_solution, ik_thread_struct, grasp_candidate, constraint_fn)) + if (!findIKSolution(pregrasp_ik_solution, ik_thread_struct, grasp_candidate)) { ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Unable to find PRE-grasp IK solution"); grasp_candidate->grasp_filtered_code_ = GraspFilterCode::PREGRASP_FILTERED_BY_IK; @@ -434,7 +422,8 @@ std::size_t GraspFilter::filterGraspsHelper(std::vector& gras std::make_shared(grasp_candidates, planning_scene_clone, link_transform, 0, // this is filled in by OpenMP kin_solvers_[arm_jmg->getName()][thread_id], robot_states_[thread_id], - solver_timeout_, filter_pregrasp, visualize, thread_id, target_object_id); + solver_timeout_, filter_pregrasp, visualize || collision_verbose_, thread_id, + target_object_id, visual_tools_, collision_verbose_speed_); ik_thread_structs[thread_id]->ik_seed_state_ = ik_seed_state; } @@ -443,7 +432,6 @@ std::size_t GraspFilter::filterGraspsHelper(std::vector& gras start_time = ros::Time::now(); // Loop through poses and find those that are kinematically feasible - omp_set_num_threads(num_threads); #pragma omp parallel for schedule(dynamic) for (std::size_t grasp_id = 0; grasp_id < grasp_candidates.size(); ++grasp_id) @@ -573,8 +561,7 @@ bool GraspFilter::processCandidateGrasp(const IkThreadStructPtr& ik_thread_struc } bool GraspFilter::findIKSolution(std::vector& ik_solution, const IkThreadStructPtr& ik_thread_struct, - const GraspCandidatePtr& grasp_candidate, - const moveit::core::GroupStateValidityCallbackFn& constraint_fn) const + const GraspCandidatePtr& grasp_candidate) const { robot_state::RobotState state = ik_thread_struct->planning_scene_->getCurrentState(); if (ik_thread_struct->ik_seed_state_.size() == grasp_candidate->grasp_data_->arm_jmg_->getActiveJointModels().size()) @@ -592,7 +579,7 @@ bool GraspFilter::findIKSolution(std::vector& ik_solution, const IkThrea ab->getAttachedLinkName(), ab->getDetachPosture()); bool ik_success = state.setFromIK(grasp_candidate->grasp_data_->arm_jmg_, ik_thread_struct->ik_pose_.pose, - ik_thread_struct->timeout_, constraint_fn); + ik_thread_struct->timeout_, ik_thread_struct->constraint_fn_); // Results if (ik_success) @@ -854,11 +841,22 @@ void GraspFilter::setACMFingerEntry(const std::string& object_name, bool allowed ROS_DEBUG_STREAM_NAMED(logger_name, "" << object_name.c_str() << ", " << (allowed ? "true" : "false")); // Lock planning scene - for (std::size_t i = 0; i < ee_link_names.size(); ++i) + if (allowed) + { + for (const auto& link_name : ee_link_names) + { + ROS_DEBUG_NAMED(logger_name, "collisions between %s and %s : allowed", object_name.c_str(), link_name.c_str()); + scene->getAllowedCollisionMatrixNonConst().setEntry(object_name, link_name, true); + } + } + else { - ROS_DEBUG_NAMED(logger_name, "collisions between %s and %s : %s", object_name.c_str(), ee_link_names[i].c_str(), - allowed ? "allowed" : "not allowed"); - scene->getAllowedCollisionMatrixNonConst().setEntry(object_name, ee_link_names[i], allowed); + for (const auto& link_name : ee_link_names) + { + ROS_DEBUG_NAMED(logger_name, "collisions between %s and %s : not allowed", object_name.c_str(), + link_name.c_str()); + scene->getAllowedCollisionMatrixNonConst().removeEntry(object_name, link_name); + } } // Debug current matrix diff --git a/src/grasp_planner.cpp b/src/grasp_planner.cpp index 75d5fde..f2ae9d8 100644 --- a/src/grasp_planner.cpp +++ b/src/grasp_planner.cpp @@ -50,7 +50,7 @@ constexpr char ENABLED_PARENT_NAME[] = "grasp_planner"; // for namespacing logg constexpr char ENABLED_SETTINGS_NAMESPACE[] = "moveit_grasps/planner"; GraspPlanner::GraspPlanner(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools) - : nh_("~"), visual_tools_(visual_tools) + : nh_("~"), name_(ENABLED_PARENT_NAME), visual_tools_(visual_tools) { loadEnabledSettings(); } @@ -70,7 +70,7 @@ bool GraspPlanner::planAllApproachLiftRetreat(std::vector& gr const planning_scene::PlanningSceneConstPtr& planning_scene, const std::string& grasp_object_id) { - ROS_INFO_STREAM_NAMED("grasp_planner", "Planning all remaining grasps with approach lift retreat cartesian path"); + ROS_INFO_STREAM_NAMED(name_, "Planning all remaining grasps with approach lift retreat cartesian path"); // For each remaining grasp, calculate entire approach, lift, and retreat path. // Remove those that have no valid path @@ -86,14 +86,14 @@ bool GraspPlanner::planAllApproachLiftRetreat(std::vector& gr if (isEnabled("verbose_cartesian_filtering")) { - ROS_INFO_STREAM_NAMED("grasp_planner", ""); - ROS_INFO_STREAM_NAMED("grasp_planner", "Attempting to plan cartesian grasp path #" - << count++ << ". " << grasp_candidates.size() << " remaining."); + ROS_INFO_STREAM_NAMED(name_, ""); + ROS_INFO_STREAM_NAMED(name_, "Attempting to plan cartesian grasp path #" + << count++ << ". " << grasp_candidates.size() << " remaining."); } if (!planApproachLiftRetreat(*grasp_it, robot_state, planning_scene, verbose_cartesian_filtering, grasp_object_id)) { - ROS_INFO_STREAM_NAMED("grasp_planner", "Grasp candidate was unable to find valid cartesian waypoint path"); + ROS_INFO_STREAM_NAMED(name_, "Grasp candidate was unable to find valid cartesian waypoint path"); grasp_it = grasp_candidates.erase(grasp_it); // not valid } @@ -125,7 +125,7 @@ bool GraspPlanner::planAllApproachLiftRetreat(std::vector& gr // If no grasp candidates had valid paths, then we return false if (grasp_candidates.size() == 0) { - ROS_DEBUG_STREAM_NAMED("grasp_planner", "No valid grasp plan possible"); + ROS_DEBUG_STREAM_NAMED(name_, "No valid grasp plan possible"); return false; } return true; @@ -148,6 +148,8 @@ bool GraspPlanner::planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, const planning_scene::PlanningSceneConstPtr& planning_scene, bool verbose_cartesian_filtering, const std::string& grasp_object_id) { + static const std::string logger_name = name_ + ".planApproachLiftRetreat"; + EigenSTL::vector_Isometry3d waypoints; GraspGenerator::getGraspWaypoints(grasp_candidate, waypoints); @@ -183,27 +185,26 @@ bool GraspPlanner::planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, moveit::core::RobotStatePtr start_state(new moveit::core::RobotState(*robot_state)); if (!grasp_candidate->getPreGraspState(start_state)) { - ROS_WARN_STREAM_NAMED("grasp_planner.waypoints", "Unable to set pregrasp"); + ROS_WARN_STREAM_NAMED(logger_name, "Unable to set pregrasp"); return false; } if (!computeCartesianWaypointPath(grasp_candidate, planning_scene, start_state, waypoints, grasp_object_id)) { - ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "Unable to plan approach lift retreat path"); + ROS_DEBUG_STREAM_NAMED(logger_name, "Unable to plan approach lift retreat path"); return false; } // Feedback - ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "Found valid and complete waypoint manipulation path for grasp " - "candidate"); + ROS_DEBUG_STREAM_NAMED(logger_name, "Found valid and complete waypoint manipulation path for grasp " + "candidate"); // Show visuals if (show_cartesian_waypoints) { - ROS_INFO_STREAM_NAMED("grasp_planner.waypoints", "Visualize end effector position of cartesian path for " - << grasp_candidate->segmented_cartesian_traj_.size() - << " segments"); + ROS_INFO_STREAM_NAMED(logger_name, "Visualize end effector position of cartesian path for " + << grasp_candidate->segmented_cartesian_traj_.size() << " segments"); visual_tools_->publishTrajectoryPoints(grasp_candidate->segmented_cartesian_traj_[APPROACH], grasp_candidate->grasp_data_->parent_link_, rviz_visual_tools::YELLOW); visual_tools_->publishTrajectoryPoints(grasp_candidate->segmented_cartesian_traj_[LIFT], @@ -245,6 +246,8 @@ bool GraspPlanner::computeCartesianWaypointPath(GraspCandidatePtr& grasp_candida const EigenSTL::vector_Isometry3d& waypoints, const std::string& grasp_object_id) { + static const std::string logger_name = name_ + ".computeCartesianWaypointPath"; + // End effector parent link (arm tip for ik solving) const moveit::core::LinkModel* ik_tip_link = grasp_candidate->grasp_data_->parent_link_; @@ -265,8 +268,8 @@ bool GraspPlanner::computeCartesianWaypointPath(GraspCandidatePtr& grasp_candida // Check for kinematic solver if (!grasp_candidate->grasp_data_->arm_jmg_->canSetStateFromIK(ik_tip_link->getName())) { - ROS_ERROR_STREAM_NAMED("grasp_planner.waypoints", "No IK Solver loaded - make sure moveit_config/kinamatics.yaml " - "is loaded in this namespace"); + ROS_ERROR_STREAM_NAMED(logger_name, "No IK Solver loaded - make sure moveit_config/kinamatics.yaml " + "is loaded in this namespace"); return false; } @@ -277,14 +280,14 @@ bool GraspPlanner::computeCartesianWaypointPath(GraspCandidatePtr& grasp_candida { if (attempts > 0) { - ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "Attempting IK solution, attempt # " << attempts + 1); + ROS_DEBUG_STREAM_NAMED(logger_name, "Attempting IK solution, attempt # " << attempts + 1); } attempts++; moveit::core::RobotStatePtr start_state_copy(new moveit::core::RobotState(*start_state)); if (!grasp_candidate->getPreGraspState(start_state_copy)) { - ROS_ERROR_STREAM_NAMED("grasp_planner.waypoints", "Unable to set pregrasp"); + ROS_ERROR_STREAM_NAMED(logger_name, "Unable to set pregrasp"); return false; } @@ -295,7 +298,9 @@ bool GraspPlanner::computeCartesianWaypointPath(GraspCandidatePtr& grasp_candida { std::vector ee_link_names = grasp_candidate->grasp_data_->ee_jmg_->getLinkModelNames(); for (const auto& ee_link : ee_link_names) + { scene->getAllowedCollisionMatrixNonConst().setEntry(grasp_object_id, ee_link, true); + } } // Collision check moveit::core::GroupStateValidityCallbackFn constraint_fn = @@ -313,7 +318,7 @@ bool GraspPlanner::computeCartesianWaypointPath(GraspCandidatePtr& grasp_candida if (!grasp_candidate->getGraspStateClosedEEOnly(start_state_copy)) { - ROS_ERROR_STREAM_NAMED("grasp_planner", "Unable to set pregrasp"); + ROS_ERROR_STREAM_NAMED(name_, "Unable to set pregrasp"); return false; } @@ -339,7 +344,7 @@ bool GraspPlanner::computeCartesianWaypointPath(GraspCandidatePtr& grasp_candida suction_voxel_co.operation = moveit_msgs::CollisionObject::ADD; if (!scene->processAttachedCollisionObjectMsg(aco)) { - ROS_WARN_STREAM_NAMED("grasp_planner", "Failed to attach: " << aco.object.id); + ROS_WARN_STREAM_NAMED(name_, "Failed to attach: " << aco.object.id); } else { @@ -366,16 +371,16 @@ bool GraspPlanner::computeCartesianWaypointPath(GraspCandidatePtr& grasp_candida robot_state::MaxEEFStep(max_step), robot_state::JumpThreshold(jump_threshold), constraint_fn, kinematics::KinematicsQueryOptions()); - ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "valid_approach_percentage: " << valid_approach_percentage - << " \tvalid_lift_retreat_" - "percentage: " - << valid_lift_retreat_percentage); + ROS_DEBUG_STREAM_NAMED(logger_name, "valid_approach_percentage: " << valid_approach_percentage + << " \tvalid_lift_retreat_" + "percentage: " + << valid_lift_retreat_percentage); // The retreat has to work for the most part but doesn't need to be perfect double min_allowed_valid_lift_retreat_percentage = 0.90; if (valid_approach_percentage == 1 && valid_lift_retreat_percentage >= min_allowed_valid_lift_retreat_percentage) { - ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "Found valid cartesian path"); + ROS_DEBUG_STREAM_NAMED(logger_name, "Found valid cartesian path"); valid_path_found = true; break; } @@ -383,8 +388,8 @@ bool GraspPlanner::computeCartesianWaypointPath(GraspCandidatePtr& grasp_candida if (!valid_path_found) { - ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "UNABLE to find valid waypoint cartesian path after " - << MAX_IK_ATTEMPTS << " attempts"); + ROS_DEBUG_STREAM_NAMED(logger_name, "UNABLE to find valid waypoint cartesian path after " << MAX_IK_ATTEMPTS + << " attempts"); return false; } diff --git a/src/suction_grasp_filter.cpp b/src/suction_grasp_filter.cpp index 5167dcb..4060af4 100644 --- a/src/suction_grasp_filter.cpp +++ b/src/suction_grasp_filter.cpp @@ -32,10 +32,13 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Dave Coleman - Desc: Filters grasps based on kinematic feasibility and collision +/* Author: Michael Lautman + Desc: Filters suction grasps based on kinematic feasibility and collision */ +// Std lib +#include // any_of + // moveit_grasps #include #include @@ -56,56 +59,24 @@ namespace moveit_grasps // Constructor SuctionGraspFilter::SuctionGraspFilter(const robot_state::RobotStatePtr& robot_state, const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools) - : GraspFilter::GraspFilter(robot_state, visual_tools), name_("suction_grasp_filter"), suction_voxel_overlap_cutoff_(0) + : GraspFilter::GraspFilter(robot_state, visual_tools) + , name_("suction_grasp_filter") + , suction_voxel_overlap_cutoff_(-1) { } -bool SuctionGraspFilter::filterGraspsBySuctionVoxelOverlapCutoff(std::vector& grasp_candidates) +bool SuctionGraspFilter::filterGraspBySuctionVoxelOverlapCutoff( + const SuctionGraspCandidatePtr& suction_grasp_candidate) const { - static const std::string logger_name = name_ + ".filter_grasps_by_suction_voxel_overlap"; + std::vector suction_voxel_enabled = + suction_grasp_candidate->getSuctionVoxelEnabled(suction_voxel_overlap_cutoff_); - // Pre-filter for suction voxel overlap - std::size_t count = 0; - std::size_t valid_grasps = 0; - for (std::size_t ix = 0; ix < grasp_candidates.size(); ++ix) + if (std::none_of(suction_voxel_enabled.begin(), suction_voxel_enabled.end(), [](bool enabled) { return enabled; })) { - ROS_DEBUG_STREAM_NAMED(logger_name, "---------------\nGrasp candidate: " << count++); - bool valid = false; - auto suction_grasp_candidate = std::dynamic_pointer_cast(grasp_candidates[ix]); - if (!suction_grasp_candidate) - { - ROS_ERROR_NAMED(logger_name, "grasp_candidate is not castable as SuctionGraspCandidatePtr"); - return 0; - } - - std::vector suction_voxel_enabled = - suction_grasp_candidate->getSuctionVoxelEnabled(suction_voxel_overlap_cutoff_); - for (const bool& voxel_enabled : suction_voxel_enabled) - { - if (voxel_enabled) - { - valid = true; - ++valid_grasps; - break; - } - } - if (!valid) - { - grasp_candidates[ix]->grasp_filtered_code_ = SuctionGraspFilterCode::GRASP_FILTERED_BY_SUCTION_VOXEL_OVERLAP; - } + suction_grasp_candidate->grasp_filtered_code_ = SuctionGraspFilterCode::GRASP_FILTERED_BY_SUCTION_VOXEL_OVERLAP; + return true; } - return valid_grasps; -} - -std::size_t SuctionGraspFilter::filterGraspsHelper(std::vector& grasp_candidates, - const planning_scene::PlanningScenePtr& planning_scene, - const robot_model::JointModelGroup* arm_jmg, - const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp, - bool visualize, const std::string& target_object_id) -{ - filterGraspsBySuctionVoxelOverlapCutoff(grasp_candidates); - return GraspFilter::filterGraspsHelper(grasp_candidates, planning_scene, arm_jmg, seed_state, filter_pregrasp, - visualize, target_object_id); + return false; } void SuctionGraspFilter::printFilterStatistics(const std::vector& grasp_candidates) const @@ -139,20 +110,26 @@ bool SuctionGraspFilter::processCandidateGrasp(const IkThreadStructPtr& ik_threa // Helper pointer GraspCandidatePtr& grasp_candidate = ik_thread_struct->grasp_candidates_[ik_thread_struct->grasp_id]; - auto suction_grasp_data = std::dynamic_pointer_cast(grasp_candidate->grasp_data_); - if (!suction_grasp_data) + auto suction_grasp_candidate = std::dynamic_pointer_cast(grasp_candidate); + if (!suction_grasp_candidate) { - ROS_ERROR_STREAM_NAMED(logger_name, "Could not cast GraspCandidatePtr->GraspDataPtr as " - "SuctionGraspDataPtr"); + ROS_ERROR_STREAM_NAMED(logger_name, "Could not cast GraspCandidatePtr as " + "SuctionGraspCandidatePtr"); grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_INVALID; return false; } - auto suction_grasp_candidate = std::dynamic_pointer_cast(grasp_candidate); - if (!suction_grasp_candidate) + // filter by suction voxel overlap cutoff + if (filterGraspBySuctionVoxelOverlapCutoff(suction_grasp_candidate)) { - ROS_ERROR_STREAM_NAMED(logger_name, "Could not cast GraspCandidatePtr as " - "SuctionGraspCandidatePtr"); + return false; + } + + auto suction_grasp_data = std::dynamic_pointer_cast(grasp_candidate->grasp_data_); + if (!suction_grasp_data) + { + ROS_ERROR_STREAM_NAMED(logger_name, "Could not cast GraspCandidatePtr->GraspDataPtr as " + "SuctionGraspDataPtr"); grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_INVALID; return false; } @@ -183,13 +160,6 @@ bool SuctionGraspFilter::processCandidateGrasp(const IkThreadStructPtr& ik_threa ik_thread_struct->planning_scene_); } - if (!removeAllSuctionCupCO(suction_grasp_data, ik_thread_struct->planning_scene_)) - { - ROS_ERROR_STREAM_NAMED(logger_name, "Failed to detach all active suction cups"); - grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_INVALID; - return false; - } - if (!filer_results) { ROS_DEBUG_STREAM_NAMED(logger_name, "Candidate grasp invalid");