No "loadRobotGraspData" called in "simple pick place" demo hydro

[ltaoist]

Hi Dave,

I am running movit and pick_place on ROS Hydro.

Currently, when Itry to run the pick and place demo, I get an error message stating "no motion planning found".

When I compare the clam_pick_place/src/simple_pick_place.cpp in hydro and groovy, I saw that the "loadRobotGraspData" function is not called in the hydro version.

For this reason, grasp_data_ is not initialized and then pick or place cannot compute any motion plan. I saw that for the Hydro version, you deleted this call in the following commit: c341a81)

I also found that in hydro, loadRobotGraspData became a part of the moveit API and it is now a method of grasp_data_ .

It seems the functionality is the same as in Groovy. Except that in Hydro, the grasp data values are assigned through a yaml file while in groovy they are assigned within the code.

I tried to fix this. So I modified simple_pick_place.cpp by adding grasp_data_. loadRobotGraspData(nh, EE_GROUP) before the line moveit_simple_grasps_.reset. I also added a config file like the following one: reem_grasp_data.yaml and baxter_grasp_data.yaml and finally loaded through roslaunch.

My code currently works, but the blocks are picked up by the wrist. I think the clam gripper posture is incorrect. I think the error maybe in my config file.

Here are my changes to "simple_pick_place.cpp" :

  if(!grasp_data_.loadRobotGraspData(nh, EE_GROUP))
  {
      ROS_ERROR_STREAM_NAMED("simple_pick_place", "Cannot load end_effector");
      return 1;
  }
  // ---------------------------------------------------------------------------------------------
  // Load grasp generator
  moveit_simple_grasps_.reset(new moveit_simple_grasps::SimpleGrasps(rviz_tools_));

And here is my launch file:

<launch>

  <arg name="debug" default="true"/>
  <rosparam command="load" file="$(find clam_pick_place)/config/clam_grasp_data.yaml"/>

  <group>
    <node name="$(anon simple_pick_place)" type="simple_pick_place" pkg="clam_pick_place">
    </node>   
  </group>

</launch>

And here is the yaml file :

base_link: 'base_link'

gripper_group:
  end_effector_name: 'gripper_group'
  joints : ['gripper_finger_joint']
  pregrasp_posture : [0.0]
  grasp_posture : [1.0]
  end_effector_parent_link : 'gripper_roll_link'
  // what should the following be ?
  pregrasp_time_from_start : 4.0
  grasp_time_from_start : 4.0
  grasp_pose_to_eef : [0,0,0]

I would appreciate any feedback on this. We almost got this working!

[rojas70]

Dave, we are not sure of the parameters:
pregrasp_posture : [0.0], grasp_posture : [1.0]

It's not clear what they mean. Are these distances along the x-axis away from the centroid of the object to be grapsed?

And similarly for grasp_pose_to_eef : [0,0,0] in 3D space?

[davetcoleman]

@ltaoist , thanks for your effort, I'm impressed what you have figured out so far.

My original project, block_grasp_generator was made for the ClamArm. I have since spun that project into two projects - moveit_simple_grasps and moveit_visual_tools - in an effort to make the more portable with any robot. So now it runs on Baxter and REEM (Pal Robotics) as well as some other projects of mine.

However, because I don't maintain the ClamArm project anymore all those changes hadn't been synced back downstream. I spent a couple hours today (and I'm kicking myself for it) trying to port those changes back to the ClamArm for you. I can't promise much other time. Here is the commit:
beb9c39

I essentially made a whole new file, and its very simple:
https://github.com/davetcoleman/clam/blob/beb9c39a3a2318a2369a367f19504ae23a50c651/clam_pick_place/src/block_pick_place.cpp

All the functionality for block pick place has been moved to moveit_simple_grasps, so make sure you build that from source, too, and have the latest. I've also moved the Clam grasp config you made (and I fixed it):
https://github.com/davetcoleman/moveit_simple_grasps/blob/hydro-devel/config/clam_grasp_data.yaml

Note you will need to merge this PR under review as well:
davetcoleman/moveit_simple_grasps#9

There are sill some issues with the controllers, but they are because of MoveIt changes (not this code) and I don't have time / will power to work on them because I work on other robots these days.