Safety is triggered with grepper open/close commands

[mmurooka]

As shown in the video below, when the gripper's open/close commands are sent, safety is triggered and the open/close cannot be completed.

[warning] Gripper safety triggered on R_UTHUMB

https://user-images.githubusercontent.com/6636600/235350928-027f1642-d43c-4dee-8471-d5375878a571.mp4

As shown in the graph below, the gripper joint does not track well.

Below is the same graph for the case of mc_openrtm + Choreonoid. This graph shows good tracking.

For reference, the PD gains for joint control in mujoco are as follows: Increasing the PD gain did not improve the results.

[info] [mc_mujoco] R_UTHUMB, pgain = 30.0, dgain = 3.0
[info] [mc_mujoco] R_LTHUMB, pgain = 30.0, dgain = 3.0
[info] [mc_mujoco] R_UINDEX, pgain = 30.0, dgain = 3.0
[info] [mc_mujoco] R_LINDEX, pgain = 30.0, dgain = 3.0
[info] [mc_mujoco] R_ULITTLE, pgain = 30.0, dgain = 3.0
[info] [mc_mujoco] R_LLITTLE, pgain = 30.0, dgain = 3.0

[rohanpsingh]

Hi @mmurooka san, If I reduce the maximum velocity percentage (from 0.250 to about 0.050), opening/closing works as expected and safety is not triggered. Is it the same in choreonoid? If the grippers can work fine at higher max velocity in choreonoid, maybe the inertial parameters of the gripper links in mujoco are wrong?

[mmurooka]

If the grippers can work fine at higher max velocity in choreonoid

Yes.

Below is the same graph for the case of mc_openrtm + Choreonoid. This graph shows good tracking.

This graph shows data from Choreonoid with the default parameters (i.e., maximum velocity percentage is 0.250).

the inertial parameters of the gripper links in mujoco are wrong?

The inertia parameters seem to be consistent (and the other parameters also seem to be consistent at a glance).

MuJoCo model: https://github.com/isri-aist/jvrc_mj_description/blob/bfb2bf25966ddfe0d0248385ef7e2586963c86a0/xml/jvrc1.xml#L310

Choreonoid model: https://github.com/jrl-umi3218/mc_openrtm/blob/master/projects/JVRC1/model/JVRC1/main.in.wrl#L776-L778

[mmurooka]

In the mc_rtc log in mc_mujoco, I found that the alphaOut (command velocity) of the gripper joints is always zero. For other joints (e.g. leg joints), the alphaOut is set correctly.

[mmurooka]

In the mc_rtc log in mc_mujoco, I found that the alphaOut (command velocity) of the gripper joints is always zero. For other joints (e.g. leg joints), the alphaOut is set correctly.

The above was reproduced in both logs of mc_mujoco and Choreonoid (via mc_openrtm). This means that the issue is not with mc_mujoco, but with mc_rtc. In the case of Choreonoid, the PDcontroller RTC calculates the target velocity independently of mc_rtc, so the safety triggering issue did not occur.

[rohanpsingh]

Yes, I confirmed alphaOut for gripper joints is zero even in case of HRP5P. And HRP5P grippers work fine in mc_mujoco

[mmurooka]

And HRP5P grippers work fine in mc_mujoco

Perhaps the velocity limit of the gripper joints was originally small, so that even with imperfect PD control, the safety trigger threshold was not exceeded. If you check the graphs for the tracking of the gripper joints, I would expect MuJoCo to have poorer performance than Choreonoid (or real robot).

[gergondet]

The grippers are always position controlled outside of the QP in mc_rtc so alphaOut is not filled for them. This can be fixed in mc_rtc but as a quick fix, I tried the following:

diff --git a/src/mj_sim.cpp b/src/mj_sim.cpp
index 04d3325..a9ca294 100644
--- a/src/mj_sim.cpp
+++ b/src/mj_sim.cpp
@@ -649,7 +649,7 @@ void MjRobot::updateControl(const mc_rbdyn::Robot & robot)
     if(jIndex != -1)
     {
       mj_next_ctrl_q[ctrl_idx] = robot.mbc().q[jIndex][0];
-      mj_next_ctrl_alpha[ctrl_idx] = robot.mbc().alpha[jIndex][0];
+      mj_next_ctrl_alpha[ctrl_idx] = (mj_next_ctrl_q[ctrl_idx] - mj_prev_ctrl_q[ctrl_idx]) / 0.005;
       mj_next_ctrl_jointTorque[ctrl_idx] = robot.mbc().jointTorque[jIndex][0];
       ctrl_idx++;
     }

This still tracks poorly.

I then changed the PDgains from 30/3 to 500/24. I almost got there but it was not tracking the closing well. It turns out with the most recent modification on the gripper kinematic there's a self-collision between some of the fingers joints and the wrist link. After filtering those, the grippers can track without issue even at max speed.

I have pushed the changes I have made to the model. With the default velocity setting (0.25) it can track without the patch in mc_mujoco. I have opened an issue in mc_rtc to track the underlying issue.

[mmurooka]

Thank you @gergondet

It turns out with the most recent modification on the gripper kinematic there's a self-collision

I hadn't noticed this..

After the underlying issue is resolved on the mc_rtc side, the PD gains may be changed to be the same as for Choreonoid (currently only gripper joints have different gains).

[gergondet]

After the underlying issue is resolved on the mc_rtc side, the PD gains may be changed to be the same as for Choreonoid (currently only gripper joints have different gains).

I will give it a look after the fix happens in mc_rtc. I just put arbitrarily high gains to check the tracking issue after the temporary fix but I hand't realized the gains were updated on Choreonoid side.

[gergondet]

Now that the issue is patched on mc_rtc side I gave it a try with the 100/10 gain. It's ok for the default velocity setting but still fails to track at 100% velocity. I haven't check in Choreonoid yet if the same issue happens.

[mmurooka]

Thank you. I tried it with a non-patched version of mc_rtc, but gripper opening/closing worked with Choreonoid at 100/10 gain even at 100% velocity.

I don't necessarily insist on having the gains exactly the same for Choreonoid and MuJoCo, so even if you need different gains to match the behavior of Choreonoid and MuJoCo, closing this issue is not a problem for me.

[gergondet]

Ok, after giving it a few more tries to bring the gains closer to Choreonoid I have decided to give up for now and keep the gain we have that works correctly at 100% speed after the issue has been addressed on mc_rtc side.