parsing mujoco equality connect constraints: default vs zero configuration

[RussTedrake]

What happened?

After testing more mujoco models with loop joints, it seems that #22446 actually made some of them worse. (even though I think that PR is implemented correctly)

The problem is, perhaps, a bit fundamental. To help parse mujoco constraints, we changed AddBallConstraint to allow

If p_BQ is std::nullopt, then p_BQ will be computed so that the constraint is satisfied for the default configuration at Finalize() time

note the default configuration there.

In mujoco, the equality connect doc says

When using this specification, the constraint is assumed to be satisfied in the configuration in which the model is defined.

that's equivalent to our "zero" configuration, not our default configuration.

This breaks the cassie model in the mujoco menagerie.

Possible solutions:

1. we could make a breaking change to the AddBallConstraint, and use the zero configuration instead of the default configuration.
2. we could pass a configuration to AddBallConstraint? But typically we don't have a position vector concept until after finalize -- so that seems bad, too.
3. we could revisit the idea of trying to enable kinematic queries for a pre-finalized plant (e.g. at parse time).

@joemasterjohn and others -- wdyt?

Version

No response

What operating system are you using?

No response

What installation option are you using?

No response

Relevant log output

No response

[RussTedrake]

having thought about it a bit today, I think a reasonable way forward would be to add one more optional argument to the AddBallConstraint method: an enum which offers to use either the default configuration or the "zero configuration".
The "zero configuration" is, to an extent, already a concept in Drake. e.g. https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_joint.html#aa7e3799ff0a2d7f2dd697c7f71e522a2

[joemasterjohn]

My first idea was a plant-wide option that set's the "default" configuration mode, but I think per-constraint is a better idea.
I can draft up a PR to do this.

I still find the MuJoCo doc a bit confusing as written. Did you happen to find a section that would lead a user to the correct conclusion that "in the configuration in which the model is defined." means the zero configuration? I would assume that since the ref attribute is part of the model, the reference configuration would be "in which the model is defined".

[joemasterjohn]

Hmm, also does it use the zero configuration when 0 is outside of the limits of a joint? I can't seem to find this information in the docs.

[joemasterjohn]

WIP branch is here fyi: https://github.com/joemasterjohn/drake/tree/constraint_default_state_mode

[RussTedrake]

I agree that the docs are lacking on both points. The cassie model has been my verification. In that example, both the zero configuration and the default (because it's often zero) are actually outside of some joint limits. But using that zero position (no clamping to the limits) is the only way to make the linkage closed by obvious visual inspection.

[RussTedrake]

I do want to be very careful that my analysis is correct.

My next step would be to visualize the cassie model before using drake before the ref support landed. It looks like this:

Then after the ref support landed, one linkage is visibly wrong. I can try to run it with your branch in the morning.

That model is strange in many ways. Not only are the defaults outside of the joint limits, but if the constraint is closed in the zero configuration, and the default configuration is not the zero configuration, then presumably there will be large internal forces requires at time=0 to close the linkage. It seems very odd.

[jwnimmer-tri]

If it's feasible to either read (in code) or explore (with experiments) what Mujoco is actually doing, an ideal next step would be to open a pull request again their docs with a precise specification, which then we could check that both implementation adhere to. I seem to recall that @sherm1 did that for SDFormat specs many years back. Building multibody parsers around a solid, unambiguous specification is really the best option when feasible. Cross-checking with robot species is also helpful, but will be more brittle.

[RussTedrake]

This is the default position running on master

and if I AdvanceTo(1.0), then I get

(So running the dynamics did not close the linkage)