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

• can't get padding to work! moveit/moveit#1330 (comment)
• Non-uniform padding of non-convex, long meshes moveit/geometric_shapes#76 (comment)

[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.

moveit/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

Lines 202 to 214 in 3a9e63a

	configureDefaultPadding();
	scene_->getCollisionEnvNonConst()->setPadding(default_robot_padd_);
	scene_->getCollisionEnvNonConst()->setScale(default_robot_scale_);
	for (const std::pair<const std::string, double>& it : default_robot_link_padd_)
	{
	scene_->getCollisionEnvNonConst()->setLinkPadding(it.first, it.second);
	}
	for (const std::pair<const std::string, double>& it : default_robot_link_scale_)
	{
	scene_->getCollisionEnvNonConst()->setLinkScale(it.first, it.second);
	}
	scene_->propogateRobotPadding();

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.

moveit/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h

Line 1087 in 3a9e63a

collision_detection::CollisionEnvPtr cenv_; // never NULL

Collision check with padd/scale parameter is executed when checkCollision function is called (ros-planning ignores padd/scale self-collision but sbgisen don't)

moveit/moveit_core/planning_scene/src/planning_scene.cpp

Lines 487 to 498 in 3a9e63a

	void PlanningScene::checkCollision(const collision_detection::CollisionRequest& req,
	collision_detection::CollisionResult& res,
	const moveit::core::RobotState& robot_state,
	const collision_detection::AllowedCollisionMatrix& acm) const
	{
	// check collision with the world using the padded version
	getCollisionEnv()->checkRobotCollision(req, res, robot_state, acm);
	// do self-collision checking with the padded version of the robot
	if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
	getCollisionEnv()->checkSelfCollision(req, res, robot_state, acm);
	}

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.

moveit/moveit_core/planning_scene/src/planning_scene.cpp

Line 156 in 3a9e63a

setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create());

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.

moveit/moveit_core/collision_detection/src/collision_env.cpp

Line 111 in 3a9e63a

void CollisionEnv::setPadding(double padding)

moveit/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp

Line 437 in 3a9e63a

void CollisionEnvFCL::updatedPaddingOrScaling(const std::vector<std::string>& links)

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.

moveit/moveit_core/collision_detection_fcl/src/collision_common.cpp

Lines 902 to 903 in 3a9e63a

	shapes::ShapePtr scaled_shape(shape->clone());
	scaled_shape->scaleAndPadd(scale, padding);

This is also used in Bullet

moveit/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp

Lines 405 to 409 in 3a9e63a

	if (fabs(getLinkScale(link->name) - 1.0) >= std::numeric_limits<double>::epsilon() ||
	fabs(getLinkPadding(link->name)) >= std::numeric_limits<double>::epsilon())
	{
	shape->scaleAndPadd(getLinkScale(link->name), getLinkPadding(link->name));
	}

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]

#24 due to trying to sync with the latest branch, it was requested to temporarily revert (#26).
Reopening this issue as a reminder that the padding visual is not enabled at the moment.