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 
12 
13 // Forward declaration.
14 // This forward declaration is made in order to be able to add a reference
15 // to the parent body without the collision element ever using the RigidBody or
16 // any of its methods.
17 // This is particularly useful when the physics engine (at the RigidBody or
18 // RigidBodyTree scope) needs to retrieve the parent body (for instance to
19 // query its world transform).
20 template <typename T>
21 class RigidBody;
22 
23 namespace drake {
24 namespace multibody {
25 namespace collision {
26 typedef uintptr_t ElementId;
27 
42 class Element : public DrakeShapes::Element {
43  public:
48  Element();
49 
56  const Eigen::Isometry3d& T_element_to_local =
57  Eigen::Isometry3d::Identity());
58 
64  Element(const Eigen::Isometry3d& T_element_to_local,
65  const RigidBody<double>* body);
66 
73  Element(const DrakeShapes::Geometry& geometry,
74  const Eigen::Isometry3d& T_element_to_local,
75  const RigidBody<double>* body);
76 
77  ~Element() override {}
78 
79  Element* clone() const override;
80 
81  ElementId getId() const;
82 
88  void set_anchored() { is_anchored_ = true; }
89 
96  bool is_anchored() const { return is_anchored_; }
97 
103  bool CanCollideWith(const Element *other) const;
104 
113  void AddToCollisionClique(int clique_id);
114 
119  int get_num_cliques() const;
120 
126  const std::vector<int>& collision_cliques() const;
127 
131  const RigidBody<double>* get_body() const;
132 
134  void set_body(const RigidBody<double> *body);
135 
139  void set_collision_filter(const bitmask &group,
140  const bitmask &ignores);
141 
143  return collision_filter_group_;
144  }
145 
147  return collision_filter_ignores_;
148  }
149 
153  friend std::ostream& operator<<(std::ostream&, const Element&);
154 
155  private:
156  // Provide a copy constructor for use by our clone() implementation.
157  // Delete all of the other operations.
158  Element(const Element&) = default;
159  void operator=(const Element&) = delete;
160  Element(Element&&) = delete;
161  void operator=(Element&&) = delete;
162 
163  bool is_anchored_{false};
164  const RigidBody<double>* body_{};
165 
166  // Collision cliques are defined as a set of collision elements that do not
167  // collide.
168  // Collision cliques in Drake are represented simply by an integer.
169  // A collision element can belong to more than one clique.
170  //
171  // Conceptually it would seem like std::set is the right fit for
172  // Element::collision_cliques_. However, std::set is really good for
173  // dynamically adding elements to a set that needs to change.
174  // Once you are done adding elements to your set, access time is poor when
175  // compared to a simple std::vector (nothing faster than scanning a vector of
176  // adjacent entries in memory).
177  // Here adding elements to the cliques vector only happens during problem
178  // setup by the user or from a URDF/SDF file. What we really want is that once
179  // this vector is setup, we can query it very quickly during simulation.
180  // This is done in Element::CanCollideWith() which to be Order(N)
181  // requires the entries in CollisionElement::collision_cliques_ to be sorted.
182  // By arbitrary convention, the ordering is monotonically increasing.
183  std::vector<int> collision_cliques_;
184 
185  // A bitmask that determines the collision groups that this element is part
186  // of. If the i-th bit is set this rigid body belongs to the i-th collision
187  // group. An element can belong to multiple collision groups.
188  drake::multibody::collision::bitmask collision_filter_group_{kDefaultGroup};
189 
190  // A bitmask that determines which collision groups this element can *not*
191  // collide with. Thus, if the i-th bit is set this element is not checked
192  // for collisions with elements in the i-th group.
193  drake::multibody::collision::bitmask collision_filter_ignores_{kNoneMask};
194 
195  public:
196  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
197 };
198 
199 } // namespace collision
200 } // namespace multibody
201 } // namespace drake
uintptr_t ElementId
Definition: element.h:26
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:96
std::bitset< kMaxNumCollisionFilterGroups > bitmask
Definition: collision_filter.h:21
Definition: automotive_demo.cc:88
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:142
std::unique_ptr< Geometry > geometry
Definition: element.h:63
ElementId getId() const
Definition: element.cc:37
Definition: collision_filter.h:13
void set_anchored()
Flags this collision element to be anchored, i.e.
Definition: element.h:88
const bitmask & get_collision_filter_ignores() const
Definition: element.h:146
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:77
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:42