Computing kinematics and dynamics for a subset of MultibodyPlant

[edrumwri]

We need a way to compute kinematics and dynamics quantities for a subset of bodies/coordinates in MultibodyPlant. Reproducing initial discussion from #12270:

The prevalent pattern for dealing with this in Drake has been to construct a MBP(+SG) for each combination of bodies that one is interested in (e.g., a MBP+SG containing a robot and manipulands for simulation plus a MBP containing only the robot for inverse dynamics control). That pattern works poorly if the sets of bodies of interest are variable.

A non-exhaustive list of quantities / operations we'd like to be able to compute a subset for / apply to a subset include:

• Generalized inertia + Coriolis terms (alternatively, inverse dynamics computation for a subset of coordinates)
• Spatial-velocity Jacobian (via CalcJacobianSpatialVelocity(.))- we do actually require the Jacobian matrix rather than a faster method to compute the operation J*v, where v is some vector. Note that computing a subset of J is a very common operation for computing inverse kinematics (e.g., you want to compute an IK solution for just one arm of a humanoid) and I bet this ability would ease @ggould-tri's pain as he described it in #12849.
• The "bias term" for computing spatial acceleration, via CalcBiasForJacobianSpatialVelocity(.)
• CalcForceElementsContribution(.)
• MakeActuationMatrix(.) our use of this is not likely optimal- we're using this matrix to determine the actuator forces needed to realize a generalized acceleration. Regardless, I have yet to find (in MBP) an efficient solution to our present problem of computing the actuation forces necessary to realize a generalized acceleration for a plant with many (tens or hundreds) of bodies and only a single robot.
• Contact force computation: note that get_generalized_contact_forces_output_port(.) already allows us to compute the contact forces acting on just a model instance, but the contact forces are first computed for the entire plant!

Performance is key, so, e.g., computing the full generalized inertia matrix and then obtaining the necessary blocks from it is unworkable.

cc @siyuanfeng-tri He immediately grokked both what we're trying to do and the performance implications in #12703, so I expect this feature request will resonate with him as well.

[sherm1]

Thanks for this helpful list, Evan! Many of the items can't be computed for arbitrary sets of bodies, but require that the bodies form a subtree in the full multibody tree. It's also not clear to me what it would mean to do inverse dynamics on a subset of bodies -- is there some restriction in your mind that the subset would be a full subtree (like a robot arm) that is not connected to anything else? Not clear to me how that ought to be dealt with in an API. Ideas?

[amcastro-tri]

ditto @sherm1's comment, we might want to prune that list a bit based on that comment.

Re:

Regardless, I have yet to find (in MBP) an efficient solution to our present problem of computing the actuation forces necessary to realize a generalized acceleration for a plant

Are you using something like the InverseDynamicsController? (that's what we usually do). In that case I propose the solution is to enhance the API for InverseDynamicsController so that it knows about the "robot" within a model of "the entire world" and gives you an actuation that you can plug straight into MBP. Would that do @edrumwri? I could open a separate issue for this.

[siyuanfeng-tri]

i am guessing evan has a giant mbp model with a robot with tens or hundreds boxes, and he doesn't want to do inverse dynamics on everything, only a subset (e.g. his robot), and that subset is likely determined at runtime based on q and qdot of the full system.

[amcastro-tri]

am guessing evan has a giant mbp model with a robot with tens or hundreds boxes

yeap, that's the application I have in mind. The solution I propose would address that.

and that subset is likely determined at runtime

you mean the world changes (more/less boxes) but the robot stays the same right?

[siyuanfeng-tri]

yeap, that's the application I have in mind. The solution I propose would address that.

not if evan wants to compensate for the different weighted boxes in his controller?

you mean the world changes (more/less boxes) but the robot stays the same right?

yes, but different boxes would come near and interact with the robot at run time.

[edrumwri]

Many of the items can't be computed for arbitrary sets of bodies, but require that the bodies form a subtree in the full multibody tree. It's also not clear to me what it would mean to do inverse dynamics on a subset of bodies -- is there some restriction in your mind that the subset would be a full subtree (like a robot arm) that is not connected to anything else?

Right! I have yet to think much about API changes to effect the necessary modifications. It seems like a number of people are struggling with getting the current API to do what they want, like @ggould-tri in #12849, @EricCousineau-TRI in #12270, and @rcory in #12203. So perhaps it's time to think about the MBP API v2.0? I'd be happy to contribute to that discussion as just one of many interested parties.

Separately from the API though, @amcastro-tri can you say how amenable the MBP code would be to performing computations on part of the plant? I worry that it's going to require a major engineering effort.

[edrumwri]

Are you using something like the InverseDynamicsController? (that's what we usually do). In that case I propose the solution is to enhance the API for InverseDynamicsController so that it knows about the "robot" within a model of "the entire world" and gives you an actuation that you can plug straight into MBP. Would that do @edrumwri? I could open a separate issue for this.

Not perfectly the same to what I've described in this issue, but #12203 touches on this too. In #12203's case, it's mainly a usability issue. But to answer your question, I don't know whether a number of separate issues should be created since many of them seem interrelated. I'll happily comment in a new issue if you decide to create one.

[amcastro-tri]

@siyuanfeng-tri, I still don't understand the problem to be solved. If the approach is "use some sort of inverse dynamics controller", it doesn't matter how the world around the robot changes, the controller will try to compensate for the errors regardless of how external disturbances are generated (either by new boxes of changes in current boxes), no?

As I said before with similar issues to this one before (like #12203) we need a snippet of code that solves the actual problem, even if hacky, Then we can iterate on that problem to define a better API.

Thus far I see two well defined separate problems:

• Ability to compute things like Jacobians and biases on a subset of bodies.
• Make the InverseDynamicsController interface better with a full model of the world (I believe that'd solve #12203).

Does that list make any sense? anything else? I thin separate issues for each well defined item would be best.

[siyuanfeng-tri]

it doesn't matter how the world around the robot changes

Depends on implementation details. For example, if it needs to cancel some external forces, it needs to deal with a J^T * F part somewhere. And depending on how you write the system, that J term potentially depends on other part of the mbp.

I think your list is a good starting point.

[EricCousineau-TRI]

I would like to pose an alternative solution: Rather than adding a whole bunch of permutations of APIS for subsets of a plant, I would recommend a different approach: Being able to create new plants from sub-trees (or sub-graphs) of an existing plant.

Then you simply use your existing plant, and just take what you need, and you don't need add new API overloads for subsets. Additionally, the physical consistency should be handled by MBP's own checks without any need for additional effort?

This approach works when you already know your subset ahead of time. If your subset is dynamic, then it works, but is not performant, but... I dunno what you'd do anyways? (as you'll have more downstream dynamics stuff...)

[EricCousineau-TRI]

I would actually like to post this request as a separate issue, as I would like it for reasons beyond those explicitly enumerated here. (Well, I guess the reason is, "And new feature and use existing API, but don't do extra work byeond that" ... ?) e.g. creating controllers on subsets, without having to specify coordinates for those subsets

[EricCousineau-TRI]

~MATLAB version: https://www.mathworks.com/help/robotics/ref/rigidbodytree.addsubtree.html~

Er, meant to post this elsewhere.