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