Gravity compensation of joint impendence controller

[JakobThumm]

We are currently conformance-checking our safety models. It seems like the gravity compensation of the controller is not well-tuned. The green capsules show how the robot should look like when given the desired joint position [2.5, 1.5, 0.0, 0.0, 0.0, 0.0], and the actual robot is significantly lower due to the gravity in simulation: When disabling gravity, this matches perfectly:

Could you check if the gravity compensation is correct @EdoardoRomiti?

[EdoardoRomiti]

Hi @JakobThumm can you better indicate what is the controller you are using? Can you link any repo or file where the controller is implemented? I imagine it is a XBot plugin. And also what you mean by disabling gravity? How can I reproduce this issue?

[manuelvogel12]

We wrote a Xbot Plugin ourselves: https://github.com/manuelvogel12/sara-shield/blob/main/safety_shield/src/sara_shield_xbot2.cc

The important parts are: setDefaultControlMode(ControlMode::Position() + ControlMode::Velocity()); and _robot->setPositionReference(_q) _robot->move();

We give it a position and everything works fine. But if we visualize the robot in RVIZ and additionally visualize where we assume the robot arm to be, there is this mismatch. (Only noticeable in positions where the arm is fully extended forward/backwards.)

When disabling gravity simulation of Gazebo (left panel --> Physics --> Gravity --> z --> set to 0), this mismatch goes away.

[EdoardoRomiti]

We wrote a Xbot Plugin ourselves: https://github.com/manuelvogel12/sara-shield/blob/main/safety_shield/src/sara_shield_xbot2.cc

Okay so I see the issue, you've not implemented any gravity compensation in this plugin (and it's not a feature active by default) so the behavior is normal. You should see the mismatch goes to zero if you either:

• raise the stiffnesses,
• implement a gravity compensation (gcomp) yourself.

For implementing gcomp you can have a look at this plugin as reference: https://github.com/ADVRHumanoids/xbot2_examples/blob/v2.10/src/gcomp_example/gcomp_example.cpp

To have gcomp you have two options:

1. Modify your plugin so to compute the gcomp term and send torque references as well. This requires to modify the control mode as such:

   setDefaultControlMode(ControlMode::Position() + ControlMode::Velocity() + ControlMode::Effort());

   and compute the gravity term (like is done in the example plugin)

   // compute gcomp torque
   _robot->model().computeGravityCompensation(_tauref);
   _robot->model().setJointEffort(_tauref);
   _robot->setReferenceFrom(_robot->model(), Sync::Effort);

2. A second simpler option is just to use the example plugin in parallel to your sara shield plugin. You'll need to launch both plugins and one will take care of sending effort references (gcomp_example) and the other to send position and velocity references (sara_shield_xbot2).

[manuelvogel12]

okay, thanks for the explanation

[JakobThumm]

Thanks Edoardo!