melodic noetic   Show EOL distros: 

DOCUMENTATION

This package calculates graspable regions at unknown (or known) objects.

HAF_GRASPING calculates grasp points for unknown and known objects represented by object point cloud data. For scientific foundation see: '''D. Fischinger, A. Weiss, M. Vincze: "Learning Grasps with Topographic Features", The International Journal of Robotics Research. (2015)'''

SIMPLE START: HOW TO USE HAF_GRASPING

This part shows how to get the code running and visualizes the grasp calculation for a point cloud in RVIZ:

CREATE CATKIN WORKSPACE

>> mkdir -p ~/haf_grasping_ws/src
>> cd ~/haf_grasping_ws/src
>> catkin_init_workspace

OR navigate to an existing *src* folder of your ROS workspace:

cd location_of_workspace/src

DOWNLOAD CODE (and add the path to the environment variable ROS_PACKAGE_PATH if needed)

>> git clone https://github.com/davidfischinger/haf_grasping.git

Execute "make" in the libsvm folder:

>> roscd haf_grasping/libsvm-3.12
>> make

CATKIN_MAKE OF PACKAGE HAF_GRASPING

>> roscd haf_grasping/../..
>> catkin_make -DCMAKE_BUILD_TYPE=Release
>> source devel/setup.sh

Start calculation server (does the work), haf_client (small program including the class that shows how to use haf_grasping) and a visualization in rviz:

>> roslaunch haf_grasping haf_grasping_all.launch

Publish the path of a point cloud to calculate grasp points on this object with the gripper approaching direction along the z-axis:

>> rostopic pub /haf_grasping/input_pcd_rcs_path std_msgs/String "$(rospack find haf_grasping)/data/pcd2.pcd" -1

Alternatively, publish a point cloud to the ROS topic: /haf_grasping/depth_registered/single_cloud/points_in_lcs:

>> roscd haf_grasping/data
>> rosrun pcl_ros pcd_to_pointcloud pcd2.pcd cloud_pcd:=/haf_grasping/depth_registered/single_cloud/points_in_lcs

EXPLANATION FOR THE RVIZ VISUALIZATION

RVIZ will now visualize the point cloud with its corresponding frame (blue indicates the z-axis).

Bigger grey rectangle: indicates the area where heights can be used for grasp calculation

Inner grey rectangle: defines the area which is searched for potential grasps (grasp centers).

Long red line: indicates the closing direction (for a two finger gripper)

Red/green spots: indicate the positions where grasps are really tested for the current gripper roll (ignoring points where no calculation is needed, e.g. no data there)

Green bars: indicate identified potential grasps available. The height of the bars indicates the grasp evaluation score (the higher the better)

Black arrow: indicates the best grasp position found and the approaching direction (for a parallel two finger gripper)

Visualization of the grasp calculation process in rviz:

Red line indicates currently tested gripper opening direction, red points indicate positions where no grasp was found, green bars indicate positions where grasps were found - the higher the bar, the better the quality.

Finally the top grasp (position and approaching direction of the gripper) is shown by the black arrow together with the gripper orientation (red line).

ROS API

ACTION SERVER

The package haf_grasping provides one SimpleActionServer:

• /calc_grasppoints_svm_action_server: requests the calculation of a grasp for a given point cloud. A simple implementation of the client can be found here: calc_grasppoints_action_client.cpp: :

  • action definition:

    • goal: GraspInput.msg defined by a point cloud and a number of settings for the action server

    • result: std_msgs/String defining the best grasp found (see below)
    • feedback: std_msgs/String
  • the object must have been previously segmented from the background
  • the action will then:
    • calculate the best grasp for a parallel gripper approaching the tool center point along the z-axis of the coordinate frame.

PARAMETER SETTING AND PROVIDED SERVICES

There are a number of parameters that can be set in the launch file haf_grasping_all.launch or via a service call.

Approach vector: x,y,z-direction of the approach vector. The tool center point of a gripper approaches the object in the opposite direction of this vector. For the default vector (0,0,1) the gripper is going down the z-axis towards the origin. Service to set the approach vector to (1,1,1):

>> rosservice call /haf_grasping/set_approach_vector "approach_vector:  x: 1.0
  y: 1.0
  z: 1.0"

Grasp_center: the x-,y-,z-position, that is the center of the area where grasps are searched. Set grasp search center (in m) to (x=0.1,y=0,z=0) with a service call:

>> rosservice call /haf_grasping/set_grasp_center "graspsearchcenter:
   x: 0.10
   y: 0.0
   z: 0.0"

Grasp_area_size: the size of the area where grasps should be detected. Set rectangle to 16x10 centimeter:

>> rosservice call /haf_grasping/set_grasp_search_area_size "grasp_search_size_x: 16 grasp_search_size_y: 10"

Grasp_calculation_time_max: maximal time in seconds until a grasp has to be returned. Set max time to 3 sec:

