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