diff --git a/include/moveit_grasps/suction_grasp_candidate.h b/include/moveit_grasps/suction_grasp_candidate.h index 5e89e89..5342dee 100644 --- a/include/moveit_grasps/suction_grasp_candidate.h +++ b/include/moveit_grasps/suction_grasp_candidate.h @@ -65,15 +65,22 @@ class SuctionGraspCandidate : public GraspCandidate void setSuctionVoxelOverlap(std::vector suction_voxel_overlap); + void setSuctionVoxelInCollision(std::vector voxel_in_collision); + std::vector getSuctionVoxelOverlap(); std::vector getSuctionVoxelEnabled(double cutoff); + std::vector getSuctionVoxelInCollision(); + EIGEN_MAKE_ALIGNED_OPERATOR_NEW protected: // A vector of fractions maped to suction gripper voxels. [0,1] representing the fraction of the // suction voxel that overlaps the object std::vector suction_voxel_overlap_; + // Whether the voxel is colliding with any objects other than the target. + // If it is, then using the voxel risks attaching to other unintended objects + std::vector voxel_in_collision_; }; // class diff --git a/include/moveit_grasps/suction_grasp_filter.h b/include/moveit_grasps/suction_grasp_filter.h index a88b63e..203e215 100644 --- a/include/moveit_grasps/suction_grasp_filter.h +++ b/include/moveit_grasps/suction_grasp_filter.h @@ -109,15 +109,15 @@ class SuctionGraspFilter : public GraspFilter const planning_scene::PlanningScenePtr& planning_scene); /** - * \brief Add a collision object in the location of every active suction voxel. This is used for collision + * \brief Add a collision object in the location of active suction voxel. This is used for collision * checking between the suction cups and other objects in the scene that you do not want to pick up * \param grasp_data - A pointer to a SuctionGraspData with a populated SuctionVoxelMatrix * \param planning_scene - A pointer to a planning scene where we will attach the collision objects * \param collision_object_names - Output, the collision object names added to the planning scene */ - bool attachActiveSuctionCupCO(const SuctionGraspDataPtr& grasp_data, const std::vector& suction_voxel_enabled, - const planning_scene::PlanningScenePtr& planning_scene, - std::vector& collision_object_names); + bool attachAllSuctionCupCO(const SuctionGraspDataPtr& grasp_data, + const planning_scene::PlanningScenePtr& planning_scene, + std::vector& collision_object_names); /* \brief a method for transforming from voxel index to voxel collision object ID used by attachActiveSuctionCupCO and * removeAllSuctionCupCO */ diff --git a/src/demo/suction_grasp_pipeline_demo.cpp b/src/demo/suction_grasp_pipeline_demo.cpp index 6af4afa..5e803c2 100644 --- a/src/demo/suction_grasp_pipeline_demo.cpp +++ b/src/demo/suction_grasp_pipeline_demo.cpp @@ -60,6 +60,7 @@ // Parameter loading #include +#include namespace moveit_grasps_demo { static const std::string LOGNAME = "grasp_pipeline_demo"; @@ -114,11 +115,10 @@ class GraspPipelineDemo "grasping_planning_scene"); planning_scene_monitor_->getPlanningScene()->setName("grasping_planning_scene"); - robot_model_loader::RobotModelLoaderPtr robot_model_loader; - robot_model_loader = std::make_shared("robot_description"); + robot_model_loader_ = std::make_shared("robot_description"); // Load the robot model - robot_model_ = robot_model_loader->getModel(); + robot_model_ = robot_model_loader_->getModel(); arm_jmg_ = robot_model_->getJointModelGroup(planning_group_name_); // --------------------------------------------------------------------------------------------- @@ -413,9 +413,9 @@ class GraspPipelineDemo double& y_width, double& z_height) { // Generate random cuboid - double xmin = 0.125; + double xmin = 0.225; double xmax = 0.250; - double ymin = 0.125; + double ymin = 0.225; double ymax = 0.250; double zmin = 0.200; double zmax = 0.500; @@ -437,14 +437,22 @@ class GraspPipelineDemo object_name = "pick_target"; visual_tools_->generateRandomCuboid(object_pose, x_depth, y_width, z_height, pose_bounds, cuboid_bounds); - visual_tools_->publishCollisionCuboid(object_pose, x_depth, y_width, z_height, object_name, rviz_visual_tools::RED); + visual_tools_->publishCollisionCuboid(object_pose, x_depth, y_width, z_height, object_name, rviz_visual_tools::BLUE); visual_tools_->publishAxis(object_pose, rviz_visual_tools::MEDIUM); visual_tools_->trigger(); + Eigen::Isometry3d pose_eigen; + Eigen::fromMsg(object_pose, pose_eigen); + visual_tools_->publishCollisionCuboid(pose_eigen * Eigen::Translation3d(x_depth, 0.0, 0.0), x_depth, y_width, z_height, "box_plus_x", rviz_visual_tools::RED); + visual_tools_->publishCollisionCuboid(pose_eigen * Eigen::Translation3d(-x_depth, 0.0, 0.0), x_depth, y_width, z_height, "box_minus_x", rviz_visual_tools::RED); + visual_tools_->publishCollisionCuboid(pose_eigen * Eigen::Translation3d(0.0, y_width, 0.0), x_depth, y_width, z_height, "box_plus_y", rviz_visual_tools::RED); + visual_tools_->publishCollisionCuboid(pose_eigen * Eigen::Translation3d(0.0, -y_width, 0.0), x_depth, y_width, z_height, "box_minus_y", rviz_visual_tools::RED); + visual_tools_->trigger(); + bool success = true; double timeout = 5; // seconds ros::Rate rate(100); - while (success && !planning_scene_monitor_->getPlanningScene()->knowsFrameTransform(object_name)) + while (success && !planning_scene_monitor_->getPlanningScene()->knowsFrameTransform(object_name) && !planning_scene_monitor_->getPlanningScene()->knowsFrameTransform("box_minus_y")) { rate.sleep(); success = rate.cycleTime().toSec() < timeout; @@ -456,6 +464,9 @@ class GraspPipelineDemo // A shared node handle ros::NodeHandle nh_; + // Robot model loader + robot_model_loader::RobotModelLoaderPtr robot_model_loader_; + // Tool for visualizing things in Rviz moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; diff --git a/src/suction_grasp_candidate.cpp b/src/suction_grasp_candidate.cpp index 4c26564..dbb86e9 100644 --- a/src/suction_grasp_candidate.cpp +++ b/src/suction_grasp_candidate.cpp @@ -44,6 +44,7 @@ SuctionGraspCandidate::SuctionGraspCandidate(const moveit_msgs::Grasp& grasp, co const Eigen::Isometry3d& cuboid_pose) : GraspCandidate::GraspCandidate(grasp, std::dynamic_pointer_cast(grasp_data), cuboid_pose) , suction_voxel_overlap_(grasp_data->suction_voxel_matrix_->getNumVoxels()) + , voxel_in_collision_(grasp_data->suction_voxel_matrix_->getNumVoxels()) { } @@ -57,11 +58,24 @@ std::vector SuctionGraspCandidate::getSuctionVoxelOverlap() return suction_voxel_overlap_; } +void SuctionGraspCandidate::setSuctionVoxelInCollision(std::vector voxel_in_collision) +{ + voxel_in_collision_ = voxel_in_collision; +} + +std::vector SuctionGraspCandidate::getSuctionVoxelInCollision() +{ + return voxel_in_collision_; +} + std::vector SuctionGraspCandidate::getSuctionVoxelEnabled(double suction_voxel_cutoff) { std::vector suction_voxel_enabled(suction_voxel_overlap_.size()); for (std::size_t voxel_ix = 0; voxel_ix < suction_voxel_enabled.size(); ++voxel_ix) - suction_voxel_enabled[voxel_ix] = suction_voxel_overlap_[voxel_ix] >= suction_voxel_cutoff; + { + suction_voxel_enabled[voxel_ix] = + (suction_voxel_overlap_[voxel_ix] >= suction_voxel_cutoff && !voxel_in_collision_[voxel_ix]); + } return suction_voxel_enabled; } diff --git a/src/suction_grasp_filter.cpp b/src/suction_grasp_filter.cpp index 8b7700a..fe81bd7 100644 --- a/src/suction_grasp_filter.cpp +++ b/src/suction_grasp_filter.cpp @@ -103,9 +103,11 @@ std::size_t SuctionGraspFilter::filterGraspsHelper(std::vector& grasp_candidates) const @@ -157,42 +159,96 @@ bool SuctionGraspFilter::processCandidateGrasp(const IkThreadStructPtr& ik_threa return false; } + // First filter without taking suction pads into account. + if (!GraspFilter::processCandidateGrasp(ik_thread_struct)) + { + ROS_DEBUG_STREAM_NAMED(logger_name, "Candidate grasp invalid"); + return false; + } + + // Create an AllowedCollisionMatrix with all collisions allowed. + // We can then add the suction voxels and check collision results to see which suction voxels must be disabled + // We first copy the matrix from the planning scene to ensure it has the robot links in it + collision_detection::AllowedCollisionMatrix suction_acm( + ik_thread_struct->planning_scene_->getAllowedCollisionMatrix()); + + // We must add entries for all world objects that may not be in the collision matrix yet, and then disable collision + // checking for all + for (const std::string& world_object : ik_thread_struct->planning_scene_->getWorld()->getObjectIds()) + { + suction_acm.setEntry(world_object, true); + } + suction_acm.setEntry(true); + std::vector collision_object_names; - if (!attachActiveSuctionCupCO(suction_grasp_data, - suction_grasp_candidate->getSuctionVoxelEnabled(suction_voxel_overlap_cutoff_), - ik_thread_struct->planning_scene_, collision_object_names)) + if (!attachAllSuctionCupCO(suction_grasp_data, ik_thread_struct->planning_scene_, collision_object_names)) { - ROS_ERROR_STREAM_NAMED(logger_name, "Failed to attch active suction cups as collision objects in the planning " + ROS_ERROR_STREAM_NAMED(logger_name, "Failed to attach active suction cups as collision objects in the planning " "scene"); grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_INVALID; return false; } + // Since we just added the suction voxels as an ACO, the default is to check collision with all world objects. + // We disable collision checking just for the box we want to pick if (!ik_thread_struct->grasp_target_object_id_.empty() && !collision_object_names.empty()) { - setACMFingerEntry(ik_thread_struct->grasp_target_object_id_, true, collision_object_names, - ik_thread_struct->planning_scene_); + ROS_DEBUG_STREAM_NAMED(logger_name, "Allowing collisions between suction voxels and " + << ik_thread_struct->grasp_target_object_id_); + for (const auto& voxel_name : collision_object_names) + { + suction_acm.setEntry(ik_thread_struct->grasp_target_object_id_, voxel_name, true); + } } - bool filter_results = GraspFilter::processCandidateGrasp(ik_thread_struct); + robot_state::RobotState grasp_state = ik_thread_struct->planning_scene_->getCurrentState(); + grasp_state.setJointGroupPositions(grasp_candidate->grasp_data_->arm_jmg_, grasp_candidate->grasp_ik_solution_); + grasp_state.update(); + collision_detection::CollisionResult::ContactMap contacts; + ik_thread_struct->planning_scene_->getCollidingPairs(contacts, grasp_state, suction_acm); - // Cleanup ACM changes - if (!ik_thread_struct->grasp_target_object_id_.empty() && !collision_object_names.empty()) + std::vector voxel_in_collision(suction_grasp_data->suction_voxel_matrix_->getNumVoxels()); + + for (auto& contact_collection : contacts) { - setACMFingerEntry(ik_thread_struct->grasp_target_object_id_, false, collision_object_names, - ik_thread_struct->planning_scene_); + for (auto& contact_pair : contact_collection.second) + { + for (std::size_t voxel_ix = 0; voxel_ix < suction_grasp_data->suction_voxel_matrix_->getNumVoxels(); ++voxel_ix) + { + std::string voxel_id = suctionVoxelIxToCollisionObjectId(voxel_ix); + if (contact_pair.body_name_1 == voxel_id || contact_pair.body_name_2 == voxel_id) + { + voxel_in_collision[voxel_ix] = true; + } + } + ROS_DEBUG_STREAM_NAMED(logger_name, "Contact between " << contact_pair.body_name_1 << " and " + << contact_pair.body_name_2); + } } - if (!removeAllSuctionCupCO(suction_grasp_data, ik_thread_struct->planning_scene_)) + suction_grasp_candidate->setSuctionVoxelInCollision(voxel_in_collision); + + if (std::find(voxel_in_collision.begin(), voxel_in_collision.end(), false) == voxel_in_collision.end()) { - ROS_ERROR_STREAM_NAMED(logger_name, "Failed to detach all active suction cups"); - grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_INVALID; + ROS_DEBUG_NAMED(logger_name, "All voxels filtered for candidate grasp"); + grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_IK; return false; } - if (!filter_results) + // Debugging code + if (false) { - ROS_DEBUG_STREAM_NAMED(logger_name, "Candidate grasp invalid"); + moveit_msgs::DisplayRobotState display_robot_state_msg; + robot_state::robotStateToRobotStateMsg(grasp_state, display_robot_state_msg.state, true); + visual_tools_->publishRobotState(display_robot_state_msg); + visual_tools_->trigger(); + } + // End debugging code + + 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; } @@ -242,7 +298,6 @@ bool SuctionGraspFilter::removeAllSuctionCupCO(const SuctionGraspDataPtr& grasp_ { // Set the aco name suction_voxel_co.id = suctionVoxelIxToCollisionObjectId(voxel_ix); - setACMFingerEntry(suction_voxel_co.id, false, ee_links, planning_scene); // Check if the ACO already exists moveit_msgs::AttachedCollisionObject aco; @@ -255,15 +310,6 @@ bool SuctionGraspFilter::removeAllSuctionCupCO(const SuctionGraspDataPtr& grasp_ ROS_WARN_STREAM_NAMED(logger_name, "Failed to processACOMsg for: " << suction_voxel_aco.object.id); return false; } - - // Check if the collision object was successfully detached - aco = moveit_msgs::AttachedCollisionObject(); - if (planning_scene->getAttachedCollisionObjectMsg(aco, suction_voxel_aco.object.id)) - { - ROS_WARN_STREAM_NAMED(logger_name, "Failed to detach object: " << suction_voxel_aco.object.id); - ROS_DEBUG_STREAM_NAMED(logger_name, "Found : \n" << aco); - return false; - } } // Check if the collision object exists and remove it moveit_msgs::CollisionObject co; @@ -276,24 +322,14 @@ bool SuctionGraspFilter::removeAllSuctionCupCO(const SuctionGraspDataPtr& grasp_ ROS_WARN_STREAM_NAMED(logger_name, "Failed to processCollisionObjectMsg for: " << suction_voxel_co.id); return false; } - - // Check to make sure the object is no longer in the planning scene - co = moveit_msgs::CollisionObject(); - if (planning_scene->getCollisionObjectMsg(co, suction_voxel_co.id)) - { - ROS_WARN_STREAM_NAMED(logger_name, "Failed to remove object " << suction_voxel_co.id << " from planning scene"); - ROS_DEBUG_STREAM_NAMED(logger_name, "Found : \n" << co); - return false; - } } } return true; } -bool SuctionGraspFilter::attachActiveSuctionCupCO(const SuctionGraspDataPtr& grasp_data, - const std::vector& suction_voxel_enabled, - const planning_scene::PlanningScenePtr& planning_scene, - std::vector& collision_object_names) +bool SuctionGraspFilter::attachAllSuctionCupCO(const SuctionGraspDataPtr& grasp_data, + const planning_scene::PlanningScenePtr& planning_scene, + std::vector& collision_object_names) { static const std::string logger_name = name_ + ".attach_active_suction_cup_co"; @@ -315,8 +351,8 @@ bool SuctionGraspFilter::attachActiveSuctionCupCO(const SuctionGraspDataPtr& gra collision_object_names.resize(num_voxels); ROS_DEBUG_STREAM_NAMED(logger_name, "~~~~~~~~~~~"); - for (std::size_t ix = 0; ix < suction_voxel_enabled.size(); ++ix) - ROS_DEBUG_STREAM_NAMED(logger_name, "voxel_" << ix << ":\t" << suction_voxel_enabled[ix]); + for (std::size_t ix = 0; ix < num_voxels; ++ix) + ROS_DEBUG_STREAM_NAMED(logger_name, "voxel_" << ix); // Get EE_JMG link names for setting ACM enabled / disabled std::vector ee_links = grasp_data->ee_jmg_->getLinkModelNames(); @@ -331,7 +367,6 @@ bool SuctionGraspFilter::attachActiveSuctionCupCO(const SuctionGraspDataPtr& gra } // Assign collision object names for output collision_object_names[voxel_ix] = suctionVoxelIxToCollisionObjectId(voxel_ix); - setACMFingerEntry(collision_object_names[voxel_ix], true, ee_links, planning_scene); // Create an AttachedCollisionObject moveit_msgs::AttachedCollisionObject suction_voxel_aco; @@ -349,9 +384,11 @@ bool SuctionGraspFilter::attachActiveSuctionCupCO(const SuctionGraspDataPtr& gra suction_voxel_co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = suction_voxel->x_width_; suction_voxel_co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = suction_voxel->y_width_; suction_voxel_co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = grasp_data->grasp_max_depth_; + suction_voxel_co.operation = moveit_msgs::CollisionObject::ADD; // The set the attached object pose suction_voxel_co.primitive_poses.resize(1); + // TODO: Check that the last translation here is the right direction (it seems maybe wrong in rviz on demo) Eigen::Isometry3d ik_link_to_voxel_center = ik_link_to_tcp * Eigen::Translation3d(suction_voxel->center_point_) * Eigen::Translation3d(0, 0, -grasp_data->grasp_max_depth_ / 2.0 + .01); suction_voxel_co.primitive_poses[0] = tf2::toMsg(ik_link_to_voxel_center); @@ -364,11 +401,8 @@ bool SuctionGraspFilter::attachActiveSuctionCupCO(const SuctionGraspDataPtr& gra bool aco_exists = planning_scene->getAttachedCollisionObjectMsg(aco, suction_voxel_aco.object.id); // If the suction voxel is not attached to the robot in the planning scene but should be - if (suction_voxel_enabled[voxel_ix] && !aco_exists) + if (!aco_exists) { - // Mark object to be added - suction_voxel_co.operation = moveit_msgs::CollisionObject::ADD; - // Attach the collision object if (!planning_scene->processAttachedCollisionObjectMsg(suction_voxel_aco)) { @@ -376,52 +410,6 @@ bool SuctionGraspFilter::attachActiveSuctionCupCO(const SuctionGraspDataPtr& gra logger_name, "Failed to process processAttachedCollisionObjectMsg for: " << suction_voxel_aco.object.id); return false; } - - // Check if the collision object was successfully attached - if (!planning_scene->getAttachedCollisionObjectMsg(aco, suction_voxel_aco.object.id)) - { - ROS_WARN_STREAM_NAMED(logger_name, "Object: " << suction_voxel_aco.object.id << " not attached to the robot"); - return false; - } - } - // If the suction voxel is attached to the robot in the planning scene but shouldn't be - else if (!suction_voxel_enabled[voxel_ix] && aco_exists) - { - // Mark object to be removed - suction_voxel_co.operation = moveit_msgs::CollisionObject::REMOVE; - - // Dettach the collision object - if (!planning_scene->processAttachedCollisionObjectMsg(suction_voxel_aco)) - { - ROS_WARN_STREAM_NAMED(logger_name, - "Failed to processAttachedCollisionObjectMsg for: " << suction_voxel_aco.object.id); - return false; - } - - // Check if the collision object was successfully detached - if (planning_scene->getAttachedCollisionObjectMsg(aco, suction_voxel_aco.object.id)) - { - ROS_WARN_STREAM_NAMED(logger_name, "Failed to detach object: " << suction_voxel_aco.object.id); - ROS_DEBUG_STREAM_NAMED(logger_name, "Found : \n" << aco); - return false; - } - - // Remove Collision object from planning scene - if (!planning_scene->processCollisionObjectMsg(suction_voxel_co)) - { - ROS_WARN_STREAM_NAMED(logger_name, - "Failed to process collision object msg for: " << suction_voxel_aco.object.id); - return false; - } - - // Check to make sure the object is no longer in the planning scene - moveit_msgs::CollisionObject co; - if (planning_scene->getCollisionObjectMsg(co, suction_voxel_co.id)) - { - ROS_WARN_STREAM_NAMED(logger_name, "Failed to remove object " << suction_voxel_co.id << " from planning scene"); - ROS_DEBUG_STREAM_NAMED(logger_name, "Found : \n" << co); - return false; - } } } diff --git a/test/suction_grasp_unit_tests.cpp b/test/suction_grasp_unit_tests.cpp index a27f796..94bcbf6 100644 --- a/test/suction_grasp_unit_tests.cpp +++ b/test/suction_grasp_unit_tests.cpp @@ -351,7 +351,7 @@ TEST_F(SuctionGraspUnitTests, TestFilterSuctionIKHighOverlap) // Ensure that we do not filter all grasps when trying to pick up an object larger than a suction voxel and surrounded // on all sides -TEST_F(SuctionGraspUnitTests, DISABLED_TestFilterSuctionIKLowOverlap) +TEST_F(SuctionGraspUnitTests, TestFilterSuctionIKLowOverlap) { // This test is identical to TestFilterSuctionIKHighOverlap except with a lower overlap cutoff // -----------------------------------