>> rosservice call /haf_grasping/set_grasp_calculation_time_max "max_calculation_time:
  secs: 3
  nsecs: 0"

Show_only_best_grasp: defines if only the best grasp should be published or each grasp with an evaluation exceeding a threshold (only top grasps per roll angle). Set this value to publish only the overall top-rated grasp:

>> rosservice call /haf_grasping/set_show_only_best_grasp "show_only_best_grasp: true"

Gripper opening width: this factor f is used to simulate a gripper that is opened 1/f-times the maximal gripper opening width (e.g.: value 3 => gripper only one third opened):

>> rosservice call /haf_grasping/set_gripper_opening_width "gripper_opening_width: 3"

These values can also be set before launching the haf_client in the launch file launch/haf_grasping_all.launch. Here are the default values:

     ...
        <!-- PARAMETER DEFINITION -->

        <!-- define center position of area which is searched for grasps -->
        <rosparam param="grasp_search_center"> [0.0,0.0,0.0] </rosparam>

        <!-- define grasp search size, i.e. the region size which is
         searched for grasps; limit x: 0-18; limit y: 0-32 -->
        <rosparam param="grasp_search_size_x"> 18 </rosparam>
        <rosparam param="grasp_search_size_y"> 30 </rosparam>

        <!-- define approach direction of gripper (without gripper roll):
         moving direction is reversed (AV = (0,0,1) <=> gripper moving along -z-axis ) -->
        <rosparam param="gripper_approach_vector"> [0,0,1] </rosparam>

        <!-- define maximal calculation time max_calculation_time for finding grasps (sec) -->
        <rosparam param="max_calculation_time"> 40 </rosparam>

        <!-- define if only best grasp should be visualized or more
             (if evaluation is higher than threshold) -->
        <rosparam param="show_only_best_grasp"> false </rosparam>

        <!-- define the topic for incoming point clouds -->
        <rosparam param="input_pc_topic">/haf_grasping/depth_registered/single_cloud
                                         /points_in_lcs</rosparam>

        <!-- define default point cloud frame (if generated from pcd) -->
        <rosparam param="base_frame">base_link2</rosparam>

        <!-- define default gripper_width factor: if value is n, this means
             gripper is tested with 1/n-times the max. gripper opening width -->
        <rosparam param="gripper_width">1</rosparam>


        <!-- SVM Parameters -->

        <!-- define path of feature file for SVM grasp classification -->
        <param name="feature_file_path" value="$(find haf_grasping)/data/Features.txt" />

        <!-- define path relative to haf-package of range/scaling file for features -->
        <param name="range_file_path" value="$(find haf_grasping)/data/range21062012_allfeatures" />

        <!-- define path of SVM classifier (model) -->
        <param name="svmmodel_file_path" value="$(find haf_grasping)/data/all_features.txt.scale.model" />

HAF-GRASPING CLIENT - CODE EXPLAINED

In calc_grasppoints_action_client.cpp we subscribe to a point_cloud topic and start the following callback when a point cloud comes in:

   1 //get goal (input point cloud) for grasp calculation,
   2 //send it to grasp action server and receive result
   3 void CCalcGrasppointsClient::get_grasp_cb(const sensor_msgs::PointCloud2ConstPtr& pc_in)
   4 {
   5         ROS_INFO("\nFrom calc_grasppoints_action_client: point cloud received");
   6 
   7         // create the action client
   8         // true causes the client to spin its own thread
   9         actionlib::SimpleActionClient<haf_grasping::CalcGraspPointsServerAction>
  10                    ac("calc_grasppoints_svm_action_server", true);
  11 
  12         ROS_INFO("Waiting for action server to start.");
  13         // wait for the action server to start
  14         ac.waitForServer(); //will wait for infinite time
  15 
  16         ROS_INFO("Action server started, sending goal.");
  17         // send a goal to the action
  18         haf_grasping::CalcGraspPointsServerGoal goal;
  19         goal.graspinput.input_pc = *pc_in;
  20 
  21         goal.graspinput.grasp_area_center = this->graspsearchcenter;
  22 
  23         // set size of grasp search area
  24         goal.graspinput.grasp_area_length_x = this->grasp_search_size_x;
  25         goal.graspinput.grasp_area_length_y = this->grasp_search_size_y;
  26 
  27         // set max grasp calculation time
  28         goal.graspinput.max_calculation_time = this->grasp_calculation_time_max;
  29 
  30         //send goal
  31         ac.sendGoal(goal);
  32 
  33         //wait for the action to return
  34         bool finished_before_timeout = ac.waitForResult(ros::Duration(50.0));
  35 
  36         if (finished_before_timeout)
  37         {
  38             actionlib::SimpleClientGoalState state = ac.getState();
  39             boost::shared_ptr<const
  40           haf_grasping::CalcGraspPointsServerResult_<std::allocator<void> > > result =
  41           ac.getResult();
  42             ROS_INFO("Result: %s", (*(result)).result.data.c_str());
  43             ROS_INFO("Action finished: %s",state.toString().c_str());
  44         }
  45         else
  46             ROS_INFO("Action did not finish before the time-out.");
  47 }

