-
Notifications
You must be signed in to change notification settings - Fork 1.3k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Defining collision filter groups across model instances #14785
Comments
This is very helpful, thank you. Here are a random collection of thoughts as I consider this kind of feature.
On the whole, I lean towards, as @sherm1 calls it, "the principle of least astonishment" and, for me, that means relationships between things should be explicit with the minimum of hidden machinery. |
My 2 cents: I'd like to see us work this out first in terms of objects known to Drake rather than files. Urdf and sdf files are just a convenient way to define Drake ModelInstances, which combined define a Drake System. I think what we're looking for is a way to define collision filters that span multiple ModelInstances. Consider a System comprised of two identical Panda arms defined by reading panda.urdf twice into instances An alternative (or addition) might be to define a distinct kind of collision filter group, say a "global collision filter", that automatically combines with any other global collision filter of the same name in the same Drake System. That would allow urdfs to be combinable without a higher-level system specification. Those couldn't be scoped since no individual urdf can know the names of the model instances that will be created; they would just be globals like externs in C. |
@sherm1 If you're willing to do post-parsing coding, you already have the API you need to declare arbitrary collision filters between any bodies, regardless of their provenance or model instance affiliation. At the end of the day, What the current API doesn't have is the ability to articulate collision filter groups based on what was stated in URDF/SDF files. (The API will allow you to determine if collision between two geometries is filtered, but gives absolutely zero explanation for why.) Named collision filter groups in urdf and sdf only exist during the parsing of a specific file. The lifespan of such a group is limited to the time of the function invocation for that particular file, and the scope of impact they have is limited to the bodies introduced by that file in that parsing invocation. (I.e., parsing the same file twice will not cause cross-model instance collision filtering). So, to use the current API to achieve the affect in the OP, the C++ code would have to have a lot of hard-coded knowledge about the contents of the URDF/SDF files being parsed. Persisting those aspects that are currently thrown out would help, but would be incredibly brittle depending on what information is actually given in the specification files. So, we have a few options:
|
Got it, thanks. That third option sounds great! Meanwhile, maybe the "global collision filter" would be sufficient? Though it would have to persist post-parsing so the "linker" could connect the geometries. |
Can you elaborate on the concept of the "global collision filter"?
Further thoughts? |
Your point 2 above is what I was thinking of. We would invent a way to mark a collision group name "global" (or maybe "shared" is better?). Something like:
These groups would have to persist post-parsing so that they can be combined once all instances of a global group have been seen. Regarding transitivity, I think we don't want it. In the example you gave the urdf defining the table would provide a global group for the table geometry ("table_geometry_global"). Then each robot urdf would create ( |
In my mind, they would exist as a hidden data of the
We certainly don't want accidental transitivity. :) In your example, I recognize the key is the table declares a group and the arm declares a group. The arm also declares the filter -- i.e., itself with the table's group. The key here is that each time we parse the arm, we're creating a new group that "happens" to share a name with a previously parsed group -- they don't all dump into the same group. While referencing collision filter groups in other files for the purpose of declaring filtering would be perfectly valid, the contents of any single group can only consist of bodies defined in the same "instance" of the file as the group is created. Further thoughtsPossible tags:I can conceive of a pair of tags that work together:
Name collisionsWhen declaring a filtered relationship between two groups, it should always be a group declared in the current file with a group declared either in the same file or another file. What if the name appears multiple times (see the arm example above). What should we do?
Arbitrary orderIt would be important to make sure that we don't require strict dependencies on the order in which things are defined. I.e., you have to parse the files in just the right order, or things blow up. If you've got circular collision filtering, that'd be impossible. And, even in the context of a single file, we don't require that. Fortunately, the idea of persisting these declarations beyond the lifespan of the file is perfectly consistent with this property, so no worries there. |
Regarding name collisions: I think we would want to allow any urdf to declare a global group containing some local geometry. If a same-named global group appears in another urdf (or by multiple readings of the same urdf) then the all the mentioned geometries should be mutually filtered. (I'm not sure if that addresses your questions above.) |
@Syler Much of this discussion has spiraled into technical nitty gritty, but we've lost sight of the origin and I'd like to revisit your specific problem. Looking at the original post, you've outlined some tentative syntax and the idea that you want the geometries of the "base of the Franka and the base of the 2DOF arm" to be filtered for collisions. It'll be good to step away from hypothetical implementation and return application. Could you elaborate on your application?
A greater insight in your application/workflow will help inform our efforts to make sure the motivating problem is actually getting efficiently addressed. Thanks. |
Here's a pic with a little less clutter, hope it makes things a little clearer. We use a 2DOF arm to hold a bowl that the Franka arm serves food into. You can check out video of it in action here - see Meet Alfred 2.0, With Amazing New Features! When planning, we don't want to smack the center assembly of the robot either with the 2DOF arm/bowl or the Franka. Snippet of how we're constructing our system model: // created a plant that will later receive the two robots.
robots_plant_ = builder_.AddSystem<MultibodyPlant>(time_step);
momap::log()->debug("SM:Build: Creating robot model parser with {} entities.",
robot_entities.size());
for (const auto& entity : robot_entities) {
mb_entity_map_.emplace(entity->name, entity);
// Parse the robot's urdf
const auto robot_model_index {
Parser(robots_plant_, scene_graph_)
.AddModelFromFile(entity->urdf, entity->name)};
model_index_map_.emplace(entity->name, std::move(robot_model_index));
log()->trace("SysModel:Build: added robot: {}", entity->name);
}
for (const auto& entity : robot_entities) {
// get child frame of urdf that is used as root
auto& child_frame {robots_plant_->GetFrameByName(
entity->world_frame, model_index_map_.at(entity->name))};
robots_plant_->WeldFrames(robots_plant_->world_frame(), child_frame,
entity->X_WorldToEntity);
}
// Now the model is complete.
robots_plant_->Finalize(); The reason we want the collisions to be excluded is there is overlapping collision geo in the cylindrical base part of both the 2DOF arm model and the Franka model. We've had other use cases where the same functionality was desired, like for large static parts of our workspace with partially overlapping collision geometry from different system models. |
I like @sherm1 's suggestion of a global/shared collision filter group definition, it would address all of our use cases |
Thanks for the explanation! A follow-up Q: from the picture it looks as though collision forces between the base geometries wouldn't affect motion at all (since both parts are immobile). Is that right? If so, is the goal of filtering to improve performance or is there another reason? |
The primary reason for filtering is to avoid states from incorrectly getting flagged as invalid/in collision during planning. If the Franka and ancillary arm base are not part of the same collision filter group, all states get flagged as in collision. |
@5yler can you clarify "flagged as in collision during planning"? How is that happening? Are you making a particular query call to SceneGraph and then making a determination about state validity? Is there a temporary workaround where you can exclude unwanted collision pairs in code? |
More than likely, this is referring to what I'll be looking into this ticket a bit this week. I think having a single SDFormat file that uses We might also be able to extend the model directives syntax to offer the same effect via yaml (instead of xml). |
Relevant to: RobotLocomotion#14785 This patch extends the parsing of collision filter groups so that collision filters can be defined across multiple robots by using SDFormat model composition. Summary of changes: * add CollisionFilterGroupResolver, with unit test * port existing parser modules to use it * modernize a few sdf_parser errors * demonstrate multi-robot filters in sdf_parser_test
Relevant to: RobotLocomotion#14785 This patch extends the parsing of collision filter groups so that collision filters can be defined across multiple robots by using SDFormat model composition. Summary of changes: * add CollisionFilterGroupResolver, with unit test * port existing parser modules to use it * modernize a few sdf_parser errors * demonstrate multi-robot filters in sdf_parser_test
Relevant to: RobotLocomotion#14785 This patch extends the parsing of collision filter groups so that collision filters can be defined across multiple robots by using SDFormat model composition. Summary of changes: * add CollisionFilterGroupResolver, with unit test * port existing parser modules to use it * modernize a few sdf_parser errors * demonstrate multi-robot filters in sdf_parser_test
Relevant to: RobotLocomotion#14785 This patch extends the parsing of collision filter groups so that collision filters can be defined across multiple robots by using SDFormat model composition. Summary of changes: * add CollisionFilterGroupResolver, with unit test * port existing parser modules to use it * modernize a few sdf_parser errors * demonstrate multi-robot filters in sdf_parser_test
Relevant to: RobotLocomotion#14785 This patch extends the parsing of collision filter groups so that collision filters can be defined across multiple robots by using SDFormat model composition. Summary of changes: * add CollisionFilterGroupResolver, with unit test * port existing parser modules to use it * modernize a few sdf_parser errors * demonstrate multi-robot filters in sdf_parser_test
Relevant to: RobotLocomotion#14785 This patch extends the parsing of collision filter groups so that collision filters can be defined across multiple robots by using SDFormat model composition. Summary of changes: * add CollisionFilterGroupResolver, with unit test * port existing parser modules to use it * modernize a few sdf_parser errors * demonstrate multi-robot filters in sdf_parser_test
Now that #17175 is close to landing, it's worth looking at some of the things it didn't solve. Some may want doing now/soon, some never, and some may merit an issue to work on later. I believe these three should all happen. The first two could happen soon. The "subgraph" thing may have to wait for other patches to land.
These three are not strictly necessary, and might even be bad ideas.
|
(1) Seems worth doing. (2) Can you provide an example (maybe a file) where this would be useful? Or is it maybe just easier to implement it correctly than try to explain in our documentation and warnings that it's unsupported? (3) That pull request is doomed. I don't think we need to work on filters for that anytime soon. (4) This might provide an easier onramp to this feature, versus someone needing to learn how SDFormat includes work. The choice here probably depends on how much effort it would take. (5,6) Does the OP's example demonstrate any need for this? If not, punt and ignore. |
(1) Yup. I don't think there are dragons here, just needs more tests and doc. |
Relevant to: RobotLocomotion#14785 This patch extends the parsing of collision filter groups so that collision filters can be defined across multiple robots by using SDFormat model composition. Summary of changes: * add CollisionFilterGroupResolver, with unit test * port existing parser modules to use it * modernize a few sdf_parser errors * demonstrate multi-robot filters in sdf_parser_test
* [parsing] Enable multi-robot collision filter groups Relevant to: #14785 This patch extends the parsing of collision filter groups so that collision filters can be defined across multiple robots by using SDFormat model composition. Summary of changes: * add CollisionFilterGroupResolver, with unit test * port existing parser modules to use it * modernize a few sdf_parser errors * demonstrate multi-robot filters in sdf_parser_test
Relevant to: RobotLocomotion#14785 This patch implements a stronger enforcement of model encapsulation for collision filters. Previously, filters defined outside any model could refer to the world body or parts of the default model by scoped names, or to default model bodies by unqualified names. Now, those references are parse errors, and using a model index from outside the parse as a parameter is a programming error. There might ever be a reason (I couldn't think of one) to define collision filters against the world body; I am fine for that to involve a special privileged name and be future work. I couldn't think of any reason why a model file should be able to refer to bodies in the default model, which (in the current implementation of Parser), could never be populated from model files.
#17241 is the first attempt to address (1). |
re (2), my current inclination is to punt. As far as I can tell, there is always the workaround of adding a layer of nested |
Splitting (4) to its own issue: #17242. |
Relevant to: RobotLocomotion#14785 This patch implements a stronger enforcement of model encapsulation for collision filters. Previously, filters defined outside any model could refer to the world body or parts of the default model by scoped names, or to default model bodies by unqualified names. Now, those references are parse errors, and using a model index from outside the parse as a parameter is a programming error. There might ever be a reason (I couldn't think of one) to define collision filters against the world body; I am fine for that to involve a special privileged name and be future work. I couldn't think of any reason why a model file should be able to refer to bodies in the default model, which (in the current implementation of Parser), could never be populated from model files.
Relevant to: RobotLocomotion#14785 This patch implements a stronger enforcement of model encapsulation for collision filters. Previously, filters defined outside any model could refer to the world body or parts of the default model by scoped names, or to default model bodies by unqualified names. Now, those references are parse errors, and using a model index from outside the parse as a parameter is a programming error. There might ever be a reason (I couldn't think of one) to define collision filters against the world body; I am fine for that to involve a special privileged name and be future work. I couldn't think of any reason why a model file should be able to refer to bodies in the default model, which (in the current implementation of Parser), could never be populated from model files.
Relevant to: RobotLocomotion#14785 This patch implements a stronger enforcement of model encapsulation for collision filters. Previously, filters defined outside any model could refer to the world body or parts of the default model by scoped names, or to default model bodies by unqualified names. Now, those references are parse errors, and using a model index from outside the parse as a parameter is a programming error. There might ever be a reason (I couldn't think of one) to define collision filters against the world body; I am fine for that to involve a special privileged name and be future work. I couldn't think of any reason why a model file should be able to refer to bodies in the default model, which (in the current implementation of Parser), could never be populated from model files.
…on (#17241) * [parsing] Restrict collision filters to models within a parse operation Relevant to: #14785 This patch implements a stronger enforcement of model encapsulation for collision filters. Previously, filters defined outside any model could refer to the world body or parts of the default model by scoped names, or to default model bodies by unqualified names. Now, those references are parse errors, and using a model index from outside the parse as a parameter is a programming error. There might ever be a reason (I couldn't think of one) to define collision filters against the world body; I am fine for that to involve a special privileged name and be future work. I couldn't think of any reason why a model file should be able to refer to bodies in the default model, which (in the current implementation of Parser), could never be populated from model files.
…on (RobotLocomotion#17241) * [parsing] Restrict collision filters to models within a parse operation Relevant to: RobotLocomotion#14785 This patch implements a stronger enforcement of model encapsulation for collision filters. Previously, filters defined outside any model could refer to the world body or parts of the default model by scoped names, or to default model bodies by unqualified names. Now, those references are parse errors, and using a model index from outside the parse as a parameter is a programming error. There might ever be a reason (I couldn't think of one) to define collision filters against the world body; I am fine for that to involve a special privileged name and be future work. I couldn't think of any reason why a model file should be able to refer to bodies in the default model, which (in the current implementation of Parser), could never be populated from model files.
…#17175) * [parsing] Enable multi-robot collision filter groups Relevant to: RobotLocomotion#14785 This patch extends the parsing of collision filter groups so that collision filters can be defined across multiple robots by using SDFormat model composition. Summary of changes: * add CollisionFilterGroupResolver, with unit test * port existing parser modules to use it * modernize a few sdf_parser errors * demonstrate multi-robot filters in sdf_parser_test
…on (RobotLocomotion#17241) * [parsing] Restrict collision filters to models within a parse operation Relevant to: RobotLocomotion#14785 This patch implements a stronger enforcement of model encapsulation for collision filters. Previously, filters defined outside any model could refer to the world body or parts of the default model by scoped names, or to default model bodies by unqualified names. Now, those references are parse errors, and using a model index from outside the parse as a parameter is a programming error. There might ever be a reason (I couldn't think of one) to define collision filters against the world body; I am fine for that to involve a special privileged name and be future work. I couldn't think of any reason why a model file should be able to refer to bodies in the default model, which (in the current implementation of Parser), could never be populated from model files.
…#17175) * [parsing] Enable multi-robot collision filter groups Relevant to: RobotLocomotion#14785 This patch extends the parsing of collision filter groups so that collision filters can be defined across multiple robots by using SDFormat model composition. Summary of changes: * add CollisionFilterGroupResolver, with unit test * port existing parser modules to use it * modernize a few sdf_parser errors * demonstrate multi-robot filters in sdf_parser_test
…on (RobotLocomotion#17241) * [parsing] Restrict collision filters to models within a parse operation Relevant to: RobotLocomotion#14785 This patch implements a stronger enforcement of model encapsulation for collision filters. Previously, filters defined outside any model could refer to the world body or parts of the default model by scoped names, or to default model bodies by unqualified names. Now, those references are parse errors, and using a model index from outside the parse as a parameter is a programming error. There might ever be a reason (I couldn't think of one) to define collision filters against the world body; I am fine for that to involve a special privileged name and be future work. I couldn't think of any reason why a model file should be able to refer to bodies in the default model, which (in the current implementation of Parser), could never be populated from model files.
…on (RobotLocomotion#17241) * [parsing] Restrict collision filters to models within a parse operation Relevant to: RobotLocomotion#14785 This patch implements a stronger enforcement of model encapsulation for collision filters. Previously, filters defined outside any model could refer to the world body or parts of the default model by scoped names, or to default model bodies by unqualified names. Now, those references are parse errors, and using a model index from outside the parse as a parameter is a programming error. There might ever be a reason (I couldn't think of one) to define collision filters against the world body; I am fine for that to involve a special privileged name and be future work. I couldn't think of any reason why a model file should be able to refer to bodies in the default model, which (in the current implementation of Parser), could never be populated from model files.
I while ago I posted on Stack Overflow inquiring about the possibility of defining collision filter groups across model instances. It seems this functionality is currently lacking.
In the multibody URDF parser, loading collision filter groups is restricted to the those present in the same model instance.
What I would like to do is in URDF A specify:
and in URDF B specify:
But currently attempting to do this results in the following error:
Would it be possible to add an optional ignored collision filter group type, for example:
which would not throw such an error?
A specific example use case if helpful:
I'm using a multibody system with a 7DOF Franka Panda arm whose base geometry is overlapping with a 2DOF robot. I want to specify that the base of the Franka and the base of the 2DOF arm are part of the same collision filter group despite being part of different URDF files.
The text was updated successfully, but these errors were encountered: