Moveit planning scene collision padding

edit

After the fruitful discussion with FSound I came to a partial solution to add padding to the peripheral elements of the robot cell.

The first assumption is that the whole model of the cell is available in the parameter server under /robot_description. This should anyhow be the case. Next, I prepared the padding values as described by FSound and loaded them into their appropriate namespace.

I then created a Python script that goes through the URDF and adds the objects to the Scene with the add_mesh() method.

Here are a few slices of the code.

First the imports that help you parse the URDF and the path to the mesh:

from urdf_parser_py.urdf import URDF
import rospkg 

I then created a helper function that returns the full path of the mesh from the path available in the URDF:

get_full_mesh_path(mesh_path_from_urdf):
    relative_path = mesh_path_from_urdf.replace("package://","")    
    package_name = relative_path.split('/')[0]
    package_path = rospkg.RosPack().get_path(package_name)
    file_path = package_path + relative_path.replace(package_name, "") 

Finally, I loop through the list of objects to add and add them to the scene:

urdf = URDF.from_parameter_server('/robot_description')
scene = moveit_commander.PlanningSceneInterface()
for object in objects_to_add:
    link = urdf.link_map[object]
    mesh_path = get_full_mesh_path(link.visual.geometry.filename)
    link_pose = PoseStamped()
    link_pose.pose.orientation.w = 1
    link_pose.header.frame_id = link.name
    scene.add_mesh(link.name, link_pose, mesh_path) 

Since this is the top result on Google for "moveit default_robot_padding", I though having a working example might be good. Here's how I have set the default_robot_padding parameter:

Add a file padding.yaml, for example in <robot>_moveit_config/config, with the following content

# parameters for padded robot (not used for self collision checks)
default_robot_padding: 0.05
default_robot_scale: 1.0

and load the parameters from padding.yaml into the correct namespace in a launch file somewhere, for example in planning_context.launch

<!-- Load padding -->
<group ns="move_group">
  <group ns="$(arg robot_description)_planning">
    <rosparam command="load" file="$(find <robot>_moveit_config)/config/padding.yaml" />
  </group>
</group>