Drake
rigid_body.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <iostream>
4 #include <map>
5 #include <memory>
6 #include <set>
7 #include <string>
8 #include <utility>
9 #include <vector>
10 
11 #include <Eigen/Dense>
12 
19 
20 template <typename T>
21 class RigidBody {
22  public:
23  RigidBody();
24 
25  /**
26  * Returns a clone of this RigidBody.
27  *
28  * *Important note!* The following are not cloned:
29  * - the joint
30  * - the parent %RigidBody
31  * - the visual elements
32  * - the collision elements
33  *
34  * The parent is *not* cloned because the reference to it can only be
35  * determined by the RigidBodyTree (which owns both this body and the parent
36  * body). Both the parent and the joint are expected to be set by calling
37  * add_joint().
38  *
39  * The visual and collision elements will be cloned pending identified need.
40  */
41  std::unique_ptr<RigidBody<T>> Clone() const;
42 
43  /**
44  * Returns the name of this rigid body.
45  */
46  const std::string& get_name() const;
47 
48  /**
49  * Sets the name of this rigid body.
50  */
51  void set_name(const std::string& name);
52 
53  /**
54  * Returns the name of the model defining this rigid body.
55  */
56  const std::string& get_model_name() const;
57 
58  /**
59  * Sets the name of the model defining this rigid body.
60  */
61  void set_model_name(const std::string& name);
62 
63  /**
64  * Returns the ID of the model instance to which this rigid body belongs.
65  */
66  int get_model_instance_id() const;
67 
68  /**
69  * Sets the ID of the model instance to which this rigid body belongs.
70  */
71  void set_model_instance_id(int model_instance_id);
72 
73  /**
74  * Sets the parent joint through which this rigid body connects to its parent
75  * rigid body.
76  *
77  * @param[in] joint The parent joint of this rigid body. Note that this
78  * rigid body assumes ownership of this joint.
79  */
80  // TODO(liang.fok): Deprecate this method in favor of add_joint().
81  // This requires, among potentially other things, updating the parsers.
82  void setJoint(std::unique_ptr<DrakeJoint> joint);
83 
84  /**
85  * Adds degrees of freedom to this body by connecting it to @p parent with
86  * @p joint. The body takes ownership of the joint.
87  *
88  * This method aborts with an error message if the user attempts to assign a
89  * joint to a body that already has one.
90  *
91  * Note that this is specifically a tree joint and that by "parent" we mean a
92  * body that is closer to "world" in the tree topology.
93  *
94  * The @p parent pointer is copied and stored meaning its lifetime must
95  * exceed the lifetime of this RigidBody.
96  *
97  * @param[in] parent The RigidBody this body gets connected to.
98  * @param[in] joint The DrakeJoint connecting this body to @p parent and
99  * adding degrees of freedom to this body.
100  * @returns A pointer to the joint just added to the body.
101  */
102  template<typename JointType>
103  JointType* add_joint(RigidBody* parent, std::unique_ptr<JointType> joint) {
104  if (joint_ != nullptr) {
106  "Attempting to assign a new joint to a body that already has one");
107  }
108  set_parent(parent);
109  setJoint(move(joint));
110  return static_cast<JointType*>(joint_.get());
111  }
112 
113  /**
114  * An accessor to this rigid body's parent joint. By "parent joint" we
115  * mean the joint through which this rigid body connects to its parent rigid
116  * body in the rigid body tree.
117  *
118  * @return The parent joint of this rigid body.
119  */
120  const DrakeJoint& getJoint() const {
121  DRAKE_DEMAND(joint_ != nullptr);
122  return *joint_;
123  }
124 
125  /**
126  * An accessor to this rigid body's mutable inboard joint. Also called "parent joint".
127  *
128  * @throws std::runtime_error if there is no joint (joint == nullptr)
129  * @return The mutable inboard joint of this rigid body.
130  */
132  DRAKE_THROW_UNLESS(joint_ != nullptr);
133  if (body_index_ == 0)
134  throw std::runtime_error("This method cannot be called on world body");
135  return *joint_;
136  }
137 
138  /**
139  * Reports if the body has a parent joint.
140  */
141  bool has_joint() const { return joint_ != nullptr; }
142 
143  /**
144  * Sets the parent rigid body. This is the rigid body that is connected to
145  * this rigid body's joint.
146  *
147  * @param[in] parent A pointer to this rigid body's parent rigid body.
148  */
149  void set_parent(RigidBody* parent);
150 
151  /**
152  * Returns a const pointer to this rigid body's parent rigid body.
153  */
154  const RigidBody* get_parent() const { return parent_; }
155 
156  /**
157  * Returns whether this RigidBody has a "parent", which is a RigidBody that is
158  * connected to this RigidBody via a DrakeJoint and is closer to the root of
159  * the RigidBodyTree relative to this RigidBody. In other words, the parent
160  * RigidBody is part of a kinematic path from this RigidBody to the root of
161  * the RigidBodyTree. Thus, by definition, all RigidBody objects should have a
162  * parent RigidBody except for the RigidBodyTree's root, which is the world.
163  */
164  bool has_parent_body() const { return parent_ != nullptr; }
165 
166  // TODO(liang.fok): Remove this deprecated method prior to Release 1.0.
167  DRAKE_DEPRECATED("Please use has_parent_body().")
168  bool hasParent() const;
169 
170  /**
171  * Checks if a particular rigid body is the parent of this rigid body.
172  *
173  * @param[in] other The potential parent of this rigid body.
174  * @return true if the supplied rigid body parameter other is the parent of
175  * this rigid body.
176  */
177  bool has_as_parent(const RigidBody& other) const {
178  return parent_ == &other;
179  }
180 
181 
182  /// Sets the "body index" of this `RigidBody`. The "body index" is the
183  /// index of this `RigidBody` within the vector of `RigidBody` objects
184  /// within the `RigidBodyTree`.
185  /// Users should NOT call this method. It is only here to be used
186  /// internally by RigidBodyTree.
187  void set_body_index(int body_index);
188 
189  /**
190  * Returns the "body index" of this `RigidBody`. This is the index within the
191  * vector of `RigidBody` objects within the `RigidBodyTree`.
192  */
193  int get_body_index() const { return body_index_; }
194 
195  /**
196  * Sets the start index of this rigid body's mobilizer joint's contiguous
197  * generalized coordinates `q` (joint position state variables) within the
198  * full RigidBodyTree generalized coordinate vector.
199  *
200  * For more details about the semantics of @p position_start_index, see the
201  * documentation for RigidBodyTree.
202  */
203  void set_position_start_index(int position_start_index);
204 
205  /**
206  * Returns the start index of this body's parent jont's position states; see
207  * RigidBody::set_position_start_index() for more information.
208  */
209  int get_position_start_index() const { return position_start_index_; }
210 
211  /**
212  * Sets the start index of this rigid body's mobilizer joint's contiguous
213  * generalized velocity `v` (joint velocity state variables) within the full
214  * RigidBodyTree generalized velocity vector.
215  *
216  * For more details about the semantics of @p velocity_start_index, see the
217  * documentation for RigidBodyTree.
218  */
219  void set_velocity_start_index(int velocity_start_index);
220 
221  /**
222  * Returns the start index of this body's parent jont's velocity states; see
223  * RigidBody::set_velocity_start_index() for more information.
224  */
225  int get_velocity_start_index() const { return velocity_start_index_; }
226 
227  void AddVisualElement(const DrakeShapes::VisualElement& elements);
228 
230 
231  // TODO(SeanCurtis-TRI): This shouldn't be called publicly. Collision elements
232  // have to be processed in the context of the rigid body tree. Long term,
233  // this will be displaced into SceneGraph. Short term, just don't call
234  // it. If you need to add a collision element to a body, add it through
235  // RigidBodyTree::addCollisionElement.
236  /**
237  * Adds the given collision @p element to the body with the given group name.
238  * @param[in] group_name The collision element's group name.
239  * @param[in] element The element to associate with the rigid body.
240  */
241  void AddCollisionElement(const std::string& group_name,
243 
244  /**
245  * @returns A reference to an `std::vector` of collision elements that
246  * represent the collision geometry of this rigid body.
247  */
248  const std::vector<drake::multibody::collision::ElementId>&
249  get_collision_element_ids() const { return collision_element_ids_; }
250 
251  /**
252  * Returns a reference to an `std::vector` of collision elements that
253  * represent the collision geometry of this rigid body.
254  */
255  std::vector<drake::multibody::collision::ElementId>&
257 
258  /**
259  * Returns a map of collision element group names to vectors of collision
260  * element IDs. These are the collision element groups created through calls
261  * to RigidBody::AddCollisionElementToGroup().
262  */
263  const std::map<std::string,
264  std::vector<drake::multibody::collision::ElementId>>&
266 
267  /**
268  * Returns a map of collision element group names to vectors of collision
269  * element IDs. These are the collision element groups created through calls
270  * to RigidBody::AddCollisionElementToGroup().
271  */
272  std::map<std::string, std::vector<drake::multibody::collision::ElementId>>&
274 
275  /**
276  * Reports if there is a path in this tree from this body to the world where
277  * all joints are *fixed*. This method throws an exception if the
278  * RigidBodyTree is invalid in that:
279  * - This node is the descendant of a parentless node that is *not* the
280  * world node, or
281  * - This node does not have a valid DrakeJoint.
282  */
283  bool IsRigidlyFixedToWorld() const;
284 
285  /**
286  * Reports `X_WBₙ`, the pose of this body, `Bₙ`, in the world frame based on
287  * the *rigid* kinematic path from `Bₙ` to `W`. As such,
288  * the world-fixed pose is only defined for bodies that are rigidly fixed to
289  * the world.
290  *
291  * For this body, with depth `n` in the tree, `Bₙ`, `X_WBₙ` is defined as:
292  *
293  * `X_WBₙ ≡ X_WB₁ * X_B₁B₂ * ... * X_Bₙ₋₂Bₙ₋₁ * X_Bₙ₋₁Bₙ`
294  *
295  * `X_Bₖ₋₁Bₖ` represents the transform from one body's frame
296  * (`Bₖ`) to its parent's frame (Bₖ₋₁). By construction, body `Bₖ` has a
297  * single inboard joint. This joint defines several frames, discussed in
298  * @ref rigid_body_tree_frames, including its parent frame: `Pₖ ≡ Bₖ₋₁`. This
299  * allows us to compute `X_Bₖ₋₁Bₖ` as follows:
300  * - `X_Bₖ₋₁Bₖ = X_PₖBₖ` because `Pₖ ≡ Bₖ₋₁`
301  * - `X_PₖBₖ ≡ X_PₖFₖ * X_FₖMₖ(q) * X_MₖBₖ`, where:
302  * - `X_MₖBₖ = I` in Drake's implementation.
303  * - `X_FₖMₖ(q) = I` because we only follow FixedJoint instances.
304  *
305  *
306  * If the body is not rigidly fixed to the world, an exception will be thrown.
307  * @return `X_WBₙ`.
308  * @see IsRigidlyFixedToWorld
309  */
310  Eigen::Isometry3d ComputeWorldFixedPose() const;
311 
312  // TODO(amcastro-tri): Change to is_adjacent_to().
313  // TODO(SeanCurtis-TRI): This method is only used by the collision clique
314  // calculation. Maybe it would be better if the name reflected this: e.g.,
315  // is_collision_adjacent(), or some such thing.
316  /**
317  * Reports if this body is considered "adjacent" to the given body.
318  *
319  * "Adjacency" refers to the idea that the bodies are connected to each other
320  * in the rigid body tree by a non-floating joint. By this definition,
321  * a rigid body is *not* adjacent to itself.
322  *
323  * In the degenerate case where one rigid body is a parent of the other, but
324  * with no joint assigned, the rigid bodies will be considered adjacent.
325  * Conversely, the degenerate case where a joint is assigned, but the parent
326  * relationship is not set, the rigid bodies will *not* be considered
327  * adjacent.
328  * @param[in] other The body to test against this body.
329  * @returns `true` if the bodies are "adjacent".
330  */
331  bool adjacentTo(const RigidBody& other) const;
332 
333  /**
334  * Returns `true` if this body should be checked for collisions
335  * with the @p other body. CanCollideWith should be commutative: A can
336  * collide with B implies B can collide with A.
337  * @param[in] other The body to query against.
338  * @returns `true` if collision between this and other should be tested.
339  */
340  bool CanCollideWith(const RigidBody& other) const;
341 
343  const std::string& group_name,
344  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
345  std::vector<drake::multibody::collision::ElementId>& ids) const;
346 
348  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
349  std::vector<drake::multibody::collision::ElementId>& ids) const;
350 
351  /**
352  * Returns the points on this rigid body that should be checked for collision
353  * with the environment. These are the contact points that were saved by
354  * RigidBody::set_contact_points().
355  */
356  const Eigen::Matrix3Xd& get_contact_points() const;
357 
358  /**
359  * Saves the points on this rigid body that should be checked for collision
360  * between this rigid body and the environment. These contact points can be
361  * obtained through RigidBody::get_contact_points().
362  */
363  void set_contact_points(const Eigen::Matrix3Xd& contact_points);
364 
365  /**
366  * Sets the mass of this rigid body.
367  */
368  void set_mass(double mass);
369 
370  /**
371  * Returns the mass of this rigid body.
372  */
373  double get_mass() const;
374 
375  /**
376  * Sets the center of mass of this rigid body. The center of mass is expressed
377  * in this body's frame.
378  */
379  void set_center_of_mass(const Eigen::Vector3d& center_of_mass);
380 
381  /**
382  * Gets the center of mass of this rigid body. The center of mass is expressed
383  * in this body's frame.
384  */
385  const Eigen::Vector3d& get_center_of_mass() const;
386 
387  /**
388  * Sets the spatial inertia of this rigid body.
389  */
391  inertia_matrix);
392 
393  /**
394  * Returns the spatial inertia of this rigid body.
395  */
397  return spatial_inertia_;
398  }
399 
400  /**
401  * Transforms all of the visual, collision, and inertial elements associated
402  * with this body to the proper joint frame. This is necessary, for instance,
403  * to support SDF loading where the child frame can be specified independently
404  * from the joint frame. In our RigidBodyTree classes, the body frame IS the
405  * joint frame.
406  *
407  * @param transform_body_to_joint The transform from this body's frame to the
408  * joint's frame.
409  */
411  const Eigen::Isometry3d& transform_body_to_joint);
412 
413  public:
414  friend std::ostream& operator<<(
415  std::ostream& out, const RigidBody<double>& b);
416 
417  public:
418  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
419 
420  typedef std::vector<drake::multibody::collision::Element*>
422  typedef typename CollisionElementsVector::iterator CollisionElementsIterator;
423 
424  CollisionElementsIterator collision_elements_begin() {
425  return collision_elements_.begin();
426  }
427 
428  CollisionElementsIterator collision_elements_end() {
429  return collision_elements_.end();
430  }
431 
432  /**
433  * Reports the total number of *registered* collision elements attached to
434  * this body. See Model::AddElement() for definition of "registered".
435  */
437  return static_cast<int>(collision_elements_.size());
438  }
439 
440  private:
441  // TODO(tkoolen): It's very ugly, but parent, dofnum, and pitch also exist
442  // currently (independently) at the RigidBodyTree level to represent the
443  // featherstone structure. This version is for the kinematics.
444 
445  // The "parent" joint of this rigid body. This is the joint through which this
446  // rigid body connects to the rest of the rigid body tree.
447  std::unique_ptr<DrakeJoint> joint_;
448 
449  // The name of this rigid body.
450  std::string name_;
451 
452  // TODO(liang.fok) Remove this member variable, see:
453  // https://github.com/RobotLocomotion/drake/issues/3053
454  // The name of the model that defined this rigid body.
455  std::string model_name_;
456 
457  // A unique ID for the model instance to which this body belongs.
458  int model_instance_id_{0};
459 
460  // The rigid body that's connected to this rigid body's joint.
461  RigidBody* parent_{nullptr};
462 
463  // The index of this rigid body in the rigid body tree.
464  int body_index_{0};
465 
466  // The starting index of this rigid body's joint's position value(s) within
467  // the parent tree's state vector.
468  int position_start_index_{0};
469 
470  // The starting index of this rigid body's joint's velocity value(s) within
471  // the parent tree's state vector.
472  int velocity_start_index_{0};
473 
474  // A list of visual elements for this RigidBody.
475  DrakeShapes::VectorOfVisualElements visual_elements_;
476 
477  // A list of collision element IDs of collision elements that represent the
478  // geometry of this rigid body.
479  std::vector<drake::multibody::collision::ElementId> collision_element_ids_;
480 
481  // A map of groups of collision element IDs. This is just for conveniently
482  // accessing particular groups of collision elements. The groups do not imply
483  // anything in terms of how the collision elements relate to each other.
484  std::map<std::string, std::vector<drake::multibody::collision::ElementId>>
485  collision_element_groups_;
486 
487  // The contact points this rigid body has with its environment.
488  Eigen::Matrix3Xd contact_points_;
489 
490  // The mass of this rigid body.
491  double mass_{0};
492 
493  // The center of mass of this rigid body.
494  Eigen::Vector3d center_of_mass_;
495 
496  // The spatial inertia of this rigid body.
497  drake::SquareTwistMatrix<double> spatial_inertia_;
498 
499  // TODO(SeanCurtis-TRI): This data is only used in the compilation of the
500  // body. As such, it should be moved into a factory so that the runtime
501  // class only has runtime data.
502  CollisionElementsVector collision_elements_;
503 };
void set_name(const std::string &name)
Sets the name of this rigid body.
Definition: rigid_body.cc:47
double get_mass() const
Returns the mass of this rigid body.
Definition: rigid_body.cc:239
const std::string & get_name() const
Returns the name of this rigid body.
Definition: rigid_body.cc:44
RigidBody()
Definition: rigid_body.cc:22
void set_position_start_index(int position_start_index)
Sets the start index of this rigid body&#39;s mobilizer joint&#39;s contiguous generalized coordinates q (joi...
Definition: rigid_body.cc:82
std::unique_ptr< RigidBody< T > > Clone() const
Returns a clone of this RigidBody.
Definition: rigid_body.cc:28
int get_model_instance_id() const
Returns the ID of the model instance to which this rigid body belongs.
Definition: rigid_body.cc:58
const Eigen::Vector3d & get_center_of_mass() const
Gets the center of mass of this rigid body.
Definition: rigid_body.cc:249
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
Provides a portable macro for use in generating compile-time warnings for use of code that is permitt...
Provides a convenient wrapper to throw an exception when a condition is unmet.
void set_model_instance_id(int model_instance_id)
Sets the ID of the model instance to which this rigid body belongs.
Definition: rigid_body.cc:61
const RigidBody * get_parent() const
Returns a const pointer to this rigid body&#39;s parent rigid body.
Definition: rigid_body.h:154
void set_center_of_mass(const Eigen::Vector3d &center_of_mass)
Sets the center of mass of this rigid body.
Definition: rigid_body.cc:244
A joint defines a spatial relationship between two rigid bodies.
Definition: drake_joint.h:66
bool has_parent_body() const
Returns whether this RigidBody has a "parent", which is a RigidBody that is connected to this RigidBo...
Definition: rigid_body.h:164
std::map< std::string, std::vector< drake::multibody::collision::ElementId > > & get_mutable_group_to_collision_ids_map()
Returns a map of collision element group names to vectors of collision element IDs.
Definition: rigid_body.cc:128
int get_position_start_index() const
Returns the start index of this body&#39;s parent jont&#39;s position states; see RigidBody::set_position_sta...
Definition: rigid_body.h:209
#define DRAKE_ABORT_MSG(message)
Aborts the program (via ::abort) with a message showing at least the given message (macro argument)...
Definition: drake_assert.h:51
bool adjacentTo(const RigidBody &other) const
Reports if this body is considered "adjacent" to the given body.
Definition: rigid_body.cc:174
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef std::vector< drake::multibody::collision::Element * > CollisionElementsVector
Definition: rigid_body.h:421
int get_velocity_start_index() const
Returns the start index of this body&#39;s parent jont&#39;s velocity states; see RigidBody::set_velocity_sta...
Definition: rigid_body.h:225
bool has_as_parent(const RigidBody &other) const
Checks if a particular rigid body is the parent of this rigid body.
Definition: rigid_body.h:177
CollisionElementsIterator collision_elements_begin()
Definition: rigid_body.h:424
void set_velocity_start_index(int velocity_start_index)
Sets the start index of this rigid body&#39;s mobilizer joint&#39;s contiguous generalized velocity v (joint ...
Definition: rigid_body.cc:87
#define DRAKE_THROW_UNLESS(condition)
Evaluates condition and iff the value is false will throw an exception with a message showing at leas...
Definition: drake_throw.h:23
int get_body_index() const
Returns the "body index" of this RigidBody.
Definition: rigid_body.h:193
Definition: collision_filter.h:13
void set_contact_points(const Eigen::Matrix3Xd &contact_points)
Saves the points on this rigid body that should be checked for collision between this rigid body and ...
Definition: rigid_body.cc:229
bool appendCollisionElementIdsFromThisBody(const std::string &group_name, std::vector< drake::multibody::collision::ElementId > &ids) const
Definition: rigid_body.cc:187
CollisionElementsVector::iterator CollisionElementsIterator
Definition: rigid_body.h:422
bool IsRigidlyFixedToWorld() const
Reports if there is a path in this tree from this body to the world where all joints are fixed...
Definition: rigid_body.cc:133
Provides Drake&#39;s assertion implementation.
DrakeJoint & get_mutable_joint()
An accessor to this rigid body&#39;s mutable inboard joint.
Definition: rigid_body.h:131
#define DRAKE_DEMAND(condition)
Evaluates condition and iff the value is false will trigger an assertion failure with a message showi...
Definition: drake_assert.h:45
void ApplyTransformToJointFrame(const Eigen::Isometry3d &transform_body_to_joint)
Transforms all of the visual, collision, and inertial elements associated with this body to the prope...
Definition: rigid_body.cc:214
bool hasParent() const
Definition: rigid_body.cc:76
CollisionElementsIterator collision_elements_end()
Definition: rigid_body.h:428
const std::string & get_model_name() const
Returns the name of the model defining this rigid body.
Definition: rigid_body.cc:50
const std::vector< drake::multibody::collision::ElementId > & get_collision_element_ids() const
Definition: rigid_body.h:249
const DrakeJoint & getJoint() const
An accessor to this rigid body&#39;s parent joint.
Definition: rigid_body.h:120
Eigen::Isometry3d ComputeWorldFixedPose() const
Reports X_WBₙ, the pose of this body, Bₙ, in the world frame based on the rigid kinematic path from...
Definition: rigid_body.cc:155
bool has_joint() const
Reports if the body has a parent joint.
Definition: rigid_body.h:141
void set_mass(double mass)
Sets the mass of this rigid body.
Definition: rigid_body.cc:234
#define DRAKE_DEPRECATED(message)
Use DRAKE_DEPRECATED("message") to discourage use of particular classes, typedefs, variables, non-static data members, functions, arguments, enumerations, and template specializations.
Definition: drake_deprecated.h:33
JointType * add_joint(RigidBody *parent, std::unique_ptr< JointType > joint)
Adds degrees of freedom to this body by connecting it to parent with joint.
Definition: rigid_body.h:103
std::vector< drake::multibody::collision::ElementId > & get_mutable_collision_element_ids()
Returns a reference to an std::vector of collision elements that represent the collision geometry of ...
Definition: rigid_body.cc:115
bool CanCollideWith(const RigidBody &other) const
Returns true if this body should be checked for collisions with the other body.
Definition: rigid_body.cc:181
The term rigid body implies that the deformations of the body under consideration are so small that t...
Definition: fixed_offset_frame.h:16
drake::eigen_aligned_std_vector< VisualElement > VectorOfVisualElements
Definition: visual_element.h:54
void AddCollisionElement(const std::string &group_name, drake::multibody::collision::Element *element)
Adds the given collision element to the body with the given group name.
Definition: rigid_body.cc:104
Definition: visual_element.h:12
const Eigen::Matrix3Xd & get_contact_points() const
Returns the points on this rigid body that should be checked for collision with the environment...
Definition: rigid_body.cc:224
Eigen::Matrix< Scalar, kTwistSize, kTwistSize > SquareTwistMatrix
A six-by-six matrix.
Definition: eigen_types.h:143
void set_body_index(int body_index)
Sets the "body index" of this RigidBody.
Definition: rigid_body.cc:79
void AddVisualElement(const DrakeShapes::VisualElement &elements)
Definition: rigid_body.cc:93
const std::map< std::string, std::vector< drake::multibody::collision::ElementId > > & get_group_to_collision_ids_map() const
Returns a map of collision element group names to vectors of collision element IDs.
Definition: rigid_body.cc:122
void set_spatial_inertia(const drake::SquareTwistMatrix< double > &inertia_matrix)
Sets the spatial inertia of this rigid body.
Definition: rigid_body.cc:254
void set_model_name(const std::string &name)
Sets the name of the model defining this rigid body.
Definition: rigid_body.cc:53
int get_num_collision_elements() const
Reports the total number of registered collision elements attached to this body.
Definition: rigid_body.h:436
void set_parent(RigidBody *parent)
Sets the parent rigid body.
Definition: rigid_body.cc:71
void setJoint(std::unique_ptr< DrakeJoint > joint)
Sets the parent joint through which this rigid body connects to its parent rigid body.
Definition: rigid_body.cc:66
const drake::SquareTwistMatrix< double > & get_spatial_inertia() const
Returns the spatial inertia of this rigid body.
Definition: rigid_body.h:396
const DrakeShapes::VectorOfVisualElements & get_visual_elements() const
Definition: rigid_body.cc:98
The underyling primitive class used for collision analysis.
Definition: element.h:43
friend std::ostream & operator<<(std::ostream &out, const RigidBody< double > &b)
Definition: rigid_body.cc:259