Pick example from moveit_commander

[Tobias Fromm]

Hello Ioan and moveit_users,

I've tried to run the pick demo from moveit_commander on our robot, but
the motion planner keeps giving me strange errors:
-----
[ INFO] [1381496234.147557423, 254.573000000]: Planning attempt 1 of at
most 1
[ INFO] [1381496234.147954643, 254.573000000]: Added plan for pipeline
'pick'. Queue is now of size 1
[ERROR] [1381496234.155924990, 254.580000000]: close up = 0.018182
[ INFO] [1381496234.160568851, 254.586000000]: Manipulation plan failed
at stage 'approach & translate' on thread 3
[ WARN] [1381496234.161139832, 254.586000000]: All supplied grasps
failed. Retrying last grasp in verbose mode.
[ INFO] [1381496234.161240559, 254.586000000]: Re-added last failed plan
for pipeline 'pick'. Queue is now of size 1
[ INFO] [1381496234.165029126, 254.590000000]: Found a contact between
'arm_4_link' (type 'Robot link') and 'asus_camera_link' (type 'Robot
link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1381496234.165074309, 254.590000000]: Collision checking is
considered complete (collision was found and 0 contacts are stored)
[ERROR] [1381496234.165119971, 254.590000000]: close up = 0.018182
[ INFO] [1381496234.169614767, 254.595000000]: Found a contact between
'arm_4_link' (type 'Robot link') and 'asus_camera_link' (type 'Robot
link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1381496234.169660631, 254.595000000]: Collision checking is
considered complete (collision was found and 0 contacts are stored)
[ERROR] [1381496234.169723300, 254.595000000]: close up = 0.018182
[ INFO] [1381496234.173846483, 254.599000000]: Manipulation plan failed
at stage 'approach & translate' on thread 1
[ INFO] [1381496234.173932149, 254.599000000]: Pickup completed after
0.013469 seconds
-----

My code is like this:

-----
scene = PlanningSceneInterface()
robot = RobotCommander()
rospy.sleep(1)

grasp = manipulation_msgs.msg.Grasp()
grasp.id = "blah"
grasp.grasp_pose.header.frame_id = "base_link"
grasp.grasp_pose.header.stamp = rospy.Time.now()
grasp.grasp_pose.pose.position.x = .68
grasp.grasp_pose.pose.position.y = .3
grasp.grasp_pose.pose.position.z = .5
grasp.grasp_pose.pose.orientation.w = .7
grasp.grasp_pose.pose.orientation.x = -.08
grasp.grasp_pose.pose.orientation.y = .7
grasp.grasp_pose.pose.orientation.z = -.08
goal.possible_grasps.append( grasp )

scene.remove_world_object("box")

p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 1.0
p.pose.position.y = 0.3
p.pose.position.z = 0.5
scene.add_box("box", p, (0.15, 0.05, 0.3))

rospy.sleep(1)

robot.arm.pick("box")

-----

The grasp pose I've defined is shown in the first of the attached
screenshots, so the planner should be able to find a solution per se.
I've also tried not giving any grasp information at all and simply
picking the object like in the pick.py example, but that spawned similar
errors.

Those messages are strange because they refer to "arm_4_link" and
"asus_camera_link" which have nothing to do with each other (see second
screenshot). I'm not sure, though, whether using the group "arm" is
correct (for group definitions, see below).
Don't worry about the red color of the ring on the end effector; it's
actually colored red in the URDF.

One more information: I'm using ROS Groovy and unfortunately, I won't
have much chance to switch to Hydro in near future, in case that's
necessary.

Does anybody have an idea what's going wrong here? How could I possibly
find out more about what's actually failing?

