Drake
body.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <vector>
5 
6 #include "drake/common/autodiff.h"
7 #include "drake/common/drake_copyable.h"
8 #include "drake/common/unused.h"
9 #include "drake/multibody/multibody_tree/frame.h"
10 #include "drake/multibody/multibody_tree/multibody_tree_element.h"
11 #include "drake/multibody/multibody_tree/multibody_tree_indexes.h"
12 #include "drake/multibody/multibody_tree/multibody_tree_topology.h"
13 #include "drake/multibody/multibody_tree/spatial_inertia.h"
14 
15 namespace drake {
16 namespace multibody {
17 
18 // Forward declaration for BodyFrame<T>.
19 template<typename T> class Body;
20 
21 /// A %BodyFrame is a material Frame that serves as the unique reference frame
22 /// for a Body.
23 ///
24 /// Each Body B, regardless of whether it represents a rigid body or a
25 /// flexible body, has a unique body frame for which we use the same symbol B
26 /// (with meaning clear from context). The body frame is also referred to as
27 /// a _reference frame_ in the literature for flexible body mechanics modeling
28 /// using the Finite Element Method. All properties of a body are defined with
29 /// respect to its body frame, including its mass properties and attachment
30 /// locations for joints, constraints, actuators, geometry and so on. Run time
31 /// motion of the body is defined with respect to the motion of its body frame.
32 /// We represent a body frame by a %BodyFrame object that is created whenever a
33 /// Body is constructed and is owned by the Body.
34 ///
35 /// Note that the %BodyFrame associated with
36 /// a body does not necessarily need to be located at its center of mass nor
37 /// does it need to be aligned with the body's principal axes, although, in
38 /// practice, it frequently is.
39 /// For flexible bodies, %BodyFrame provides a representation for the body's
40 /// reference frame. The flexible degrees of freedom associated with a flexible
41 /// body describe the body's deformation in this frame. Therefore, the motion of
42 /// a flexible body is defined by the motion of its %BodyFrame, or reference
43 /// frame, plus the motion of the material points on the body with respect to
44 /// its %BodyFrame.
45 ///
46 /// A %BodyFrame and Body are tightly coupled concepts; neither makes sense
47 /// without the other. Therefore, a %BodyFrame instance is constructed in
48 /// conjunction with its Body and cannot be
49 /// constructed anywhere else. However, you can still access the frame
50 /// associated with a body, see Body::get_body_frame().
51 /// This access is more than a convenience; you can use the %BodyFrame to
52 /// define other frames on the body and to attach other multibody elements
53 /// to it.
54 ///
55 /// @tparam T The scalar type. Must be a valid Eigen scalar.
56 template <typename T>
57 class BodyFrame final : public Frame<T> {
58  public:
60 
62  const systems::Context<T>&) const override {
63  return Isometry3<T>::Identity();
64  }
65 
67  const systems::Context<T>&,
68  const Isometry3<T>& X_FQ) const override {
69  return X_FQ;
70  }
71 
72  protected:
73  // Frame<T>::DoCloneToScalar() overrides.
74  std::unique_ptr<Frame<double>> DoCloneToScalar(
75  const MultibodyTree<double>& tree_clone) const override;
76 
77  std::unique_ptr<Frame<AutoDiffXd>> DoCloneToScalar(
78  const MultibodyTree<AutoDiffXd>& tree_clone) const override;
79 
80  private:
81  // Body<T> and BodyFrame<T> are natural allies. A BodyFrame object is created
82  // every time a Body object is created and they are associated with each
83  // other.
84  friend class Body<T>;
85 
86  // Make BodyFrame templated on any other scalar type a friend of
87  // BodyFrame<T> so that CloneToScalar<ToAnyOtherScalar>() can access
88  // private methods from BodyFrame<T>.
89  template <typename> friend class BodyFrame;
90 
91  // Only Body objects can create BodyFrame objects since Body is a friend of
92  // BodyFrame.
93  explicit BodyFrame(const Body<T>& body) : Frame<T>(body) {}
94 
95  // Helper method to make a clone templated on any other scalar type.
96  // This method holds the common implementation for the different overrides to
97  // DoCloneToScalar().
98  template <typename ToScalar>
99  std::unique_ptr<Frame<ToScalar>> TemplatedDoCloneToScalar(
100  const MultibodyTree<ToScalar>& tree_clone) const;
101 };
102 
103 // Forward declarations for Body<T>.
104 template<typename T> class MultibodyTree;
105 
106 /// @cond
107 // Internal implementation details. Users should not access implementations
108 // in this namespace.
109 namespace internal {
110 template <typename T>
111 // Attorney-Client idiom to grant MultibodyTree access to a selected set of
112 // private methods in Body.
113 // BodyAttorney serves as a "proxy" to the Body class but only providing an
114 // interface to a selected subset of methods that should be accessible to
115 // MultibodyTree. These methods are related to the construction and finalize
116 // stage of the multibody system.
117 class BodyAttorney {
118  private:
119  // MultibodyTree keeps a list of mutable pointers to each of the body frames
120  // in the system and therefore it needs mutable access.
121  // Notice this method is private and therefore users do not have access to it
122  // even in the rare event they'd attempt to peek into the "internal::"
123  // namespace.
124  static BodyFrame<T>& get_mutable_body_frame(Body<T>* body) {
125  return body->get_mutable_body_frame();
126  }
127  friend class MultibodyTree<T>;
128 };
129 } // namespace internal
130 /// @endcond
131 
132 /// %Body provides the general abstraction of a body with an API that
133 /// makes no assumption about whether a body is rigid or deformable and neither
134 /// does it make any assumptions about the underlying physical model or
135 /// approximation.
136 /// As an element or component of a MultibodyTree, a body is a
137 /// MultibodyTreeElement, and therefore it has a unique index of type BodyIndex
138 /// within the multibody tree it belongs to.
139 ///
140 /// A %Body contains a unique BodyFrame; see BodyFrame class documentation for
141 /// more information.
142 ///
143 /// @tparam T The scalar type. Must be a valid Eigen scalar.
144 template <typename T>
145 class Body : public MultibodyTreeElement<Body<T>, BodyIndex> {
146  public:
148 
149  /// Creates a %Body with a BodyFrame associated with it.
150  Body() : body_frame_(*this) {}
151 
152  /// Returns the number of generalized positions q describing flexible
153  /// deformations for this body. A rigid body will therefore return zero.
154  virtual int get_num_flexible_positions() const = 0;
155 
156  /// Returns the number of generalized velocities v describing flexible
157  /// deformations for this body. A rigid body will therefore return zero.
158  virtual int get_num_flexible_velocities() const = 0;
159 
160  /// Returns a const reference to the associated BodyFrame.
161  const BodyFrame<T>& get_body_frame() const {
162  return body_frame_;
163  }
164 
165  /// Returns the index of the node in the underlying tree structure of
166  /// the parent MultibodyTree to which this body belongs.
168  return topology_.body_node;
169  }
170 
171  /// Returns the mass of this body stored in `context`.
172  virtual T get_mass(const MultibodyTreeContext<T> &context) const = 0;
173 
174  /// Computes the center of mass `p_BoBcm_B` (or `p_Bcm` for short) of this
175  /// body measured from this body's frame origin `Bo` and expressed in the body
176  /// frame B.
177  virtual const Vector3<T> CalcCenterOfMassInBodyFrame(
178  const MultibodyTreeContext<T>& context) const = 0;
179 
180  /// Computes the SpatialInertia `I_BBo_B` of `this` body about its frame
181  /// origin `Bo` (not necessarily its center of mass) and expressed in its body
182  /// frame `B`.
183  /// In general, the spatial inertia of a body is a function of state.
184  /// Consider for instance the case of a flexible body for which its spatial
185  /// inertia in the body frame depends on the generalized coordinates
186  /// describing its state of deformation. As a particular case, the spatial
187  /// inertia of a RigidBody in its body frame is constant.
188  virtual SpatialInertia<T> CalcSpatialInertiaInBodyFrame(
189  const MultibodyTreeContext<T>& context) const = 0;
190 
191  /// NVI (Non-Virtual Interface) to DoCloneToScalar() templated on the scalar
192  /// type of the new clone to be created. This method is mostly intended to be
193  /// called by MultibodyTree::CloneToScalar(). Most users should not call this
194  /// clone method directly but rather clone the entire parent MultibodyTree if
195  /// needed.
196  /// @sa MultibodyTree::CloneToScalar()
197  template <typename ToScalar>
198  std::unique_ptr<Body<ToScalar>> CloneToScalar(
199  const MultibodyTree<ToScalar>& tree_clone) const {
200  return DoCloneToScalar(tree_clone);
201  }
202 
203  protected:
204  /// @name Methods to make a clone templated on different scalar types.
205  ///
206  /// These methods are meant to be called by MultibodyTree::CloneToScalar()
207  /// when making a clone of the entire tree or a new instance templated on a
208  /// different scalar type. The only const argument to these methods is the
209  /// new MultibodyTree clone under construction. Specific %Body subclasses
210  /// might specify a number of prerequisites on the cloned tree and therefore
211  /// require it to be at a given state of cloning (for instance requiring that
212  /// the cloned tree already contains all the frames in the world as in the
213  /// original tree.) See MultibodyTree::CloneToScalar() for a list of
214  /// prerequisites that are guaranteed to be satisfied during the cloning
215  /// process.
216  ///
217  /// @{
218 
219  /// Clones this %Body (templated on T) to a body templated on `double`.
220  virtual std::unique_ptr<Body<double>> DoCloneToScalar(
221  const MultibodyTree<double>& tree_clone) const = 0;
222 
223  /// Clones this %Body (templated on T) to a body templated on AutoDiffXd.
224  virtual std::unique_ptr<Body<AutoDiffXd>> DoCloneToScalar(
225  const MultibodyTree<AutoDiffXd>& tree_clone) const = 0;
226 
227  /// @}
228 
229  private:
230  // Only friends of BodyAttorney (i.e. MultibodyTree) have access to a selected
231  // set of private Body methods.
232  friend class internal::BodyAttorney<T>;
233 
234  // Implementation for MultibodyTreeElement::DoSetTopology().
235  // At MultibodyTree::Finalize() time, each body retrieves its topology
236  // from the parent MultibodyTree.
237  void DoSetTopology(const MultibodyTreeTopology& tree_topology) final {
238  topology_ = tree_topology.get_body(this->get_index());
239  body_frame_.SetTopology(tree_topology);
240  }
241 
242  // MultibodyTree has access to the mutable BodyFrame through BodyAttorney.
243  BodyFrame<T>& get_mutable_body_frame() {
244  return body_frame_;
245  }
246 
247  // Body frame associated with this body.
248  BodyFrame<T> body_frame_;
249 
250  // The internal bookkeeping topology struct used by MultibodyTree.
251  BodyTopology topology_;
252 };
253 
254 } // namespace multibody
255 } // namespace drake
Eigen::Transform< Scalar, 3, Eigen::Isometry > Isometry3
An Isometry templated on scalar type.
Definition: eigen_types.h:106
MultibodyTreeContext is an object that contains all the information needed to uniquely determine the ...
Definition: multibody_tree_context.h:40
Definition: automotive_demo.cc:89
Data structure to store the topological information associated with a Body.
Definition: multibody_tree_topology.h:38
std::unique_ptr< Frame< double > > DoCloneToScalar(const MultibodyTree< double > &tree_clone) const override
Clones this Frame (templated on T) to a frame templated on double.
Definition: body.cc:24
Context is an abstract base class template that represents all the inputs to a System: time...
Definition: query_handle.h:10
Body provides the general abstraction of a body with an API that makes no assumption about whether a ...
Definition: body.h:19
MultibodyTree provides a representation for a physical system consisting of a collection of interconn...
Definition: body.h:104
BodyNodeIndex get_node_index() const
Returns the index of the node in the underlying tree structure of the parent MultibodyTree to which t...
Definition: body.h:167
const BodyFrame< T > & get_body_frame() const
Returns a const reference to the associated BodyFrame.
Definition: body.h:161
A BodyFrame is a material Frame that serves as the unique reference frame for a Body.
Definition: body.h:57
Frame is an abstract class representing a material frame (also called a physical frame), meaning that it is associated with a material point of a Body.
Definition: frame.h:41
Eigen::Matrix< Scalar, 3, 1 > Vector3
A column vector of size 3, templated on scalar type.
Definition: eigen_types.h:34
Data structure to store the topological information associated with an entire MultibodyTree.
Definition: multibody_tree_topology.h:372
friend class BodyFrame
Definition: body.h:89
Isometry3< T > CalcOffsetPoseInBody(const systems::Context< T > &, const Isometry3< T > &X_FQ) const override
Given the offset pose X_FQ of a frame Q in this frame F, this method computes the pose X_BQ of frame ...
Definition: body.h:66
std::unique_ptr< Body< ToScalar > > CloneToScalar(const MultibodyTree< ToScalar > &tree_clone) const
NVI (Non-Virtual Interface) to DoCloneToScalar() templated on the scalar type of the new clone to be ...
Definition: body.h:198
This class represents the physical concept of a Spatial Inertia.
Definition: spatial_inertia.h:87
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33
Isometry3< T > CalcPoseInBodyFrame(const systems::Context< T > &) const override
Returns the pose X_BF of this frame F in the body frame B associated with this frame.
Definition: body.h:61