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
fe4431e
commit 5b04ba2
Showing
22 changed files
with
878 additions
and
175 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
170 changes: 170 additions & 0 deletions
170
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,170 @@ | ||
#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); | ||
} | ||
|
||
CollisionFilterGroupResolver::~CollisionFilterGroupResolver() {} | ||
|
||
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; } | ||
if (!ParseScopedName(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()) { | ||
if (plant_->HasModelInstanceNamed(scoped_name.instance_name)) { | ||
body_model = plant_->GetModelInstanceByName(scoped_name.instance_name); | ||
} else { | ||
diagnostic.Error(fmt::format("body with name '{}' not found", | ||
qualified)); | ||
continue; | ||
} | ||
} | ||
|
||
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, | ||
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 |
122 changes: 122 additions & 0 deletions
122
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,122 @@ | ||
#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); | ||
|
||
~CollisionFilterGroupResolver(); | ||
|
||
// 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); | ||
|
||
// @returns a reference to the map of defined groups. | ||
const std::map<std::string, geometry::GeometrySet>& groups() const { | ||
return groups_; | ||
} | ||
|
||
// @returns a reference to the set of pairs of groups. | ||
const std::set<SortedPair<std::string>>& pairs() const { return pairs_; } | ||
|
||
private: | ||
bool CheckLegalName(const drake::internal::DiagnosticPolicy& diagnostic, | ||
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.