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 
10 #include "drake/multibody/collision/collision_filter.h"
11 #include "drake/multibody/rigid_body_plant/compliant_material.h"
12 #include "drake/multibody/shapes/drake_shapes.h"
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  * Reports the number of cliques to which this element belongs.
118  * @returns The number of cliques.
119  */
120  int get_num_cliques() const;
121 
122  /**
123  * Provides access to the set of cliques to which this element belongs.
124  * @returns A reference to the clique set (as a montonically increasing
125  * ordered list).
126  */
127  const std::vector<int>& collision_cliques() const;
128 
129  /** Returns a pointer to the `RigidBody` to which this `Element`
130  * is attached.
131  */
132  const RigidBody<double>* get_body() const;
133 
134  /** Sets the `RigidBody` this collision element is attached to. */
135  void set_body(const RigidBody<double> *body);
136 
137  /** Sets the collision filter state of the element: the groups to which this
138  * element belongs and the groups that it should ignore.
139  */
140  void set_collision_filter(const bitmask &group,
141  const bitmask &ignores);
142 
144  return collision_filter_group_;
145  }
146 
148  return collision_filter_ignores_;
149  }
150 
151  /**
152  * A toString method for this class.
153  */
154  friend std::ostream& operator<<(std::ostream&, const Element&);
155 
157  return compliant_material_;
158  }
159 
161  const systems::CompliantMaterial& material) {
162  compliant_material_ = material;
163  }
164 
165  private:
166  // Provide a copy constructor for use by our clone() implementation.
167  // Delete all of the other operations.
168  Element(const Element&) = default;
169  void operator=(const Element&) = delete;
170  Element(Element&&) = delete;
171  void operator=(Element&&) = delete;
172 
173  bool is_anchored_{false};
174  const RigidBody<double>* body_{};
175 
176  // Collision cliques are defined as a set of collision elements that do not
177  // collide.
178  // Collision cliques in Drake are represented simply by an integer.
179  // A collision element can belong to more than one clique.
180  //
181  // Conceptually it would seem like std::set is the right fit for
182  // Element::collision_cliques_. However, std::set is really good for
183  // dynamically adding elements to a set that needs to change.
184  // Once you are done adding elements to your set, access time is poor when
185  // compared to a simple std::vector (nothing faster than scanning a vector of
186  // adjacent entries in memory).
187  // Here adding elements to the cliques vector only happens during problem
188  // setup by the user or from a URDF/SDF file. What we really want is that once
189  // this vector is setup, we can query it very quickly during simulation.
190  // This is done in Element::CanCollideWith() which to be Order(N)
191  // requires the entries in CollisionElement::collision_cliques_ to be sorted.
192  // By arbitrary convention, the ordering is monotonically increasing.
193  std::vector<int> collision_cliques_;
194 
195  // A bitmask that determines the collision groups that this element is part
196  // of. If the i-th bit is set this rigid body belongs to the i-th collision
197  // group. An element can belong to multiple collision groups.
198  drake::multibody::collision::bitmask collision_filter_group_{kDefaultGroup};
199 
200  // A bitmask that determines which collision groups this element can *not*
201  // collide with. Thus, if the i-th bit is set this element is not checked
202  // for collisions with elements in the i-th group.
203  drake::multibody::collision::bitmask collision_filter_ignores_{kNoneMask};
204 
205  // The compliant contact model parameters for this element.
206  systems::CompliantMaterial compliant_material_;
207 
208  public:
209  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
210 };
211 
212 } // namespace collision
213 } // namespace multibody
214 } // 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:82
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:86
void set_body(const RigidBody< double > *body)
Sets the RigidBody this collision element is attached to.
Definition: element.cc:46
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:89
constexpr bitmask kDefaultGroup(1)
void AddToCollisionClique(int clique_id)
Adds this element to the clique specified by the given clique id.
Definition: element.cc:65
Definition: geometry.h:32
Element()
Default constructor.
Definition: element.cc:15
const bitmask & get_collision_filter_group() const
Definition: element.h:143
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:160
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:147
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)
Definition: element.h:10
friend std::ostream & operator<<(std::ostream &, const Element &)
A toString method for this class.
Definition: element.cc:93
int get_num_cliques() const
Reports the number of cliques to which this element belongs.
Definition: element.cc:78
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:156