Implement effort limits input port

[amcastro-tri]

Is your feature request related to a problem? Please describe. Some hardware platforms allow to update effort limits dynamically. E.g. grippers. However our implicit PD controllers use a fixed effort limit value from JointActuator::effort_limit().

Describe the solution you'd like We'd provide an MbP::get_effort_limits_input_port() so that external systems to the plant can dynamically change effort limits as the simulation progresses.

Describe alternatives you've considered

@jwnimmer-tri proposes:

u_pd = -Kp⋅(q − qd) - Kd⋅(v − vd)
u_pd_sat = clamp(u_pd, pd_limit_u_lower, pd_limit_u_upper)
u = clamp(u_pd_sat + u_ff, -e, e)

@amcastro-tri proposes to implement:

ũ = -Kp⋅(q − qd) - Kd⋅(v − vd) + u_ff
e_lower = max(u_lower, -e)
e_upper = min(u_upper, e)
u = max(e_lower, min(e_upper, ũ))

There is of course also the "why not both?" option:

u_pd = -Kp⋅(q − qd) - Kd⋅(v − vd)
u_pd_sat = clamp(u_pd, pd_limit_u_lower, pd_limit_u_upper)
u_pdff_sat = clamp(u_pd_sat + u_ff, net_limit_u_lower, net_limit_u_upper)
u = clamp(u_pdff_sat, -e, e)

Additional context Slack conversation in Drake developers.

cc'ing @jwnimmer-tri, @EricCousineau-TRI, @joemasterjohn, @dmcconachie-tri

[jwnimmer-tri]

I'll start by reminding the audience that we already have a (constant) max effort limit available as a property of the actuator itself. That limit would remain intact and unchanged, and always be a limit of last resort. (It's named e in the math above.)

The question in this issue revolves around enforcing even narrower limits based on some time-varying control policy (like "don't grasp that tomoto too hard!" but still "grab that spatula pretty strongly").

To summarize, the question for a roboticist is when computing the net actuation when under implicit pd control, should we allow for clamping (1) the pd term only, or (2) the pd+ff term only, or (3) each in turn, with distinct limits? Those are the three choices of math shown in the above.

Any time we enhance (change) the math for implicit PD, we need to screw around with MbP input ports and their sizes (in order to input the "arguments" of the math -- in this case, the various limits), which is a pretty big pain in the butt. So, we should try to think carefully and get it right the first time, so that we don't need to keep changing the code over and over again.

The context where this came up was trying to simulate the WSG controller using implicit PD. Our current WSG models use explicit PD. It's probably worth looking at the WSG docs (e.g., here) when responding here.

Hopefully a roboticist can provide some suggestions.

[RussTedrake]

I think we all agree that the fundamental challenge here is that the implicit PD is breaking abstractions. Details about WSG controller modeling should happen outside of MbP, not inside it. (The same problem happens when implementing joint stiffness control; we have the gravity comp outside of MbP but the implicit PD inside; the abstraction/encapsulation is broken.). Of course, we need implicit PD for simulating stiff controllers.

The most ideal solution, imho, would be making SAP general enough to move to the level of an Integrator, and all Systems could declare their constraints which define the time-stepper. Another sign that we need this is the fact that we are inventing a second constraint language inside MbP, but we should really have that at the level of all Systems. I understand that's far out of scope for this issue, but I want to at least plant that seed.

From the robotics perspective, the only "clamp" that I feel is fundamental is the final one that looks approximately like a current control on the motor controller, written: u = clamp(u_pd + u_ff, -e, e) above, which is what I think we have already. All of the other clamps fall into the territory of "a driver control design choice made by robotics company X" could be basically anything, no?

[jwnimmer-tri]

I agree with the general observations that MbP should have reused (i.e., should be refactored moving forward to reuse) more vocabulary from our symbolic engine and our solvers and systems constraints languages, instead of reinventing the wheel.

One question / clarification, though: I understand how state constraints (like a mimic joint, i.e., coupler constraint) on MbP relate to system constraints and/or integrator constraints. I don't quite follow how it relates to implicit PD. For PD control we have commanded_state (or at least commanded_position) input data (i.e., coming from the user / controller, not merely a function of the MbP's current state), and we have some gains, and together they boil down to actuation input u. Are you saying that we should use the constraint vocabulary for that? It seems like the constraint would need to be a function of state and user input, not just the state. Do we have a formulation of constraints that involve non-state input?

