Update subframe tutorial with object pose

[felixvd]

This cleans up the subframes tutorial and updates it with object poses. Depends on moveit/moveit#2037

Checklist

• Required by CI: Code is auto formatted using clang-format
• While waiting for someone to review your request, please consider reviewing another open pull request to support the maintainers

[davetcoleman]

Need to fix conflict

[davetcoleman]

Friendly ping, I tried to fix it myself but am unsure what stays.

[v4hn]

Ping @felixvd. The feature got merged :-)

[felixvd]

Right! Can't test right now because my simulation is stuck like this and I don't have time to fix my setup, but it should work.

[gautz]

With panda_moveit_config on melodic-devel branch I get

RLException: while processing /home/gah/robotkit_ws/src/panda_moveit_config/launch/planning_context.launch:
Invalid <param> tag: Cannot load command parameter [robot_description]: command [['/opt/ros/noetic/lib/xacro/xacro', '/opt/ros/noetic/share/franka_description/robots/panda_arm_hand.urdf.xacro']] returned with code [2]. 

Param xml is <param if="$(eval arg('load_robot_description') and arg('load_gripper'))" name="$(arg robot_description)" command="$(find xacro)/xacro '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'"/>
The traceback for the exception was written to the log file

So I used moveit/panda_moveit_config#83 then the tutorial launches
Typing 1 in the tutorial gives me following error:

ros.moveit_core.robot_model: Loading robot model 'panda'...
ros.moveit_core.robot_model.empty_collision_geometry: Link panda_leftfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
ros.moveit_core.robot_model.empty_collision_geometry: Link panda_rightfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
ros.moveit_core.robot_model: Loading robot model 'panda'...
ros.moveit_core.robot_model.empty_collision_geometry: Link panda_leftfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
ros.moveit_core.robot_model.empty_collision_geometry: Link panda_rightfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
ros.moveit_ros_planning.planning_scene_monitor: Starting planning scene monitor
ros.moveit_ros_planning.planning_scene_monitor: Listening to '/move_group/monitored_planning_scene'
ros.moveit_core.planning_scene: Empty quaternion found in pose message. Setting to neutral orientation.
ros.moveit_core.planning_scene: Empty quaternion found in pose message. Setting to neutral orientation.
ros.moveit_ros_move_group.MoveAction: Planning request received for MoveGroup action. Forwarding to planning pipeline.
ros.moveit_core.collision_detection.fcl: Found a contact between 'panda_hand' (type 'Robot link') and 'cylinder' (type 'Robot attached'), which constitutes a collision. Contact information is not stored.
ros.moveit_core.collision_detection.fcl: Collision checking is considered complete (collision was found and 0 contacts are stored)
ros.moveit_ros_planning: Start state appears to be in collision with respect to group panda_arm
ros.moveit_ros_planning: Unable to find a valid state nearby the start state (using jiggle fraction of 0.050000 and 100 sampling attempts). Passing the original planning request to the planner.
ros.moveit_planners_ompl.model_based_planning_context: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
ros.moveit_planners_ompl.ompl: panda_arm/panda_arm: Skipping invalid start state (invalid state)
ros.moveit_planners_ompl.ompl: panda_arm/panda_arm: Motion planning start tree could not be initialized!
ros.moveit_planners_ompl.ompl: No solution found after 0.000226 seconds
ros.moveit_planners_ompl.ompl: Goal sampling thread never did any work.
ros.moveit_planners_ompl.model_based_planning_context: Unable to solve the planning problem

[felixvd]

How annoying. I will not be able to do anything about it this week, but if you check the collision geometry of the hand, you might find that the cylinder and/or box just need to be moved down a little. Maybe that will help you fix the issue locally for now.

[gautz]

Collisions were detected between panda_arm and cylinder and between box and cylinder. There may be an issue with the recent upstream franka repo.
os.moveit_core.robot_model.empty_collision_geometry: Link panda_leftfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.

SOLUTION (workaround?):

• Before attaching the cylinder add att_coll_object.touch_links = std::vector<std::string>{ "panda_hand", "panda_leftfinger", "panda_rightfinger" };
• Change cylinder.subframe_poses[0].position.z = 0.04; (previously 0.03)

[v4hn]

This lay around for long enough. Removing the visualization code in favor of the TODO to explain the TfPublisher capability is debatable, but nobody will touch it to clean this up for now.

The tutorial works great when using roslaunch moveit_resources_panda_moveit_config demo.launch and has collisions between hand and attached object with upstream franka_description/panda_moveit_config. Maybe @rhaschke can take a look because he worked on the ACM default collisions for just these use-cases.

Personally I think if things work fine with out internal resources here that's fine by me.