Ability to lock/unlock joints from within a simulation

[ggould-tri]

We are now at n=2 cases where someone has wanted to do joint locking/unlocking based on the current state of a simulation:

• https://drakedevelopers.slack.com/archives/C2WBPQDB7/p1697058667553179
• https://drakedevelopers.slack.com/archives/C2CHRT98E/p1700593913640549

Currently joints can only be locked by something with non-const access to the MBP Context; that means that nothing running inside of the Diagram has the necessary ability.

Jeremy proposes a "locked joints" input port on the multibody plant as the pure-dynamics solution to this problem, which seems very sensible, albeit somewhat discontinuous. Other solutions are probably also possible.

This feature probably has a low reward-vs-effort tradeoff for now; this issue is just to track it and memorialize the discussions about it for future reference and in case more use cases appear.

[xuchenhan-tri]

Enabling/disabling constraints has the same flavor and seems to be a desired feature too.

https://stackoverflow.com/questions/77427378/breakable-joint-in-drake

[jwnimmer-tri]

It's too bad this ticket didn't obey the feature request template. It is completely lacking in describing the problem, which is the critical requirements-gathering phase of designing any solution.

As it stands, we should just close it unless we can actually capture some meaningful amount of backstory here.

[sherm1]

This can be done today by using an event handler or the Simulator's monitor function to interrupt the simulation (via EventStatus, causing a return from AdvanceTo()), modify parameters, resume simulation.

A possibly nicer solution would be to add a fourth event handler type that has write access to the parameters -- so we'd have publish, discrete update, unrestricted update, really unrestricted update or something like that. Russ didn't like that IIRC in a discussion long ago since it makes the parameters look like state variables, but physically they are indistinguishable so that doesn't bother me.

[jwnimmer-tri]

A possibly nicer solution would be to add a fourth event handler type that has write access to the parameters ...

There's nowhere to declare that event, though. Only the plant can declare events that change the plant context.

So if we imagine a plant function like MbP::DeclareJointLockDynamics(std::function<vector<bool>, const Context&> calc_locking); then we might as well switch the lock data from Parameters to State anyway, no new event type needed.

---

Again, this has all been discussed elsewhere and the issue summary is astonishingly lacking and it's probably not a good use of our time to propose solutions until the actual problem that needs solving is carefully summarized, to allow for thoughtful responses.

[RussTedrake]

I do think we want a feature for this: turning on/off constraints (or similarly locking/unlocking joints) from within a simulation. My inclination is also for having an input port.

Here are two specific use cases i've heard requested: 1) A very minimal model for a suction gripper. When the user sets the "suction" input port to high, then an object that is close to the suction gripper gets a weld joint added between the gripper and the object. 2) Don't simulate the physics of objects (e.g. piles of boxes) that are > that some threshold distance away from the robot of interest. Locking will reduce the computation whereas weld joints, I believe, will not.

I'm happy to say that we should factor these into two, possibly more detailed, feature requests. But I do think it's an important feature request. Imo, forcing the user to stop the simulation to add/remove constraints or lock/unlock joints breaks the encapsulation that the systems framework is intended to provide.

[sherm1]

If the lock/unlock booleans were state variables rather than parameters we wouldn't be having this discussion. IIRC we didn't do that because the presence of extra state variables was going to be problematic for some analysis you wanted to be able to do, Russ. Is that still a problem?

[RussTedrake]

I see. Would they at least be abstract state? I definitely find it inelegant to have those in the state space of every multibodyplant. But I am also sympathetic to the fact that shoving things into parameters isn't always the fix.

In practice, yes -- we do have a lot of code (trajectory optimization, etc) that starts by confirming that the system has only continuous state (or only discrete state and simple periodic updates) for analysis. Putting in other states would definitely at least throw a wrench in things.

[sherm1]

Yeah, they'd be abstract state of type bool.

[RussTedrake]

Follow-up thought: SAP constraints get added specifically by the user. I could see the active/inactive status of those constraints living as abstract states of type bool. The simple models would still stay simple, and it is only by opting in (declaring constraints), that the state gets this clutter.

We could potentially do the same for joint locking? For the user to opt in to enabling joint locking for a particular joint/body? Only then does it get added as an abstract bool to the state? So the simple models stay clean?

[sherm1]

How about opt-out rather than opt-in for joint locking? We could apply the same to constraint enable/disable and any other auxiliary state variables. That way users would get the full functionality by default but those who want an uncluttered state could say so.

[jwnimmer-tri]

FYI I've posted TRI's internal prototype of a item sleeping in a gist here, to help unblock people in the meantime.