RESULT

Detected grasps are visualized in rviz and returned as result to the service client. Furthermore, they are published to the topic /haf_grasping/grasp_hypothesis_with_eval as string value and have the following format:

>> rostopic echo /haf_grasping/grasp_hypothesis_with_eval
data: 55     0.07 0.08 0.158921   0.07 0.02 0.158921     0 0 1     0.07 0.05 0.158921 90
       |      |     |    |          |    |      |        | | |      |     |     |      |
      val gp1(x,    y,   z)     gp2(x,   y,     z )   av(x,y,z) gcp(x,    y,    z)    roll

val: scaled evaluation of the grasp: 99 is the top value, -20 is the worst possible value, meaning that no grasp was found at all (normally indicating a data or software issue, e.g. an issue with the tf system or no make executed in the libsvm folder)

gp1(x,y,z): x-,y-,z-coordinates of the first grasp point

gp2(x,y,z): x-,y-,z-coordinates of the second grasp point

av(x,y,z) : x-,y-,z-coordinates of the approach vector

gcp(x,y,z): x-,y-,z-coordinates of the grasp center point, i.e. the middle point between gp1 and gp2

roll: gripper orientation in degrees

The returned grasps indicate (in a redundant way) how a gripper with two opposite fingers should approach an object: the gripper-specific approach vector that goes through the tool center point should approach the object along the object specific (returned) approach vector (av), which goes to the grasp center point (gcp) whereby the roll is defined by roll respectively by the heuristically estimated grasp points (gp1,gp2).

GRASP REALIZATION WITH SIMULATION

A crucial part for successful grasping is path planning and grasp (fine) planning. We achieved the best results by using a simulation software (OpenRAVE) to calculate a collision free straight approach trajectory for a gripper with fixed orientation in the following way.

The OpenRAVE simulator is used for path planning including determination of an appropriate distance between the manipulator and the object before closing the manipulator. OpenRAVE tries to approach the object mesh (i.e. an unsegmented mesh of all objects in the scene [rosnode with conversion from point cloud to mesh can be provided]) using the calculated approach vector and manipulator roll angle until a collision occurs. Then it sets the manipulator position back by a standoff value which is dependent on the object position: in recent work, we do not use a fixed standoff of 1 cm, but start with a standoff value of 1 mm. If this standoff leads to a collision of the gripper fingers with the table top or the ground (used in simulation), the standoff is increased until the closing fingers do not collide with the table or the ground any more. Then the actual grasp points, i.e. contact points of the fingers with the object mesh in the simulation, are calculated. From the resulting gripper position, OpenRAVE calculates the manipulator position about 7 cm away and searches for a collision free path to place the manipulator there. For the last 7 cm to the object OpenRAVE calculates a straight path to the object if one exists. We chose 7 cm as a practical trade-off between a higher grasping robustness (regarding calibration inaccuracy or incomplete data) achieved by a straight approach trajectory with fixed manipulator orientation and the challenge to find inverse kinematics solutions for such trajectories.

An OpenRAVE implementation for the PR2 can be found here: http://pr.cs.cornell.edu/grasping/rect_data/data.php ('''code''', '''Readme'''). OpenRAVE implementations for an 7-DOF Schunk/AMTEC arm, a Kuka LWR and the service robot '''Hobbit''' can be provided on request.

LIBSVM

Grasp points and approach vectors are detected using Support Vector Machines. We have included LIBSVM to work as our classifier (go to folder libsvm-3.12 and type "make" after checking out):

Chih-Chung Chang and Chih-Jen Lin, LIBSVM : a library for support vector machines. ACM Transactions on Intelligent Systems and Technology, 2:27:1--27:27, 2011. Software available at http://www.csie.ntu.edu.tw/~cjlin/libsvm

CITATION

If you like this package and use it in your own work, please cite our '''IJRR paper''':

@article{DBLP:journals/ijrr/FischingerWV15,
  author    = {David Fischinger and Astrid Weiss and Markus Vincze},
  title     = {Learning Grasps with Topographic Features},
  journal   = {I. J. Robotic Res.},
  volume    = {34},
  number    = {9},
  pages     = {1167--1194},
  year      = {2015},
  url       = {http://dx.doi.org/10.1177/0278364915577105},
  doi       = {10.1177/0278364915577105},
  timestamp = {Fri, 31 Jul 2015 01:00:00 +0200},
  biburl    = {http://dblp.uni-trier.de/rec/bib/journals/ijrr/FischingerWV15},
  bibsource = {dblp computer science bibliography, http://dblp.org}
}

REPORT A BUG

Use GitHub to report bugs or submit feature requests. [View active issues]
or contact the package maintainer directly.