RobotLocomotion
Allow updating multibody constraints in the context with new specs values after Finalize()
Is your feature request related to a problem? Please describe. We are attempting to model an item in a soft bag as a runtime-mutable pendulum, and we wish to leverage the multibody distance constraints. However, currently Drake does not allow updating the constraints specs after Finalize().
Describe the solution you'd like Allow updating multibody constraints in the context with new specs values. Currently we only care about updating distance, but we are glad to see other specs becoming mutable too.
Describe alternatives you've considered We can also use bushing for this purpose, but (we heard that) constraints are better for numerics in SAP.
Additional context The slack discussion is here
so what I hear is that we'd like constraint specs to be parameters in the context. Is that right?
Per discussion in the Slack thread above.
The particular use case is that of modeling suction cup grippers. The model would then consist of welding a specific manipuland to the end effector. Not only the body index of the manipuland needs to be modifiable, but also the relative pose to it, which is only known at runtime.
Therefore, the requested feature is that all constraint parameters (including body index and relative poses) be parameters in the context so that they can be modified at runtime.
This plus the active/deactive feature already in MbP should allow for a simple gripper model that doesn't require fine detail modeling of contact nor suction.
... be parameters in the context so that they can be modified at runtime.
Of course parameters are better than nothing, but remember that parameters cannot be changed by diagram events. So, like we currently have with joint locking, switching the suction mode would need to happen via some beyond-the-simulator time stepper, which raises question of reproducibility and composability.
It's probably still worth implementing the parameter version, but we shouldn't be under the illusion that it solves the problem robustly.
parameters cannot be changed by diagram events
True. Maybe that's more of a system feature that we don't have? Should we consider parameters to be exposed as input ports? Doesn't Simulink allow this? cc'ing @sherm1
Yes, it's probable that doing it correctly would require new features from the framework. See also #20571.
Even having the ability to change the constraint specs programmatically (not via ports) post-finalize would already be a huge improvement. Like the way that joint locking is being done in practice now, we could then make these changes outside of the main simulation loop. Please let's not block the simpler feature waiting for a new framework capability.
I wasn't trying to propose blocking the quick fix now. I said "parameters are better than nothing".
Same here. I was mostly trying to decide parameter vs input. IMO, we should land the parameter version first, which essentially is a different flavor of "state". A framework solution to externally update those parameters (say via some sort of input port) can be added later when/if needed.
My thought is that parameterizing the floating point aspects of the constraint (distance and attachment points), plus enable/disable would be straightforward and useful. Changing which bodies are involved is a structural change and would be better handled as part of a separate effort to allow runtime addition and removal of constraints during simulation.
I think that for the uses cases we talked about in the above Slack thread, being able to change the bodies involved is a must, since the modeled gripper can grasp one of many objects in the scene. In terms of code, I don't see why the whole constraint' specs could not be a parameter, I believe it is just a matter of storing two additional indexes in the params struct
I see, that's good -- does that mean you don't have any persistent matrix structures so can deal with the structural change on the fly?
I believe so @sherm1, I'll play with this sometime this today and report back. Usually constriants are nice in that way, unlike our tree topology which we build once at the beginning of time.
Looking into the code, this is what I am proposing:
- Take
internal::DistanceConstraintSpecout ofinternal::so that users can see it. - Make
DistanceConstraintSpeca proper class rather than a a lil struct, for validity checks. - Declare numerical parameters and shove
DistanceConstraintSpecin there (much like what we do for inertias for instance) - Provide MbP APIs to set and reterive
DistanceConstraintSpecfrom the context.
BTW I noticed recently that Constraints in MbP aren't MultibodyElements (as are Joints, Bodies, Forces, Frames, etc.). That means they don't get the common features of elements: name, index, ordinal, model instance, is_ephemeral. Is there a reasonable way to include them in the MultibodyElement framework?
I'd say no for the scope of this issue. That'd require having a first class citizen MultibodyElement to represent constraints. I believe that's more work than what we need right now, and with little immediate return IMO.
If you see my plan outlined above, the API implied (MbP::SetDistanceConstraintSpecs(Context*, const MultibodyConstraintId, DistanceConstraintSpec&), I believe we can push an implementation today that doesn't require all that plumbing.