Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

parsing mujoco equality connect constraints: default vs zero configuration #22462

Open
RussTedrake opened this issue Jan 14, 2025 · 11 comments
Open
Assignees
Labels

Comments

@RussTedrake
Copy link
Contributor

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 RussTedrake self-assigned this Jan 14, 2025
@rpoyner-tri rpoyner-tri added the component: multibody plant MultibodyPlant and supporting code label Jan 14, 2025
@RussTedrake
Copy link
Contributor Author

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
Copy link
Contributor

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
Copy link
Contributor

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
Copy link
Contributor

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

@RussTedrake
Copy link
Contributor Author

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
Copy link
Contributor Author

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:
image
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
Copy link
Collaborator

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
Copy link
Contributor Author

This is the default position running on master
image
and if I AdvanceTo(1.0), then I get
image
(So running the dynamics did not close the linkage)

@RussTedrake
Copy link
Contributor Author

RussTedrake commented Jan 18, 2025

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:
Image

@yuvaltassa
Copy link

yuvaltassa commented Jan 18, 2025

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
Copy link
Contributor Author

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?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

No branches or pull requests

5 participants