Custom Impedance control problem

[Hydran00]

Hi @mhubii,

The following issue is not strictly related to this repo but more on the KUKA Fast Robot Interface and its capabilities, however I think this discussion could be useful.

We noticed an overshoot problem when using our implementation of the impedance control (both joint and cartesian), which uses your hardware interface. It seems that for some reason when using the proprietary JointImpedanceControlMode with stiffness and damping set to zero some terms are not compensated correctly.

To be more clear we implemented a joint impedance mode that follows the classic equation (derivation here):

$$\tau_{ext} = \boldsymbol{K}(\boldsymbol{q}_d - \boldsymbol{q}) + \boldsymbol{D}(\boldsymbol{\dot{q}}_d - \boldsymbol{\dot{q}}) + \boldsymbol{\hat{M}}(\boldsymbol{q})\boldsymbol{\ddot{q}}_d + \boldsymbol{\hat{c}}(\boldsymbol{q},\boldsymbol{\dot{q}}) + \boldsymbol{\hat{g}}(q) + \boldsymbol{\hat{h}}(q,\dot{q})$$

and we omitted gravity $g$, friction $h$ and coriolis term $c$ since they should be already compensated by KUKA.
Then, since our desired joint velocity and acceleration is zero we use the following control input:

$$\tau = \boldsymbol{K}(\boldsymbol{q}_d - \boldsymbol{q}) -\boldsymbol{D}\boldsymbol{\dot{q}}$$

which is the torque we send to the hardware interface, implemented here, however the obtained behaviour is different from the KUKA joint impedance control with the same stiffness and damping parameters (critically damped value) so there should be some missing term that depends on $K$ and $D$.

We also plotted the torque difference for joint A1 between our controller (connected via FRI and KUKA impedance controller gains set to zero) and the KUKA impedance controller with the same gains as our controller. In the first image you can see the torque comparison, while on the second the joint trajectory of the same joint. This should be the cause of our overshoots. This is confirmed also by the fact that both our cartesian and joint impedance controllers do not show overshooting in the gazebo simulation.

Have you ever noticed this issue?

[mhubii]

hi @Hydran00 and thank you for the very detailed report. There is a lot to take in. Just trying to understand.

In essence, the commanded torque (on the robot) deviates from the requested torque (as computed by your implementation), hence the actual robot overshoots? I have to admit that this is a little beyond what we are doing.

The dynamic parameters in the URDF are not very accurate. Could this cause the issue?

I have to say that your repository is starting to be quite awesome! I am very much looking forward to try it out and use it for our own research. Maybe it makes sense to add a link to your repo somewhere on the landing page here, possibly just add it to the repos files.

[Hydran00]

Hi @mhubi, thank you for the reply.

In essence, the comparison is done with the following setting:

our ROS2 control:

• Sending the overlay torque $\tau$ with stiffness set to $K$ and damping set to $D$ to the FRI
• Stiffness and Damping gains of the Sunrise joint impedance control mode set to zero in the LBRServer.java to simulate a simple gravity and Coriolis compensation

Kuka impedance control:

• Sending $\tau=0$ overlay torque to the FRI
• Stiffness set to be the same as $K$ and damping the same as $D$ in the Sunrise joint impedance control mode in the LBRServer.java

So what we noticed is that the two commanded torques are different, and this may be the cause of the overshoot issue (which is visible also in the video in our README.md.)

We did not understand which term (that should depend on $K$ and $D$ since it is the only difference between the total torque actuated) is missing.
Furthermore, the dynamics parameters are not involved in the computation of $\tau$ so they should not be the problem.

You might wonder how we did that plot, we did it by using our controller and plotting $\tau$ that should be sent to the robot, but then we overwrote it to zero before sending it to FRI. At the same time, stiffness and damping are set to $K$ and $D$ in the sunrise application, and we plot the torques using the ones published in /lbr/state/commanded_torque by your hw interface. Note that we considered just the joint 1 because /lbr/state/commanded_torque accounts also for the gravity contribution, which since it is zero for joint 1, it allows us to make a fair comparison.

With respect to the possibility of adding our repo to your landing page, we would really appreciate that. 🙂