Drake: Implicit PD control in MultibodyPlant

[amcastro-tri]

Problem Statement

I recently examined a number of our robot simulations. In particular, @calderpg-tri's and @EricCousineau-TRI's sims. I was trying to evaluate performance and stability. The conclusion is the same for both: stability (which prevents us from using larger time steps and thus having faster sims) stems from the explicit treatment of PD control in our grippers. That is;

• Grippers are usually formed by small, very light, fingers.
• PD gains usually need to be stiff to emulate the low-level control in real hardware.
• High PD gains + small finger masses == instability.

Proposed Solution

The solution I propose is that our discrete solver takes care of PD gains (with force limits to emulate real applications) implicitly.

Proposed API

This however requires an API design and I'd like feedback on this. My preferred option/workflow thus far is something along:

• MBP::AddPdController(const Joint& joint, double Pgain, double Dgain, double force_limit);
• At MBP::Finalize() MBP would create a new port, MBP::get_desired_pd_positions(), that would be of vector type and have an entry per each PD controlled position.
• The application layer then would wire the pd_positions port to whoever produces the desired positions (an LCM subscriber, a controller system, etc.)
• get_actuation_input_port() and get_applied_generalized_force_input_port() could still be used to "add" external contributions if needed. That is, all contributions are added together.

Thoughts? other ideas and/or alternatives for this API?

[jwnimmer-tri]

Related to #11551 perhaps?

[amcastro-tri]

yes @jwnimmer-tri, thanks. The API above (or similar) would eliminate all that huge modeling mentioned in #11551.

[ggould-tri]

@jwnimmer-tri Very much so! The high gains do not emulate low-level control (the WSG-50 is actually quite slow) but rather model the physical linkage that Drake is unable to handle.

[calderpg-tri]

Yes, the bad WSG model is the root cause for all of our simulated gripper woes - the Panda gripper is simulated as if it were a WSG, just with different geometry.

[ggould-tri]

We should distinguish between the gains that maintain finger symmetry (which are in reality almost infinitely stiff) and the gains that control the finger separation (which are in reality low and bounded). A central issue of #11551 is that those two are handled the same way.

IMO at no point should we expose an API that treats each finger's position separately. If the mathematical demands of Drake cannot be satisfied with the 1dof reality of the gripper, then the two dofs should be center (incredibly high force) and width (limited force, stiff joint limit) to sanely model the toothed belt linkage.

(With the 1/10 mm accurate calipers I was using at the time I was never able to measured any asymmetry of the schunk fingers at any force level I could attain up to putting my full body weight on the finger bases. The symmetry of schunk fingers is a more fundamental truth than most of the things we model as weld joints.)

[amcastro-tri]

I agree completely with you @ggould-tri. However I am trying to solver one problem at a time. I see two problems to be solved:

1. Implicit PD control
2. Constraints; which in this case would enforce both fingers move together.

I am attempting to tackle (1), which is much lower hanging fruit. For now yes, implementing (1) would still require you to treat each finger separately (you do now) but what does it buy you? 5 to 10X faster sim. I think ok as a first step?

[calderpg-tri]

Even without performance improvements, I think the benefits of getting rid of the current model and replacing it with something easier to work with would be worth it.

In the absence of a specific plan for adding constraints that this gets in the way of, I'm willing to accept that we keep treating the gripper as 2-DoF in return for better stability and speed.

[EricCousineau-TRI]

Abstractly, I'm sad about this weird boundary between the System integrators and the MBP solver that requires coupling like embedding a PD controller so that the solver can handle the mixing.

Concretely / pragmatically, however, I'm happy for a speed-up!

Re: symmetry, constraints, agreed w/ Grant that it would be nice-to-have, but agree w/ Ale that moving forward in perf. would be great.

Thoughts? other ideas and/or alternatives for this API?

Is there any feasible solution towards keeping the PD control decoupled from MBP itself, and still keep it w/in Systems? (perhaps, even in just a way visible to users? Under the hood, do whatevs?)
\cc @sherm1

[sherm1]

Is there any feasible solution towards keeping the PD control decoupled from MBP itself, and still keep it w/in Systems?

Implicit integration can be applied at the Diagram level so could provide similar performance without MBP knowing about PD control -- in theory! How to achieve that in practice is a research project for us at the moment. OTOH MBP's discrete solver (of necessity localized to within MBP) can get us speed now. But it can only operate on elements whose internals are visible to MBP. Currently that applies to contact, e.g., but the PD forces are opaque.

[jwnimmer-tri]

... the two [gripper] dofs should be center (incredibly high force) and width (limited force, stiff joint limit) to sanely model the toothed belt linkage.

