Add option to use padded robot for self-collisions

[nyxrobotics]

Summary Import parameters to set the avoidance distance for each link when planning with moveit. You can read more about it in the PR I wrote in the Reference.

Purpose

• Enable/Disable self-collision checking
• Prevent self collision in the moveit operation.

Reference

• moveit/#2840

[nyxrobotics]

In soar I tested with the following parameters. By setting the head padding large, the arm will not pass near the head.

• planning_context.launch

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

• move_group.launch

  <!-- Start the actual move_group node/action server -->
  <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
  <!-- Set the display variable, in case OpenGL code is used internally -->
  <env name="DISPLAY" value="$(optenv DISPLAY :0)"/>
  <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
  <param name="sense_for_plan/max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
  <param name="default_planning_pipeline" value="$(arg pipeline)"/>
  <param name="capabilities" value="$(arg capabilities)"/>
  <param name="disable_capabilities" value="$(arg disable_capabilities)"/>
  <!-- do not copy dynamics information from /joint_states to internal robot monitoring
       default to false, because almost nothing in move_group relies on this information -->
  <param name="monitor_dynamics" value="false"/>
  <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
  <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)"/>
  <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)"/>
  <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)"/>
  <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)"/>
  <param name="max_safe_path_cost" value="1"/>
  <param name="jiggle_fraction" value="0.05" />
  <param name="start_state_max_dt" value="0.5" />
  <param name="max_sampling_attempts" value="100" />
  <param name="capabilities" value=""/>
  <param name="use_padding_self_collisions" value="true"/>
  <param name="disable_capabilities" value="move_group/ClearOctomapService"/>
  </node>

• padding.yaml

  # parameters for padded robot (not used for self collision checks)
  default_robot_padding: 0.004
  default_robot_scale: 1.0
  default_robot_link_padding: {"arm_link7" : 0.004, "right_finger_extension_link" : 0.004, "left_finger_extension_link" : 0.004, "hand_camera_link" : 0.01, "head_pan_link" : 0.05, "head_tilt_link" : 0.05}

[nyxrobotics]

The ability to set padding for each link originally existed, and the effect of this change was to reflect it in the self collision.