Tune admittance controller

[sangteak601]

I’ve set up the admittance controller, but I’m having difficulties tuning it. If anyone could help me with this, that would be really helpful.
You can follow these steps to start peg in hole example.

[sangteak601]

Screencast.from.2024.07.25.07.11.26.webm

@pac48 @dyackzan
This is what I'm experiencing now. What would be a problem here?

[sangteak601]

Screencast.from.2024.07.25.08.03.29.webm

This is without admittance controller.

[dyackzan]

It looks like you're probably getting positive feedback here. This could happen if your FTS frame axes are not properly aligned between your admittance controller configuration and your robot model(s). If you echo the FTS readings while you run this, I expect the FTS readings shoot up once contact is made.

[dyackzan]

Try negating these lines in mujoco_system.cpp first. Also ensure that your FTS frames are aligned between the URDF and the Mujoco model

[sangteak601]

@dyackzan Thanks for the reply. I will try to make them negative.
It seems direction of mujoco force sensor has a problem or at least unclear behavior. I found some similar issues

[sangteak601]

I'm not sure whether this is desired behavior or not, but this was the best I can do...
panda_peginhole.webm

[sangteak601]

@dyackzan @pac48
Hi guys,
I tried different values for stiffness and damping. It seems the motion doesn't change a lot.
I'm uploading data. Do you guys have any thoughts?

Also, there is another problem. When motion planning goal is canceled, robot goes out of control and moves crazily. You can see this at the end of this video.

1. mass = 30, damping = 0, stiffness = 0.01
   rosbag2_2024_08_07-19_33_38.zip
   

2. mass = 30, damping = 0, stiffness = 0.1
   rosbag2_2024_08_07-19_36_11.zip
   

3. mass = 30, damping = 0, stiffness = 1
   rosbag2_2024_08_07-19_37_43.zip
   

4. mass = 30, damping = 0.1, stiffness = 1
   rosbag2_2024_08_07-19_38_59.zip
   

5. mass = 30, damping = 1, stiffness = 1
   rosbag2_2024_08_07-19_40_41.zip
   

6. mass = 30, damping = 10, stiffness = 1
   rosbag2_2024_08_07-19_42_55.zip
   

[sangteak601]

@dyackzan
You can reproduce the problem with following setup.

• mujoco_ros2_control : peg_in_hole branch
• mujoco_ros2_control_examples : peg-in-hole branch
• run ros2 launch peg_in_hole peg_in_hole.py

[dyackzan]

I am getting better performance/less oscillation with the following admittance parameters:

      mass:
        - 8.0  # x
        - 8.0  # y
        - 8.0  # z
        - 8.0  # rx
        - 8.0  # ry
        - 8.0  # rz
      damping_ratio:  # damping can be used instead: zeta = D / (2 * sqrt( M * S ))
        - 30.  # x
        - 30.  # y
        - 30.  # z
        - 30.  # rx
        - 30.  # ry
        - 30.  # rz
      stiffness:
        - 60.  # x
        - 60.  # y
        - 60.  # z
        - 60.  # rx
        - 60.  # ry
        - 60.  # rz

As for the jump at the end, that is occurring because there is a big spike in the FTS readings right when the command is cancelled for some reason. You can see this spike in the PlotJuggler plot below that I recorded while running the demo:

To debug further, I would recommend you take a look at your ros2_mujoco system code to ensure that the jump in FTS values is actually coming through from MuJoCo and is being passed through correctly. Then we can rule out any issues coming from your ros2_mujoco package.

Once you check that and verify that MuJoCo is actually sending this discontinuous FTS reading, then you can start tweaking the MuJoCo simulation setup. Here are a couple ideas it may help to look closer into:

1. Is the convex decomposition causing an issue somehow? Try a simplified contact task (just pushing a flat peg against a flat surface) and see if this behavior is replicated.
2. Are your MuJoCo contact solver settings (like solref and solimp) configured correctly? See the MuJoCo Contact section of their documentation for more details.

[sangteak601]

@dyackzan Thanks David. It looks much better with new parameters!
I will take a look at the FTS issue.