From the robotics perspective, the only "clamp" that I feel is fundamental is the final one that looks approximately like a current control on the motor controller, written: u = clamp(u_pd + u_ff, -e, e) above, which is what I think we have already.

Yes. (Assuming you term e to be a setup-time constant, i.e., a parameter of the model -- not time-varying.)

All of the other clamps fall into the territory of "a driver control design choice made by robotics company X" could be basically anything, no?

Yes.

I suppose the question is -- we have a relatively important short-term goal of turning on implicit PD for our simulated grippers, to improve simulation speed. Even though the options for control design are unbounded as you say, could we settle on one of the modeling choices above in order to expand the set of control designs we can simulate efficiently, hopefully in a way that can best support foreseeable (common) use cases, and that we are least unhappy with? In other words, which of the design(s) are most likely to capture "a driver control design choice made by robotics company X" for values of X we're familiar with.

[dmcconachie-tri]

@EricCousineau-TRI can you comment on what the WSG does currently, as well how it operates/we intend to use it in LUA mode?

[jwnimmer-tri]

FYI From my quick skim of the current state of affairs, everything in Anzu uses the LCM driver @anzu//tools:schunk_driver (not the ROS driver), and our Schunk LCM message does not have any u_ff command, and so u_ff is always zero/disconnected/ignored, and so all three formulas above are equally suitable / interchangeable.

The lua mode looks like it might add some PD deadbanding below |v| < 0.05mm/s but I'm not sure how much it matters to simulate that? If we must simulate it, then I guess that's yet another thing MbP PD will somehow need to deal with.

That's all just current-state-of-affairs though. (Please correct me if I got it wrong.) I don't know about any planned / future changes.

[RussTedrake]

I don't quite follow how it relates to implicit PD.

Certainly system constraints should be able to work with inputs, too. If we don't have that yet, we should be able to.

I agree that we have a short-term need. I just want to call out that, for me, this is very reminiscent of what started happening in the old matlab Drake -- we tried to shove more and more "systems" (at the time it was thinking like we today call multibody forces) into the multibody system in order to use them with multibody features. The fact that the Wing class, for instance, is outside of MultibodyPlant now is a major success in my eyes. But now we are slipping back into the trap of shoving more dynamics into the plant.

In other words, which of the design(s) are most likely to capture "a driver control design choice made by robotics company X" for values of X we're familiar with.

I don't have a strong opinion about that, I'm afraid. I think the WSG driver is pretty unique (and complicated), but it is one that we use very often. It seems reasonable for grippers that having a limit on (total?) clamping force on the input port is reasonable... my instinct is that this should still be at the level of clamp(u_pd + u_ff). That is the force exerted on the manipuland (by the fingers), and seems the most relevant.

By the principle of least astonishment. I would say that if I passed in a force limit and a too-large u_ff, then I think most people would not be surprised to see it clamped. I think some people might be surprised to ever see forces that exceeded a force limit if it was passed in.

But again, I haven't studied the WSG controller details as carefully as some others on the team have.

[EricCousineau-TRI]

can you comment on what the WSG does currently, as well how it operates/we intend to use it in LUA mode?

Our usage of the WSG via Lua scripting is to do position control via velocity control with force limits. The code we use:

-- pd controller
local e = cmd_pos - pos
local de = cmd_vel - vel
local act_vel = cmd_kp * e + cmd_kd * de

if math.abs(act_vel) < CMD_VEL_DEADBAND then
    act_vel = 0.0
end

-- command
mc.speed(act_vel)

-- force limit
if mc.blocked() then
    mc.force(cmd_blocked_force_limit)
else
    mc.force(cmd_travel_force_limit)
end

We do not have the ability to provide feedforward force (that I know of). mc.force() is for setting the force limit.

Has someone tested out mechanical coupling of fingers (with a basic position coupler), adding reflected inertia, and some estimate of friction? Would those help to stabilize simulation time steps?