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