Go to the documentation of this file.
1 #pragma once
3 #include <bitset>
4 #include <string>
5 #include <unordered_map>
6 #include <utility>
7 #include <vector>
9 #include "drake/common/drake_copyable.h"
11 // forward declaration
12 template <typename U>
13 class RigidBody;
15 namespace drake {
16 namespace multibody {
17 namespace collision {
18 /** The maximum width of the collision filter group bitmasks. */
19 constexpr int kMaxNumCollisionFilterGroups = 128;
21 typedef std::bitset<kMaxNumCollisionFilterGroups> bitmask;
23 // Constants
24 // The empty bit mask. Used in the membership group mask represents no
25 // membership. As the ignore mask, the body ignores nothing.
26 constexpr bitmask kNoneMask(0);
27 // The membership bit mask indicating the CFG to which *all* bodies belong. A
28 // body can be made invisible (from a collision perspective) by having setting
29 // its ignore mask to kDefaultGroup.
30 constexpr bitmask kDefaultGroup(1);
32 /**
33  The specification of a collision filter group: its name, bodies that belong
34  to it, and the names of collision filter groups that it ignores. This
35  class is used for initialization and not run-time calculations.
37  A collision filter group is a mechanism for cheaply culling pairs of collision
38  elements from consideration during collision detection. One collision filter
39  group associates a set of bodies with a set of *ignored* collision filter
40  groups. At runtime, when a pair of bodies @f$(A, B)@f$ are determined to be a
41  collision candidate, their collision filter group membership is examined.
43  Given the following definitions:
45  - @f$G(A) ≜ \{g^A_0, g^A_1, ..., g^A_n\}@f$ is the set of all groups
46  to which @f$A@f$ belongs,
47  - @f$I(f) ≜ \{g^f_0, g^f_1, ..., g^f_m\}@f$ is the set set of all
48  groups that group @f$f@f$ ignores,
49  - @f$I(A) ≜ \{I(g^A_0) \cap I(g^A_1) \cap ... \cap I(g^A_n)\}@f$
50  such that @f$g^A_i \in G(A)@f$ is the set of all groups that @f$A@f$
51  ignores.
53  Then, the pair @f$(A, B)@f$ will be filtered if:
55  @f$I(A) \cap G(B) \ne \emptyset \lor I(B) \cap G(A) \ne \emptyset@f$.
57  In other words, if either body belongs to a group which is ignored by *any*
58  group the other body belongs to.
60  @tparam T A valid Eigen scalar type.
61  */
62 template <typename T>
64  public:
67  /**
68  Default constructor required by use in std::unordered_map.
69  */
72  /**
73  @param name The name for the collision filter group.
74  @param id The bit id for this collision filter group.
75  */
76  CollisionFilterGroup(const std::string& name, int id);
78  int get_mask_id() const { return mask_id_; }
80  void add_body(const RigidBody<T>& body) { bodies_.push_back(&body); }
82  std::vector<const RigidBody<T>*>& get_bodies() { return bodies_; }
84  const std::vector<std::string>& get_ignore_groups() const {
85  return ignore_groups_;
86  }
88  void add_ignore_group(const std::string& group_name) {
89  ignore_groups_.push_back(group_name);
90  }
92  private:
93  std::string name_{};
94  int mask_id_{};
95  std::vector<const RigidBody<T>*> bodies_{};
96  std::vector<std::string> ignore_groups_{};
97 };
99 // TODO(SeanCurtis-TRI): Per discussion in issue #4729, this will eventually
100 // be expanded to include programmatic manipulation of groups during
101 // construction, and, eventually, dynamic manipulation of groups during
102 // simulation.
103 /**
104  This class provides management utilities for the definition of collision filter
105  groups for RigidBodyTree instances.
107  The intent of the manager is to serve as an accumulator during the
108  *construction* process. It serves as an intermediate representation of the
109  collision filter group semantics. A unique instance should be owned by each
110  RigidBodyTree instance.
112  The manager is used in construction *sessions*. The first session begins upon
113  construction and ends with a call to Clear(). Each call to Clear() implicitly
114  marks the beginning of the next session. Collision filter groups
115  defined during a single session must all have unique group *names*. Names
116  repeated in different sessions are treated as being *different* collision
117  filter groups. The names of the groups are only maintained during the session.
118  During RigidBodyTree compilation, the names are replaced with integers
119  in the filter mechanism identifiers.
121  The group manager can only handle a finite number of groups
122  (kMaxNumCollisionFilterGroups). Each session contributes towards reaching
123  that total. If the maximum number of groups has been reached, subsequent
124  efforts to define a new collision filter group will cause exceptions to be
125  thrown.
127  There are several implications of this design:
129  - Collision filter groups have a scope limited to a single URDF file. That
130  means a collision filter group *cannot* be configured between bodies
131  defined in different URDF files.
132  - Even if the same file is parsed multiple times, leading to collision
133  filter groups with identical names in each session, the groups will be
134  considered different. Bodies from one parsing of the file will not be
135  in groups visible to another parsing.
136  - A user cannot change the collision filter groups a body belongs to or
137  which groups it ignores outside of the URDF specification.
139  @tparam T A valid Eigen scalar type.
140  */
141 template <typename T>
143  public:
146  /** Default constructor. */
149  /**
150  Based on the current specification, builds the appropriate collision filter
151  bitmasks and assigns them to the previously provided rigid bodies.
152  */
153  void CompileGroups();
155  /**
156  Attempts to define a new collision filter group. The given name *must*
157  be unique in this session. Duplicate names or attempting to add more
158  collision filter groups than the system can handle will lead to failure.
159  @param name The unique name of the new group.
160  @throws std::logic_error in response to failure conditions.
161  */
162  void DefineCollisionFilterGroup(const std::string& name);
164  // Note: unlike the other public methods of this class, this method does *not*
165  // throw an exception upon failure. The reason is two-fold:
166  // 1) This method has a single fail condition (which can be succinctly
167  // communicated with a boolean.)
168  // 2) The caller has more context to why this was called and is better able
169  // to provide a meaningful error message. More particularly, this code
170  // cannot create an error message including the body name without creating
171  // a circular dependency between drakeRBM and drakeCollision.
172  /**
173  Adds a RigidBody to a collision filter group. The process will fail if the
174  group cannot be found.
175  @param group_name The name of the collision filter group to add the body
176  to.
177  @param body The body to add.
178  @returns False if the group could not be found.
179  */
180  bool AddCollisionFilterGroupMember(const std::string& group_name,
181  const RigidBody<T>& body);
183  /**
184  Adds a collision group to the set of groups ignored by the specified
185  collision filter group. Will fail if the specified group name
186  does not refer to an existing collision filter group. (Although, the
187  target group name need not exist at this time.)
188  @param group_name The name of the group to modify.
189  @param target_group_name The name of the group to ignore.
190  @throws std::logic_error in response to failure conditions.
191  */
192  void AddCollisionFilterIgnoreTarget(const std::string& group_name,
193  const std::string& target_group_name);
195  /**
196  Reports the collision filter group assigned to the given group name.
197  @param group_name The group name to query.
198  @returns the assigned group id (kInvalidGroupId if an valid group name).
199  */
200  int GetGroupId(const std::string& group_name);
202  /**
203  Returns the group membership bitmask for the given @p body. If there is
204  no information for this body, the zero bitmask will be returned. This
205  should only be called *after* CompileGroups.
206  */
207  const bitmask& get_group_mask(const RigidBody<T>& body);
209  /**
210  Returns the ignored group bitmask for the given @p body. If there is
211  no information for this body, the zero bitmask will be returned. This
212  should only be called *after* CompileGroups.
213  */
214  const bitmask& get_ignore_mask(const RigidBody<T>& body);
216  /**
217  Clears the cached collision filter group specification data from the current
218  session. It does *not* reset the counter for available collision filter
219  groups. This is what makes it possible for a file to be read multiple times
220  in sequence, but to have each parsing produce a unique set of collision
221  filter groups. Or if two files are parsed, and both use a common name for a
222  collision filter group. They are treated as unique collision filter groups
223  and all of those groups count against the *total* number of groups supported
224  by a single instance of the manager.
225  */
226  void Clear();
228  const std::unordered_map<const RigidBody<T>*, std::pair<bitmask, bitmask>>&
229  body_groups() const { return body_groups_; }
231  /**
232  Reported value for group names that do not map to known collision filter
233  groups.
234  */
235  static const int kInvalidGroupId;
237  private:
238  // Attempts to provision a group id for the next group. Throws an exception
239  // if this manager is out of ids.
240  int acquire_next_group_id();
242  // The next available collision filter group identifier. Assumes that 0
243  // is the default group (which is implicitly consumed.)
244  int next_id_{1};
246  // Map between group names and its collision filter group specification.
247  std::unordered_map<std::string,
249  collision_filter_groups_{};
251  // Mappings between a RigidBody and the bitmasks that define its group
252  // membership and the groups it ignores. This is populated during
253  // CompileGroups. The pair is: (group mask, ignore mask).
254  std::unordered_map<const RigidBody<T>*, std::pair<bitmask, bitmask>>
255  body_groups_;
256 };
257 } // namespace collision
258 } // namespace multibody
259 } // namespace drake
std::bitset< kMaxNumCollisionFilterGroups > bitmask
Definition: collision_filter.h:21
Definition: automotive_demo.cc:90
constexpr bitmask kDefaultGroup(1)
constexpr int kMaxNumCollisionFilterGroups
The maximum width of the collision filter group bitmasks.
Definition: collision_filter.h:19
The specification of a collision filter group: its name, bodies that belong to it, and the names of collision filter groups that it ignores.
Definition: collision_filter.h:63
Default constructor required by use in std::unordered_map.
Definition: collision_filter.cc:17
static const int kInvalidGroupId
Reported value for group names that do not map to known collision filter groups.
Definition: collision_filter.h:235
Definition: collision_filter.h:13
const std::vector< std::string > & get_ignore_groups() const
Definition: collision_filter.h:84
void add_ignore_group(const std::string &group_name)
Definition: collision_filter.h:88
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN defaults the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:57
void add_body(const RigidBody< T > &body)
Definition: collision_filter.h:80
This class provides management utilities for the definition of collision filter groups for RigidBodyT...
Definition: collision_filter.h:142
const std::unordered_map< const RigidBody< T > *, std::pair< bitmask, bitmask > > & body_groups() const
Definition: collision_filter.h:229
constexpr bitmask kNoneMask(0)
int get_mask_id() const
Definition: collision_filter.h:78
The term rigid body implies that the deformations of the body under consideration are so small that t...
Definition: fixed_offset_frame.h:16
std::vector< const RigidBody< T > * > & get_bodies()
Definition: collision_filter.h:82
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33