parsing: Use libsdformat's Interface API to parse URDF files #15140
Conversation
There was a problem hiding this comment.
+@EricCousineau-TRI for feature review or delegation, please
Reviewable status: LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers (waiting on @EricCousineau-TRI)
There was a problem hiding this comment.
Reviewed 10 of 14 files at r1.
Reviewable status: 11 unresolved discussions, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers (waiting on @azeey and @EricCousineau-TRI)
a discussion (no related file):
This also removes additional implicit model frames created for nested models that were missed in #15099.
Is it possible to split this out in into a separate / smaller PR?
multibody/parsing/detail_sdf_parser.cc, line 527 at r1 (raw file):
data_source.DemandExactlyOne(); sdf::ParserConfig parser_config = sdf::ParserConfig::GlobalConfig();
It smells bad to repeatedly register a callback for the global configuration.
If it's OK in this case, can you please describe why?
FWIW I don't think this is OK, as this mutation doesn't seem thread-safe in a global context (due to singleton)?
multibody/parsing/detail_sdf_parser.cc, line 533 at r1 (raw file):
}); parser_config.RegisterCustomModelParser(
It smells bad to repeatedly register a new custom parser for the global configuration.
If it's OK in this case, can you please describe why?
FWIW I don't this is OK, as this mutation doesn't seem thread-safe in a global context (due to singleton); also, it admits unbounded growth of sdf::ParserConfig::Implementation::customParsers!
Beyond that, though, the precedence of RegisterCustomModelParser does "last-one-wins" when a compatible parser is found, so it would be functionally OK.
multibody/parsing/detail_sdf_parser.cc, line 961 at r1 (raw file):
// For the libsdformat API, see: // http://sdformat.org/tutorials?tut=composition_proposal&cat=pose_semantics_docs&branch=feature-composition-interface-api&repo=https%3A%2F%2Fgithub.com%2FEricCousineau-TRI%2Fsdf_tutorials-1#1-5-minimal-libsdformat-interface-types-for-non-sdformat-models
Can you replace this with the now-merged form of this proposal?
multibody/parsing/detail_sdf_parser.cc, line 967 at r1 (raw file):
// To test re-parsing an SDFormat document, but in complete isolation. Tests // out separate model formats. constexpr char kExtForcedNesting[] = ".forced_nesting_sdf";
As much as I love this for testing, I don't think we should have this enabled by default for public API.
Can you make this feature internal test-only, disabled by default, and enabled via something like internal::EnableForcedNestingSdfForTesting() (ideally within a scoped context via ScopedExit)?
multibody/parsing/detail_sdf_parser.cc, line 971 at r1 (raw file):
void AddBodiesToInterfaceModel(const MultibodyPlant<double>& plant, ModelInstanceIndex model_instance, const sdf::InterfaceModelPtr& interface_model) {
FYI I forgot if there's a readability requirement / suggestion regarding mutable data within const shared_ptr<T>&.
From brief perusal, it seems like mutable data within a shared_ptr has been passed as const shared_ptr<T>&, e.g.:
drake/common/symbolic_expression_cell.cc
Lines 2095 to 2097 in 9d5b2e4
Line 429 in 9d5b2e4
multibody/parsing/detail_sdf_parser.cc, line 985 at r1 (raw file):
ModelInstanceIndex model_instance, const sdf::InterfaceModelPtr& interface_model) { for (FrameIndex fi(0); fi < plant.num_frames(); ++fi) {
nit Can you either use index or expand fully to frame_index?
(if you use index, can you replace link_index, joint_index, etc. with index as well in thijs new code?)
multibody/parsing/detail_sdf_parser.cc, line 993 at r1 (raw file):
plant.HasBodyNamed(frame.name(), model_instance)) { // Skip unnamed frames, and __model__ since it's already added. Also // skip frames with the same name as a link since those are not explicit
nit "not explicit frames" is a bit more confusing than just saying "are frames implicitly added".
multibody/parsing/detail_sdf_parser.cc, line 993 at r1 (raw file):
plant.HasBodyNamed(frame.name(), model_instance)) { // Skip unnamed frames, and __model__ since it's already added. Also // skip frames with the same name as a link since those are not explicit
nit For "not explicit", it is not immediately clear in which context this is implicit and why that is nuanced; Drake is the "sender", SDFormat is the "receiver", and it's unclear if we skip this because it's implicit in both, one or the other, etc.
Can you briefly qualify this?
multibody/parsing/detail_sdf_parser.cc, line 1014 at r1 (raw file):
} // This assumes that parent models will have their parsing start before child
nit Can you describe why we this assumption is "safe"?
(e.g. "because this is how parsing happens "?)
multibody/parsing/detail_sdf_parser.cc, line 1039 at r1 (raw file):
ModelInstanceIndex main_model_instance; // New instances will have indices starting from cur_num_models int cur_num_models = plant->num_model_instances();
BTW I'd prefer doing pre- and post-parsing set differencing, as it makes it clearer.
However, this seems fine for now.
multibody/parsing/detail_sdf_parser.cc, line 1113 at r1 (raw file):
RigidTransformd X_PM = RigidTransformd::Identity(); if (model_instance != main_model_instance) { // The pose of the main (non-nested) model will be updated by the
nit The flow of this comment is a bit confusing.
Can you ensure the "pose of main" is outside of the if statement?
(either via splitting the "however" sentence off, or moving full comment?)
multibody/parsing/test/sdf_parser_test/interface_api_test/top.sdf, line 1 at r1 (raw file):
<?xml version="1.0"?>
For completeness, can you exercise placement_frame here too? (e.g. a dummy table with a mug flipped upside down, or something like that?)
There was a problem hiding this comment.
First pass done, PTAL!
Reviewable status: 11 unresolved discussions, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers (waiting on @azeey and @EricCousineau-TRI)
There was a problem hiding this comment.
Reviewable status: 12 unresolved discussions, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers (waiting on @azeey and @EricCousineau-TRI)
multibody/parsing/BUILD.bazel, line 27 at r1 (raw file):
) filegroup(
I believe this is a subset of what is provided by :test_models.
Can you try deleting this target, and using :test_models instead?
(IMO, this is better scoped, but it's odd to have intersecting file groups)
There was a problem hiding this comment.
Reviewable status: 9 unresolved discussions, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers (waiting on @azeey and @EricCousineau-TRI)
a discussion (no related file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
This also removes additional implicit model frames created for nested models that were missed in #15099.
Is it possible to split this out in into a separate / smaller PR?
Done. #15208
multibody/parsing/BUILD.bazel, line 27 at r1 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
I believe this is a subset of what is provided by
:test_models.Can you try deleting this target, and using
:test_modelsinstead?
(IMO, this is better scoped, but it's odd to have intersecting file groups)
Done. It wasn't working before because it wasn't picking up the .forced_nesting_sdf extension.
multibody/parsing/detail_sdf_parser.cc, line 527 at r1 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
It smells bad to repeatedly register a callback for the global configuration.
If it's OK in this case, can you please describe why?
FWIW I don't think this is OK, as this mutation doesn't seem thread-safe in a global context (due to singleton)?
Done. I moved the creation of a sdf::ParserConfig object outside of LoadSdfso we won't be repeatedly registering a callback.
multibody/parsing/detail_sdf_parser.cc, line 533 at r1 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
It smells bad to repeatedly register a new custom parser for the global configuration.
If it's OK in this case, can you please describe why?
FWIW I don't this is OK, as this mutation doesn't seem thread-safe in a global context (due to singleton); also, it admits unbounded growth of
sdf::ParserConfig::Implementation::customParsers!Beyond that, though, the precedence of
RegisterCustomModelParserdoes "last-one-wins" when a compatible parser is found, so it would be functionally OK.
Done. I moved the creation of a sdf::ParserConfig object outside of LoadSdf.
multibody/parsing/detail_sdf_parser.cc, line 961 at r1 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
Can you replace this with the now-merged form of this proposal?
Done
multibody/parsing/detail_sdf_parser.cc, line 967 at r1 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
As much as I love this for testing, I don't think we should have this enabled by default for public API.
Can you make this feature internal test-only, disabled by default, and enabled via something like
internal::EnableForcedNestingSdfForTesting()(ideally within a scoped context viaScopedExit)?
Done. I'm passing in a flag test_sdf_forced_nesting which is set to true only during testing.
multibody/parsing/detail_sdf_parser.cc, line 985 at r1 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit Can you either use
indexor expand fully toframe_index?(if you use
index, can you replacelink_index,joint_index, etc. withindexas well in thijs new code?)
Done. Used index for all instances of *_index in the new code.
multibody/parsing/detail_sdf_parser.cc, line 993 at r1 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit "not explicit frames" is a bit more confusing than just saying "are frames implicitly added".
Done
multibody/parsing/detail_sdf_parser.cc, line 993 at r1 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit For "not explicit", it is not immediately clear in which context this is implicit and why that is nuanced; Drake is the "sender", SDFormat is the "receiver", and it's unclear if we skip this because it's implicit in both, one or the other, etc.
Can you briefly qualify this?
Done. I've tried to clarify this a little more. Let me know if it's sufficient.
multibody/parsing/detail_sdf_parser.cc, line 1014 at r1 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit Can you describe why we this assumption is "safe"?
(e.g. "because this is how parsing happens "?)
Done.
multibody/parsing/detail_sdf_parser.cc, line 1113 at r1 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit The flow of this comment is a bit confusing.
Can you ensure the "pose of main" is outside of the
ifstatement?
(either via splitting the "however" sentence off, or moving full comment?)
Done. I moved the whole comment outside of the if block.
multibody/parsing/test/sdf_parser_test/interface_api_test/table_and_mug.forced_nesting_sdf, line 14 at r2 (raw file):
<pose relative_to="table::top">0 0 0 1.570796 0 0</pose> </include> </model>
Working: Adding a fixed joint between table::top and mug::top seems to cause a change in the pose of mug::top and results in a failing test.
multibody/parsing/test/sdf_parser_test/interface_api_test/table_and_mug.forced_nesting_sdf, line 14 at r2 (raw file):
<pose relative_to="table::top">0 0 0 1.570796 0 0</pose> </include> </model>
Working: If I add a joint between table::top and mug::top, the joint is not added to the InterfaceModel of table_and_mug because the joint is now associated with the model instance of mug. If anything references the joint frame in the SDFormat file, it will cause an error. I'm going to see if there's a way to keep track of the original model each joint belonged to.
multibody/parsing/test/sdf_parser_test/interface_api_test/top.sdf, line 1 at r1 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
For completeness, can you exercise
placement_framehere too? (e.g. a dummy table with a mug flipped upside down, or something like that?)
Done.
There was a problem hiding this comment.
Ok, I have addressed all the feedback. PTAL.
Reviewable status: 7 unresolved discussions, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers (waiting on @EricCousineau-TRI)
multibody/parsing/BUILD.bazel, line 27 at r1 (raw file):
Previously, azeey (Addisu Z. Taddese) wrote…
Done. It wasn't working before because it wasn't picking up the
.forced_nesting_sdfextension.
Oops, that wasn't in r2. It's deleted in r3.
multibody/parsing/detail_sdf_parser.cc, line 521 at r3 (raw file):
std::string root_dir; // TODO(marcoag) ensure that we propagate the right ParserConfig instance. sdf::ParserConfig parser_config;
FYI, I have moved this section to a new function CreateNewParserConfig.
multibody/parsing/test/sdf_parser_test/interface_api_test/table_and_mug.forced_nesting_sdf, line 14 at r2 (raw file):
Previously, azeey (Addisu Z. Taddese) wrote…
Working: Adding a fixed joint between
table::topandmug::topseems to cause a change in the pose ofmug::topand results in a failing test.
Done. Fixed in #15290
multibody/parsing/test/sdf_parser_test/interface_api_test/table_and_mug.forced_nesting_sdf, line 14 at r2 (raw file):
Previously, azeey (Addisu Z. Taddese) wrote…
Working: If I add a joint between
table::topandmug::top, the joint is not added to theInterfaceModeloftable_and_mugbecause the joint is now associated with the model instance ofmug. If anything references the joint frame in the SDFormat file, it will cause an error. I'm going to see if there's a way to keep track of the original model each joint belonged to.
Done. #15279
There was a problem hiding this comment.
Reviewed 2 of 14 files at r1, 4 of 8 files at r2, 3 of 5 files at r3.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers (waiting on @azeey and @EricCousineau-TRI)
a discussion (no related file):
Second pass done, PTAL!
Will wait until prereq PRs merge to review further.
Thanks!
multibody/parsing/detail_sdf_parser.cc, line 1048 at r3 (raw file):
// skip frames with the same name as a link since those are frames added // by Drake and are considered implicit by SDFormat. Sending such frames // to SDFormat would imply that these frames are explicit (i.e frames
BTW In other instances, I've seen i.e. (trailing period) in lieu of i.e
$ git rev-parse --short HEAD
46cd5c87a6
$ grep -rnI '\bi\.e\.' | wc -l
753
$ grep -rnI '\bi\.e[^\.]' | wc -l
11
There was a problem hiding this comment.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers (waiting on @azeey and @EricCousineau-TRI)
multibody/parsing/detail_sdf_parser.cc, line 993 at r1 (raw file):
Previously, azeey (Addisu Z. Taddese) wrote…
Done. I've tried to clarify this a little more. Let me know if it's sufficient.
OK Yup, well clarified and sufficient!
azeey
force-pushed
the
sdf_interface_api
branch
2 times, most recently
from
24a7c8b to
feb8db7
Compare
There was a problem hiding this comment.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers (waiting on @EricCousineau-TRI)
a discussion (no related file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
Second pass done, PTAL!
Will wait until prereq PRs merge to review further.Thanks!
All the prereq PRs have merged and I have fixed all merge conflicts. PTAL.
multibody/parsing/detail_sdf_parser.cc, line 1048 at r3 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
BTW In other instances, I've seen
i.e.(trailing period) in lieu ofi.e$ git rev-parse --short HEAD 46cd5c87a6 $ grep -rnI '\bi\.e\.' | wc -l 753 $ grep -rnI '\bi\.e[^\.]' | wc -l 11
Changed ti i.e.,
There was a problem hiding this comment.
+@rpoyner-tri for platform review, please!
Reviewed 3 of 5 files at r4, 1 of 1 files at r5.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee rpoyner-tri(platform) (waiting on @azeey, @EricCousineau-TRI, and @rpoyner-tri)
a discussion (no related file):
Previously, azeey (Addisu Z. Taddese) wrote…
All the prereq PRs have merged and I have fixed all merge conflicts. PTAL.
OK Sweet!
multibody/parsing/detail_ignition.h, line 22 at r5 (raw file):
// Helper function to express a RigidTransform instance as an // ignition::math::Pose3d instance. ignition::math::Pose3d ToIgnPose3d(const math::RigidTransformd& pose);
nit Ign isn't a well-understood abbreviation for Drake developers.
Can you expand to ToIgnitionPose3d?
multibody/parsing/detail_sdf_parser.cc, line 1222 at r5 (raw file):
return ResolveUri(_input, package_map, "."); }); // TODO(#15018): This means that unrecognized elements won't be shown to a
FYI @marcoag This will create merge conflict (as it should) for #15317
multibody/parsing/test/detail_sdf_parser_test.cc, line 2016 at r5 (raw file):
{ // Frame T represents the frame top::table_and_mug::mug::top const RigidTransformd X_WT_expected(RollPitchYawd(1.570796, 0.0, 0.0),
nit Can you use more precision (here and in SDFormat file)?
There was a problem hiding this comment.
Reviewed 9 of 14 files at r1, 3 of 8 files at r2, 2 of 5 files at r3, 4 of 5 files at r4, 1 of 1 files at r5.
Reviewable status: 4 unresolved discussions (waiting on @azeey)
multibody/parsing/detail_sdf_parser.h, line 44 at r5 (raw file):
extention
typo
multibody/parsing/detail_sdf_parser.h, line 79 at r5 (raw file):
extention
typo
There was a problem hiding this comment.
Reviewable status: 2 unresolved discussions (waiting on @EricCousineau-TRI and @rpoyner-tri)
multibody/parsing/detail_ignition.h, line 22 at r5 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit
Ignisn't a well-understood abbreviation for Drake developers.Can you expand to
ToIgnitionPose3d?
Done.
multibody/parsing/detail_sdf_parser.h, line 44 at r5 (raw file):
Previously, rpoyner-tri (Rick Poyner (rico)) wrote…
extentiontypo
Done.
multibody/parsing/detail_sdf_parser.h, line 79 at r5 (raw file):
Previously, rpoyner-tri (Rick Poyner (rico)) wrote…
extentiontypo
Done.
multibody/parsing/test/detail_sdf_parser_test.cc, line 2016 at r5 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit Can you use more precision (here and in SDFormat file)?
Done.
There was a problem hiding this comment.
Reviewed 6 of 6 files at r6.
Reviewable status:complete! all discussions resolved, LGTM from assignees rpoyner-tri(platform),EricCousineau-TRI(platform) (waiting on @azeey)
There was a problem hiding this comment.
Reviewed 6 of 6 files at r6.
Reviewable status:
|
There is no "release notes: none" label here, but I'm not sure that the commit message would make sense to a user as part of the release notes? (It seems like an implementation detail.) Could you reply with some suggested bullet point text that I can paste into the notes, that explains the effect of this change for an end-user of Drake? Thanks! |
|
Does this work?
|
|
That's much better, but begs the question of whether there is any user-visible change from it? Perhaps the conversion process was brittle or broken, so that including URDFs used to be effectively impossible, but is possible now? |
|
I think the main issues is that the URDF->SDFormat conversion doesn't preserve elements SDFormat doesn't know about (e.g. Here's the updated note:
|
|
multibody/parsing/detail_sdf_parser.cc, line 1231 at r6 (raw file):
This captures a long-lived reference to a stack variable. (I'll fix in a forthcoming follow-up.) Code quote: &parser_config |
Closes #14295.
This allows Drake to parse URDF files bypassing libsdformat's native URDF parser. It makes use of libsdformat's Interface API by registering a custom parser for
*.urdffiles.This also removes additional implicit model frames created for nested models that were missed in #15099.
A large portion of this PR was taken from @EricCousineau-TRI's prototype
//cc @EricCousineau-TRI
Needs: #15290
This change is