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)

[RussTedrake]

Ok, I've spent more time with the mujoco docs. The match between MuJoCo concepts and Drake concepts is actually pretty clean, but the naming choices are a bit unfortunate:

MuJoCo has three concepts:

• qpos -- is the current configuration
• qpos0 -- is their "zero" or "reference" configuration, described nicely here
• "the configuration where the model was defined" is used in the equality constraint docs. The fact that the docs do not use qpos0 here, which they use consistently everywhere else, along with my experience with the cassie model, leads me to believe that this is not qpos0. I do think it would be a documentation improvement if the mujoco equality docs clearly stated that this was not qpos0, and perhaps even gave it a name (like qdefined or something).

Drake has three concepts:

• positions in the current context
• default positions
• zero positions

Roughly speaking

• mujoco's qpos <=> drake's positions in the context
• mujoco's qpos0 <=> drake's default positions. The last lines of this section of the mujoco docs clearly state that

the function mj_resetData. It sets mjData.qpos equal to the model reference configuration mjModel.qpos0, mjData.mocap_pos and mjData.mocap_quat equal to the corresponding fixed body poses from mjModel; and all other state and control variables to 0.

qpos0 in mujoco is also used for the rest length of joint springs, etc, so we will likely need to use it elsewhere in the parser when we implement those.

• mujoco's "configuration where the model was defined" <=> drake's zero positions. this is the one that I believe we need to correctly compute the equality constraint kinematics post-finalize.

My only nagging concern is just how strange the cassie model in the menagerie is. It defines joint limits that do not contain zero, so qpos0 is outside of the joint limits. And because ref is set for one of the joints in the leg kinematic loop, but not others, I think that qpos0 also does not satisfy the equality constraints. So either the Cassie model gets a massive facepunch of constraint forces at time zero to satisfy the constraints, or the mujoco reset function does extra work to resolve the constraints, even thought the resetData doc does not say so. (I would like to confirm the answer to that, but even if we want to resolve constraints in Drake's SetDefaultContext, I think that would be an additional thing we could do after this issue + Joe's PR)

My current proposal:

• land Joe's fix. I think it is a correct interpretation of mujoco's behavior and is needed.
• add a summary of this discussion as a comment somewhere in the Drake mujoco parser doc.
• maybe: add logic to resolve constraints in MultibodyConstraint's SetDefaultContext()?

fyi @yuvaltassa @kevinzakka .

BTW -- I resolved the other texture/material issues with the cassie parsing, so all of the menagerie models now look a lot better in meshcat, cassie included:

[yuvaltassa]

Please excuse the unclear documentation (now updated), "the configuration at which the model is defined" == mjData.qpos0. Please let us know if anything else is unclear. Thanks for catching.

[RussTedrake]

Thanks @yuvaltassa. But I'm surprised. For the cassie model when I use the ref position to compute the equality connect constraints, the loop is visibly not closed. I thought that the ref position => qpos0, but that could not be the position in which the constraint kinematics were evaluated?

[yuvaltassa]

I'm not sure what you mean. When I load the Cassie and turn on constraint visualisation (red and blue spheres at the two anchor points), the loop is clearly closed at qpos0 and remains that way.

Cassie.mp4

[RussTedrake]

Thanks @yuvaltassa . I appreciate it. My qpos0 (default position) , with an almost identical gui, looks like this:

Apart from my default values being projected onto the joint limits (which only effects the lower leg), the values match. The knee position in your qpos0 corresponds to a left-knee of zero in my parser. I guess it's on me. I'll keep digging.

[yuvaltassa]

I suspect you are not correctly parsing the joint/ref attribute, see here. The semantic of the attribute is to redefine qpos[i]=0 for joint i.

[RussTedrake]

thanks. it was when i added parsing support for ref that things went sideways (that was the discussion above). something else is wrong.

[RussTedrake]

When I comment out the "ref" for the right leg, I looks just like yuval's initial conditions

[RussTedrake]

When i take that same model (with the refs in the right light commented out) into mujoco, I get

and it's not until I run that I see something different happening in the right leg:

So the ref is not impacting the initial frame in the visualization. That seems inconsistent with the docs and your comments above @yuvaltassa ?

[RussTedrake]

I think I see my error. The text description in https://mujoco.readthedocs.io/en/stable/overview.html#joint-reference says that setting "ref=-45" means that the configuration in the model file should be interpreted as having a joint angle of -45. I implemented it as the model in the mjcf is 0, but you should start instead at 45. That's not a great mapping to how we do things in Drake, but I think I can fix it. Sorry for being slow.

[yuvaltassa]

Looks like you figured it out.

As always, if you there is any way we can improve the MJCF reference in a way that would avoid future confusion, please let us know 🤓

[RussTedrake]

I've got the fix coming. The default configuration is nicely resolved now. Thanks!

One thing that is still different for me: the default positions of the foot-cranks are outside of the joint limits:
https://github.com/google-deepmind/mujoco_menagerie/blob/5e6af47a49bf8ae27581615b54045107b20ce584/agility_cassie/cassie.xml#L124
The default position (qpos0) is zero here, but the range="-140 -30". @yuvaltassa -- are your default joint limits just really soft?

[yuvaltassa]

Nice catch!

1. All constraints in MuJoCo are softish by default. As a side note, we're considering adding named sets of global defaults as alternatives. Drake has great defaults; chapeau.
2. The out-of-range-ankle is curious indeed. My guess is that the CAD preceded the decision to tighten the range? The model is Agility's, maybe @danielbennettagility would know?

[danielbennettagility]

I'll admit, it's been a while since I've worked on Cassie, so the details are escaping me. I suspect that this isn't an error, it's just that when we exported the models there wasn't a particular effort spent making sure that the zero position was physically meaningful. The MuJoCo model was also created a bit after I moved on from working on Cassie, so I'm not sure if my experience is exactly relevant here. For example, when we zeroed the hardware, the pose was with the toes pointed down, where it looks above like the zero position has the toes pointed up. I can ask around to see if anyone can give any more clarity if you think it would be helpful

[RussTedrake]

Thanks @danielbennettagility . The model can work as it is, but I do think it would be more elegant to resolve this. Drake's solver takes constraints seriously with the default parameters, so if I drop the model in as-is, depending on which workflow we're using, that inconsistency will either be resolved with a nonlinear optimization or by a large constraint force at time zero. A small fix to that file would allow the developers to choose how it's resolved rather than relying on the solvers.

It's not a blocker for me. I just called it out when I saw it (and I wanted to make sure that it wasn't coming from a mistake in Drake's parser!)