Drake
collision_filter.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <bitset>
4 #include <string>
5 #include <unordered_map>
6 #include <utility>
7 #include <vector>
8 
10 
11 // forward declaration
12 template <typename U>
13 class RigidBody;
14 
15 namespace drake {
16 namespace multibody {
17 namespace collision {
18 /** The maximum width of the collision filter group bitmasks. */
19 constexpr int kMaxNumCollisionFilterGroups = 128;
20 
21 typedef std::bitset<kMaxNumCollisionFilterGroups> bitmask;
22 
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);
31 
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.
36 
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.
42 
43  Given the following definitions:
44 
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.
52 
53  Then, the pair @f$(A, B)@f$ will be filtered if:
54 
55  @f$I(A) \cap G(B) \ne \emptyset \lor I(B) \cap G(A) \ne \emptyset@f$.
56 
57  In other words, if either body belongs to a group which is ignored by *any*
58  group the other body belongs to.
59 
60  @tparam T A valid Eigen scalar type.
61  */
62 template <typename T>
64  public:
66 
67  /**
68  Default constructor required by use in std::unordered_map.
69  */
71 
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);
77 
78  int get_mask_id() const { return mask_id_; }
79 
80  void add_body(const RigidBody<T>& body) { bodies_.push_back(&body); }
81 
82  std::vector<const RigidBody<T>*>& get_bodies() { return bodies_; }
83 
84  const std::vector<std::string>& get_ignore_groups() const {
85  return ignore_groups_;
86  }
87 
88  void add_ignore_group(const std::string& group_name) {
89  ignore_groups_.push_back(group_name);
90  }
91 
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 };
98 
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.
106 
107  The intent of the manager is to serve as an accumulator during the *parsing*
108  process. It serves as an intermediate representation of the collision filter
109  group semantics. A unique instance should be owned by each RigidBodyTree
110  instance.
111 
112  The manager is used in parsing *sessions*. By design, one session maps to
113  parsing a single file. The session ends with a call to Clear() (called in
114  RigidBodyTree::compile()). Multiple sessions can be run on a single manager
115  (this naturally arises from parsing multiple files). Collision filter groups
116  defined during a single session must all have unique group *names*. Names
117  repeated in different sessions are treated as being *different* collision
118  filter groups. The names of the groups are only maintained during parsing.
119  During compilation, the names are replaced with integer identifiers.
120 
121  The group manager can handle a finite number of groups
122  (kMaxNumCollisionFilterGroups). Each session contributes towards reaching
123  that total. If the maximum number of groups has been defined, subsequent
124  efforts to define a new collision filter group will cause exceptions to be
125  thrown.
126 
127  There are several implications of this design:
128 
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.
138 
139  @tparam T A valid Eigen scalar type.
140  */
141 template <typename T>
143  public:
145 
146  /** Default constructor. */
148 
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();
154 
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. In
159  the event of failure, an exception is thrown.
160  @param name The unique name of the new group.
161  */
162  void DefineCollisionFilterGroup(const std::string& name);
163 
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);
182 
183  /**
184  Adds a collision group to the set of groups ignored by the specified
185  collision filter group. Will fail if the specified 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.) An exception is thrown
188  upon failure.
189  @param group_name The name of the group to modify.
190  @param target_group_name The name of the group to ignore.
191  */
192  void AddCollisionFilterIgnoreTarget(const std::string& group_name,
193  const std::string& target_group_name);
194 
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);
201 
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);
208 
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);
215 
216  // TODO(SeanCurtis-TRI): Kill this method when matlab dependencies are
217  // removed. There is a corresponding method on the RigidBodyTree.
218  /**
219  Directly set the masks for a body. The values will remain in the current
220  session (i.e., until Clear() is called). This is a convenience function for
221  Matlab integration. The Matlab parser handles the mapping of collision
222  filter group names to ids and passes the mapped ids directly the manager for
223  when the tree gets compiled. The input bitmasks are in no way validated.
224  Bitmasks initialized in this manner are *not* guaranteed to include
225  membership in the DEFAULT_GROUP.
226  */
227  void SetBodyCollisionFilters(const RigidBody<T>& body, const bitmask& group,
228  const bitmask& ignores);
229 
230  /**
231  Clears the cached collision filter group specification data from the current
232  session. It does *not* reset the counter for available collision filter
233  groups. This is what makes it possible for a file to be read multiple times
234  in sequence, but to have each parsing produce a unique set of collision
235  filter groups. Or if two files are parsed, and both use a common name for a
236  collision filter group. They are treated as unique collision filter groups
237  and all of those groups count against the *total* number of groups supported
238  by a single instance of the manager.
239  */
240  void Clear();
241 
242  /**
243  Reported value for group names that do not map to known collision filter
244  groups.
245  */
246  static const int kInvalidGroupId;
247 
248  private:
249  // Attempts to provision a group id for the next group. Throws an exception
250  // if this manager is out of ids.
251  int acquire_next_group_id();
252 
253  // The next available collision filter group identifier. Assumes that 0
254  // is the default group (which is implicitly consumed.)
255  int next_id_{1};
256 
257  // Map between group names and its collision filter group specification.
258  std::unordered_map<std::string,
260  collision_filter_groups_{};
261 
262  // Mappings between a RigidBody and the bitmasks that define its group
263  // membership and the groups it ignores. This is populated during
264  // CompileGroups. The pair is: (group mask, ignore mask).
265  std::unordered_map<const RigidBody<T>*, std::pair<bitmask, bitmask>>
266  body_groups_;
267 };
268 } // namespace collision
269 } // namespace multibody
270 } // namespace drake
std::bitset< kMaxNumCollisionFilterGroups > bitmask
Definition: collision_filter.h:21
Definition: automotive_demo.cc:88
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
CollisionFilterGroup()
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:246
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
#define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname)
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
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
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
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
Provides careful macros to selectively enable or disable the special member functions for copy-constr...