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. 🙂

[mhubii]

sorry again for the late response, I am in the middle of something. I'll spend the weekend to try out your controllers, hopefully get a clue regarding this issue.

Also, I'll see how to best integrate your controllers to enhance the capabilities of this stack.

[Hydran00]

I believe we have finally found the solution—at least for the Cartesian impedance controller. The issue was with how we were computing the damping coefficient. We had been using the equation:

$$D = 2 \xi \sqrt{K}$$

However, this formula is only valid for unit-mass dynamical systems or robots. While it works well for low-payload robots, the KUKA’s high inertia required a more accurate approach. The correct way to compute the damping term—taking into account the Cartesian inertia matrix—is explained this paper through the so-called double diagonalization, which I implemented here.
If you are interested, I followed this stackoverflow explanation.

We did the torque comparison again (this time with our Cartesian impedance control vs the KUKA original Cartesian impedance) control, and we obtained the following plot.

Here you can see the torque computed for joint 1 from our controller (in green) against KUKA one (blue).

We are quite happy with the result, since we managed to replicate almost perfectly the same torque that the proprietary controller computes. There is still some missing torque that is probably due to the dynamical model (urdf) inaccuracies which affect the damping coefficient computation via double diagonalization.

[tvercaut]

Not sure how helpful that is but it reminds me a bit of an issue our friends in Paris were looking into. This provides a summary of what they were trying to do and how they solved it:

Jimmy Da Silva, Saman Vafadar, Thibault Chandanson, Guillaume Morel. Force control of the KUKA LBR Med without external force sensor. 11 th edition of Conference on New Technologies for Computer and Robot Assisted Surgery, Apr 2022, Naples, Italy. ⟨hal-03768357⟩

There is more detail in @JimmyDaSilva's thesis (Sections 3.3-3.5):

Jimmy Da Silva. Automation of pedicle screw placement by coupling distal bone bio-impedance measurements and robotics. Robotics [cs.RO]. Sorbonne Université, 2022. English. ⟨NNT : 2022SORUS313⟩. ⟨tel-03900424⟩

[Hydran00]

Hi @tvercaut,

thank you for sharing that interesting resource. I had a look at the PhD thesis you linked. The problem you had seems a bit different from mine.

In particular (correct me if I am wrong) you were trying to design an input wrench for the KUKA's cartesian impedance control which achieve better tracking. So your new dynamics will account both for the KUKA's impedance control law and your input force. What we are trying to do instead is to being able to cancel the KUKA's impedance control law from the equation (setting stiffness and damping to zero) and using $$F_e$$ to impose our impedance law.

This allows us to design variable impedance control strategies that are not possible with original KUKA's impedance control, since it does not allow changing stiffness at runtime.