(WIP) Disable individual suction voxels from collision

include/moveit_grasps/suction_grasp_candidate.h

@@ -65,15 +65,22 @@ class SuctionGraspCandidate : public GraspCandidate
 
   void setSuctionVoxelOverlap(std::vector<double> suction_voxel_overlap);
 
+  void setSuctionVoxelInCollision(std::vector<bool> voxel_in_collision);
+
   std::vector<double> getSuctionVoxelOverlap();
 
   std::vector<bool> getSuctionVoxelEnabled(double cutoff);
 
+  std::vector<bool> 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<double> 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<bool> voxel_in_collision_;
 
 };  // class
 

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<bool>& suction_voxel_enabled,
-                                const planning_scene::PlanningScenePtr& planning_scene,
-                                std::vector<std::string>& collision_object_names);
+  bool attachAllSuctionCupCO(const SuctionGraspDataPtr& grasp_data,
+                             const planning_scene::PlanningScenePtr& planning_scene,
+                             std::vector<std::string>& collision_object_names);
 
   /* \brief a method for transforming from voxel index to voxel collision object ID used by attachActiveSuctionCupCO and
    * removeAllSuctionCupCO */

src/demo/suction_grasp_pipeline_demo.cpp

@@ -60,6 +60,7 @@
 // Parameter loading
 #include <rosparam_shortcuts/rosparam_shortcuts.h>
 
+#include <tf2_eigen/tf2_eigen.h>
 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_model_loader::RobotModelLoader>("robot_description");
+    robot_model_loader_ = std::make_shared<robot_model_loader::RobotModelLoader>("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_;
 

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<GraspData>(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<double> SuctionGraspCandidate::getSuctionVoxelOverlap()
   return suction_voxel_overlap_;
 }
 
+void SuctionGraspCandidate::setSuctionVoxelInCollision(std::vector<bool> voxel_in_collision)
+{
+  voxel_in_collision_ = voxel_in_collision;
+}
+
+std::vector<bool> SuctionGraspCandidate::getSuctionVoxelInCollision()
+{
+  return voxel_in_collision_;
+}
+
 std::vector<bool> SuctionGraspCandidate::getSuctionVoxelEnabled(double suction_voxel_cutoff)
 {
   std::vector<bool> 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;
 }