MoveIt Pick and Place problem

[郭佳文]

Hi,

I'm trying to use the high level function pick() from move_group_interface::MoveGroup. The arm would not open the gripper and just move directly across the object. The problem is shown in the attachment problem-1.png and problem-2.png.

Here is my procedure:

1)Fill in the message moveit_msgs/CollisionObject and publish it to the topic, /collision_object.
The source code is here.
The visualization of the planning scene is in the attachment planning_scene.png

2)Write a node that use the pick function provided by move_group_interface::MoveGroup.
The source code is here.

3)Run the necessary launches and nodes.
roslaunch pr2_gazebo pr2_empty_world.launch
roslaunch pr2_moveit_config move_group.launch
rosrun rviz rviz
rosrun the node that publish the collision object.
rosrun the node that call the pick function.

My platform is Ubuntu 12.10 64bit, and ROS Groovy. All the ROS related packages are installed using apt-get.

How to solve the problem. If there is a detailed step by step tutorial, please show me. That would be really helpful and be greatly appreciated.

Thanks,

-Albert

[Dave Hershberger]

Hi Albert, sorry for the slow reply.

It seems to me like you are missing any sort of "grasp planning".

Your code calls group.pick("test_obj").  That pick() function has an optional second parameter which specifies exactly the grasp to use.  If you fill out a moveit_msgs/Grasp message and pass it as the second argument, pick() should use that Grasp when it approaches the object.

There is a bunch of code inside the MoveGroup pick_place_action_capability which tries to figure out grasps to use when you leave it blank.  Some of that is here:

https://github.com/ros-planning/moveit_ros/blob/hydro-devel/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp#L417

in the fillGrasps() function.  In there it first tries to call a grasp planning service named "database_grasp_planning".  If you don't have a service like that running, it falls back to a default grasp which seems to involve setting all the end-effector joints to their max value in the "open" phase and to their min value in the "close" phase.  (around this line:

https://github.com/ros-planning/moveit_ros/blob/hydro-devel/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp#L539

In answer to your question about pick-and-place documentation, I will say that MoveIt's pick and place functionality is not fully developed.  I do not believe there is a tutorial for it.

So you could try to understand and run the grasp-planning system, or you could just fill out a Grasp message that you know will be useful in your situation and pass that to group.pick().

Dave

[郭佳文]

Hi Dave,

Thanks for your reply. I still got some problems to ask.

First, is the grasp necessary for the pick() function? For my case the gripper did not open at all if the grasp plan is not provided. The default grasp seemed to not work in my case.

Secondly, how do I provide a database for the grasp, like "grasp planning service named "database_grasp_planning"" you mentioned in the post? Any document for that?

By the way, is it better to use MoveIt in Hydro or in Groovy? Here "better" can mean more stable or more functions to use, etc.

Thanks,

-Albert

Dave Hershberger於 2014年4月29日星期二UTC+8上午12時32分53秒寫道：

[waldez junior]

Hi Albert, 

you might have done it already, but there's a chance this demo code may help you.

https://github.com/ros-planning/moveit_pr2/blob/hydro-devel/pr2_moveit_tutorials/pick_place/src/pick_place_tutorial.cpp

Regards,

Waldez Jr

[郭佳文]

Hi,
Waldez, thanks for the link. That really helps for filling in the grasp. I followed the tutorial and provided the grasp to the pick() function. However, the problem is still similar to the original post--the gripper would not open. I've taken several screenshot and attached them in the post.

The gripper would not open and would not cross the object either. You can see from the screenshot that only the gripper tip will contact the collision object, and the object is following the motion of the arm.

I just followed the tutorial and filled in grasp. Here is the code snippet:
--------------------------------------------------------------------
  std::vector<moveit_msgs::Grasp> grasps;

  geometry_msgs::PoseStamped p;
  p.header.frame_id = "base_footprint";
  p.pose.position.x = 0.32;
  p.pose.position.y = -0.7;
  p.pose.position.z = 0.5;
  p.pose.orientation.x = 0;
  p.pose.orientation.y = 0;
  p.pose.orientation.z = 0;
  p.pose.orientation.w = 1;
  moveit_msgs::Grasp g;
  g.grasp_pose = p;

  g.pre_grasp_approach.direction.vector.x = 1.0;
  g.pre_grasp_approach.direction.header.frame_id = "r_wrist_roll_link";
  g.pre_grasp_approach.min_distance = 0.2;
  g.pre_grasp_approach.desired_distance = 0.4;

  g.post_grasp_retreat.direction.header.frame_id = "base_footprint";
  g.post_grasp_retreat.direction.vector.z = 1.0;
  g.post_grasp_retreat.min_distance = 0.1;
  g.post_grasp_retreat.desired_distance = 0.25;

  g.pre_grasp_posture.joint_names.resize(1, "r_gripper_joint");
  g.pre_grasp_posture.points.resize(1);
  g.pre_grasp_posture.points[0].positions.resize(1);
  g.pre_grasp_posture.points[0].positions[0] = 1;

  g.grasp_posture.joint_names.resize(1, "r_gripper_joint");
  g.grasp_posture.points.resize(1);
  g.grasp_posture.points[0].positions.resize(1);
  g.grasp_posture.points[0].positions[0] = 0;

  grasps.push_back(g);
--------------------------------------------------------------------
I've switched the platform to Hydro now.

How to solve this, thanks!

-Albert

waldez junior於 2014年5月3日星期六UTC+8上午10時41分07秒寫道：

[Johannes P]

Hi,

if you have problems with pick() and place() functions, you could also implement your own functions for manipulation. I have created a own pick/place routine with basic functions
("setPoseTarget() + move()" for the arm and "setJointValueTarget()+move()" for the end-effector):
(1) move manipulator above/before the object
(2) move eef straight "into" the object
(3) close endeffector (+ attach object)
(4) move back to (1) position

At the end it's more or less the same as pick/place, with the advantage, that you have full control over each manipulation step...

[Morfologus]

Hi Albert!
Have you managed to pick and place an object after all?