How involved is it for us to prototype this? If we can replace one particular model without touching any MbP APIs, that seems like it could be an even quicker win to improve our performance?

[ggould-tri]

Unfortunately we moved away from that model (the old linkage) precisely because it had bad simulator performance (I assume worse than the current bad performance?). All roads lead to where we are, I'm afraid, unless stiff bushings somehow save us?

[EricCousineau-TRI]

Unfortunately we moved away from that model (the old linkage) [...]

I forgot; was this b/c it was using a simple holonomic constraint (h(q) = q[left] + q[right] = 0), or did it do something more complex to reflect the actual mechanism? starts digging in git

[EricCousineau-TRI]

Er, yeah, the pre-PR state looks heckin' complex to just to get the above holonomic constraint:
https://github.com/RobotLocomotion/drake/pull/8819/files#diff-97e742d11a51ce336c1d6fc51358f0b18d60f5730b1a46346c127811e922fa92

But if we did just the math, and ignored the (overly?) complex mechanics modeling, may be good?
Is there a quick hack to try this and see what it does for stiffness, as Jeremy said?

[amcastro-tri]

I couldn't quite figure out what that looks like, but it'd seem left_pusher has two parents? were thee loop joints in that model?

unless stiff bushings somehow save us?

Same problem, they are treated explicitly and they'd blow up. We could incorporate them implicitly, but that'd require more software changes.

[sherm1]

unless stiff bushings somehow save us?

Same problem, they are treated explicitly

Although bushings are already MBP force elements so could be treated implicitly without any API changes.

[EricCousineau-TRI]

If the goal is to have something quick to compute, not stiff, why not use a simple holonomic constraint? (and just let it effectively be "feedforward"?)

[sherm1]

why not use a simple holonomic constraint?

That is exactly the right solution here since the PD controller is being used in lieu! But MBP does not currently support any constraints :(. That's been on our to-do list for years but hasn't managed to win the priority war (and "simple" isn't the right adjective for this project). Also in general PD controllers aren't attempting to emulate a constraint so the problem Alejandro wants to address would still exist if we had a proper constraint implementation.

[EricCousineau-TRI]

[...] would still exist if we had a proper constraint implementation.

Gotcha! Hence Ale trying to decouple constraints from stiffness from the start - my b for switching around :facepalm:

[jwnimmer-tri]

... so the problem Alejandro wants to address would still exist if we had a proper constraint implementation.

If the goals of adding implicit PD extend beyond the problem of simulating parallel grippers sans constraints, then I think for your API design work you should evaluate more use cases than just the gripper problem. If you can write down one or two more distinct use cases, then you are more likely to choose an API that will meet those future needs as well, as you evaluate whether a proposed API is suitable.

[RussTedrake]

Just to be clear... there are two PD gains in the gripper model -- one set to make the fingers open and close, but the stiffer set is to make the two fingers act as one DOF. In the schunk this is a physical constraint implemented with a mechanical linkage. We should be able to address it correctly with a proper constraint implementation. And it is the stiffer of the two gains by a factor of 10. Correct?

[jwnimmer-tri]

@RussTedrake To clarify, there are two different controllers:

(1) The one you linked, SchunkWsgPdController, which has two pairs of pd gains, yes.
(2) The SchunkWsgController which embeds a SchunkWsgPlainController, which has only one set of pid gains.

Most (but not all) of the applications in Anzu are using (2).

[amcastro-tri]

Also in general PD controllers aren't attempting to emulate a constraint so the problem Alejandro wants to address would still exist if we had a proper constraint implementation.

Exactly what @sherm1 said.

Having constraints and implicit PDs are two orthogonal capabilities. IMO we must have them both.
This issue is about implicit PDs, not constraints, even though I agree we should address that as well. However I believe the implicit PDs will quickly pay off with minimum effort.

[amcastro-tri]

then I think for your API design work you should evaluate more use cases than just the gripper problem.

Good observation @jwnimmer-tri. I gave that some thought. I believe another very common use case will be when users want to impose motions without having to add constraints. In that case they'd have stiff PD gains and no force limits. The API above would enable that.

[jwnimmer-tri]

At MBP::Finalize() MBP would create a new port ...

I wonder if having the WSG controller implementation happen partially pre-finalize and partially post-finalize is going to be a burden on the calling-code. I suggest you prototype a sketch of how the calling code will operate (e.g., by pseudocode patching a current demo to use the new API) before you invest into the MbP implementation too much.

[amcastro-tri]

Thanks @jwnimmer-tri, that's actually my plan yes. If I see it pays off I'll come back and report what I found out.