forked from RobotLocomotion/drake
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[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
- Loading branch information
1 parent
245e69f
commit 9bbf389
Showing
22 changed files
with
887 additions
and
170 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
164 changes: 164 additions & 0 deletions
164
multibody/parsing/detail_collision_filter_group_resolver.cc
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,164 @@ | ||
#include "drake/multibody/parsing/detail_collision_filter_group_resolver.h" | ||
|
||
#include "drake/multibody/parsing/scoped_names.h" | ||
|
||
namespace drake { | ||
namespace multibody { | ||
namespace internal { | ||
|
||
using drake::internal::DiagnosticPolicy; | ||
using geometry::GeometrySet; | ||
using parsing::GetInstanceScopeName; | ||
using parsing::ParseScopedName; | ||
using parsing::PrefixName; | ||
using parsing::ScopedName; | ||
|
||
CollisionFilterGroupResolver::CollisionFilterGroupResolver( | ||
MultibodyPlant<double>* plant) : plant_(plant) { | ||
DRAKE_DEMAND(plant != nullptr); | ||
} | ||
|
||
void CollisionFilterGroupResolver::AddGroup( | ||
const DiagnosticPolicy& diagnostic, | ||
const std::string& group_name, | ||
const std::set<std::string>& body_names, | ||
std::optional<ModelInstanceIndex> model_instance) { | ||
if (model_instance) { | ||
DRAKE_DEMAND(*model_instance < plant_->num_model_instances()); | ||
} | ||
DRAKE_DEMAND(!group_name.empty()); | ||
if (!CheckLegalName(diagnostic, group_name, "group name")) { return; } | ||
ScopedName scoped_group_name = ParseScopedName(group_name); | ||
if (!scoped_group_name.instance_name.empty()) { | ||
diagnostic.Error(fmt::format("group name '{}' cannot be a scoped name", | ||
FullyQualify(group_name, model_instance))); | ||
return; | ||
} | ||
if (body_names.empty()) { | ||
diagnostic.Error(fmt::format("group '{}' has no members", | ||
FullyQualify(group_name, model_instance))); | ||
return; | ||
} | ||
|
||
geometry::GeometrySet geometry_set; | ||
for (const auto& body_name : body_names) { | ||
DRAKE_DEMAND(!body_name.empty()); | ||
if (!CheckLegalName(diagnostic, body_name, "body name")) { continue; } | ||
const std::string qualified = FullyQualify(body_name, model_instance); | ||
ScopedName scoped_name = ParseScopedName(qualified); | ||
|
||
std::optional<ModelInstanceIndex> body_model; | ||
if (!scoped_name.instance_name.empty() && | ||
plant_->HasModelInstanceNamed(scoped_name.instance_name)) { | ||
body_model = plant_->GetModelInstanceByName(scoped_name.instance_name); | ||
} | ||
|
||
const Body<double>* body = FindBody(scoped_name.name, body_model); | ||
if (!body) { | ||
diagnostic.Error(fmt::format("body with name '{}' not found", qualified)); | ||
continue; | ||
} | ||
geometry_set.Add(plant_->GetBodyFrameIdOrThrow(body->index())); | ||
} | ||
groups_.insert({FullyQualify(group_name, model_instance), geometry_set}); | ||
} | ||
|
||
void CollisionFilterGroupResolver::AddPair( | ||
const DiagnosticPolicy& diagnostic, | ||
const std::string& group_name_a, | ||
const std::string& group_name_b, | ||
std::optional<ModelInstanceIndex> model_instance) { | ||
DRAKE_DEMAND(!group_name_a.empty()); | ||
DRAKE_DEMAND(!group_name_b.empty()); | ||
if (model_instance) { | ||
DRAKE_DEMAND(*model_instance < plant_->num_model_instances()); | ||
} | ||
bool a_ok = CheckLegalName(diagnostic, group_name_a, "group name"); | ||
bool b_ok = CheckLegalName(diagnostic, group_name_b, "group name"); | ||
if (!(a_ok && b_ok)) { | ||
return; | ||
} | ||
|
||
// Store group pairs by fully qualified name. The groups don't need to | ||
// actually be defined until Resolve() time. | ||
const std::string name_a = FullyQualify(group_name_a, model_instance); | ||
const std::string name_b = FullyQualify(group_name_b, model_instance); | ||
|
||
// These two group names are allowed to be identical, which means the | ||
// bodies inside this collision filter group should be collision excluded | ||
// among each other. | ||
pairs_.insert({name_a, name_b}); | ||
} | ||
|
||
void CollisionFilterGroupResolver::Resolve(const DiagnosticPolicy& diagnostic) { | ||
for (const auto& [name_a, name_b] : pairs_) { | ||
const GeometrySet* set_a = CheckGroup(diagnostic, name_a); | ||
const GeometrySet* set_b = CheckGroup(diagnostic, name_b); | ||
if (!(set_a && set_b)) { | ||
continue; | ||
} | ||
plant_->ExcludeCollisionGeometriesWithCollisionFilterGroupPair( | ||
{name_a, *set_a}, {name_b, *set_b}); | ||
} | ||
groups_.clear(); | ||
pairs_.clear(); | ||
} | ||
|
||
bool CollisionFilterGroupResolver::CheckLegalName( | ||
const drake::internal::DiagnosticPolicy& diagnostic, | ||
const std::string_view name, | ||
const std::string& description) const { | ||
DRAKE_DEMAND(!name.empty()); | ||
|
||
const std::string_view delim(parsing::internal::kScopedNameDelim); | ||
// The main objective here is to avoid aborting with a dubious message in | ||
// ParseScope(). There are numerous other degenerate name cases, but those | ||
// likely won't abort in ParseScope() and will be caught later on by full | ||
// qualification and lookup. | ||
bool legal = (name.front() != delim.front()) && | ||
(name.back() != delim.back()); | ||
if (!legal) { | ||
diagnostic.Error(fmt::format("{} '{}' can neither begin nor end with '{}'", | ||
description, name, delim)); | ||
} | ||
return legal; | ||
} | ||
|
||
std::string CollisionFilterGroupResolver::FullyQualify( | ||
const std::string& name, | ||
std::optional<ModelInstanceIndex> model_instance) const { | ||
if (!model_instance) { | ||
// Names found in global scope are just themselves. | ||
return name; | ||
} | ||
|
||
// Treat the name as relative and just prefix it with the model instance | ||
// name. Misuses of scoping will fail in lookup. | ||
return PrefixName(GetInstanceScopeName(*plant_, *model_instance), name); | ||
} | ||
|
||
const GeometrySet* CollisionFilterGroupResolver::CheckGroup( | ||
const DiagnosticPolicy& diagnostic, const std::string& group_name) const { | ||
auto iter = groups_.find(group_name); | ||
if (iter == groups_.end()) { | ||
diagnostic.Error( | ||
fmt::format("collision filter group with name '{}' not found", | ||
group_name)); | ||
return nullptr; | ||
} | ||
return &iter->second; | ||
} | ||
|
||
const Body<double>* CollisionFilterGroupResolver::FindBody( | ||
const std::string& name, | ||
std::optional<ModelInstanceIndex> model_instance) { | ||
ModelInstanceIndex model = model_instance.value_or(default_model_instance()); | ||
if (plant_->HasBodyNamed(name, model)) { | ||
return &plant_->GetBodyByName(name, model); | ||
} | ||
return {}; | ||
} | ||
|
||
} // namespace internal | ||
} // namespace multibody | ||
} // namespace drake |
112 changes: 112 additions & 0 deletions
112
multibody/parsing/detail_collision_filter_group_resolver.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,112 @@ | ||
#pragma once | ||
|
||
#include <map> | ||
#include <optional> | ||
#include <set> | ||
#include <string> | ||
|
||
#include "drake/common/diagnostic_policy.h" | ||
#include "drake/common/drake_copyable.h" | ||
#include "drake/common/sorted_pair.h" | ||
#include "drake/geometry/geometry_set.h" | ||
#include "drake/multibody/plant/multibody_plant.h" | ||
|
||
namespace drake { | ||
namespace multibody { | ||
namespace internal { | ||
|
||
// Collects and resolves collision filter group information, potentially across | ||
// multiple models. Uses the same scoped-name semantics as SDFormat and model | ||
// directives to allow reference and resolution of names across models. | ||
// | ||
// All methods use the supplied MultibodyPlant as the source of truth for | ||
// existence and naming of models and bodies. The Resolve() step writes the | ||
// finished collision filtering rules to the plant. | ||
// | ||
// All methods may emit warnings or errors to a passed diagnostic policy | ||
// channel. | ||
// | ||
// @note model_instance: All methods use a model instance parameter to denote | ||
// the current scope where information is found during parsing. Its name is | ||
// used to expand the relative names found to absolute names for further | ||
// internal processing. If a model instance value is not provided, names are | ||
// evaluated with respect to the "root" of the plant. | ||
// | ||
// TODO(rpoyner-tri): clarify the roles of the world instance (0) and the | ||
// default model instance (1). | ||
// | ||
class CollisionFilterGroupResolver { | ||
public: | ||
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(CollisionFilterGroupResolver) | ||
|
||
// The @p plant parameter is aliased, and must outlive the resolver. | ||
// @pre plant is not nullptr. | ||
explicit CollisionFilterGroupResolver(MultibodyPlant<double>* plant); | ||
|
||
// Adds a collision filter group. Locates bodies by name immediately, and | ||
// emits errors for illegal names, empty groups, or missing bodies. | ||
// | ||
// @param diagnostic The error-reporting channel. | ||
// @param group_name The name of the group being defined. | ||
// @param body_names The names of the bodies that are group members. | ||
// @param model_instance The current model, for scoping purposes. | ||
// @see model_instance note above. | ||
// | ||
// @pre group_name is not empty. | ||
// @pre no member strings of body_names are empty. | ||
// @pre model_instance has a valid index or else no value. | ||
void AddGroup( | ||
const drake::internal::DiagnosticPolicy& diagnostic, | ||
const std::string& group_name, | ||
const std::set<std::string>& body_names, | ||
std::optional<ModelInstanceIndex> model_instance); | ||
|
||
// Adds a group pair. Emits diagnostics for illegal names. Two distinct | ||
// names will resolve to an exclude-between-groups rule; identical names will | ||
// resolve to an exclude-within-group rule. Group names may refer to a group | ||
// not defined until later; group existence will be checked in the Resolve() | ||
// step. | ||
// | ||
// @param diagnostic The error-reporting channel. | ||
// @param group_name_a The name of a defined group. | ||
// @param group_name_b The name of a defined group. | ||
// @param model_instance The current model, for scoping purposes. | ||
// @see model_instance note above. | ||
// | ||
// @pre neither group_name_a nor group_name_b is empty. | ||
// @pre model_instance has a valid index or else no value. | ||
void AddPair( | ||
const drake::internal::DiagnosticPolicy& diagnostic, | ||
const std::string& group_name_a, | ||
const std::string& group_name_b, | ||
std::optional<ModelInstanceIndex> model_instance); | ||
|
||
// Resolves group pairs to rules, and clears all internal bookkeeping | ||
// state. Emits diagnostics for undefined groups. | ||
// | ||
// @note Clearing state allows the object to be reused for subsequent | ||
// parsing. It is up to clients of this class to Resolve() at the end of a | ||
// defined parsing operation, where resolution across models makes sense. | ||
void Resolve(const drake::internal::DiagnosticPolicy& diagnostic); | ||
|
||
private: | ||
bool CheckLegalName(const drake::internal::DiagnosticPolicy& diagnostic, | ||
const std::string_view name, | ||
const std::string& description) const; | ||
std::string FullyQualify( | ||
const std::string& name, | ||
std::optional<ModelInstanceIndex> model_instance) const; | ||
const geometry::GeometrySet* CheckGroup( | ||
const drake::internal::DiagnosticPolicy& diagnostic, | ||
const std::string& group_name) const; | ||
const Body<double>* FindBody( | ||
const std::string& name, | ||
std::optional<ModelInstanceIndex> model_instance); | ||
MultibodyPlant<double>* const plant_; | ||
std::map<std::string, geometry::GeometrySet> groups_; | ||
std::set<SortedPair<std::string>> pairs_; | ||
}; | ||
|
||
} // namespace internal | ||
} // namespace multibody | ||
} // namespace drake |
Oops, something went wrong.