atenpas / gpd

Detect 6-DOF grasp poses in point clouds
BSD 2-Clause "Simplified" License
598 stars 233 forks source link

How to change the configuration so only the grasp from above are selected ??? #110

Closed gachiemchiep closed 3 years ago

gachiemchiep commented 3 years ago

Hello guys. @atenpas @sisco0

Sorry for another silly question.

We're using gpd as a ros node. The gpd_ros node read pointcloud from realsense camera. Output on rviz look like this.

gpd_01

But we want grasps to be detected similar to this.

ros2_grasp_library

We checked the code of ros2_grasp_library and found that they're preprocessing the pointcloud as : filter_workspace -> remove_plane -> voxelize -> normalization . And in this gpd the preprocessing is as : removeNans -> filter_workspace -> voxelize -> normalization.

The sampleAbovePlane method inside cloud.cpp are exactly the same as remove_plane method of ros2_grasp_library. So we enabled "sample_above_plane = 1" inside config file, but a segmentation fault occured and the node dies.

void Cloud::sampleAbovePlane() {
  double t0 = omp_get_wtime();
  printf("Sampling above plane ...\n");
  std::vector<int> indices(0);
  pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  seg.setInputCloud(cloud_processed_);
  seg.setOptimizeCoefficients(true);
  seg.setModelType(pcl::SACMODEL_PLANE);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setDistanceThreshold(0.01);
// This line caused the problem
  seg.segment(*inliers, *coefficients);

--> we switched from Ubuntu 16.04 into Ubuntu 18.04 and the segmentation fault disappeared. I guess this problem is related to pcl. so we will deal with it by ourself.

The unsolved problem is how to change the configuration so only the detected grasp from above are generated as this image

ros2_grasp_library

Would you mind take a look at this and help us overcome this. Thanks

sisco0 commented 3 years ago

Related code: https://github.com/atenpas/gpd/blob/96fc9bcf17ee6669cabf9cd0e66f8e62ba617e08/src/gpd/util/cloud.cpp#L407-L435

atmagopal commented 3 years ago

@gachiemchiep You could change the direction parameter in the .cfg file that you are using with GPD. For example, in eigen_params.cfg, the approach direction filter parameters are:

# Filtering of candidates based on their approach direction
#   filter_approach_direction: turn filtering on/off
#   direction: direction to compare against
#   angle_thresh: angle in radians above which grasps are filtered
filter_approach_direction = 1
direction = 1 0 0
thresh_rad = 2.0

Mind you, the direction vector values are set relative to the camera coordinate frame. So in your case I'd assume direction = 0 0 1. Please try this.

To see how the approach direction filter works: https://github.com/atenpas/gpd/blob/06f029f1eb5bf508b5a9a1745e08be243ffad42f/src/gpd/grasp_detector.cpp#L422-L450

atenpas commented 3 years ago

@atmagopal's answer is the right way to go. Just as a small tip: it might be easier to use the filter if your point cloud has already been transformed into a frame that is related to how the robot approaches the object. For example, in a typical ROS robot frame, x is forward and direction would just be a simple unit vector. Then you could use this filter to avoid grasping objects from behind (from the viewpoint of the robot).

atmagopal commented 3 years ago

@gachiemchiep Transforming the point cloud frame would also require transforming the camera source position. Just wanted to remark that. Please correct if I'm wrong.

gachiemchiep commented 3 years ago

@atenpas Our robot will approach the object from above (as same as the z axis (blue color one)). Thanks for the tip.

@atmagopal I used your option and now only the grasp from above are selected

The configuration file : https://gist.github.com/gachiemchiep/600afb8b0f09abdff934abe70bae82ee

Unfortunately, the grasp are only available the our plastic duck, for the remote controller and the driver, the grasp aren't available. ... image

Inside the all_axes_vino_12channels.cfg, I found these parameters :

hand_axes = 0 1 2
deepen_hand = 2

# Filtering of grasps which are too low on (i.e. too close to) support surface (e.g., table or floor)
#   filter_table_side_grasps: turn this filter on/off
#   vertical_axis: the vertical axis in the point cloud frame
#   angle_thresh: threshold to determine which grasps are considered to be side grasps
#   table_height: the height of the support surface (along vertical axis)
#   table_thresh: thresold below which grasps are considered to close too the support surface
filter_table_side_grasps = 1
vertical_axis = 1 0 0
angle_thresh = 0.1
table_height = 0.0
table_thresh = 0.05

Should I modify those abve values too?

atmagopal commented 3 years ago

@gachiemchiep I find the param files and code well-documented. You should feel free to make the changes and test. In your case, I would reduce the table_tresh value it to generate grasp on the objects closer to the surface, or disable the filter even.

But meanwhile, if you think the issue you raised here has been solved, please close this issue. :)

atenpas commented 3 years ago

@gachiemchiep Please open a separate issue for the next problem. (I suspect it has to do with the samples -- do you feed the whole point cloud to GPD?)

gachiemchiep commented 3 years ago

@atmagopal @atenpas you guys are right. I will close this issue.

@atenpas Yes, I feed the whole point cloud to GPD.