Drake
element.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <cstdint>
4 #include <memory>
5 #include <utility>
6 #include <vector>
7 
8 #include <Eigen/Dense>
9 
13 
14 // Forward declaration.
15 // This forward declaration is made in order to be able to add a reference
16 // to the parent body without the collision element ever using the RigidBody or
17 // any of its methods.
18 // This is particularly useful when the physics engine (at the RigidBody or
19 // RigidBodyTree scope) needs to retrieve the parent body (for instance to
20 // query its world transform).
21 template <typename T>
22 class RigidBody;
23 
24 namespace drake {
25 namespace multibody {
26 namespace collision {
27 typedef uintptr_t ElementId;
28 
29 /**
30  * The underyling primitive class used for collision analysis. Collisions
31  * between `RigidBody` instances are determined by performing geometry analysis
32  * on the `RigidBody` instances' corresponding collision `Element`s. As such,
33  * The `Element` combines three pieces of data:
34  * - the geometric definition of the element's colliding shape (a `Geometry`
35  * instance referred to as its "geometry").
36  * - the transform `T_element_to_local` (T_LE) giving the pose of the element's
37  * frame E in the body's local frame L, with the sense that p_L = T_LE * p_E
38  * for points measured and expressed in frames L and E, respectively.
39  * - the `RigidBody` instance to which this element belongs (its "body"). A
40  * `RigidBody` *can* possess multiple collision `Element`s, but an `Element`
41  * can only belong to a single `RigidBody`.
42  */
43 class Element : public DrakeShapes::Element {
44  public:
45  /**
46  * Default constructor.
47  * The element's pose will be the identity with no geometry or rigid body.
48  */
49  Element();
50 
51  /**
52  * Geometry constructor. Defines geometry and pose but no body.
53  * @param[in] geometry The colliding geometry.
54  * @param[in] T_element_to_local The pose (defaults to identity).
55  */
57  const Eigen::Isometry3d& T_element_to_local =
58  Eigen::Isometry3d::Identity());
59 
60  /**
61  * Body constructor. Defines body and pose but no geometry.
62  * @param[in] T_element_to_local The pose (defaults to identity).
63  * @param[in] body The associated rigid body.
64  */
65  Element(const Eigen::Isometry3d& T_element_to_local,
66  const ::RigidBody<double>* body);
67 
68  /**
69  * Full constructor.
70  * @param[in] geometry The colliding geometry.
71  * @param[in] T_element_to_local The pose.
72  * @param[in] body The associated rigid body.
73  */
74  Element(const DrakeShapes::Geometry& geometry,
75  const Eigen::Isometry3d& T_element_to_local,
76  const ::RigidBody<double>* body);
77 
78  ~Element() override {}
79 
80  Element* clone() const override;
81 
82  ElementId getId() const;
83 
84  /**
85  * Flags this collision element to be anchored, i.e. it does not move
86  * relative to the world frame.
87  * @see Element::is_anchored().
88  */
89  void set_anchored() { is_anchored_ = true; }
90 
91  /**
92  * Returns `true` if the shape does not move relative to the world frame.
93  * For instance, terrain geometry would commonly be anchored. This property
94  * allows the collision engine to perform certain optimizations not
95  * generally possible for moving objects.
96  */
97  bool is_anchored() const { return is_anchored_; }
98 
99  /**
100  * Returns true if this element should be checked for collisions
101  * with the @p other object. CanCollideWith() is commutative;
102  * A can collide with B implies B can collide with A.
103  */
104  bool CanCollideWith(const Element *other) const;
105 
106  /**
107  * Adds this element to the clique specified by the given clique id.
108  *
109  * The clique may be a previously existing clique or a new clique. If the
110  * element already belongs to the clique, there will be no change.
111  *
112  * @param[in] clique_id The clique to which this element will belong to.
113  */
114  void AddToCollisionClique(int clique_id);
115 
116  /**
117  * Adds the cliques of the given `element` to `this` element's cliques.
118  *
119  * @param[in] element The element whose cliques will be copied.
120  */
121  void AddCliquesFromElement(const Element& element);
122 
123  /**
124  * Reports the number of cliques to which this element belongs.
125  * @returns The number of cliques.
126  */
127  int get_num_cliques() const;
128 
129  /**
130  * Provides access to the set of cliques to which this element belongs.
131  * @returns A reference to the clique set (as a monotonically increasing
132  * ordered list).
133  */
134  const std::vector<int>& collision_cliques() const;
135 
136  /** Returns a pointer to the `RigidBody` to which this `Element`
137  * is attached.
138  */
139  const ::RigidBody<double>* get_body() const;
140 
141  /** Sets the `RigidBody` this collision element is attached to. */
142  void set_body(const ::RigidBody<double>* body);
143 
144  /** Sets the collision filter state of the element: the groups to which this
145  * element belongs and the groups that it should ignore.
146  */
147  void set_collision_filter(const bitmask& group, const bitmask& ignores);
148 
149  /** Updates the collision filter state of the element; *adds* the groups to
150  which this element belongs and the groups it ignores. */
151  void merge_collision_filter(const bitmask& group, const bitmask& ignores);
152 
154  return collision_filter_group_;
155  }
156 
158  return collision_filter_ignores_;
159  }
160 
161  /**
162  * A toString method for this class.
163  */
164  friend std::ostream& operator<<(std::ostream&, const Element&);
165 
167  return compliant_material_;
168  }
169 
171  const systems::CompliantMaterial& material) {
172  compliant_material_ = material;
173  }
174 
175  private:
176  // Provide a copy constructor for use by our clone() implementation.
177  // Delete all of the other operations.
178  Element(const Element&) = default;
179  void operator=(const Element&) = delete;
180  Element(Element&&) = delete;
181  void operator=(Element&&) = delete;
182 
183  bool is_anchored_{false};
184  const ::RigidBody<double>* body_{};
185 
186  // Collision cliques are defined as a set of collision elements that do not
187  // collide.
188  // Collision cliques in Drake are represented simply by an integer.
189  // A collision element can belong to more than one clique.
190  //
191  // Conceptually it would seem like std::set is the right fit for
192  // Element::collision_cliques_. However, std::set is really good for
193  // dynamically adding elements to a set that needs to change.
194  // Once you are done adding elements to your set, access time is poor when
195  // compared to a simple std::vector (nothing faster than scanning a vector of
196  // adjacent entries in memory).
197  // Here adding elements to the cliques vector only happens during problem
198  // setup by the user or from a URDF/SDF file. What we really want is that once
199  // this vector is setup, we can query it very quickly during simulation.
200  // This is done in Element::CanCollideWith() which to be Order(N)
201  // requires the entries in CollisionElement::collision_cliques_ to be sorted.
202  // By arbitrary convention, the ordering is monotonically increasing.
203  std::vector<int> collision_cliques_;
204 
205  // A bitmask that determines the collision groups that this element is part
206  // of. If the i-th bit is set this rigid body belongs to the i-th collision
207  // group. An element can belong to multiple collision groups.
208  bitmask collision_filter_group_{kDefaultGroup};
209 
210  // A bitmask that determines which collision groups this element can *not*
211  // collide with. Thus, if the i-th bit is set this element is not checked
212  // for collisions with elements in the i-th group.
213  bitmask collision_filter_ignores_{kNoneMask};
214 
215  // The compliant contact model parameters for this element.
216  systems::CompliantMaterial compliant_material_;
217 
218  public:
219  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
220 };
221 
222 } // namespace collision
223 } // namespace multibody
224 } // namespace drake
uintptr_t ElementId
Definition: element.h:27
const std::vector< int > & collision_cliques() const
Provides access to the set of cliques to which this element belongs.
Definition: element.cc:92
void set_collision_filter(const bitmask &group, const bitmask &ignores)
Sets the collision filter state of the element: the groups to which this element belongs and the grou...
Definition: element.cc:96
bool is_anchored() const
Returns true if the shape does not move relative to the world frame.
Definition: element.h:97
std::bitset< kMaxNumCollisionFilterGroups > bitmask
Definition: collision_filter.h:21
Definition: automotive_demo.cc:90
constexpr bitmask kDefaultGroup(1)
void AddToCollisionClique(int clique_id)
Adds this element to the clique specified by the given clique id.
Definition: element.cc:67
void merge_collision_filter(const bitmask &group, const bitmask &ignores)
Updates the collision filter state of the element; adds the groups to which this element belongs and ...
Definition: element.cc:102
Definition: geometry.h:32
Element()
Default constructor.
Definition: element.cc:15
const bitmask & get_collision_filter_group() const
Definition: element.h:153
std::unique_ptr< Geometry > geometry
Definition: element.h:63
ElementId getId() const
Definition: element.cc:37
void set_compliant_material(const systems::CompliantMaterial &material)
Definition: element.h:170
Definition: collision_filter.h:13
void set_anchored()
Flags this collision element to be anchored, i.e.
Definition: element.h:89
const bitmask & get_collision_filter_ignores() const
Definition: element.h:157
bool CanCollideWith(const Element *other) const
Returns true if this element should be checked for collisions with the other object.
Definition: element.cc:48
~Element() override
Definition: element.h:78
The set of per-object compliant material parameters with one material applied to each collision objec...
Definition: compliant_material.h:54
constexpr bitmask kNoneMask(0)
void set_body(const ::RigidBody< double > *body)
Sets the RigidBody this collision element is attached to.
Definition: element.cc:46
void AddCliquesFromElement(const Element &element)
Adds the cliques of the given element to this element&#39;s cliques.
Definition: element.cc:82
Definition: element.h:10
friend std::ostream & operator<<(std::ostream &, const Element &)
A toString method for this class.
Definition: element.cc:108
int get_num_cliques() const
Reports the number of cliques to which this element belongs.
Definition: element.cc:88
const ::RigidBody< double > * get_body() const
Returns a pointer to the RigidBody to which this Element is attached.
Definition: element.cc:44
Eigen::Isometry3d T_element_to_local
Definition: element.h:62
Element * clone() const override
Definition: element.cc:35
The underyling primitive class used for collision analysis.
Definition: element.h:43
const systems::CompliantMaterial & compliant_material() const
Definition: element.h:166