[parsing] Add support for using merge-includes with custom parsed models #16727
Conversation
There was a problem hiding this comment.
Reviewable status: 1 unresolved discussion, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey and @EricCousineau-TRI)
a discussion (no related file):
Thanks!
Looking at this, I am a bit concerned as you mentioned - this is starting to couple quite a few things, and it may become rather hard to track. Also, some of the tracking that is being done is akin to multibody_plant_subgraph, but may not be reusable in that context.
Mayhaps we should implement a /dev/ or internal-only version of mutlibody_plant_subgraph.
In this case, it'd only be for composition and/or merging, not for anything else -- e.g., we could encapsulate it all into a single function in ::dev:: or ::internal::, not worry about removing stuff, inverse mappings, state mapping, etc.
Then we can consolidate compose w/ or w/o merge into that logic, w/o spreading it elsewhere; then, if we think the internal subgraph API could be hoisted, we could do so.
WDYT?
There was a problem hiding this comment.
Reviewable status: 1 unresolved discussion, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @EricCousineau-TRI)
a discussion (no related file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
Thanks!
Looking at this, I am a bit concerned as you mentioned - this is starting to couple quite a few things, and it may become rather hard to track. Also, some of the tracking that is being done is akin to
multibody_plant_subgraph, but may not be reusable in that context.Mayhaps we should implement a
/dev/or internal-only version ofmutlibody_plant_subgraph.
In this case, it'd only be for composition and/or merging, not for anything else -- e.g., we could encapsulate it all into a single function in::dev::or::internal::, not worry about removing stuff, inverse mappings, state mapping, etc.Then we can consolidate compose w/ or w/o merge into that logic, w/o spreading it elsewhere; then, if we think the internal subgraph API could be hoisted, we could do so.
WDYT?
I haven't had a chance to look carefully at mutlibody_plant_subgraph yet, but what you suggest sounds reasonable. I'll work on a prototype.
|
a discussion (no related file): Previously, azeey (Addisu Z. Taddese) wrote…
I've added |
There was a problem hiding this comment.
Reviewed 3 of 7 files at r2, all commit messages.
Reviewable status: 10 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey and @EricCousineau-TRI)
a discussion (no related file):
Previously, azeey (Addisu Z. Taddese) wrote…
I've added
MultibodyPlantSubgraphand used it to implement the merge-include. Can you give it a look? I'll cleanupdetail_multibody_plant_subgraph.h(move functions to.ccfile, address TODOs, etc) if this is going in the right direction.
OK Awesome! Yeah, looks like it's going in very much the right direction - thanks!!!
multibody/parsing/detail_multibody_plant_subgraph.h, line 1 at r2 (raw file):
#pragma once
nit Can you add brief comment pointing to Python prototype this is derived from, and why it's only a subset of functionality?
multibody/parsing/detail_multibody_plant_subgraph.h, line 23 at r2 (raw file):
template <typename T> std::vector<T> indices_to_vector(int num_items) {
nit Can you rename these functions to be CamelCase (as most are not O(1))?
Sorry for not mentioning that, but our C++ style guide and Python style guide conflict in that regard.
multibody/parsing/detail_multibody_plant_subgraph.h, line 128 at r2 (raw file):
} void PrintBodies() const {
nit Super helpful for debugging! However, can you instead pass ostream& as argument?
multibody/parsing/detail_multibody_plant_subgraph.h, line 210 at r2 (raw file):
std::set<ModelInstanceIndex> model_instances_; friend MultibodyPlantElementsMap;
nit Rather than using friend, can you expose read-only accessors? (and mutators, if necessary?)
multibody/parsing/detail_multibody_plant_subgraph.h, line 298 at r2 (raw file):
std::unique_ptr<Joint<double>> joint; // TODO(azeey) Not sure if this matches the python prototype.
nit My reading on this indicates that this matches the prototype.
Was there something you were concerned about?
multibody/parsing/detail_multibody_plant_subgraph.h, line 298 at r2 (raw file):
std::unique_ptr<Joint<double>> joint; // TODO(azeey) Not sure if this matches the python prototype.
nit Consider adding TODO for using double dispatch.
multibody/parsing/detail_multibody_plant_subgraph.h, line 366 at r2 (raw file):
std::map<ModelInstanceIndex, ModelInstanceIndex> model_instances_; friend MultibodyPlantSubgraph;
nit Rather than using friend, can you expose read-only accessors? (and mutators, if necessary?)
multibody/parsing/detail_multibody_plant_subgraph.h, line 400 at r2 (raw file):
} using RemapFuncT = std::function<ModelInstanceIndex(
nit Can you remove trailing T, and either use trailing Func or Function?
$ git log -n 1 --oneline --no-decorate
a6005354c9 Python bindings for LcmPublisherSystem and Simulator (#16762)
$ grep -rnI -E 'using [A-Z].*?[a-z]T =' | wc -l
2
$ grep -rnI -E 'using [A-Z].*?[a-z]Func =' | wc -l
9
$ grep -rnI -E 'using [A-Z].*?[a-z]Function =' | wc -l
12multibody/parsing/detail_multibody_plant_subgraph.h, line 444 at r2 (raw file):
} // TODO(azeey) Copy geometries (if applicable).
BTW Yup, will def. be applicable!
multibody/parsing/detail_sdf_parser.cc, line 942 at r2 (raw file):
} ModelInstanceIndex AddModelInstanceIfReusable(
Still not sure if I get why model instances need to be reusable if we now have subgraph functionality.
Can I ask why?
multibody/parsing/detail_sdf_parser.cc, line 1411 at r2 (raw file):
}; if (subgraph_info.is_merged) {
BTW Nice!!!
multibody/parsing/detail_urdf_parser.h, line 48 at r2 (raw file):
geometry::SceneGraph<double>* scene_graph = nullptr); std::string MergeModelFromUrdf(
nit Unused?
There was a problem hiding this comment.
Reviewable status: 3 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @EricCousineau-TRI)
multibody/parsing/detail_multibody_plant_subgraph.h, line 1 at r2 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit Can you add brief comment pointing to Python prototype this is derived from, and why it's only a subset of functionality?
Done in 1aef15b
multibody/parsing/detail_multibody_plant_subgraph.h, line 23 at r2 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit Can you rename these functions to be
CamelCase(as most are notO(1))?Sorry for not mentioning that, but our C++ style guide and Python style guide conflict in that regard.
Done in f41285b
multibody/parsing/detail_multibody_plant_subgraph.h, line 128 at r2 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit Super helpful for debugging! However, can you instead pass
ostream&as argument?
Yup, these were just for debugging and I was planning to remove these before merging. Do you think we should keep them?
multibody/parsing/detail_multibody_plant_subgraph.h, line 210 at r2 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit Rather than using
friend, can you expose read-only accessors? (and mutators, if necessary?)
Done in 98b885a
multibody/parsing/detail_multibody_plant_subgraph.h, line 298 at r2 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit My reading on this indicates that this matches the prototype.
Was there something you were concerned about?
I saw this comment in the python prototype
# N.B. We use `type(x) == cls`, not `isinstance(x, cls)`, so that we
# know we recreate the exact types.
Isn't dynamic_cast more like isinstance? That's where my concern came from.
multibody/parsing/detail_multibody_plant_subgraph.h, line 298 at r2 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit Consider adding TODO for using double dispatch.
Done in f41285b. Double dispatch would be a lot nicer.
multibody/parsing/detail_multibody_plant_subgraph.h, line 366 at r2 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit Rather than using
friend, can you expose read-only accessors? (and mutators, if necessary?)
Done in 98b885a
multibody/parsing/detail_multibody_plant_subgraph.h, line 400 at r2 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit Can you remove trailing
T, and either use trailingFuncorFunction?$ git log -n 1 --oneline --no-decorate a6005354c9 Python bindings for LcmPublisherSystem and Simulator (#16762) $ grep -rnI -E 'using [A-Z].*?[a-z]T =' | wc -l 2 $ grep -rnI -E 'using [A-Z].*?[a-z]Func =' | wc -l 9 $ grep -rnI -E 'using [A-Z].*?[a-z]Function =' | wc -l 12
Done in f41285b
multibody/parsing/detail_sdf_parser.cc, line 942 at r2 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
Still not sure if I get why model instances need to be reusable if we now have subgraph functionality.
Can I ask why?
As shown below, the custom parsed models have to be added into the destination plant before calling AddModelsFromSpecification to add the non-custom models. In the case where a non-custom model merge-includes a custom model, the model instance for the non-custom model will have to be created during AddMultibodyPlantSubgraphsToPlant. When AddModelsFromSpecification subsequently processes the non-custom model, it will also try to create a model instance for it and fail unless we keep track of reusable model instances.
multibody/parsing/detail_sdf_parser.cc, line 1411 at r2 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
BTW Nice!!!
👍
multibody/parsing/detail_urdf_parser.h, line 48 at r2 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit Unused?
Done in 27855ef
|
multibody/parsing/detail_multibody_plant_subgraph.h, line 444 at r2 (raw file): Previously, EricCousineau-TRI (Eric Cousineau) wrote…
Am I correct in thinking this is not necessary for our current use case? |
There was a problem hiding this comment.
Reviewed 1 of 8 files at r1, 2 of 7 files at r2, 1 of 2 files at r3.
Reviewable status: 5 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey and @EricCousineau-TRI)
multibody/parsing/detail_multibody_plant_subgraph.h, line 128 at r2 (raw file):
Previously, azeey (Addisu Z. Taddese) wrote…
Yup, these were just for debugging and I was planning to remove these before merging. Do you think we should keep them?
Yup! I think it'd be good to keep them around.
multibody/parsing/detail_multibody_plant_subgraph.h, line 298 at r2 (raw file):
Previously, azeey (Addisu Z. Taddese) wrote…
I saw this comment in the python prototype
# N.B. We use `type(x) == cls`, not `isinstance(x, cls)`, so that we # know we recreate the exact types.Isn't
dynamic_castmore likeisinstance? That's where my concern came from.
Per VC, these joints should already be final, so dynamic_cast<> shouldn't allow "wiggle room".
multibody/parsing/detail_multibody_plant_subgraph.h, line 444 at r2 (raw file):
Previously, azeey (Addisu Z. Taddese) wrote…
Am I correct in thinking this is not necessary for our current use case?
Per VC, yes, geometries will be necessary when composing models.
However, we should ensure it stays optional (e.g. don't parse into intermediate sub-scene graph if final MbP doesn't have associated scene graph).
Additionally, I think that we don't need to connect the (MbP, SG) pair, so we shouldn't need a temporary DiagramBuilder.
multibody/parsing/detail_sdf_parser.cc, line 942 at r2 (raw file):
Previously, azeey (Addisu Z. Taddese) wrote…
As shown below, the custom parsed models have to be added into the destination plant before calling
AddModelsFromSpecificationto add the non-custom models. In the case where a non-custom model merge-includes a custom model, the model instance for the non-custom model will have to be created duringAddMultibodyPlantSubgraphsToPlant. WhenAddModelsFromSpecificationsubsequently processes the non-custom model, it will also try to create a model instance for it and fail unless we keep track of reusable model instances.
Per VC, understood!
Requestions:
- Can you add comment about one-directional dependence in the parsing code (meriting the ordering you chose, and need for get-or-create?)
multibody/parsing/detail_sdf_parser.cc, line 944 at r3 (raw file):
ModelInstanceIndex AddModelInstanceIfReusable( MultibodyPlant<double>* plant, const std::string& model_name, const std::vector<ModelInstanceIndex>& reusable_model_instances) {
nit Is there any way to ensure a reusable model instance is consumed the "right" number of times?
e.g. add a map with counts, make it mutable, and ensure consuming end decrements the count and it ends at zero?
There was a problem hiding this comment.
Reviewable status: 6 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey and @EricCousineau-TRI)
multibody/parsing/detail_multibody_plant_subgraph.h, line 298 at r2 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
Per VC, these joints should already be
final, sodynamic_cast<>shouldn't allow "wiggle room".
(Can you reflect that in a comment here?)
multibody/parsing/detail_multibody_plant_subgraph.h, line 4 at r3 (raw file):
// This file is derived from the python prototype at // https://github.com/EricCousineau-TRI/repro/blob/master/drake_stuff/multibody_plant_prototypes/multibody_plant_subgraph.py.
nit Sorry to be picky, but can use a permalink (using sha) for this?
multibody/parsing/detail_sdf_parser.cc, line 942 at r2 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
Per VC, understood!
Requestions:
- Can you add comment about one-directional dependence in the parsing code (meriting the ordering you chose, and need for get-or-create?)
Ah, partial statement; also "Requestions" => "Request"
Second bullet point:
- Can you rename
resuable_model_instancesto something more informative, likesubgraph_merge_model_instances?
azeey
force-pushed
the
merge_include_interface_api
branch
3 times, most recently
from
fec3575
to
7009139
Compare
There was a problem hiding this comment.
Reviewable status: 4 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey and @EricCousineau-TRI)
multibody/parsing/detail_multibody_plant_subgraph.h, line 128 at r2 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
Yup! I think it'd be good to keep them around.
Done in a68c57d.
multibody/parsing/detail_multibody_plant_subgraph.h, line 298 at r2 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
(Can you reflect that in a comment here?)
Done in a68c57d.
multibody/parsing/detail_multibody_plant_subgraph.h, line 444 at r2 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
Per VC, yes, geometries will be necessary when composing models.
However, we should ensure it stays optional (e.g. don't parse into intermediate sub-scene graph if final MbP doesn't have associated scene graph).
Additionally, I think that we don't need to connect the (MbP, SG) pair, so we shouldn't need a temporaryDiagramBuilder.
Done. I've added SceneGraph operations in a68c57d.
multibody/parsing/detail_multibody_plant_subgraph.h, line 4 at r3 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit Sorry to be picky, but can use a permalink (using sha) for this?
Done in a68c57d
multibody/parsing/detail_multibody_plant_subgraph.cc, line 321 at r4 (raw file):
*geometry_instance_dest->illustration_properties()); } else { // Currently, RegisterVisualGeometry also assigns the Perception role to a
@EricCousineau-TRI We talked about exposing MultibodyPlant::RegisterGeometry, but I found that it doesn't have a way propagate the roles assigned to the geometry. So, I've instead used RegisterCollisionGeometry and RegisterVisualGeometry (which are already public). The only caveat is that it doesn't cover geometries that have a Perception role only.
There was a problem hiding this comment.
Reviewed 2 of 15 files at r4, all commit messages.
Reviewable status: 6 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey and @EricCousineau-TRI)
multibody/parsing/detail_multibody_plant_subgraph.h, line 187 at r5 (raw file):
}; // Ensures that current elements / topology satisifes subgraph invariants.
nit (minor) grammar "satisfy"
multibody/parsing/detail_multibody_plant_subgraph.cc, line 321 at r4 (raw file):
Previously, azeey (Addisu Z. Taddese) wrote…
@EricCousineau-TRI We talked about exposing
MultibodyPlant::RegisterGeometry, but I found that it doesn't have a way propagate the roles assigned to the geometry. So, I've instead usedRegisterCollisionGeometryandRegisterVisualGeometry(which are already public). The only caveat is that it doesn't cover geometries that have aPerceptionrole only.
Is it possible to teach RegisterGeometry to handle roles (w/o altering public workflows)?
I can help and/or solicit help if so desired.
multibody/parsing/detail_multibody_plant_subgraph.cc, line 8 at r5 (raw file):
template <typename T> std::vector<T> IndicesToVector(int num_items) {
nit Many of these internal helpers don't have declarations in header file.
Can you place them in anonymous namespace? (I assume demoted visibility will save on exported symbols in compiled binaries)
multibody/parsing/detail_multibody_plant_subgraph.cc, line 316 at r5 (raw file):
*proximity_properties); } else if (geometry_instance_dest->illustration_properties() != nullptr) { geometry_id_dest = plant_dest_->RegisterVisualGeometry(
This may be too relaxed and can permit loss / corruption of information.
Can you assert that (illustration, perception) are mutually required roles?
i.e. DRAKE_DEMAND(geometry_instance_dest->perception_properties() != nullptr)
Perhaps w/ a TODO stating that we may lose percpetion-specific properties?
There was a problem hiding this comment.
Reviewable status: 7 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey and @EricCousineau-TRI)
a discussion (no related file):
Alas, I forgot about another feature - collision filters, TODO'd here:
https://github.com/EricCousineau-TRI/repro/blob/5bc0e37a371239c096b0130c0c2ef4eca48153d8/drake_stuff/multibody_plant_prototypes/multibody_plant_subgraph.py#L130
IIRC, any collision filtering is "rendered" immediately (i.e., if you have a frame as a constituent filtering element, then only the IDs at time of declaration are filtered). For more information, see the Warning in these docs:
https://drake.mit.edu/doxygen_cxx/classdrake_1_1geometry_1_1_collision_filter_manager.html#a67386b6917b1243e34c6f092d4956386
Can you do one of the following:
(a) Support filtering for subgraphs?
(b) Explicitly disallow subgraphs of any SceneGraph that has any (non-default) collision filtering.
Ultimately, I think we'd only be feature-complete (can close the issue) once we've done (a).
Plus, the introspection to accomplish (b) may require 50% of the effort that (a) would require (rough guess).
azeey
force-pushed
the
merge_include_interface_api
branch
from
7009139
to
280de1b
Compare
There was a problem hiding this comment.
Reviewable status: 3 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey and @EricCousineau-TRI)
a discussion (no related file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
Alas, I forgot about another feature - collision filters, TODO'd here:
https://github.com/EricCousineau-TRI/repro/blob/5bc0e37a371239c096b0130c0c2ef4eca48153d8/drake_stuff/multibody_plant_prototypes/multibody_plant_subgraph.py#L130IIRC, any collision filtering is "rendered" immediately (i.e., if you have a frame as a constituent filtering element, then only the IDs at time of declaration are filtered). For more information, see the Warning in these docs:
https://drake.mit.edu/doxygen_cxx/classdrake_1_1geometry_1_1_collision_filter_manager.html#a67386b6917b1243e34c6f092d4956386Can you do one of the following:
(a) Support filtering for subgraphs?
(b) Explicitly disallow subgraphs of any SceneGraph that has any (non-default) collision filtering.Ultimately, I think we'd only be feature-complete (can close the issue) once we've done (a).
Plus, the introspection to accomplish (b) may require 50% of the effort that (a) would require (rough guess).
Working. I'll look into (a), might have to wait till next week though.
multibody/parsing/detail_multibody_plant_subgraph.h line 187 at r5 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit (minor) grammar "satisfy"
Done.
multibody/parsing/detail_multibody_plant_subgraph.cc line 321 at r4 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
Is it possible to teach
RegisterGeometryto handle roles (w/o altering public workflows)?I can help and/or solicit help if so desired.
I've added MultibodyPlant::RegisterGeometry that takes a geometry::GeometryInstance. Let me know if that's what you had in mind.
multibody/parsing/detail_multibody_plant_subgraph.cc line 8 at r5 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit Many of these internal helpers don't have declarations in header file.
Can you place them in anonymous namespace? (I assume demoted visibility will save on exported symbols in compiled binaries)
Done
multibody/parsing/detail_multibody_plant_subgraph.cc line 316 at r5 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
This may be too relaxed and can permit loss / corruption of information.
Can you assert that
(illustration, perception)are mutually required roles?
i.e.DRAKE_DEMAND(geometry_instance_dest->perception_properties() != nullptr)Perhaps w/ a TODO stating that we may lose percpetion-specific properties?
I've added MultibodyPlant::RegisterGeometry that takes a geometry::GeometryInstance, so this now replaced with an else block that adds any geometry by forwarding the GeometryInstance.
multibody/parsing/detail_sdf_parser.cc line 942 at r2 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
Ah, partial statement; also "Requestions" => "Request"
Second bullet point:
- Can you rename
resuable_model_instancesto something more informative, likesubgraph_merge_model_instances?
Done. For the comment about one-directional dependence, let me know if I should add anything else.
multibody/parsing/detail_sdf_parser.cc line 944 at r3 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit Is there any way to ensure a reusable model instance is consumed the "right" number of times?
e.g. add a map with counts, make it mutable, and ensure consuming end decrements the count and it ends at zero?
Done in 280de1b
There was a problem hiding this comment.
Sweet, subgraph bits look awesome thus far!
Reviewed 4 of 12 files at r6, all commit messages.
Reviewable status: 9 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey and @EricCousineau-TRI)
multibody/parsing/detail_multibody_plant_subgraph.cc line 316 at r5 (raw file):
Previously, azeey (Addisu Z. Taddese) wrote…
I've added
MultibodyPlant::RegisterGeometrythat takes ageometry::GeometryInstance, so this now replaced with anelseblock that adds any geometry by forwarding theGeometryInstance.
Thanks! Can I ask if we still need the if for proximity_properties != nullptr?
multibody/parsing/detail_sdf_parser.cc line 942 at r2 (raw file):
Previously, azeey (Addisu Z. Taddese) wrote…
Done. For the comment about one-directional dependence, let me know if I should add anything else.
OK Yup, this seems to cover it!
multibody/parsing/detail_sdf_parser.cc line 1029 at r6 (raw file):
model_name)); } else { --subgraph_merged_model_instances->at(model_instance);
nit Can you add DRAKE_DEMAND(subgraph_merged_model_instances->at(model_instance) > 0) before this line?
multibody/parsing/test/detail_multibody_plant_subgraph_test.cc line 102 at r6 (raw file):
// Helper struct that is returned by CastTo defined below. template <typename... C>
BTW Nice!
multibody/parsing/test/detail_multibody_plant_subgraph_test.cc line 112 at r6 (raw file):
template <typename F> void EachDo(F cb) {
nit Not sure of EachDo is fully accurate, given it only operates if RTTI is compatible.
Consider renaming to DoIfCastable?
multibody/parsing/test/detail_multibody_plant_subgraph_test.cc line 133 at r6 (raw file):
} template <typename T, typename S> struct Zip {
nit Nice! Can you add TODO to replace with std::ranges when the feature is available?
multibody/parsing/test/detail_multibody_plant_subgraph_test.cc line 207 at r6 (raw file):
EXPECT_EQ(a->dynamic_friction(), b->dynamic_friction()); } else { // TODO(azeey) The python prototype compares the values of a and b, but we
Aw, nuts! Perhaps we can add this to AbstractValue and Value<> (using SFINAE to ensure T::operator== exists)?
multibody/parsing/test/detail_multibody_plant_subgraph_test.cc line 313 at r6 (raw file):
EXPECT_EQ(joint_a.default_positions(), joint_b.default_positions()); // TODO(azeey) The python prototype has comment "Fix damping for
Ah, that was just for missing bindings - fixed in prototype:
EricCousineau-TRI/repro@9d5a4cd
Can remove TODO?
multibody/parsing/test/detail_multibody_plant_subgraph_test.cc line 315 at r6 (raw file):
// TODO(azeey) The python prototype has comment "Fix damping for // BallRpyJoint". Does this apply in C++ if (auto result =
If someone adds a new joint type to drake and to a test plant, then this testing code will miss it.
Consider adding something like this:
int num_matches{};
if (auto result = ...) {
num_matches += result.EachDo(...);
} else {...}
EXPECT_GT(num_matches, 0); // Ensure at least one of the checks was performed.
multibody/parsing/test/detail_multibody_plant_subgraph_test.cc line 548 at r6 (raw file):
} // Returns a random joint, but with an incrementing name. Note that we
nit Given we're not using a RNG, consider putting "random" in quotes.
(I think we should keep it as modulus, to ensure full coverage of joint types)
|
a discussion (no related file): Previously, azeey (Addisu Z. Taddese) wrote…
Can you take a quick look at c40c8cb. I will add tests if that looks okay to you. |
|
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
This patch is my attempt to do it, but I get a slew of compiler errors diff --git a/common/value.h b/common/value.h
index 22ce1e8ff2..c7175e6303 100644
--- a/common/value.h
+++ b/common/value.h
@@ -47,6 +47,25 @@ using ValueForwardingCtorEnabled = typename std::enable_if_t<
template <typename T>
using remove_cvref_t = std::remove_cv_t<std::remove_reference_t<T>>;
+// Adapted from https://people.eecs.berkeley.edu/~brock/blog/detection_idiom.php
+template <typename T>
+struct has_equality_operator {
+ template <typename U>
+ static constexpr decltype(std::declval<U>() == std::declval<U>(), bool())
+ test_equality_operator(int) {
+ return true;
+ }
+
+ template <typename U>
+ static constexpr bool test_equality_operator(...) {
+ return false;
+ }
+
+ static constexpr bool value = test_equality_operator<T>(int());
+};
+
+template <typename T>
+constexpr bool has_equality_operator_v = has_equality_operator<T>::value;
} // namespace internal
#endif
@@ -144,6 +163,8 @@ class AbstractValue {
/// the typeid of the most-derived type of the contained object.
std::string GetNiceTypeName() const;
+ virtual bool operator==(const AbstractValue& other) const = 0;
+
protected:
#if !defined(DRAKE_DOXYGEN_CXX)
// Use a struct argument (instead of a bare size_t) so that no code
@@ -251,6 +272,8 @@ class Value : public AbstractValue {
const std::type_info& type_info() const final;
const std::type_info& static_type_info() const final;
+ bool operator==(const AbstractValue& other) const override;
+
// A using-declaration adds these methods into our class's Doxygen.
using AbstractValue::static_type_info;
using AbstractValue::GetNiceTypeName;
@@ -809,5 +832,18 @@ const std::type_info& Value<T>::static_type_info() const {
return typeid(T);
}
+template <typename T>
+bool Value<T>::operator==(const AbstractValue& other) const {
+ const T* other_value = other.maybe_get_value<T>();
+ if (other_value == nullptr) {
+ return false;
+ }
+ if constexpr (internal::has_equality_operator_v<T>){
+ const T& value = get_value();
+ return (value == *other_value);
+ }
+ return false;
+}
+
#endif // DRAKE_DOXYGEN_CXX
} // namespace drakeA small sample of the errors: |
|
This approach is doomed to be overly brittle. We can't implement parsing this way. It's not up to the parser to know how to clone a frame. Anytime we add an optional property to the frame class (i.e., something no required by the frame's constructor), we'll need to remember to propagate it here, so that copying is total, not partial. That's unmaintainable. Ditto for all of the other element types (body, etc.). At best, we could add the "copy element from one MbP to another MbP" feature to the plant itself (so the logic is properly encapsulated) -- but really, I think the subgraph approach to parsing is fundamentally flawed and we need to discuss punting it entirely. |
|
Good points on having "clone-to-with-remap". Unclear why subgraph is fundamentally flawed. |
|
Using MbP as the intermediate representation signals an design flaw with the parser library's architecture. That holds true even if we add "MbP::CopyElementIntoOtherMbP" encapsulated helper functions. Something about the parsing passes must be mis-ordered, or else the composition specification incorrectly depends on tree-element ordering an in-order traversal or something. I think VC would be fine, but first I would like to see (in writing) a clear problem statement of the implementation challenge that this is trying to work around. I believe I understand the functional requirement (what a merge-include does), but I don't understand why it is difficult to implement. Here's some thoughts along the lines of what might be wrong, in case it helps with the write-up of the current implementation challenge: The first parsing pass should only be lifting the XML syntax into a DOM tree. (This is libsdformat's job.) A second pass should be used to map the DOM into the MbP, which is where named references will be resolved, like which bodies a joint is connecting. (This is |
Unfortunately, that is the goal for permitting interaction between SDFormat and other formats that (I prefer) it does not directly ingest 😅 Again, this is only when mixing model formats for which Drake has its own crafted (and thoroughly tested) parsing. I would readily buy the "this should only ever be done via IR" if we decide to have SDFormat be the sole interface between all model formats or iff we have our own IR that we use for our parsing. However, we have neither of those, hence the immediate rendering. Yes, this complicates the parsing passes / mechanisms necessary for gluing; however, by construction, since all models should be encapsulated / scoping only admits looking "down", there should be no actual design issues. Would you mind me scheduling a VC just to ensure we're on the same page? |
|
Sorry, I was unclear -- I don't think that the URDF or MJF tree should be jammed into a subtree in the libsdformat DOM via transmogrification. I mean that the libsdformat DOM should have a node for "external file: here's the name" and do nothing more. Just tell the me filename. I'll add it to MbP myself, thanks. No callback functions. |
I'll repeat my above -- I'm fine with that, but I need someone to point me to why this is all so difficult, first. I can't have a useful VC until I'm calibrated to the problem. Is there a draft of this code without the subgraph stuff, so that maybe I could see a failing test case (or TODO comment in the implementation) that forms the basis for why we're on this road in the first place? I'm happy to dig around in code myself to find out, but that will take time, and I would appreciate some starting points or links to prior discussions to get me going. |
|
Re: "why callbacks vs. just getting include filenames" - difficulty lies in fact that coupling (callbacks) are necessary to ensure nested models can be parsed (to whatever extent) to provide poses / relationships to libsdformat so it can construct it's pose-frame graph for posturing / attaching. Re: "why subgraph?", base..r1 - per this comment #16727 (review) - shows that attempting to do an include-merge without MbB-as-IR would require libsdformat mechanisms to creep into other parsers. Subgraph provides mechanism for modularity (as well as other benefits that I want eventually). We don't have failing tests, but I feared for durability of that approach w/ other constraints in play. Re: other links for reading, the only other one I can immediately think of is the design doc I linked above: Does this help? |
There was a problem hiding this comment.
Reviewed 26 of 31 files at r16, 7 of 8 files at r19, all commit messages.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee sammy-tri(platform), needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey)
multibody/parsing/detail_sdf_parser.cc line 1799 at r18 (raw file):
Previously, azeey (Addisu Z. Taddese) wrote…
Done in 2b0ff8a, but I'm not 100% sure if this PR addresses the author's concerns.
Gotcha, there is a chance that interleaving MuJoCo and SDFormat composition could pose some issues. Perhaps rewording then to:
Should ensure we can appropriately interleave SDFormat composition and MuJoCo composition
\cc @rpoyner-tri
multibody/parsing/test/sdf_parser_test/interface_api_test/arm_merge_include.sdf line 2 at r18 (raw file):
Previously, azeey (Addisu Z. Taddese) wrote…
Merge-include was added in 1.9, so every SDFormat file that has
//include/@mergeshould be 1.9. I couldn't find any 1.10 in theinterface_api_testdirectory.
OK Thanks!
There was a problem hiding this comment.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee sammy-tri(platform), needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey)
multibody/parsing/detail_sdf_parser.cc line 1799 at r18 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
Gotcha, there is a chance that interleaving MuJoCo and SDFormat composition could pose some issues. Perhaps rewording then to:
Should ensure we can appropriately interleave SDFormat composition and MuJoCo composition\cc @rpoyner-tri
It's been a while since I visited this. IIRC, Mujoco has some specific expectations around merging model files. I will have to take a look again at their docs, and write down the potential issues.
There was a problem hiding this comment.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee sammy-tri(platform), needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey)
multibody/parsing/detail_sdf_parser.cc line 1799 at r18 (raw file):
Previously, rpoyner-tri (Rick Poyner (rico)) wrote…
It's been a while since I visited this. IIRC, Mujoco has some specific expectations around merging model files. I will have to take a look again at their docs, and write down the potential issues.
I think my concern was with the mujoco <worldbody> element, which is documented (in their parser) to be automatically merged with other <worldbody> elements, when files are combined by their include mechanism. It seems to me now that mujoco files designed for inclusion may or may not have a <worldbody>, in which case I don't know how frames would be combined.
It could be that it's all fine, but i think we would need some test cases to work through the issues.
There was a problem hiding this comment.
Just came up with one possible style guide issue.
Reviewed 1 of 7 files at r2, 7 of 18 files at r10, 1 of 28 files at r14, 4 of 37 files at r15, 27 of 31 files at r16, 5 of 10 files at r17, 1 of 1 files at r18, 8 of 8 files at r19, all commit messages.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee sammy-tri(platform), needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey)
multibody/parsing/detail_sdf_parser.cc line 1394 at r19 (raw file):
// TODO(azeey) If any of Drakes custom parsers start supporting nested models, // this needs to be updated to keep track of them. struct InterfaceModelHelper {
based on my reading of https://drake.mit.edu/styleguide/cppguide.html#Structs_vs._Classes this struct does have invariants (modifying public fields after calling TakeSnapshot for example would break an invariant)
Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
azeey
force-pushed
the
merge_include_interface_api
branch
from
2b0ff8a
to
5a6b966
Compare
There was a problem hiding this comment.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee sammy-tri(platform), needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey)
multibody/parsing/detail_sdf_parser.cc line 1394 at r19 (raw file):
Previously, sammy-tri (Sam Creasey) wrote…
based on my reading of https://drake.mit.edu/styleguide/cppguide.html#Structs_vs._Classes this struct does have invariants (modifying public fields after calling
TakeSnapshotfor example would break an invariant)
Okay, I've converted it to a class and added accessors.
There was a problem hiding this comment.
Reviewed 6 of 6 files at r20, all commit messages.
Reviewable status: 1 unresolved discussion, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey)
Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
azeey
force-pushed
the
merge_include_interface_api
branch
from
5a6b966
to
e3afd5d
Compare
There was a problem hiding this comment.
Reviewed 1 of 1 files at r21, all commit messages.
Dismissed @jwnimmer-tri from a discussion.
Reviewable status: needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey)
multibody/parsing/detail_sdf_parser.cc line 1799 at r18 (raw file):
Previously, rpoyner-tri (Rick Poyner (rico)) wrote…
I think my concern was with the mujoco
<worldbody>element, which is documented (in their parser) to be automatically merged with other<worldbody>elements, when files are combined by theirincludemechanism. It seems to me now that mujoco files designed for inclusion may or may not have a<worldbody>, in which case I don't know how frames would be combined.It could be that it's all fine, but i think we would need some test cases to work through the issues.
Thanks (and sorry for delay)
There is a test with a MuJoCo file (see arm.xml), but there is not one without a worldbody.
@azeey Would you be able to add a small parsing test to see what happens if worldbody is not present?
(I assume worldbody is not required, so it will effectively be unanchored.)
Re: possible testing of the MuJoCo include mechanism, either in isolation or in concert w/ SDFormat, I would like to keep that out of this PR for now.
There was a problem hiding this comment.
Reviewable status: needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey)
multibody/parsing/detail_sdf_parser.cc line 1799 at r18 (raw file):
I think my concern was with the mujoco
<worldbody>element, which is documented (in their parser) to be automatically merged with other<worldbody>elements, when files are combined by theirincludemechanism.
Citation please?
All I can find in the docs is that when using MJCF include, the outermost (i.e., root) element is discarded before copy-pasting the new file into the location of the MJCF include statement -- nothing specific to <worldbody> per se.
Anyway, why should an SDFormat semantic include statement expect to inter-operate with a MJCF fragment file for a MuJoCo lexical include statement? Only MJCF files can include MJCF fragments. SDFormat only ever needs to include a normie top-level MJCF file that opens with <mujoco>.
My interpretation so far is that yes, the TODO is stale.
There was a problem hiding this comment.
Reviewed 1 of 1 files at r21, all commit messages.
Reviewable status: needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey)
There was a problem hiding this comment.
Reviewable status: needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey)
multibody/parsing/detail_sdf_parser.cc line 1799 at r18 (raw file):
Previously, jwnimmer-tri (Jeremy Nimmer) wrote…
I think my concern was with the mujoco
<worldbody>element, which is documented (in their parser) to be automatically merged with other<worldbody>elements, when files are combined by theirincludemechanism.Citation please?
All I can find in the docs is that when using MJCF include, the outermost (i.e., root) element is discarded before copy-pasting the new file into the location of the MJCF include statement -- nothing specific to
<worldbody>per se.Anyway, why should an SDFormat semantic include statement expect to inter-operate with a MJCF fragment file for a MuJoCo lexical include statement? Only MJCF files can include MJCF fragments. SDFormat only ever needs to include a normie top-level MJCF file that opens with
<mujoco>.My interpretation so far is that yes, the TODO is stale.
OK Looks like TODO is resolved, and upon review I don't think we need the additional test for the non-worldbody MJCF file.
There was a problem hiding this comment.
Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey)
a discussion (no related file):
Working: Will test in Anzu.
There was a problem hiding this comment.
Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey)
a discussion (no related file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
Working: Will test in Anzu.
Anzu-master-Drake-PR, build 232
There was a problem hiding this comment.
Reviewable status: commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @azeey)
a discussion (no related file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
Anzu-master-Drake-PR, build 232
Done!
|
Huh, |
|
@drake-jenkins-bot retest this please |
|
mypy stub generation failed: @drake-jenkins-bot linux-focal-gcc-cmake-experimental-release please |
EricCousineau-TRI
added
status: squashing now
…els (RobotLocomotion#16727) SDFormat models can now merge-include URDF and MJCF models using `<include merge="true"/>`
/cc @EricCousineau-TRI
This change is