Allow weld/unweld of predefined free bodies without rebuilding model

[sherm1]

(Further follow-on from the Mutating Multibody Models Meeting -- see #13289 for references.)

Users reported wanting to be able to do these applications without rebuilding the Drake models (my paraphrasing):

• Package pickup: Lots of free objects lying around doing nothing (e.g. a truck full of packages). Shouldn't cost anything to have them there. Robot bumps or grabs a few. Those become active. An object once grabbed gets welded to the end effector and again doesn't have its own dynamics.
• Tool change: Model includes an assortment of tools (likely just rigid bodies; might have some articulation). Upon command, robot swaps its current tool with another one. Tools briefly gain some dynamics during the process but end up welded either to ground or the end effector.

For these applications, it is feasible to predefine all the bodies that will be included in the model, but many of them will be inactive for most of simulation or other uses. All geometry will be present for perception and proximity purposes, but otherwise inactive bodies should cost very little computationally. Conditions under which they should be made active must be detectable, but the switch from active (6dof) to inactive (welded) can be done under user control at the application level. Such switching should be very fast and not involve rebuilding large structures from scratch. Existing Context should be preserved as appropriate across the switch.

Note that users also want to perform applications in which novel objects can be added to the model on the fly, and existing objects removed permanently. That is not the topic for this issue -- I believe that will be a more difficult project and we should tackle this one first.

I'm opening this for discussion to determine whether this is a correct summary of the applications to be enabled, and to discuss how we might want to implement the capability in Drake. This of course requires work in MultibodyPlant, but I suspect it will also require System framework and Simulator modifications also.

Thoughts?

[edrumwri]

To follow up on these thoughts a little.

Package pickup: Lots of free objects lying around doing nothing (e.g. a truck full of packages). Shouldn't cost anything to have them there. Robot bumps or grabs a few. Those become active. An object once grabbed gets welded to the end effector and again doesn't have its own dynamics.

Welding a grasped object is (was?) the Gazebo model, and I don't think it's good or necessary.

Conditions under which they should be made active must be detectable, but the switch from active (6dof) to inactive (welded) can be done under user control at the application level.

We'd be very happy if this were just done automatically by MBP: free bodies that come to rest are welded until some conditions are met (e.g., a new contact is made, a force is applied). Then we, and everyone else, would get the benefit of faster simulations of moderate to high complexity with no effort required on the user.

[sherm1]

Changing the sizes of states and ports without a model rebuild will be a substantial project touching several areas of Drake. @amcastro-tri proposed a smaller project that we could do more quickly and we want to see if this provides enough functionality that it would be worth doing first. This is my attempt to flesh out Alejandro's proposal; blame me for messing up the details. Please let us know if this would actually solve any problems:

Proposal

For describing the proposal, the canonical application here will be "tool change", where the "tool" might be a rigid body or a mechanism like an articulated gripper. (Hopefully there are other applications for this also.) Every tool has a designated base frame. The robot and all tools would be built into the model, but with inactive tools having their base frames welded to World. An active tool has its base frame welded to the robot end effector. All controllers, even for inactive tools like a gripper, would be wired up in the Diagram and ready to use, although the controllers would presumably be disabled for inactive tools. The State and ports are thus set up at the beginning and never change size or meaning. The number and types of bodies and joints never changes.

Under user control, the base frame weld can be retargeted between World and a frame on a moving body, via a MultibodyPlant method something like this:

void MultibodyPlant::ReweldFrame
    (const Frame& welded_frame, const Frame& target_frame);

welded_frame must currently be the child frame of a Weld joint. After the call, that same Weld joint exists but the parent frame is now target_frame rather than whatever it was before. This topology change must not create a loop. The intent is that this would be a very fast operation (microseconds). Note that Reweld is a non-physical operation since the tool will leap from where it was to its new parent.

Context re-use

Although the states retain their previous meanings, after a Reweld most Context computations must be invalidated. To ensure that, Reweld must increment the MBPlant serial number and hence invalidate all existing Contexts. But ... they can be resurrected very quickly with something like this:

void MultibodyPlant::RefreshContextAfterReweld
    (Context* previously_valid_mbplant_subcontext) const;

That method just invalidates any cache entries that are affected by rewelding, and then updates the recorded serial number in the Context so that it can be used with the revised MBPlant.

The workflow here requires that simulation return to the application level briefly (i.e. Simulator::AdvanceTo() returns as a result of some event being detected). At that point the application can perform the Reweld and Context refreshes. The app can activate and deactivate controllers also if necessary before making the next AdvanceTo() call.

Would the above very-limited model change capability solve any important application problems? If so we could implement something like this fairly quickly; if not we'll move on to a more general capability. Thoughts on utility or feasibility?

attn @edrumwri @huihuaTRI @EricCousineau-TRI @RussTedrake @mitiguy

[RussTedrake]

I see the appeal, and it's very clever. But I don't think that it addresses the primary request. My impression of the request is mainly that people want to be able to add new (floating-base) bodies into the world. For many cases, I think it would be relatively ok, albeit inelegant, to add them all at creation time and have them welded under the table and not contributing to any computational cost. But I think they would need to change from welded to floating (and back)? Not just welded to world vs welded to a robot?

[Alvinosaur]

Similar to this, I would like to request the ability to add free bodies after plant finalization. Besides grabbing objects, robots will frequently encounter new objects in their environment (added by humans, or not detected initially). Constantly reloading the entire plant and reloading all meshes can become very expensive. I apologize if this isn't the right location to request this.