Display the mesh after padding

[nyxrobotics]

Overview I want to be able to display the effect of parameters related to padding in rviz.

default_robot_padding: 1.0
default_robot_scale: 1.0
default_object_padding: 1.0
default_attached_padding: 1.0

Reference

• https://github.com/ros-planning/moveit/issues/1330#issuecomment-460062943
• https://github.com/ros-planning/geometric_shapes/issues/76#issuecomment-362474549

[jsupratman13]

It's a work in progress, and here is something I noticed when visualizing the padding/scale

original	scaled/padded (excluding mesh type)

I've extracted moveit_core::RobotModel link model information to generate the padding as shown above. However, it seems that it only takes the first collision define by the link and ignores any other collision. Need more investigation on this.

[jsupratman13]

Memo: This is where the padd/scale parameters are loaded set. https://github.com/sbgisen/moveit/blob/3a9e63a309d56ca62feb68976f8b497748b5b8a0/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp#L202-L214

The parameters are stored in the scene_ a planning_scene::PlanningScene instance under cenv_, a collision_detection::CollisionEnvPtr variable. Accessing this will give you the link padd/scale. https://github.com/sbgisen/moveit/blob/3a9e63a309d56ca62feb68976f8b497748b5b8a0/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h#L1087

Collision check with padd/scale parameter is executed when checkCollision function is called (ros-planning ignores padd/scale self-collision but sbgisen don't) https://github.com/sbgisen/moveit/blob/3a9e63a309d56ca62feb68976f8b497748b5b8a0/moveit_core/planning_scene/src/planning_scene.cpp#L487-L498

This will called the checkRobotCollision and checkSelfCollision function defined in moveit_core/collision_detection/include/collision_env.h file. Here, it gets a little tricky since it is a virtual function; this means the function will override whatever class it inherits. In this case, MoveIt has several collision detection method, such as FCL and Bullet. By default, MoveIt uses the FCL. https://github.com/sbgisen/moveit/blob/3a9e63a309d56ca62feb68976f8b497748b5b8a0/moveit_core/planning_scene/src/planning_scene.cpp#L156

Visualization part: In general, we want to modify either the https://github.com/sbgisen/moveit/tree/noetic-devel/moveit_ros/visualization/motion_planning_rviz_plugin or https://github.com/sbgisen/moveit/tree/noetic-devel/moveit_ros/visualization/planning_scene_rviz_plugin to visualize the robot model on rviz.

[jsupratman13]

Memo part2: Since MoveIt uses FCL, the updatedPaddingOrScaling function, which is called when the padd/scale parameter are configured, is referred here. https://github.com/sbgisen/moveit/blob/3a9e63a309d56ca62feb68976f8b497748b5b8a0/moveit_core/collision_detection/src/collision_env.cpp#L111 https://github.com/sbgisen/moveit/blob/3a9e63a309d56ca62feb68976f8b497748b5b8a0/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp#L437

Looking at FCL's createCollisionGeometry function, the individual link's collision are cast as geometric_shapes::Shape with the scaling and padding parameters configured within. Depending on the type of collision shape, the geometric_shapes::Shape updates the scaling and padding parameters accordingly. https://github.com/sbgisen/moveit/blob/3a9e63a309d56ca62feb68976f8b497748b5b8a0/moveit_core/collision_detection_fcl/src/collision_common.cpp#L902-L903 This is also used in Bullet https://github.com/sbgisen/moveit/blob/3a9e63a309d56ca62feb68976f8b497748b5b8a0/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp#L405-L409

Therefore, if we can extract the geometric_shapes::Shape to some structure for Rviz (such as visualization_msgs or Rviz api) we can visualize how the padding/scaling parameters affects the robot model collision.

[jsupratman13]

https://github.com/sbgisen/moveit/pull/24 due to trying to sync with the latest branch, it was requested to temporarily revert (https://github.com/sbgisen/moveit/pull/26). Reopening this issue as a reminder that the padding visual is not enabled at the moment.