Parse closed kinematic chains from SDF (and perhaps also URDF)

[RussTedrake]

SAP now supports constraints, but currently users can only define those constraints programmatically.

Closed kinematic chains are a common feature in robots (e.g. Cassie, or in many underactuated hands). We should support parsing a robot with a closed kinematic chain directly from SDF and/or URDF. The specification in URDF is such that one cannot define a closed chain without additional tags, but in SDF it is possible to define a closed chain just by adding the already defined links and joints. Presumably we throw an error in this case, but I have not verified that.

We used to have a Drake-specific "loop_joint" tag. You can still find vestiges of it in our codebase; the URDF parser even reports Error(*node, "loop joints are not supported in MultibodyPlant"); if it encounters one. I believe @sherm1 feels that it's better for the user to be able to decide where the mechanism should be split and closed with a constraint (vs the true joints), and that it's better to split in the middle of a link and then have a constraint which welds the link back together (vs a lower-DOF constraint implemented at a joint). I am ok with requiring a custom tag.

A victory condition here would be to have the four_bar.sdf and even strandebeest use constraints without having to add them programmatically. They should be specified in the robot description file.

[sherm1]

FYI I opened a related draft PR #18804 that can turn these into spanning tree + constraint models.

[rpoyner-tri]

There are potentially two pieces of work here. (1) make the parser map closed chains onto appropriate MbP features; (2) make MbP support closed chains at all. Do we believe enough of (2) is done to undertake (1)? Based on Sherm's comment above and other discussion, I'm not sure.

[jwnimmer-tri]

My intuition is that we could write a four_bar.sdf or four_bar.urdf file that uses a to-be-added custom tag to define the constraint, at which point MbP already offers everything we need. It's probably worth trying to write down that exemplar input file as the next step here.

In that design, the user would still need to point their joints in right direction by hand in the xml, and annotate the constraint by hand in xml, but it would meet the victory condition of not calling MbP API to set up the model.

[sherm1]

There are definitely two different problems, greatly differing in difficulty:

1. Allow users some way to construct tree+constraint models, or
2. Read unmodified sdf files containing only links and joints and automatically turn those into internal tree+constraint systems.

In general the second case is much harder. We probably want to provide option 1 first, if only to make sure we have suitable underlying functionality. If we want to do option 2, we need some code that can produce a reasonable tree+constraint model, which can be quite difficult. Just for a taste: massless bodies (which are quite reasonable in linkages) must never be placed as leaf nodes in the tree. #18804 solves the general problem.

[sherm1]

BTW for the automated (option 2) all we need is Weld constraints. That's all users need too but they might expect a large collection of "loop joint" constraints instead. That leads to asymmetric behavior where some joints behave differently than others (e.g. there are not q's and v's in the state corresponding to the loop joints). The method I advocate for mapping loops to trees is:

• for each kinematic loop in the system (there can be many), choose one body
• cut that body in half, breaking the loop (each body gets half the mass properties)
• weld the two halves together with a Weld constraint
• assemble and run.

With that approach the joints are all treated identically.

[jwnimmer-tri]

It sounds like you have a lot of experience here @sherm1. Could we convince you to write a four_bar.sdf (or four_bar.urdf) sample file that demonstrates that approach?

We have a sample file here:

https://github.com/RobotLocomotion/drake/blob/master/examples/multibody/four_bar/four_bar.sdf

It currently completes the loop by adding a Bushing in code:

https://github.com/RobotLocomotion/drake/blob/2511ba5022253425be9639aac73047aee894283e/examples/multibody/four_bar/passive_simulation.cc#L112-L117

The goal would be a variant of that input file where we end up with an SAP constraint (and no bushing).

[sherm1]

Sure -- do you mean the option 1 approach where we postulate the existence of a <drake::weld_constraint> tag or something like that? The option 2 sdf file leaves all the fancy stuff hidden; I'll do that one for contrast.

[jwnimmer-tri]

Yes, exactly option 1. (Doing both options is fine, too.)

The goal would be to hand that file to a software developer and say "add parsing code so that a unit which loads this file passes".

[sherm1]

See #18829 for possible sdf descriptions

[RussTedrake]

related to https://github.com/RobotLocomotion/drake/issues/17663

[jwnimmer-tri]

FYI there's some type in your last; it's a self-link back to this issue.

[RussTedrake]

oops. updated now. thanks.

[jwnimmer-tri]

@rpoyner-tri If you're looking for the next suggestion for a parsing project (after 18773), I'll upvote this one.

[sherm1]

FYI I'm working on the graph processing that can handle this as we speak. If the parser can get the links, joints, and constraints into MbP we'll be able to model correctly.

[rpoyner-tri]

Noting #20232 here, for support of closed chains from MJCF as well.

[sherm1]

Current graph processing draft is #20225

[jwnimmer-tri]

Having just re-read all of the posts, I'll summarize here for the next person who wanders by:

The call to action here is to write the SDFormat custom parsing logic (and syntax documentation) that can parse four_bar_weld.sdf using the custom weld_constraint tag:

    <drake:weld_constraint>
      <parent>Ccm</parent>
      <child>Scm</child>
    </drake:weld_constraint>

[RussTedrake]

FWIW (since I didn't know until I stumbled on the code), https://github.com/RobotLocomotion/drake/pull/19780 introduced <drake:ball_constraint> for URDF and SDF, which does enable authoring loop joints, though not in the way that @sherm1 prefers (@sherm1's preference, I believe, is sawing a bone in half and welding it back together).

[sherm1]

BTW breaking a loop at a ball joint is the one case where using a constraint-based loop joint is arguably harmless. We delete 3 differential equations and add 3 constraint equations. Although we lose the internal coordinates q and v, those usually aren't much use for a ball joint anyway, especially when modeling with a quaternion. And the alternative is to retain the 3 DEs, cut a link, and add 6 constraints to glue it back together.