Open sherm1 opened 1 month ago
@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()
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).
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.)
This is my understanding, correct me if I am wrong @RussTedrake:
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.
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.
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.
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.
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:
However that failed because we can't currently clone an un-finalized MbP.
Possible fixes:
cc @amcastro-tri @xuchenhan-tri @joemasterjohn @SeanCurtis-TRI @RussTedrake