Pick and place found collision between object and grippier

[wruixian]

Description

I was trying to run the pick and place tutorial and I ran into multiple errors: [http://docs.ros.org/en/melodic/api/moveit_tutorials/html/doc/pick_place/pick_place_tutorial.html#]

The errors seem to be raised by the line move_group.pick("object", grasps) inside pick_place_tutorial.cpp.

I am new to MoveIt but my hunch is that I might need to create an AllowedCollisionMatrix object to allow collisions with the object, but I have been tinkering for hours to no avail, any help would be much appreciated, thank you!!

Your environment

• ROS Distro: Melodic
• OS Version: Ubuntu 18.04
• MoveIt installation details as per "Getting Started" in moveit_tutorials

Backtrace or Console output

Output from pick_place_tutorial

[ INFO] [1673514004.761735849]: Loading robot model 'panda'...
[ INFO] [1673514005.857218741]: Ready to take commands for planning group panda_arm.
[ WARN] [1673514007.140055648]: Fail: ABORTED: No motion plan found. No execution attempted.
[ WARN] [1673514008.227004922]: Fail: ABORTED: Must specify group in motion plan request

Relevant output from demo.launch

[ WARN] [1673514005.859206604]: Empty quaternion found in pose message. Setting to neutral orientation.
[ WARN] [1673514005.859402198]: Empty quaternion found in pose message. Setting to neutral orientation.
[ WARN] [1673514005.859458671]: Empty quaternion found in pose message. Setting to neutral orientation.
[ INFO] [1673514005.861388876]: Loading robot model 'panda'...
[ INFO] [1673514005.974133499]: Starting planning scene monitor
[ INFO] [1673514005.977216142]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1673514006.287422869]: Constructing new MoveGroup connection for group 'panda_arm' in namespace ''
[ INFO] [1673514006.925875608]: Planning attempt 1 of at most 1
[ INFO] [1673514006.933654381]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1673514007.035511406]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0
[ WARN] [1673514007.035874239]: All supplied grasps failed. Retrying last grasp in verbose mode.
[ INFO] [1673514007.036129018]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1673514007.037573919]: Found a contact between 'object' (type 'Object') and 'panda_hand_sc' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1673514007.037765314]: Collision checking is considered complete (collision was found and 0 contacts are stored)

...

[ INFO] [1673514007.136174809]: Found a contact between 'object' (type 'Object') and 'panda_hand_sc' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1673514007.136213746]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1673514007.138494559]: IK failed
[ INFO] [1673514007.138586689]: Sampler failed to produce a state
[ INFO] [1673514007.138643800]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1673514007.139179031]: Pickup planning completed after 0.109809 seconds
[ INFO] [1673514007.270067059]: Ready to take commands for planning group panda_arm.
[ INFO] [1673514007.270137943]: Looking around: no
[ INFO] [1673514007.270156257]: Replanning: no
[ INFO] [1673514008.225551810]: Planning attempt 1 of at most 1
[ERROR] [1673514008.225681415]: Attached body 'object' not found

[welcome[bot]]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[meixiaohh]

Is the problem solved?

[Astaxuqichao]

I encounter the same issue. But I find using demo_gazebo.launch rather than demo.launch, pick_place_tutorial work well. So I guess the default "panda_hand_sc" collision model is incorrect. I modify the franka_hand.xacro in franka_description, like this:

    <xacro:link_with_sc name="hand" gazebo="${gazebo}">
      <self_collision_geometries>
        <xacro:collision_capsule xyz="0 0 0.02" direction="y" radius="${0.02+safety_distance}" length="0.1" />
        <!-- <xacro:collision_capsule xyz="0 0 0.10" direction="y" radius="${0.02+safety_distance}" length="0.1" /> -->
      </self_collision_geometries>
    </xacro:link_with_sc>

Hope that helps