Collision shape poses of gripper links are not updated correctly

[mmurooka]

Regarding gripper links, it seems that the convex shape poses are not updated correctly for calculating the collision distance in the collision constraint.

Problem details

In the following example video, the pair of R_HIP_Y_S (right thigh) and R_LTHUMB_S (right gripper finger) are added to the collision check pairs of JVRC1 (patch is mmurooka@b2ed0c5).
As you can see, when the gripper is opened and closed, the gripper link poses of the robot model and the green convex shapes are updated correctly. However, the arrow that connects the closest points of the collision check result is not updated correctly. If the robot moves the shoulder joint instead of opening and closing the gripper, it will be updated.

Example video:
https://www.dropbox.com/s/rsws9e71gthe8xj/jvrc1-collision-distance-girpper-position-20210822.mp4?dl=0

I ran the following command for this example.

$ rosrun mc_rtc_ticker mc_rtc_ticker
$ roslaunch mc_convex_visualization convex_publisher.launch robot:=JVRC1 tf_prefix:=control
$ roslaunch mc_rtc_ticker display.launch
# Add `/sch_marker` topic visualization for convex shapes used for collision checking

Corresponding video for HRP5P:
https://www.dropbox.com/s/q00n05ysdektdm6/hrp5p-collision-distance-girpper-position-20210822.mp4?dl=0

In my use case, HRP2KAI has a problem with grippers hitting thigh links while reaching an object.

Cause of a problem

I still don't know how to solve it... Below is some information that might be useful.

The pose of the collision shape is updated in the next line, and this is where the problem lies?
https://github.com/jrl-umi3218/Tasks/blob/cb38fb5e29faa94864c5d78359276da53acca003/src/QPConstr.cpp#L482

The poses for visualizing the robot model and convex shapes are based on tf, which is updated below. Gripper link poses are handled correctly in this part.

mc_rtc/plugins/ROS/src/mc_rtc_ros/ros.cpp

Line 313 in 9509307

update_tf(data.tfs[tfs_i++], X_succp_succ * mbc.parentToSon[static_cast<size_t>(j)] * X_predp_pred.inv());

This issue may essentially be related to mimic joints rather than grippers.

[gergondet]

Hello @mmurooka

The problem is that the collision shapes are attached to the gripper links. However, these links are not directly controlled by the QP but by the gripper mechanism which overrides the configuration output by the QP. When we start a new control iteration we re-inject the configuration obtained via the QP control and thus the collision shape that is considered by the QP is different from what can be visualized (one way to see it would be to disable the grippers in the RobotModule)

For now, the recommended work-around is to use an open/closed collision shape representing the gripper that is attached to the last link effectively controlled by the QP (e.g. L|RARM_LINK6 for HRP2). In general this is also better performance-wise as there's only one collision pair to check instead of however many fingers you need to test.

The better approach would be to update the gripper configuration (which is almost done now except for dynamic quantities) from the gripper control so that the QP has the right configuration for every links but I'm a little afraid of the side effects on this...

[mmurooka]

For now, the recommended work-around is to use an open/closed collision shape representing the gripper that is attached to the last link effectively controlled by the QP (e.g. L|RARM_LINK6 for HRP2). In general this is also better performance-wise as there's only one collision pair to check instead of however many fingers you need to test.

I tried this workaround.

With the following two patches for hrp2_drc_description and mc-hrp2 I tried my controller, and it seems to be working.

• Update convex shapes of L|RARM_LINK6
  https://gite.lirmm.fr/mc-hrp2/hrp2_drc_description/-/commit/68abcc52e3171d12f49fd933966655dc9efcea90
• Remove collision pairs involving finger links
  https://gite.lirmm.fr/mc-hrp2/mc-hrp2/-/commit/051e39c628249bebbf0d7c71e9657f29852806c4

In order to genereate a convex file merging multiple links, I generated a simple controller to dump merged convex vertices. (Please let me know if there was a better way)
mmurooka@5c2403f

[gergondet]

In order to genereate a convex file merging multiple links, I generated a simple controller to dump merged convex vertices. (Please let me know if there was a better way)

I think last time I had to do something like this I imported the individual meshes in Blender, reported their transformation to form the full shape and generated the convex from that global mesh so your way is definitely an improvement 👍

[mmurooka]

Collision distances for gripper links are not calculated correctly, even with the latest mc_rtc, as shown in the video.
With the introduction of canonical robots, has it become easier to resolve this cleanly?

gripper-collision-20221226.mp4

[gergondet]

Maybe... The underlying issue is the same. The convexes in the video are attached to bodies that are not controlled by the QP but their position is updated based on the QP control model.

With the introduction of canonical robots, we could use the canonical position to update the convex positions and if the gripper joints are correctly excluded from the control model it should behave nicely. However, the position update is currently done in Tasks which is not aware of the canonical model.

The position update (currently in https://github.com/jrl-umi3218/Tasks/blob/master/src/QPConstr.cpp#L471) needs to be moved to mc_rtc, this poses a small problem to get the closed loop behavior to work correctly as we need to choose the update source based on the selected feedback.