As I've been working on the PR for this, I've been finding myself basically writing mostly MultibodyPlant code and test cases. I can try this myself, but I'm not really sure I'm qualified to be writing code for MBP.

@jwnimmer-tri do you know if anyone at TRI has cycles to help out with this? (Tagging you because I can't actually remember who the MBP component owner is.)

... if the user has a discrete plant and isn't using SAP as the contact solver ...

It seems to me it would be totally fair to throw in case the user asks IRIS-ZO to obtain constraints from the plant, but the plant was non-SAP. Nobody should be using anything else anyway, these days. The other modes are kept for backcompat, not to be used in new code.

(Tagging you because I can't actually remember who the MBP component owner is.)

It's @amcastro-tri (per https://drake.mit.edu/issues.html).

... anyone at TRI has cycles to help out with this?

We probably don't have bandwidth for IRIS support (unless you can convince @hongkai-dai) but if there is a more general missing API for MultibodyPlant, we could maybe take over that part. I'm not sure what kind of code you have in play, here.

It seems to me it would be totally fair to throw in case the user asks IRIS-ZO to obtain constraints from the plant, but the plant was non-SAP. Nobody should be using anything else anyway, these days. The other modes are kept for backcompat, not to be used in new code.

Do you think it's appropriate to throw for continuous-time plants as well?

We probably don't have bandwidth for IRIS support (unless you can convince @hongkai-dai) but if there is a more general missing API for MultibodyPlant, we could maybe take over that part. I'm not sure what kind of code you have in play, here.

The functionality would be to provide minimal coordinates in the scenario where the MBP has coupler constraints (either user-specified for from drake:mimic tags). If a plant only has coupler constraints, then we can uniquely write every valid configuration as y=Ax+b, where A is a rectangular matrix whose entries are the multipliers, b is a vector whose entries are the offsets, and x lives in the lower-dimensional space.

Right now, I'm getting a list of coupler constraints and traversing over them to construct the matrix and vector. Then I use that for the IRIS stuff downstream.

Do you think it's appropriate to throw for continuous-time plants as well?

Depending on the API contract, maybe. But also, it doesn't matter? I believe if the user tries to add constraints to a continuous-time plant, the plant will immediately throw.

Right now, I'm getting a list of coupler constraints and traversing over them to construct the matrix and vector.

Is this the only "MultibodyPlant code and test cases" you're talking about? That seems pretty small. Anyway, possibly you should just share the code outline you have, and then people can weigh in on where/how to help, or what different API the MbP could offer. That could be on #dynamics slack if that's easier.

The code outline is roughly as follows:

1. Perform a topological sort of the coupler constraints, so that the "input" joint is either independent, or it is the dependent joint of an earlier coupler constraint.
2. Determine which joints are independent (and thus the "parameterization variables"), and construct an initial mapping (A, b) that simply maps those joint values to their appropriate indices.
3. Iteratively update A and b for each coupler constraint (in sorted order).

I've actually already implemented much of this in #22733. But I'm definitely a bit concerned since I just don't know how MBP deals with constraints all that well. For example, could I define a cyclic set of coupler constraints? Or redundant ones? Because I don't know that sort of thing, it's hard to be comprehensive about test cases.

Makes sense. In that case, I would actually suggest opening a draft PR with the general outline, and pinging MbP experts to help validate / review the algorithm and suggest test cases. You could ask on #dynamics or just ping Sherm & Joe & Alejandro.

Having looked at the code now, I suggest that @sherm1 should be on point for offering advice on the code / whether the approach is valid.

Thanks for the advice, Jeremy! And thanks in advance for your advice, Sherm.

I'm sorry Thomas I don't understand enough about what you're trying to do to provide any useful advice. If there are some MbP services that would make your life easier I'd be happy to discuss those. Possibly @joemasterjohn is more familiar with the application area as well as the mimic constraint internals. WDYT Joe?

The question is whether the approach in IrisZoOptions::CreateWithMimicJointsParameterization is robust against all kinds of possible plant species, both today and as MbP evolves. It has an API contract and unit tests, so that should be able to provide some leverage to understand the question.

The question is whether the approach in IrisZoOptions::CreateWithMimicJointsParameterization is robust against all kinds of possible plant species, both today and as MbP evolves.

The answer to that is certainly no but I don't think it's the right question. The general problem of finding a set of independent coordinates for a constrained multibody system is very hard -- there are lots of constraint types, constraints can be redundant or ill conditioned, redundancy and conditioning can be configuration dependent, and there are arbitrary choices to be made about which coordinates are to be declared independent. But I don't think @cohnt is trying to solve that problem. So I'm thinking the right question is whether this serves the intended purpose when applied to the systems of actual interest to the author.

That's fine. In that case the predicate "systems of actual interest" is whatever the API contract of #22733 says it allows / supports.

So maybe the first step is to iterate on what that contract could/should say, and then second decide if the implementation is safe / sound?

The worlds "mimic joints" in the function makes me think that it only need to allow mimic joints, and any/all other kinds of constraints present in the plant will result in throwing an exception. So, does the PR work correctly if any kind and number of mimic joints are present? Or maybe even that is too hard to implement, so we need say "only such-and-such kinds of mimics are allowed" and be sure to have test cases that cover both allowed and disallowed situations. Maybe the thing you can help with is itemizing the situations where kinds and groupings of mimic constraints end up being distinctly different in practice. Essentially what I think tcohn is after is expert advice in setting up that test suite.

(I was also trying not to bother Alejandro and Joe since they are heads-down on critical research, whereas at the moment you (Sherm) have a bit more bandwidth available, while also being an expert on plant topology and coordinates.)

Ultimately, what we want is the following: if someone has a urdf or sdf that uses the <drake:mimic> tag to define some of its joints as being dependent on others, then we have a very simple way of constructing a set of independent coordinates. I would like to add a programmatic way of constructing these coordinates, in a form which can be used with IRIS-ZO.

Makes sense. The algorithm in #22733 looks reasonable, though I can't say I follow it in detail (for example, it's unclear after staring at the code whether joint0 or joint1 is considered the independent one -- either could be). My suggestion would be to bulletproof it as much as possible by pre-certifying that the MultibodyPlant you are looking at is one you can handle. For example, there should be no non-coupler constraints in the system or the "independent" coordinates you pick may not be independent. Also, it looks to me that you are expecting only a single chain of coupler constraints (I might be misunderstanding); in that case you should make sure the plant doesn't contain multiple chains. It is also unclear to me what happens if there are contact constraints active -- won't that potentially spoil the independence of your chosen coordinates?

1. joint1_index is treated as the independent joint, and joint2_index is treated as the dependent joint, to match multibody::internal::CouplerConstraintSpec.
2. Throwing if there are any non-coupler constraints, if the plant is continuous-time, etc. sounds good to me. But is it possible for someone to add additional coupler constraints that form a "loop"? Is that something I need to be watching for (and writing test cases to verify)?
3. I think the algorithm should work for multiple chains of coupler constraints. (That's certainly something we need to support, e.g., for a bimanual robot with parallel-jaw grippers.)
4. IrisZo is mostly intended for downstream use with kinematic motion planning, so we're not really worried about contact constraints right now.

I do also have some concern about bloating the test cases of IrisZo with a bunch of examples that are ultimately not really related to that algorithm. And @sadraddini has requested having the ability to construct this mapping and use it outside the realm of IrisZo. So this motivates the desire to have a function in MultibodyPlant.

I don't think there is anything that would stop someone from forming a loop of coupler constraints -- that would be a special case of redundant (and possibly inconsistent) constraints. Doesn't seem like a good idea though! You could reject systems like that.

I do also have some concern about bloating the test cases of IrisZo with a bunch of examples that are ultimately not really related to that algorithm.

Yes. Having this capability exist outside of IrisZo (e.g., as a helper class or function) seems smart. Then it can be unit tested by itself, and IrisZo can just simply use it, with only 1-2 smoke tests on the integration point. All of the corner-case tests can be on the standalone component.

And sadraddini has requested having the ability to construct this mapping and use it outside the realm of IrisZo

Also a good reason to have it a be a distinct component.

So this motivates the desire to have a function in MultibodyPlant.

There are tons of components that use MultibodyPlant, but which are not member functions on the plant (see, e.g., rational forward kinematics, trajectory optimization, etc.). It doesn't need to be a MbP API, nor even in the plant nor tree directory. It could start out as a component in planning/iris.