Kinematic analysis while constructing a MultibodyPlant

[sherm1]

Dealing with loops in MuJoCo's input file requires doing some kinematic analysis using the default configuration to determine the location of attachment points.

The first attempt which seemed very sensible was:

• Clone the in-progress MbP
• Do kinematics with the clone to get the necessary info
• Apply that to the in-progress MbP
• Throw away the clone

However that failed because we can't currently clone an un-finalized MbP.

Possible fixes:

• Allow cloning of un-finalized MbP (what to do with the SceneGraph pointer?)
• Support un-finalizing (we need a way to restore SceneGraph's collision filters to their pre-finalize state)
• Other ideas?

cc @amcastro-tri @xuchenhan-tri @joemasterjohn @SeanCurtis-TRI @RussTedrake

[amcastro-tri]

@joemasterjohn propposed alternative solution for @RussTedrake's specific problem of parsing equality constraints. In his words:

Currently we require user added equality constraints (ball, weld, distance) to fully specify their kinematics at creation. The ball constraint for instance:

  MultibodyConstraintId AddBallConstraint(const RigidBody<T>& body_A,
                                          const Vector3<double>& p_AP,
                                          const RigidBody<T>& body_B,
                                          Vector3<double>> p_BQ);

We could alter the API to be closer to what's specified in MJCF's equality constraint (which specifies that what we call p_BQ should be coincident with p_AP in the default configuration):

  MultibodyConstraintId AddBallConstraint(const RigidBody<T>& body_A,
                                          const Vector3<double>& p_AP,
                                          const RigidBody<T>& body_B = world_body(),
                                          const std::optional<Vector3<double>> p_BQ = {});

Then have the plant compute and fill in the missing kinematics data at Finalize time and store it in the constraint spec. i.e. as the very last step of MbP::FinalizePlantOnly()

[RussTedrake]

I think this change to the AddBallConstraint API is strict improvement and it does resolve the immediate need. Let's do it.

(I still worry that we will need to make similar improvements in other parts of the code, but we can defer those until the need arises).

[jwnimmer-tri]

Is this the ticket we want to use? As I understand the overall request, it is actually "Parse MJCF 'equality' elements" so possibly we should open a new ticket with that specific outcome?

Or, if the only goal is to switch p_BQ to optional with kinematics computed at finalize-time, then plausibly we could use this issue but someone needs to fix the title and clarify the overview. (I am somewhat skeptical of doing only that first half open-loop, though. It runs the risk of changing the API but still not being able to parse the MJCF element because of something we failed to foresee. At minimum, we should probably start to draft and validate the parser changes, before merging the plant API change.)

[amcastro-tri]

This is my understanding, correct me if I am wrong @RussTedrake:

• This proposed ball consraint API (similarly other's like weld) would allow Russ to update the parser.
• Someone else (Russ thus far) will use this API to update the MJCF parser to parse equality constraints.
• We should close this issue and open two new ones for the Drake board: Proposed constraint APIs + MJCF feature request.

[jwnimmer-tri]

Here's another explanation of the point I tried to make above -- if you add and test and review and merge the new MbP feature without contemporaneously integrating it into the parser to confirm its utility, then we run the risk of adding a new MbP feature that doesn't actually solve the problem. In other words, the two issues are not orthogonal -- so if you do decide to split it up, someone will actively need to coordinate and de-risk their overlap.

My suggestion is to just make one new issue, for MJCF equality parsing, and leave it at that. As the person working on it develops e.g. their test cases and prototype, we will gain more insight into exactly what we need from MbP. In any case, this is not yet a priority so we don't need to worry about scheduling the work or who is doing it. We only need to precisely capture the specific request and crisply state its victory condition.

[RussTedrake]

I can open a new issue tomorrow. I already have the feature basically implemented in the mujoco parser (but I was unable to run the tests).

I would not say that it is not yet a priority. It is not yet high priority.

[RussTedrake]

21970 implements the narrow solution of generalizing the AddBallConstraint API (and also implements the parsing which uses it).

I believe @sherm1 was willing to move forward the idea that one should be able to clone an MbP pre-finalize. @sherm1 -- i'll leave it to you to decide whether we should rename this PR or open a different one.

[RussTedrake]

Note that I could not implement the world_body() default argument

const RigidBody<T>& body_B = world_body(),

in the method because mkdoc.py couldn't support it out of the box. I don't need it right now and it wasn't worth trying to fix mkdoc.