[parsing] Add support for URDF mimic element. #18728
Conversation
RussTedrake
added
release notes: feature
There was a problem hiding this comment.
@rpoyner-tri for feature review, please.
Reviewable status: 4 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers
-- commits line 8 at r1:
Q: does this constitute a breaking change?
multibody/parsing/detail_urdf_parser.cc line 606 at r1 (raw file):
if (mimic_node) { if (!plant->is_discrete() || plant->get_discrete_contact_solver() != DiscreteContactSolver::kSap) {
q: i didn't want to check the contact solver in the parser (because parsing first and then setting the contact solver after should be acceptable behavior). But currently the AddCouplerConstraint method throws if the solver is not kSap. Should we change that?
I feel that this behavior is a bit different than for other features. For instance, joint limits aren't respected by all multibody modes, but the parser does not warn nor throw if we parse joint limits and aren't using kSap.
multibody/parsing/test/detail_urdf_parser_test.cc line 48 at r1 (raw file):
UrdfParserTest() { plant_.RegisterAsSourceForSceneGraph(&scene_graph_); plant_.set_discrete_contact_solver(DiscreteContactSolver::kSap);
Note: this is related to the question above. I don't like making the parser behavior contact-solver dependent.
multibody/parsing/test/detail_urdf_parser_test.cc line 849 at r1 (raw file):
DRAKE_EXPECT_NO_THROW(plant_.GetJointByName("revolute_joint_with_mimic")); // TODO(russt): Test coupler constraint properties once constraint getters are // provided by MultibodyPlant (currently a TODO in multibody_plant.h).
This makes me sad. I don't actually have a way to confirm that my gear ratios and offsets are getting set properly, short of adding simulator as a dependency to this test and running a simulation here. Perhaps it's ok for the short term? WDYT?
There was a problem hiding this comment.
checkpoint. Generally looks good so far. pending changes around constraints and modes and error handling.
I want to look at test coverage and some closer reading still.
Reviewable status: 4 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @RussTedrake)
Previously, RussTedrake (Russ Tedrake) wrote…
Q: does this constitute a breaking change?
I suppose it's conservative to say "yes". I don't see any "deprecation" path that makes sense, so "breaking change" will at least get us prominent text in the release notes.
multibody/parsing/detail_urdf_parser.cc line 606 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
q: i didn't want to check the contact solver in the parser (because parsing first and then setting the contact solver after should be acceptable behavior). But currently the AddCouplerConstraint method throws if the solver is not kSap. Should we change that?
I feel that this behavior is a bit different than for other features. For instance, joint limits aren't respected by all multibody modes, but the parser does not warn nor throw if we parse joint limits and aren't using kSap.
IIRC joint limits are set/represented a bit differently than solver constraints. Is this the first time we've had a connection between explicit solver constraints and the parser? If so, I think we are justified in modifying the error reporting strategy.
multibody/parsing/test/detail_urdf_parser_test.cc line 48 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
Note: this is related to the question above. I don't like making the parser behavior contact-solver dependent.
Agreed. In any case, it may make sense to have unit tests for Sap and non-Sap. Presumably the non-Sap case would be an execution-time no-op, but maybe would issue a warning at parse time?
multibody/parsing/test/detail_urdf_parser_test.cc line 849 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
This makes me sad. I don't actually have a way to confirm that my gear ratios and offsets are getting set properly, short of adding simulator as a dependency to this test and running a simulation here. Perhaps it's ok for the short term? WDYT?
I think the "run a simulation" route is unnecessary heroics, and would get deleted when the getters arrive. Is there a an issue filed for "add constraint getters"? In addition to the TODO here, we could file or boost the issue.
There was a problem hiding this comment.
I have a patch to r1 that delays some constraint error checking to Finalize() time, getting it out of the parser's way. If you like, I can push it onto your dev branch, or pass it along some other way.
Reviewable status: 4 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @RussTedrake)
There was a problem hiding this comment.
Reviewable status: 4 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @RussTedrake)
a discussion (no related file):
patch to delay error checking: https://drakedevelopers.slack.com/archives/C2WBPQDB7/p1675370291647319?thread_ts=1657230165.865699&cid=C2WBPQDB7
rpoyner-tri
added
the
release notes: breaking change
There was a problem hiding this comment.
+(release notes: breaking change) Changes simulation semantics of URDF <mimic> from being ignored to being implemented, for SAP only.
Reviewable status: 5 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @RussTedrake)
There was a problem hiding this comment.
Reviewable status: 5 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @RussTedrake)
a discussion (no related file):
Previously, rpoyner-tri (Rick Poyner (rico)) wrote…
patch to delay error checking: https://drakedevelopers.slack.com/archives/C2WBPQDB7/p1675370291647319?thread_ts=1657230165.865699&cid=C2WBPQDB7
See also more discussion in that slack thread: https://drakedevelopers.slack.com/archives/C2WBPQDB7/p1657230165865699
Per nimmer's observation there, I think the patch you haver1 is the best we can do quickly, and we're still discussing the final disposition of error handling and how to get there. TL;DR my patch will break files that "work" now, so let's go with your patch.
There was a problem hiding this comment.
Reviewable status: 4 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @RussTedrake)
multibody/parsing/detail_urdf_parser.cc line 606 at r1 (raw file):
Previously, rpoyner-tri (Rick Poyner (rico)) wrote…
IIRC joint limits are set/represented a bit differently than solver constraints. Is this the first time we've had a connection between explicit solver constraints and the parser? If so, I think we are justified in modifying the error reporting strategy.
See discussion thread above and linked slack discussion. I'm disappointed but satisfied with this behavior.
There was a problem hiding this comment.
Reviewable status: 4 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @RussTedrake)
multibody/parsing/test/detail_urdf_parser_test.cc line 849 at r1 (raw file):
Previously, rpoyner-tri (Rick Poyner (rico)) wrote…
I think the "run a simulation" route is unnecessary heroics, and would get deleted when the getters arrive. Is there a an issue filed for "add constraint getters"? In addition to the TODO here, we could file or boost the issue.
Filed #18732.
RussTedrake
force-pushed
the
parse_urdf_mimic
branch
from
6b093b6 to
364fde8
Compare
There was a problem hiding this comment.
Reviewable status: 1 unresolved discussion, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @rpoyner-tri)
a discussion (no related file):
Previously, rpoyner-tri (Rick Poyner (rico)) wrote…
See also more discussion in that slack thread: https://drakedevelopers.slack.com/archives/C2WBPQDB7/p1657230165865699
Per nimmer's observation there, I think the patch you have
r1is the best we can do quickly, and we're still discussing the final disposition of error handling and how to get there. TL;DR my patch will break files that "work" now, so let's go with your patch.
Sounds good (I've read the slack now, too)
multibody/parsing/test/detail_urdf_parser_test.cc line 48 at r1 (raw file):
Previously, rpoyner-tri (Rick Poyner (rico)) wrote…
Agreed. In any case, it may make sense to have unit tests for Sap and non-Sap. Presumably the non-Sap case would be an execution-time no-op, but maybe would issue a warning at parse time?
Done. (I've narrowed the use of kSap to the mimic-specific tests, and added a test with a mimic element that is not using Sap).
There was a problem hiding this comment.
+@SeanCurtis-TRI for platform
Reviewable status: 1 unresolved discussion, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers (waiting on @rpoyner-tri)
There was a problem hiding this comment.
Reviewed 3 of 4 files at r1, 1 of 1 files at r2, all commit messages.
Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers (waiting on @SeanCurtis-TRI)
There was a problem hiding this comment.
First pass done. See comments below.
Reviewed 3 of 4 files at r1, 1 of 1 files at r2, all commit messages.
Reviewable status: 8 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers (waiting on @RussTedrake)
multibody/parsing/detail_urdf_parser.cc line 535 at r2 (raw file):
auto plant = w_.plant; JointIndex index{};
nit: By Hoisting index outside of the chain of if/else blocks, we giving the implicit promise that it will carry a valid value by the time we're done evaluating the block. However, the type == "floating" block does not set it. It might throw, but we have no guarantees that it will throw.
Therefore, this should be turned into an optional<JointIndex> and it should be validated in the if(mimic_node) block before using.
Alternatively, we can just set the index now that floating joints have actual Joints.
multibody/parsing/detail_urdf_parser.cc line 549 at r2 (raw file):
} else if (type.compare("fixed") == 0) { throw_on_custom_joint(false); index = plant->AddJoint<WeldJoint>(name, *parent_body, X_PJ,
nit: Formatting has gotten skewed; this format typically has all parameters aligned in a block. Although the block formatting doesn't work well here and won't survive clang-fomratting.
multibody/parsing/detail_urdf_parser.cc line 619 at r2 (raw file):
double offset{0.0}; if (!ParseStringAttribute(mimic_node, "joint", &joint_to_mimic)) { Error(*mimic_node,
BTW For my own edification, is this parsing sensitive to the ordering of the XML tags? And is that normal for URDF and/or Drake parsing? I.e., what if the joint exists in the file but later than the joint that mimics it?
I don't have the sense that URDF particularly has this requirement. If this is our requirement, then we should be clear about this requirement in the error message.
If this is just understood by the community at large, please ignore this.
multibody/parsing/detail_urdf_parser.cc line 638 at r2 (raw file):
const Joint<double>& joint1 = plant->GetJointByName(joint_to_mimic, model_instance_); if (joint1.num_velocities() != joint0.num_velocities()) {
BTW So, we allow a prismatic joint to mimic a revolute joint? (And vice versa.)
multibody/parsing/detail_urdf_parser.cc line 647 at r2 (raw file):
BTW While ambiguity in the URDF documentation is one issue, the more acute issue is that Drake's couple constraint only supports 1-dof joints. Perhaps it would be more honest/precise if this comment and the message indicated such.
E.g.,, warning message along the lines of:
Drake only supports the mimic element for single-dof joints. The joint '{}' (with {} dofs) is attempting to mimic joint '{}' (with {} dofs). The mimic element will be ignored.
This means we don't have to lawyer up vis a vis URDF, we're just stating what Drake does or does not do.
multibody/parsing/test/detail_urdf_parser_test.cc line 82 at r2 (raw file):
protected: PackageMap package_map_; MultibodyPlant<double> plant_{0.1};
nit: Prior to this PR whether the plant was discrete or continuous was irrelevant. Now it matters. Putting a note here will be helpful for the future.
Although, that will probably be undone based on my next note (about testing against a continuous plant). But, for now, the comment would help clarify the current state of the code and its dependencies.
multibody/parsing/test/detail_urdf_parser_test.cc line 331 at r2 (raw file):
} TEST_F(UrdfParserTest, MimicNoSap) {
nit: You're missing the test: MimicContinuous.
I recognize that the infrastructure doesn't lend itself well to tests which vary in this dimension. At the very least, I think a TODO (possibly with @rpoyner-tri's name?) indicating that the test is missing would be good.
multibody/parsing/test/detail_urdf_parser_test.cc line 332 at r2 (raw file):
TEST_F(UrdfParserTest, MimicNoSap) { EXPECT_NE(AddModelFromUrdfString(R"""(
btw: This test will break when SAP becomes the default solver. Perhaps it would be better to explicitly declare TAMSI?
Implements parsing support for the `<mimic>` element in URDF. Towards RobotLocomotion#17663. Users could have been using existing URDF models in Drake with mimic tags that were ignored. Now those models will be simulated differently if they are being simulated with DiscreteContactSolver::kSap.
RussTedrake
force-pushed
the
parse_urdf_mimic
branch
from
364fde8 to
508883f
Compare
There was a problem hiding this comment.
Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers (waiting on @rpoyner-tri and @SeanCurtis-TRI)
multibody/parsing/detail_urdf_parser.cc line 535 at r2 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit: By Hoisting
indexoutside of the chain of if/else blocks, we giving the implicit promise that it will carry a valid value by the time we're done evaluating the block. However, thetype == "floating"block does not set it. It might throw, but we have no guarantees that it will throw.Therefore, this should be turned into an
optional<JointIndex>and it should be validated in theif(mimic_node)block before using.Alternatively, we can just set the index now that floating joints have actual
Joints.
Done.
"now that floating joints have actual Joints"... yes, but not until Finalize. Add that joint here is more of a change that I want to make/test in this PR.
I've handled the floating case.
multibody/parsing/detail_urdf_parser.cc line 619 at r2 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW For my own edification, is this parsing sensitive to the ordering of the XML tags? And is that normal for URDF and/or Drake parsing? I.e., what if the joint exists in the file but later than the joint that mimics it?
I don't have the sense that URDF particularly has this requirement. If this is our requirement, then we should be clear about this requirement in the error message.
If this is just understood by the community at large, please ignore this.
It's true that the joint referenced here must have been declared earlier in the file. That is common in our parsers (if not the xml standard). For instance, we reference the body by name earlier in the joint; and don't handle the case where the link was declared after the joint.
multibody/parsing/detail_urdf_parser.cc line 638 at r2 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW So, we allow a prismatic joint to mimic a revolute joint? (And vice versa.)
Yes.
multibody/parsing/detail_urdf_parser.cc line 647 at r2 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW While ambiguity in the URDF documentation is one issue, the more acute issue is that Drake's couple constraint only supports 1-dof joints. Perhaps it would be more honest/precise if this comment and the message indicated such.
E.g.,, warning message along the lines of:
Drake only supports the mimic element for single-dof joints. The joint '{}' (with {} dofs) is attempting to mimic joint '{}' (with {} dofs). The mimic element will be ignored.
This means we don't have to lawyer up vis a vis URDF, we're just stating what Drake does or does not do.
Done.
multibody/parsing/test/detail_urdf_parser_test.cc line 82 at r2 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit: Prior to this PR whether the plant was discrete or continuous was irrelevant. Now it matters. Putting a note here will be helpful for the future.
Although, that will probably be undone based on my next note (about testing against a continuous plant). But, for now, the comment would help clarify the current state of the code and its dependencies.
Done.
multibody/parsing/test/detail_urdf_parser_test.cc line 331 at r2 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit: You're missing the test:
MimicContinuous.I recognize that the infrastructure doesn't lend itself well to tests which vary in this dimension. At the very least, I think a TODO (possibly with @rpoyner-tri's name?) indicating that the test is missing would be good.
Done. Added the TODO.
multibody/parsing/test/detail_urdf_parser_test.cc line 332 at r2 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
btw: This test will break when SAP becomes the default solver. Perhaps it would be better to explicitly declare TAMSI?
Done.
There was a problem hiding this comment.
Reviewed 2 of 2 files at r3, all commit messages.
Reviewable status: 1 unresolved discussion, needs at least two assigned reviewers (waiting on @RussTedrake)
multibody/parsing/detail_urdf_parser.cc line 635 at r3 (raw file):
ParseScalarAttribute(mimic_node, "offset", &offset); if (!index) {
BTW While correct and succinct, I get the sense that Drake tends towards !index.has_value() over implicit bool conversion. (Akin to preferring to test a pointer explicitly against nullptr.)
|
Previously, RussTedrake (Russ Tedrake) wrote…
@SeanCurtis-TRI -- waiting for you to clear this one. |
|
+@rpoyner-tri for review (not sure why reviewable thinks i need more reviewers) |
Implements parsing support for the
<mimic>element in URDF. Towards #17663.Users could have been using existing URDF models in Drake with mimic tags that were ignored. Now those models will be simulated differently if they are being simulated with DiscreteContactSolver::kSap.
This change is