Drake: Better model of the IIWA internal controller

[RussTedrake]

@pangtao22 has done some reading and some experiments, and believes that we can make a pretty simple change to better model the controller.

As seen in the manipulation station example, we normally use the inverse dynamics controller which uses a PD controller (I believe we always set the integral terms to zero) to compute a desired acceleration:

vdot_desired = Kp(q_desired - q) + Kv(v_desired - v)
then
tau_command = inverse_dynamics(q, v, vdot_desired) = M(q)vdot_desired + C(q, v)v - tau_g(q) + tau_ff
(though i was surprised that the docs claim the opposite sign on tau_ff??)

We would expect the resulting dynamics to be
M(q)vdot = M(q)vdot_desired + tau_ff + tau_ext
or
vdot = vdot_desired + M⁻¹(q)*(tau_ff + tau_ext)

(minor detail: we set v_desired by interpolating from q_desired, which is the only command we can actually send besides tau_ff).

I believe @pangtao22 believes that a more accurate representation of what is happening is that the iiwa controller is doing
tau_command = M(q)vdot + C(q,v)v - tau_g(q) + Kp(q_desired - q) + Kv(v_desired - v) + tau_ff - tau_ext
resulting in the dynamics:
Kp(q_desired - q) + Kv(v_desired - v) + tau_ff = 0

@pangtao22 -- did I get that right? In particular, I cannot remember if you think it's cancelling tau_ext, also? Please feel free to edit the equations in this comment if you want to avoid copy and paste. Once we agree on the equations, can you add some evidence in terms of the papers you found and the plots you've made so we can come to consensus on this?

Thanks!
cc @siyuanfeng-tri @mpetersen94 @sammy-tri

[pangtao22]

Thanks for getting us started on this issue! I can't edit your comment so I'll have to copy some of the equations here.
I don't think tau_command is cancelling tau_ext. Also the damping is penalizing v, not v_desired - v So the controller is doing
tau_command = M(q)vdot + C(q,v)v - tau_g(q) + Kp(q_desired - q) - Kv v (v_desired - v) + tau_ff - tau_ext
And the dynamics is:
Kp(q_desired - q) - Kv v (v_desired - v) + tau_ff + tau_ext = 0

In steady state, if tau_ff = 0, the robot should satisfy
Kp*(q_desired - q) + tau_ext = 0

I first came across this controller on the 5th page of this paper, although it talks about the kaka lightweight robot, the predecessor of the IIWA.

It turns out that the joint stiffness we set in the java applications has unit N*m/rad. I plotted Kp*(q_command - q_measured), tau_external, and their sum when the robot is doing some fairly nontrivial, but slow, tasks (tau_ff is set to 0):

[RussTedrake]

thanks. if your motions are very slow, perhaps you don't have evidence yet about whether it is using a v_desired term? I thought that it would, and i had the impression that it might be using this for friction compensation as well? But it should be relatively easy to check? If you command some higher speed terms, then you would expect it to have a lag that increases with the derivative of the commanded position. And to spell it out, the implication is that we would want to think of the robot (though not the objects it is interacting with) as a first order system?

Kvv = Kp(q_desired - q) + tau_ff + tau_ext

[pangtao22]

That is what I had hoped for, but after running some tests on the robot, I'm inclined to believe that
Kv v = Kp(q_desired - q) + tau_ff + tau_ext
is not a good model.
TLDR: the time constant predicted by this first order model does not match the step response of the robot.

Here's why:
Assuming tau_ff = 0, tau_ext = 0 and q_desired is constant (step input), if the above equation were correct, the robot dynamics would be:

(Kv / Kp) q_dot + q = q_desired    

The time constant of this system is T = Kv / Kp. Further assuming a damping ratio of 1 (which is the value shown on the pendant when Iiwa driver starts, see picture),

we can deduce that T = 2 / (sqrt(Kp)). This has two implications:

1. The higher the stiffness, the faster the convergence. For example, if both given a step input, joint 2 would converge faster than joint 6.
2. For Kp = 200, the time constant is 0.14s, which means it would take some 0.42 seconds to converge.

But neither of these can be verified.

In the plots above, a step command of magnitude 0.005 is sent at t = 9.005. q_cmd does not look like a step because of Iiwa's motion interpolator. Although the interpolation makes it harder to judge the time it takes to converge, it definitely takes shorter than 3*T in both cases. Moreover, it takes joint 2 longer than joint 7 to converge despite joint 2's higher stiffness, although this might be the motion interpolator's fault.

These observations make me think that it's really hard to pinpoint what's going on in iiwa's controller box. The only conclusion I can draw from the logs so far is that

Kp*(q_desired - q) + tau_ff + tau_ext = 0

[RussTedrake]

I've looked into this a bit more, and I think everything is now consistent/clear. Basically all of the details (even the friction compensation model) are available in

The DLR Lightweight Robot – Design and Control Concepts for Robots in Human Environments by A. Albu-Schaffer et al, 2016

and

A. Albu-Schaffer, Ch. Ott, and G. Hirzinger, “A unified passivity based control framework for position, torque and impedance control of flexible joint robots,” International Journal of Robotics Research, vol. 26, no. 1, pp. 23–39, 2007.

and the references there-in.

Not sure if we even want to simulate the motors explicitly (using the reduced elastic model of spong), but we could try if we feel we need it for fidelity.

TL;DR - I now agree that Kp and Kd are not being multiplied by M(q) -- even wikipedia has a clear justification for that -- though think the model cancellation is a bit different that what we wrote above. I also think that there is a more fundamental reason for taking in (only) theta and torque (not thetadot), and that we might have been assigning too much to the "motion interpolation" block.

[pangtao22]

After digging through the papers Russ posted and some of their references, this is the most relevant, easy-to-follow and self-contained reference I've found so far:
https://ieeexplore.ieee.org/abstract/document/4451347