Some additional data, in case that helps:
my controllers.yaml:
-----
controller_manager_ns:
pr2_controller_manager
controller_list:
- name: arm_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- arm_1_joint
- arm_2_joint
- arm_3_joint
- arm_4_joint
- arm_5_joint
- arm_6_joint
- name: gripper_controller
action_ns: gripper_command
type: GripperCommand
default: true
joints:
- gripper_base_joint_gripper_left
- gripper_base_joint_gripper_right
-----
my group definitions from the SRDF:
-----
<group name="arm">
<joint name="arm_base_joint" />
<joint name="arm_0_joint" />
<joint name="arm_1_joint" />
<joint name="arm_2_joint" />
<joint name="arm_3_joint" />
<joint name="arm_4_joint" />
<joint name="arm_5_joint" />
<joint name="arm_6_joint" />
</group>
<group name="gripper">
<joint name="ft_sensor_joint" />
<joint name="gripper_base_joint_gripper_left" />
<joint name="gripper_base_joint_gripper_right" />
<joint name="gripper_anterior_gripper_joint" />
</group>
-----

Thanks in advance, best,
Tobi

--
Tobias Fromm
Robotics Group
Jacobs University

Research I, Room 38 +49-421-200-3106
Campus Ring 1 t.f...@jacobs-university.de
28759 Bremen, Germany http://robotics.jacobs-university.de

[Ioan Sucan]

Hello Tobias,

I am sorry to hear you are having trouble with this. Thank you for the detailed report though!

One side remark first: you can actually run the Hydro version of MoveIt alongside with other groovy packages. The only thing you will have to worry about is converting between groovy & hydro versions of JointTrajectory messages. Caveat: I have not tried this, but I suspect it will work.

Back to your question,

Is your robot model using Collada files? I am wandering if this is the same problem that Dave Coleman was pointing out with the Baxter.

Does this problem (of robot links colliding) appear when doing regular planning ? (even if intermittently)

Ioan

[Fromm, Tobias]

Hi Ioan,

thanks a lot for your quick response!
If necessary, I'd try the parallel hydro/groovy method, but would like to avoid it of course.

As far as I remember, we use lots of STL files, but no DAE in our robot model. Can't tell for sure at the moment, though, because I don't have the code here right now. I'll double-check this on Monday.

But I am sure that I never even once had this problem on regular planning. Do you think the error actually stems from this suspected collision or could it also be something different? Is it possible that it has something to do with the collision checks that were performed by the MoveIt setup assistant? In that case I'd try to re-run this because we changed lots things on the URDF and didn't re-generate the collision checks.

Cheers,
Tobi

[Ioan Sucan]

Tobias,

It is very likely this is a collision matrix problem, since you changed the URDF. Please regenerate the matrix and try again. It is still suspicious you do not get this with regular planning though...

Ioan

[Tobias Fromm]

Thanks, Ioan,

that'll be my first thing to try on Monday when I have access to the
code again.
However, there a possibility to visualize any grasps in RViz? In that
case it would be easier to make sure that I give a valid grasping pose.

Cheers,
Tobi

[Ioan Sucan]

Tobias,

If you run in debug mode (debug:=true argument to your move_group.launch file) or with info:=true, the used grasps are published as markers. Different colors are used to identify when the grasp failed along the pick&place pipeline.

Ioan

[Ioan Sucan]

On Mon, Oct 14, 2013 at 6:45 PM, Steffen P <s.pfi...@gmail.com> wrote:

Hey,

I have the same problem as Tobias. I am currently also trying to debug the pick function. When I enable the debug mode for move_group and add a marker array listening to the topic /move_group/display_grasp_markers I get some markers displayed but they are displayed at a completely different position than my defined grasp. (They are displayed at the position of the gripper of the current goal state in RViz.) I am using the latest debians in groovy.

This sounds like a TF problem. Do you see any errors about transforming frames? The fact the eefs appear at clearly wrong positions suggests that.

By the way is it possible to set the goal state displayed in RViz through the moveit interface?

No, but you can disable displaying the goal state and add your own RobotState plugin, to which you can publish robot state messages. This allows you to implement the same behaviour.

Ioan

Steffen

[Steffen P]

Yes, this looks like a TF problem but I don't understand why the grasp, if a possible one was found, is executed correctly. I don't get any TF errors.