Drake
multibody_tree.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <string>
5 #include <tuple>
6 #include <type_traits>
7 #include <utility>
8 #include <vector>
9 
10 #include "drake/common/autodiff.h"
11 #include "drake/common/drake_copyable.h"
12 #include "drake/common/drake_optional.h"
13 #include "drake/multibody/multibody_tree/acceleration_kinematics_cache.h"
14 #include "drake/multibody/multibody_tree/body.h"
15 #include "drake/multibody/multibody_tree/body_node.h"
16 #include "drake/multibody/multibody_tree/force_element.h"
17 #include "drake/multibody/multibody_tree/frame.h"
18 #include "drake/multibody/multibody_tree/joints/joint.h"
19 #include "drake/multibody/multibody_tree/mobilizer.h"
20 #include "drake/multibody/multibody_tree/multibody_tree_context.h"
21 #include "drake/multibody/multibody_tree/multibody_tree_topology.h"
22 #include "drake/multibody/multibody_tree/position_kinematics_cache.h"
23 #include "drake/multibody/multibody_tree/velocity_kinematics_cache.h"
24 #include "drake/systems/framework/context.h"
25 
26 namespace drake {
27 namespace multibody {
28 
29 /// %MultibodyTree provides a representation for a physical system consisting of
30 /// a collection of interconnected rigid and deformable bodies. As such, it owns
31 /// and manages each of the elements that belong to this physical system.
32 /// Multibody dynamics elements include bodies, joints, force elements and
33 /// constraints.
34 ///
35 /// @tparam T The scalar type. Must be a valid Eigen scalar.
36 ///
37 /// Instantiated templates for the following kinds of T's are provided:
38 /// - double
39 /// - AutoDiffXd
40 ///
41 /// They are already available to link against in the containing library.
42 /// No other values for T are currently supported.
43 template <typename T>
44 class MultibodyTree {
45  public:
47 
48  /// Creates a MultibodyTree containing only a **world** body.
49  MultibodyTree();
50 
51  /// @name Methods to add new MultibodyTree elements.
52  ///
53  /// To create a %MultibodyTree users will add multibody elements like bodies,
54  /// joints, force elements, constraints, etc, using one of these methods.
55  /// Once a user is done adding multibody elements, the Finalize() method
56  /// **must** be called before invoking any %MultibodyTree method.
57  /// See Finalize() for details.
58  /// @{
59  // TODO(amcastro-tri): add at least one example of a method that requires a
60  // valid topology in this documentation.
61  // See this Reviewable comment:
62  // https://reviewable.io/reviews/robotlocomotion/drake/5583#-KgGqGisnX9uMuYDkHpx
63 
64  /// Takes ownership of `body` and adds it to `this` %MultibodyTree. Returns a
65  /// constant reference to the body just added, which will remain valid for the
66  /// lifetime of `this` %MultibodyTree.
67  ///
68  /// Example of usage:
69  /// @code
70  /// MultibodyTree<T> model;
71  /// // ... Code to define spatial_inertia, a SpatialInertia<T> object ...
72  /// const RigidBody<T>& body =
73  /// model.AddBody(std::make_unique<RigidBody<T>>(spatial_inertia));
74  /// @endcode
75  ///
76  /// @throws std::logic_error if `body` is a nullptr.
77  /// @throws std::logic_error if Finalize() was already called on `this` tree.
78  ///
79  /// @param[in] body A unique pointer to a body to add to `this`
80  /// %MultibodyTree. The body class must be specialized on the
81  /// same scalar type T as this %MultibodyTree.
82  /// @returns A constant reference of type `BodyType` to the created body.
83  /// This reference which will remain valid for the lifetime of `this`
84  /// %MultibodyTree.
85  ///
86  /// @tparam BodyType The type of the specific sub-class of Body to add. The
87  /// template needs to be specialized on the same scalar type
88  /// T of this %MultibodyTree.
89  template <template<typename Scalar> class BodyType>
90  const BodyType<T>& AddBody(std::unique_ptr<BodyType<T>> body) {
91  static_assert(std::is_convertible<BodyType<T>*, Body<T>*>::value,
92  "BodyType must be a sub-class of Body<T>.");
93  if (topology_is_valid()) {
94  throw std::logic_error("This MultibodyTree is finalized already. "
95  "Therefore adding more bodies is not allowed. "
96  "See documentation for Finalize() for details.");
97  }
98  if (body == nullptr) {
99  throw std::logic_error("Input body is a nullptr.");
100  }
101  BodyIndex body_index(0);
102  FrameIndex body_frame_index(0);
103  std::tie(body_index, body_frame_index) = topology_.add_body();
104  // These tests MUST be performed BEFORE frames_.push_back() and
105  // owned_bodies_.push_back() below. Do not move them around!
106  DRAKE_ASSERT(body_index == get_num_bodies());
107  DRAKE_ASSERT(body_frame_index == get_num_frames());
108 
109  // TODO(amcastro-tri): consider not depending on setting this pointer at
110  // all. Consider also removing MultibodyTreeElement altogether.
111  body->set_parent_tree(this, body_index);
112  // MultibodyTree can access selected private methods in Body through its
113  // BodyAttorney.
115  &internal::BodyAttorney<T>::get_mutable_body_frame(body.get());
116  body_frame->set_parent_tree(this, body_frame_index);
117  frames_.push_back(body_frame);
118  BodyType<T>* raw_body_ptr = body.get();
119  owned_bodies_.push_back(std::move(body));
120  return *raw_body_ptr;
121  }
122 
123  /// Constructs a new body with type `BodyType` with the given `args`, and adds
124  /// it to `this` %MultibodyTree, which retains ownership. The `BodyType` will
125  /// be specialized on the scalar type T of this %MultibodyTree.
126  ///
127  /// Example of usage:
128  /// @code
129  /// MultibodyTree<T> model;
130  /// // ... Code to define spatial_inertia, a SpatialInertia<T> object ...
131  /// // Notice RigidBody is a template on a scalar type.
132  /// const RigidBody<T>& body = model.AddBody<RigidBody>(spatial_inertia);
133  /// @endcode
134  ///
135  /// Note that for dependent names you must use the template keyword (say for
136  /// instance you have a MultibodyTree<T> member within your custom class):
137  ///
138  /// @code
139  /// MultibodyTree<T> model;
140  /// auto body = model.template AddBody<RigidBody>(Args...);
141  /// @endcode
142  ///
143  /// @throws std::logic_error if Finalize() was already called on `this` tree.
144  ///
145  /// @param[in] args The arguments needed to construct a valid Body of type
146  /// `BodyType`. `BodyType` must provide a public constructor
147  /// that takes these arguments.
148  /// @returns A constant reference of type `BodyType` to the created body.
149  /// This reference which will remain valid for the lifetime of `this`
150  /// %MultibodyTree.
151  ///
152  /// @tparam BodyType A template for the type of Body to construct. The
153  /// template will be specialized on the scalar type T of this
154  /// %MultibodyTree.
155  template<template<typename Scalar> class BodyType, typename... Args>
156  const BodyType<T>& AddBody(Args&&... args) {
157  static_assert(std::is_convertible<BodyType<T>*, Body<T>*>::value,
158  "BodyType must be a sub-class of Body<T>.");
159  return AddBody(std::make_unique<BodyType<T>>(std::forward<Args>(args)...));
160  }
161 
162  /// Takes ownership of `frame` and adds it to `this` %MultibodyTree. Returns
163  /// a constant reference to the frame just added, which will remain valid for
164  /// the lifetime of `this` %MultibodyTree.
165  ///
166  /// Example of usage:
167  /// @code
168  /// MultibodyTree<T> model;
169  /// // ... Define body and X_BF ...
170  /// const FixedOffsetFrame<T>& frame =
171  /// model.AddFrame(std::make_unique<FixedOffsetFrame<T>>(body, X_BF));
172  /// @endcode
173  ///
174  /// @throws std::logic_error if `frame` is a nullptr.
175  /// @throws std::logic_error if Finalize() was already called on `this` tree.
176  ///
177  /// @param[in] frame A unique pointer to a frame to be added to `this`
178  /// %MultibodyTree. The frame class must be specialized on
179  /// the same scalar type T as this %MultibodyTree.
180  /// @returns A constant reference of type `FrameType` to the created frame.
181  /// This reference which will remain valid for the lifetime of `this`
182  /// %MultibodyTree.
183  ///
184  /// @tparam FrameType The type of the specific sub-class of Frame to add. The
185  /// template needs to be specialized on the same scalar type
186  /// T of this %MultibodyTree.
187  template <template<typename Scalar> class FrameType>
188  const FrameType<T>& AddFrame(std::unique_ptr<FrameType<T>> frame) {
189  static_assert(std::is_convertible<FrameType<T>*, Frame<T>*>::value,
190  "FrameType must be a sub-class of Frame<T>.");
191  if (topology_is_valid()) {
192  throw std::logic_error("This MultibodyTree is finalized already. "
193  "Therefore adding more frames is not allowed. "
194  "See documentation for Finalize() for details.");
195  }
196  if (frame == nullptr) {
197  throw std::logic_error("Input frame is a nullptr.");
198  }
199  FrameIndex frame_index = topology_.add_frame(frame->get_body().get_index());
200  // This test MUST be performed BEFORE frames_.push_back() and
201  // owned_frames_.push_back() below. Do not move it around!
202  DRAKE_ASSERT(frame_index == get_num_frames());
203  // TODO(amcastro-tri): consider not depending on setting this pointer at
204  // all. Consider also removing MultibodyTreeElement altogether.
205  frame->set_parent_tree(this, frame_index);
206  FrameType<T>* raw_frame_ptr = frame.get();
207  frames_.push_back(raw_frame_ptr);
208  owned_frames_.push_back(std::move(frame));
209  return *raw_frame_ptr;
210  }
211 
212  /// Constructs a new frame with type `FrameType` with the given `args`, and
213  /// adds it to `this` %MultibodyTree, which retains ownership. The `FrameType`
214  /// will be specialized on the scalar type T of this %MultibodyTree.
215  ///
216  /// Example of usage:
217  /// @code
218  /// MultibodyTree<T> model;
219  /// // ... Define body and X_BF ...
220  /// // Notice FixedOffsetFrame is a template an a scalar type.
221  /// const FixedOffsetFrame<T>& frame =
222  /// model.AddFrame<FixedOffsetFrame>(body, X_BF);
223  /// @endcode
224  ///
225  /// Note that for dependent names you must use the template keyword (say for
226  /// instance you have a MultibodyTree<T> member within your custom class):
227  ///
228  /// @code
229  /// MultibodyTree<T> model;
230  /// // ... Define body and X_BF ...
231  /// const auto& frame =
232  /// model.template AddFrame<FixedOffsetFrame>(body, X_BF);
233  /// @endcode
234  ///
235  /// @throws std::logic_error if Finalize() was already called on `this` tree.
236  ///
237  /// @param[in] args The arguments needed to construct a valid Frame of type
238  /// `FrameType`. `FrameType` must provide a public constructor
239  /// that takes these arguments.
240  /// @returns A constant reference of type `FrameType` to the created frame.
241  /// This reference which will remain valid for the lifetime of `this`
242  /// %MultibodyTree.
243  ///
244  /// @tparam FrameType A template for the type of Frame to construct. The
245  /// template will be specialized on the scalar type T of
246  /// this %MultibodyTree.
247  template<template<typename Scalar> class FrameType, typename... Args>
248  const FrameType<T>& AddFrame(Args&&... args) {
249  static_assert(std::is_convertible<FrameType<T>*, Frame<T>*>::value,
250  "FrameType must be a sub-class of Frame<T>.");
251  return AddFrame(
252  std::make_unique<FrameType<T>>(std::forward<Args>(args)...));
253  }
254 
255  /// Takes ownership of `mobilizer` and adds it to `this` %MultibodyTree.
256  /// Returns a constant reference to the mobilizer just added, which will
257  /// remain valid for the lifetime of `this` %MultibodyTree.
258  ///
259  /// Example of usage:
260  /// @code
261  /// MultibodyTree<T> model;
262  /// // ... Code to define inboard and outboard frames by calling
263  /// // MultibodyTree::AddFrame() ...
264  /// const RevoluteMobilizer<T>& pin =
265  /// model.AddMobilizer(std::make_unique<RevoluteMobilizer<T>>(
266  /// inboard_frame, elbow_outboard_frame,
267  /// Vector3d::UnitZ() /*revolute axis*/));
268  /// @endcode
269  ///
270  /// A %Mobilizer effectively connects the two bodies to which the inboard and
271  /// outboard frames belong.
272  ///
273  /// @throws std::logic_error if `mobilizer` is a nullptr.
274  /// @throws std::logic_error if Finalize() was already called on `this` tree.
275  /// @throws a std::runtime_error if the new mobilizer attempts to connect a
276  /// frame with itself.
277  /// @throws std::runtime_error if attempting to connect two bodies with more
278  /// than one mobilizer between them.
279  ///
280  /// @param[in] mobilizer A unique pointer to a mobilizer to add to `this`
281  /// %MultibodyTree. The mobilizer class must be
282  /// specialized on the same scalar type T as this
283  /// %MultibodyTree. Notice this is a requirement of this
284  /// method's signature and therefore an input mobilzer
285  /// specialized on a different scalar type than that of
286  /// this %MultibodyTree's T will fail to compile.
287  /// @returns A constant reference of type `MobilizerType` to the created
288  /// mobilizer. This reference which will remain valid for the
289  /// lifetime of `this` %MultibodyTree.
290  ///
291  /// @tparam MobilizerType The type of the specific sub-class of Mobilizer to
292  /// add. The template needs to be specialized on the
293  /// same scalar type T of this %MultibodyTree.
294  template <template<typename Scalar> class MobilizerType>
295  const MobilizerType<T>& AddMobilizer(
296  std::unique_ptr<MobilizerType<T>> mobilizer) {
297  static_assert(std::is_convertible<MobilizerType<T>*, Mobilizer<T>*>::value,
298  "MobilizerType must be a sub-class of mobilizer<T>.");
299  if (topology_is_valid()) {
300  throw std::logic_error("This MultibodyTree is finalized already. "
301  "Therefore adding more mobilizers is not allowed. "
302  "See documentation for Finalize() for details.");
303  }
304  if (mobilizer == nullptr) {
305  throw std::logic_error("Input mobilizer is a nullptr.");
306  }
307  // Verifies that the inboard/outboard frames provided by the user do belong
308  // to this tree. This is a pathological case, but in theory nothing
309  // (but this test) stops a user from adding frames to a tree1 and attempting
310  // later to define mobilizers between those frames in a second tree2.
311  mobilizer->get_inboard_frame().HasThisParentTreeOrThrow(this);
312  mobilizer->get_outboard_frame().HasThisParentTreeOrThrow(this);
313  const int num_positions = mobilizer->get_num_positions();
314  const int num_velocities = mobilizer->get_num_velocities();
315  MobilizerIndex mobilizer_index = topology_.add_mobilizer(
316  mobilizer->get_inboard_frame().get_index(),
317  mobilizer->get_outboard_frame().get_index(),
318  num_positions, num_velocities);
319 
320  // This DRAKE_ASSERT MUST be performed BEFORE owned_mobilizers_.push_back()
321  // below. Do not move it around!
322  DRAKE_ASSERT(mobilizer_index == get_num_mobilizers());
323 
324  // TODO(amcastro-tri): consider not depending on setting this pointer at
325  // all. Consider also removing MultibodyTreeElement altogether.
326  mobilizer->set_parent_tree(this, mobilizer_index);
327 
328  MobilizerType<T>* raw_mobilizer_ptr = mobilizer.get();
329  owned_mobilizers_.push_back(std::move(mobilizer));
330  return *raw_mobilizer_ptr;
331  }
332 
333  /// Constructs a new mobilizer with type `MobilizerType` with the given
334  /// `args`, and adds it to `this` %MultibodyTree, which retains ownership.
335  /// The `MobilizerType` will be specialized on the scalar type T of this
336  /// %MultibodyTree.
337  ///
338  /// Example of usage:
339  /// @code
340  /// MultibodyTree<T> model;
341  /// // ... Code to define inboard and outboard frames by calling
342  /// // MultibodyTree::AddFrame() ...
343  /// // Notice RevoluteMobilizer is a template an a scalar type.
344  /// const RevoluteMobilizer<T>& pin =
345  /// model.template AddMobilizer<RevoluteMobilizer>(
346  /// inboard_frame, outboard_frame,
347  /// Vector3d::UnitZ() /*revolute axis*/);
348  /// @endcode
349  ///
350  /// Note that for dependent names _only_ you must use the template keyword
351  /// (say for instance you have a MultibodyTree<T> member within your custom
352  /// class).
353  ///
354  /// @throws std::logic_error if Finalize() was already called on `this` tree.
355  /// @throws a std::runtime_error if the new mobilizer attempts to connect a
356  /// frame with itself.
357  /// @throws std::runtime_error if attempting to connect two bodies with more
358  /// than one mobilizer between them.
359  ///
360  /// @param[in] args The arguments needed to construct a valid Mobilizer of
361  /// type `MobilizerType`. `MobilizerType` must provide a
362  /// public constructor that takes these arguments.
363  /// @returns A constant reference of type `MobilizerType` to the created
364  /// mobilizer. This reference which will remain valid for the
365  /// lifetime of `this` %MultibodyTree.
366  ///
367  /// @tparam MobilizerType A template for the type of Mobilizer to construct.
368  /// The template will be specialized on the scalar type
369  /// T of `this` %MultibodyTree.
370  template<template<typename Scalar> class MobilizerType, typename... Args>
371  const MobilizerType<T>& AddMobilizer(Args&&... args) {
372  static_assert(std::is_base_of<Mobilizer<T>, MobilizerType<T>>::value,
373  "MobilizerType must be a sub-class of Mobilizer<T>.");
374  return AddMobilizer(
375  std::make_unique<MobilizerType<T>>(std::forward<Args>(args)...));
376  }
377 
378  /// Creates and adds to `this` %MultibodyTree (which retains ownership) a new
379  /// `ForceElement` member with the specific type `ForceElementType`. The
380  /// arguments to this method `args` are forwarded to `ForceElementType`'s
381  /// constructor.
382  ///
383  /// The newly created `ForceElementType` object will be specialized on the
384  /// scalar type T of this %MultibodyTree.
385  template <template<typename Scalar> class ForceElementType>
386  const ForceElementType<T>& AddForceElement(
387  std::unique_ptr<ForceElementType<T>> force_element) {
388  static_assert(
389  std::is_convertible<ForceElementType<T>*, ForceElement<T>*>::value,
390  "ForceElementType<T> must be a sub-class of ForceElement<T>.");
391  if (topology_is_valid()) {
392  throw std::logic_error(
393  "This MultibodyTree is finalized already. Therefore adding more "
394  "force elements is not allowed. "
395  "See documentation for Finalize() for details.");
396  }
397  if (force_element == nullptr) {
398  throw std::logic_error("Input force element is a nullptr.");
399  }
400  ForceElementIndex force_element_index = topology_.add_force_element();
401  // This test MUST be performed BEFORE owned_force_elements_.push_back()
402  // below. Do not move it around!
403  DRAKE_ASSERT(force_element_index == get_num_force_elements());
404  force_element->set_parent_tree(this, force_element_index);
405  ForceElementType<T>* raw_force_element_ptr = force_element.get();
406  owned_force_elements_.push_back(std::move(force_element));
407  return *raw_force_element_ptr;
408  }
409 
410  template<template<typename Scalar> class ForceElementType, typename... Args>
411  const ForceElementType<T>& AddForceElement(Args&&... args) {
412  static_assert(std::is_base_of<ForceElement<T>, ForceElementType<T>>::value,
413  "ForceElementType<T> must be a sub-class of "
414  "ForceElement<T>.");
415  return AddForceElement(
416  std::make_unique<ForceElementType<T>>(std::forward<Args>(args)...));
417  }
418 
419  /// This method helps to create a Joint of type `JointType` between two
420  /// bodies.
421  /// The two bodies connected by this Joint object are referred to as the
422  /// _parent_ and _child_ bodies. Although the terms _parent_ and _child_ are
423  /// sometimes used synonymously to describe the relationship between inboard
424  /// and outboard bodies in multibody models, this usage is wholly unrelated
425  /// and implies nothing about the inboard-outboard relationship between the
426  /// bodies.
427  /// As explained in the Joint class's documentation, in Drake we define a
428  /// frame F attached to the parent body P with pose `X_PF` and a frame M
429  /// attached to the child body B with pose `X_BM`. This method helps creating
430  /// a joint between two bodies with fixed poses `X_PF` and `X_BM`.
431  /// Refer to the Joint class's documentation for more details.
432  ///
433  /// The arguments to this method `args` are forwarded to `JointType`'s
434  /// constructor. The newly created `JointType` object will be specialized on
435  /// the scalar type T of this %MultibodyTree.
436  ///
437  /// @param name
438  /// The name of the joint.
439  /// @param[in] parent
440  /// The parent body connected by the new joint.
441  /// @param[in] X_PF
442  /// The fixed pose of frame F attached to the parent body, measured in
443  /// the frame P of that body. `X_PF` is an optional parameter; empty curly
444  /// braces `{}` imply that frame F **is** the same body frame P. If instead
445  /// your intention is to make a frame F with pose `X_PF`, provide
446  /// `Isometry3<double>::Identity()` as your input.
447  /// @param[in] child
448  /// The child body connected by the new joint.
449  /// @param[in] X_BM
450  /// The fixed pose of frame M attached to the child body, measured in
451  /// the frame B of that body. `X_BM` is an optional parameter; empty curly
452  /// braces `{}` imply that frame M **is** the same body frame B. If instead
453  /// your intention is to make a frame F with pose `X_PF`, provide
454  /// `Isometry3<double>::Identity()` as your input.
455  ///
456  /// Example of usage:
457  /// @code
458  /// MultibodyTree<T> model;
459  /// // ... Code to define a parent body P and a child body B.
460  /// const Body<double>& parent_body =
461  /// model.AddBody<RigidBody>(SpatialInertia<double>(...));
462  /// const Body<double>& child_body =
463  /// model.AddBody<RigidBody>(SpatialInertia<double>(...));
464  /// // Define the pose X_BM of a frame M rigidly atached to child body B.
465  /// const RevoluteJoint<double>& elbow =
466  /// model.AddJoint<RevoluteJoint>(
467  /// "Elbow", /* joint name */
468  /// model.get_world_body(), /* parent body */
469  /// {}, /* frame F IS the parent body frame P */
470  /// pendulum, /* child body, the pendulum */
471  /// X_BM, /* pose of frame M in the body frame B */
472  /// Vector3d::UnitZ()); /* revolute axis in this case */
473  /// @endcode
474  ///
475  /// @see The Joint class's documentation for further details on how a Joint
476  /// is defined.
477  template<template<typename> class JointType, typename... Args>
478  const JointType<T>& AddJoint(
479  const std::string& name,
480  const Body<T>& parent, const optional<Isometry3<double>>& X_PF,
481  const Body<T>& child, const optional<Isometry3<double>>& X_BM,
482  Args&&... args) {
483  static_assert(std::is_base_of<Joint<T>, JointType<T>>::value,
484  "JointType<T> must be a sub-class of Joint<T>.");
485 
486  const Frame<T>* frame_on_parent;
487  if (X_PF) {
488  frame_on_parent = &this->AddFrame<FixedOffsetFrame>(parent, *X_PF);
489  } else {
490  frame_on_parent = &parent.get_body_frame();
491  }
492 
493  const Frame<T>* frame_on_child;
494  if (X_BM) {
495  frame_on_child = &this->AddFrame<FixedOffsetFrame>(child, *X_BM);
496  } else {
497  frame_on_child = &child.get_body_frame();
498  }
499 
500  return AddJoint(
501  std::make_unique<JointType<T>>(
502  name,
503  *frame_on_parent, *frame_on_child,
504  std::forward<Args>(args)...));
505  }
506  /// @}
507  // Closes Doxygen section.
508 
509  /// Returns the number of Frame objects in the MultibodyTree.
510  /// Frames include body frames associated with each of the bodies in
511  /// the %MultibodyTree including the _world_ body. Therefore the minimum
512  /// number of frames in a %MultibodyTree is one.
513  int get_num_frames() const {
514  return static_cast<int>(frames_.size());
515  }
516 
517  /// Returns the number of bodies in the %MultibodyTree including the *world*
518  /// body. Therefore the minimum number of bodies in a MultibodyTree is one.
519  int get_num_bodies() const { return static_cast<int>(owned_bodies_.size()); }
520 
521  /// Returns the number of joints added with AddJoint() to the %MultibodyTree.
522  int get_num_joints() const { return static_cast<int>(owned_joints_.size()); }
523 
524  /// Returns the number of mobilizers in the %MultibodyTree. Since the world
525  /// has no Mobilizer, the number of mobilizers equals the number of bodies
526  /// minus one, i.e. get_num_mobilizers() returns get_num_bodies() - 1.
527  // TODO(amcastro-tri): Consider adding a WorldMobilizer (0-dofs) for the world
528  // body. This could be useful to query for reaction forces of the entire
529  // model.
530  int get_num_mobilizers() const {
531  return static_cast<int>(owned_mobilizers_.size());
532  }
533 
534  /// Returns the number of ForceElement objects in the MultibodyTree.
536  return static_cast<int>(owned_force_elements_.size());
537  }
538 
539  /// Returns the number of generalized positions of the model.
540  int get_num_positions() const {
541  return topology_.get_num_positions();
542  }
543 
544  /// Returns the number of generalized velocities of the model.
545  int get_num_velocities() const {
546  return topology_.get_num_velocities();
547  }
548 
549  /// Returns the total size of the state vector in the model.
550  int get_num_states() const {
551  return topology_.get_num_states();
552  }
553 
554  /// Returns the height of the tree data structure of `this` %MultibodyTree.
555  /// That is, the number of bodies in the longest kinematic path between the
556  /// world and any other leaf body. For a model that only contains the _world_
557  /// body, the height of the tree is one.
558  /// Kinematic paths are created by Mobilizer objects connecting a chain of
559  /// frames. Therefore, this method does not count kinematic cycles, which
560  /// could only be considered in the model using constraints.
561  int get_tree_height() const {
562  return topology_.get_tree_height();
563  }
564 
565  /// Returns a constant reference to the *world* body.
566  const RigidBody<T>& get_world_body() const {
567  // world_body_ is set in the constructor. So this assert is here only to
568  // verify future constructors do not mess that up.
569  DRAKE_ASSERT(world_body_ != nullptr);
570  return *world_body_;
571  }
572 
573  /// Returns a constant reference to the *world* frame.
574  const BodyFrame<T>& get_world_frame() const {
575  return owned_bodies_[world_index()]->get_body_frame();
576  }
577 
578  /// Returns a constant reference to the body with unique index `body_index`.
579  /// This method aborts in Debug builds when `body_index` does not correspond
580  /// to a body in this multibody tree.
581  const Body<T>& get_body(BodyIndex body_index) const {
582  DRAKE_ASSERT(body_index < get_num_bodies());
583  return *owned_bodies_[body_index];
584  }
585 
586  /// Returns a constant reference to the frame with unique index `frame_index`.
587  /// This method aborts in Debug builds when `frame_index` does not correspond
588  /// to a frame in `this` multibody tree.
589  const Frame<T>& get_frame(FrameIndex frame_index) const {
590  DRAKE_ASSERT(frame_index < get_num_frames());
591  return *frames_[frame_index];
592  }
593 
594  /// Returns a constant reference to the mobilizer with unique index
595  /// `mobilizer_index`. This method aborts in Debug builds when
596  /// `mobilizer_index` does not correspond to a mobilizer in this multibody
597  /// tree.
598  const Mobilizer<T>& get_mobilizer(MobilizerIndex mobilizer_index) const {
599  DRAKE_ASSERT(mobilizer_index < get_num_mobilizers());
600  return *owned_mobilizers_[mobilizer_index];
601  }
602 
603  /// Returns `true` if this %MultibodyTree was finalized with Finalize() after
604  /// all multibody elements were added, and `false` otherwise.
605  /// When a %MultibodyTree is instantiated, its topology remains invalid until
606  /// Finalize() is called, which validates the topology.
607  /// @see Finalize().
608  bool topology_is_valid() const { return topology_.is_valid(); }
609 
610  /// Returns the topology information for this multibody tree. Users should not
611  /// need to call this method since MultibodyTreeTopology is an internal
612  /// bookkeeping detail. Used at Finalize() stage by multibody elements to
613  /// retrieve a local copy of their topology.
614  const MultibodyTreeTopology& get_topology() const { return topology_; }
615 
616  /// This method must be called after all elements in the tree (joints, bodies,
617  /// force elements, constraints) were added and before any computations are
618  /// performed.
619  /// It essentially compiles all the necessary "topological information", i.e.
620  /// how bodies, joints and, any other elements connect with each other, and
621  /// performs all the required pre-processing to perform computations at a
622  /// later stage.
623  ///
624  /// If the finalize stage is successful, the topology of this %MultibodyTree
625  /// is validated, meaning that the topology is up-to-date after this call.
626  /// No more multibody tree elements can be added after a call to Finalize().
627  ///
628  /// @throws std::logic_error If users attempt to call this method on an
629  /// already finalized %MultibodyTree.
630  // TODO(amcastro-tri): Consider making this method private and calling it
631  // automatically when CreateDefaultContext() is called.
632  void Finalize();
633 
634  /// Allocates a new context for this %MultibodyTree uniquely identifying the
635  /// state of the multibody system.
636  ///
637  /// @pre The method Finalize() must be called before attempting to create a
638  /// context in order for the %MultibodyTree topology to be valid at the moment
639  /// of allocation.
640  ///
641  /// @throws std::logic_error If users attempt to call this method on a
642  /// %MultibodyTree with an invalid topology.
643  std::unique_ptr<systems::LeafContext<T>> CreateDefaultContext() const;
644 
645  /// Sets default values in the context. For mobilizers, this method sets them
646  /// to their _zero_ configuration according to
647  /// Mobilizer::set_zero_configuration().
648  void SetDefaultContext(systems::Context<T>* context) const;
649 
650  /// Sets default values in the `state`. For mobilizers, this method sets them
651  /// to their _zero_ configuration according to
652  /// Mobilizer::set_zero_configuration().
653  void SetDefaultState(const systems::Context<T>& context,
654  systems::State<T>* state) const;
655 
656  /// @name Computational methods
657  /// These methods expose the computational capabilities of MultibodyTree to
658  /// compute kinematics, forward and inverse dynamics, and Jacobian matrices,
659  /// among others.
660  /// These methods follow Drake's naming scheme for methods performing a
661  /// computation and therefore are named `CalcXXX()`, where `XXX` corresponds
662  /// to the quantity or object of interest to be computed. They all take a
663  /// `systems::Context` as an input argument storing the state of the multibody
664  /// system. A `std::bad_cast` exception is thrown if the passed context is not
665  /// a MultibodyTreeContext.
666  /// @{
667 
668  /// Computes into the position kinematics `pc` all the kinematic quantities
669  /// that depend on the generalized positions only. These include:
670  /// - For each body B, the pose `X_BF` of each of the frames F attached to
671  /// body B.
672  /// - Pose `X_WB` of each body B in the model as measured and expressed in
673  /// the world frame W.
674  /// - Across-mobilizer Jacobian matrices `H_FM` and `H_PB_W`.
675  /// - Body specific quantities such as `com_W` and `M_Bo_W`.
676  ///
677  /// Aborts if `pc` is nullptr.
679  const systems::Context<T>& context,
680  PositionKinematicsCache<T>* pc) const;
681 
682  /// Computes all the kinematic quantities that depend on the generalized
683  /// velocities and stores them in the velocity kinematics cache `vc`.
684  /// These include:
685  /// - Spatial velocity `V_WB` for each body B in the model as measured and
686  /// expressed in the world frame W.
687  /// - Spatial velocity `V_PB` for each body B in the model as measured and
688  /// expressed in the inboard (or parent) body frame P.
689  ///
690  /// @pre The position kinematics `pc` must have been previously updated with a
691  /// call to CalcPositionKinematicsCache().
692  ///
693  /// Aborts if `vc` is nullptr.
695  const systems::Context<T>& context,
696  const PositionKinematicsCache<T>& pc,
697  VelocityKinematicsCache<T>* vc) const;
698 
699  /// Computes all the kinematic quantities that depend on the generalized
700  /// accelerations that is, the generalized velocities' time derivatives, and
701  /// stores them in the acceleration kinematics cache `ac`.
702  /// These include:
703  /// - Spatial acceleration `A_WB` for each body B in the model as measured and
704  /// expressed in the world frame W.
705  ///
706  /// @param[in] context
707  /// The context containing the state of the %MultibodyTree model.
708  /// @param[in] pc
709  /// A position kinematics cache object already updated to be in sync with
710  /// `context`.
711  /// @param[in] vc
712  /// A velocity kinematics cache object already updated to be in sync with
713  /// `context`.
714  /// @param[in] known_vdot
715  /// A vector with the generalized accelerations for the full %MultibodyTree
716  /// model.
717  /// @param[out] ac
718  /// A pointer to a valid, non nullptr, acceleration kinematics cache. This
719  /// method aborts if `ac` is nullptr.
720  ///
721  /// @pre The position kinematics `pc` must have been previously updated with a
722  /// call to CalcPositionKinematicsCache().
723  /// @pre The velocity kinematics `vc` must have been previously updated with a
724  /// call to CalcVelocityKinematicsCache().
726  const systems::Context<T>& context,
727  const PositionKinematicsCache<T>& pc,
728  const VelocityKinematicsCache<T>& vc,
729  const VectorX<T>& known_vdot,
731 
732  /// Given the state of `this` %MultibodyTree in `context` and a known vector
733  /// of generalized accelerations `known_vdot`, this method computes the
734  /// spatial acceleration `A_WB` for each body as measured and expressed in the
735  /// world frame W.
736  ///
737  /// @param[in] context
738  /// The context containing the state of the %MultibodyTree model.
739  /// @param[in] pc
740  /// A position kinematics cache object already updated to be in sync with
741  /// `context`.
742  /// @param[in] vc
743  /// A velocity kinematics cache object already updated to be in sync with
744  /// `context`.
745  /// @param[in] known_vdot
746  /// A vector with the generalized accelerations for the full %MultibodyTree
747  /// model.
748  /// @param[out] A_WB_array
749  /// A pointer to a valid, non nullptr, vector of spatial accelerations
750  /// containing the spatial acceleration `A_WB` for each body. It must be of
751  /// size equal to the number of bodies in the MultibodyTree. This method
752  /// will abort if the the pointer is null or if `A_WB_array` is not of size
753  /// `get_num_bodies()`. On output, entries will be ordered by BodyNodeIndex.
754  /// These accelerations can be read in the proper order with
755  /// Body::get_from_spatial_acceleration_array().
756  ///
757  /// @pre The position kinematics `pc` must have been previously updated with a
758  /// call to CalcPositionKinematicsCache().
759  /// @pre The velocity kinematics `vc` must have been previously updated with a
760  /// call to CalcVelocityKinematicsCache().
762  const systems::Context<T>& context,
763  const PositionKinematicsCache<T>& pc,
764  const VelocityKinematicsCache<T>& vc,
765  const VectorX<T>& known_vdot,
766  std::vector<SpatialAcceleration<T>>* A_WB_array) const;
767 
768  /// Given the state of `this` %MultibodyTree in `context` and a known vector
769  /// of generalized accelerations `vdot`, this method computes the
770  /// set of generalized forces `tau` that would need to be applied at each
771  /// Mobilizer in order to attain the specified generalized accelerations.
772  /// Mathematically, this method computes: <pre>
773  /// tau = M(q)v̇ + C(q, v)v - tau_app - ∑ J_WBᵀ(q) Fapp_Bo_W
774  /// </pre>
775  /// where `M(q)` is the %MultibodyTree mass matrix, `C(q, v)v` is the bias
776  /// term containing Coriolis and gyroscopic effects and `tau_app` consists
777  /// of a vector applied generalized forces. The last term is a summation over
778  /// all bodies in the model where `Fapp_Bo_W` is an applied spatial force on
779  /// body B at `Bo` which gets projected into the space of generalized forces
780  /// with the geometric Jacobian `J_WB(q)` which maps generalized velocities
781  /// into body B spatial velocity as `V_WB = J_WB(q)v`.
782  /// This method does not compute explicit expressions for the mass matrix nor
783  /// for the bias term, which would be of at least `O(n²)` complexity, but it
784  /// implements an `O(n)` Newton-Euler recursive algorithm, where n is the
785  /// number of bodies in the %MultibodyTree. The explicit formation of the
786  /// mass matrix `M(q)` would require the calculation of `O(n²)` entries while
787  /// explicitly forming the product `C(q, v) * v` could require up to `O(n³)`
788  /// operations (see [Featherstone 1987, §4]), depending on the implementation.
789  /// The recursive Newton-Euler algorithm is the most efficient currently known
790  /// general method for solving inverse dynamics [Featherstone 2008].
791  ///
792  /// @param[in] context
793  /// The context containing the state of the %MultibodyTree model.
794  /// @param[in] pc
795  /// A position kinematics cache object already updated to be in sync with
796  /// `context`.
797  /// @param[in] vc
798  /// A velocity kinematics cache object already updated to be in sync with
799  /// `context`.
800  /// @param[in] known_vdot
801  /// A vector with the known generalized accelerations `vdot` for the full
802  /// %MultibodyTree model. Use Mobilizer::get_accelerations_from_array() to
803  /// access entries into this array for a particular Mobilizer. You can use
804  /// the mutable version of this method to write into this array.
805  /// @param[in] Fapplied_Bo_W_array
806  /// A vector containing the spatial force `Fapplied_Bo_W` applied on each
807  /// body at the body's frame origin `Bo` and expressed in the world frame W.
808  /// `Fapplied_Bo_W_array` can have zero size which means there are no
809  /// applied forces. To apply non-zero forces, `Fapplied_Bo_W_array` must be
810  /// of size equal to the number of bodies in `this` %MultibodyTree model.
811  /// This array must be ordered by BodyNodeIndex, which for a given body can
812  /// be retrieved with Body::get_node_index().
813  /// This method will abort if provided with an array that does not have a
814  /// size of either `get_num_bodies()` or zero.
815  /// @param[in] tau_applied_array
816  /// An array of applied generalized forces for the entire model. For a
817  /// given mobilizer, entries in this array can be accessed using the method
818  /// Mobilizer::get_generalized_forces_from_array() while its mutable
819  /// counterpart, Mobilizer::get_mutable_generalized_forces_from_array(),
820  /// allows writing into this array.
821  /// `tau_applied_array` can have zero size, which means there are no applied
822  /// forces. To apply non-zero forces, `tau_applied_array` must be of size
823  /// equal to the number to the number of generalized velocities in the
824  /// model, see MultibodyTree::get_num_velocities().
825  /// This method will abort if provided with an array that does not have a
826  /// size of either MultibodyTree::get_num_velocities() or zero.
827  /// @param[out] A_WB_array
828  /// A pointer to a valid, non nullptr, vector of spatial accelerations
829  /// containing the spatial acceleration `A_WB` for each body. It must be of
830  /// size equal to the number of bodies. This method will abort if the the
831  /// pointer is null or if `A_WB_array` is not of size `get_num_bodies()`.
832  /// On output, entries will be ordered by BodyNodeIndex.
833  /// To access the acceleration `A_WB` of given body B in this array, use the
834  /// index returned by Body::get_node_index().
835  /// @param[out] F_BMo_W_array
836  /// A pointer to a valid, non nullptr, vector of spatial forces
837  /// containing, for each body B, the spatial force `F_BMo_W` corresponding
838  /// to its inboard mobilizer reaction forces on body B applied at the origin
839  /// `Mo` of the inboard mobilizer, expressed in the world frame W.
840  /// It must be of size equal to the number of bodies in the MultibodyTree.
841  /// This method will abort if the the pointer is null or if `F_BMo_W_array`
842  /// is not of size `get_num_bodies()`.
843  /// On output, entries will be ordered by BodyNodeIndex.
844  /// To access a mobilizer's reaction force on given body B in this array,
845  /// use the index returned by Body::get_node_index().
846  /// @param[out] tau_array
847  /// On output this array will contain the generalized forces that must be
848  /// applied in order to achieve the desired generalized accelerations given
849  /// by the input argument `known_vdot`. It must not be nullptr and it
850  /// must be of size MultibodyTree::get_num_velocities(). Generalized forces
851  /// for each Mobilizer can be accessed with
852  /// Mobilizer::get_generalized_forces_from_array().
853  ///
854  /// @warning There is no mechanism to assert that either `A_WB_array` nor
855  /// `F_BMo_W_array` are ordered by BodyNodeIndex. You can use
856  /// Body::get_node_index() to obtain the node index for a given body.
857  ///
858  /// @note This method uses `F_BMo_W_array` and `tau_array` as the only local
859  /// temporaries and therefore no additional dynamic memory allocation is
860  /// performed.
861  ///
862  /// @warning `F_BMo_W_array` (`tau_array`) and `Fapplied_Bo_W_array`
863  /// (`tau_applied_array`) can actually be the same
864  /// array in order to reduce memory footprint and/or dynamic memory
865  /// allocations. However the information in `Fapplied_Bo_W_array`
866  /// (`tau_applied_array`) would be overwritten through `F_BMo_W_array`
867  /// (`tau_array`). Make a copy if data must be preserved.
868  ///
869  /// @pre The position kinematics `pc` must have been previously updated with a
870  /// call to CalcPositionKinematicsCache().
871  /// @pre The velocity kinematics `vc` must have been previously updated with a
872  /// call to CalcVelocityKinematicsCache().
873  void CalcInverseDynamics(
874  const systems::Context<T>& context,
875  const PositionKinematicsCache<T>& pc,
876  const VelocityKinematicsCache<T>& vc,
877  const VectorX<T>& known_vdot,
878  const std::vector<SpatialForce<T>>& Fapplied_Bo_W_array,
879  const Eigen::Ref<const VectorX<T>>& tau_applied_array,
880  std::vector<SpatialAcceleration<T>>* A_WB_array,
881  std::vector<SpatialForce<T>>* F_BMo_W_array,
882  EigenPtr<VectorX<T>> tau_array) const;
883 
884  /// Computes the combined force contribution of ForceElement objects in the
885  /// model. A ForceElement can apply forcing as a spatial force per body or as
886  /// generalized forces, depending on the ForceElement model. Therefore this
887  /// method provides outputs for both spatial forces per body (with
888  /// `F_Bo_W_array`) and generalized forces (with `tau_array`).
889  /// ForceElement contributions are a function of the state and time only.
890  /// The output from this method can immediately be used as input to
891  /// CalcInverseDynamics() to include the effect of applied forces by force
892  /// elements.
893  ///
894  /// @param[in] context
895  /// The context containing the state of the %MultibodyTree model.
896  /// @param[in] pc
897  /// A position kinematics cache object already updated to be in sync with
898  /// `context`.
899  /// @param[in] vc
900  /// A velocity kinematics cache object already updated to be in sync with
901  /// `context`.
902  /// @param[out] F_Bo_W_array
903  /// A pointer to a valid, non nullptr, vector of spatial forces
904  /// containing, for each body B, the total spatial force `F_Bo_W` applied at
905  /// Bo by the force elements in `this` model, expressed in the world frame
906  /// W. It must be of size equal to the number of bodies in the
907  /// MultibodyTree. This method will abort if the the pointer is null or if
908  /// `F_Bo_W_array` is not of size `get_num_bodies()`.
909  /// On output, entries will be ordered by BodyNodeIndex.
910  /// To access a mobilizer's reaction force on given body B in this array,
911  /// use the index returned by Body::get_node_index().
912  /// @param[out] tau_array
913  /// On output this array will contain the generalized forces contribution
914  /// applied by the force elements in `this` model. It must not be nullptr
915  /// and it must be of size MultibodyTree::get_num_velocities() or this
916  /// method will abort.
917  /// Generalized forces for each Mobilizer can be accessed with
918  /// Mobilizer::get_generalized_forces_from_array().
919  ///
920  /// @pre The position kinematics `pc` must have been previously updated with a
921  /// call to CalcPositionKinematicsCache().
922  /// @pre The velocity kinematics `vc` must have been previously updated with a
923  /// call to CalcVelocityKinematicsCache().
925  const systems::Context<T>& context,
926  const PositionKinematicsCache<T>& pc,
927  const VelocityKinematicsCache<T>& vc,
928  std::vector<SpatialForce<T>>* F_Bo_W_array,
929  EigenPtr<VectorX<T>> tau_array) const;
930 
931  /// Computes and returns the total potential energy stored in `this` multibody
932  /// model for the configuration given by `context`.
933  /// @param[in] context
934  /// The context containing the state of the %MultibodyTree model.
935  /// @returns The total potential energy stored in `this` multibody model.
936  T CalcPotentialEnergy(const systems::Context<T>& context) const;
937 
938  /// Computes and returns the power generated by conservative forces in the
939  /// multibody model. This quantity is defined to be positive when the
940  /// potential energy is decreasing. In other words, if `U(q)` is the potential
941  /// energy as defined by CalcPotentialEnergy(), then the conservative power,
942  /// `Pc`, is `Pc = -U̇(q)`.
943  ///
944  /// @see CalcPotentialEnergy()
945  T CalcConservativePower(const systems::Context<T>& context) const;
946 
947  /// Performs the computation of the mass matrix `M(q)` of the model using
948  /// inverse dynamics, where the generalized positions q are stored in
949  /// `context`. See CalcInverseDynamics().
950  ///
951  /// @param[in] context
952  /// The context containing the state of the %MultibodyTree model.
953  /// @param[out] H
954  /// A valid (non-null) pointer to a squared matrix in `ℛⁿˣⁿ` with n the
955  /// number of generalized velocities (get_num_velocities()) of the model.
956  /// This method aborts if H is nullptr or if it does not have the proper
957  /// size.
958  ///
959  /// The algorithm used to build `M(q)` consists in computing one column of
960  /// `M(q)` at a time using inverse dynamics. The result from inverse dynamics,
961  /// with no applied forces, is the vector of generalized forces: <pre>
962  /// tau = M(q)v̇ + C(q, v)v
963  /// </pre>
964  /// where q and v are the generalized positions and velocities, respectively.
965  /// When `v = 0` the Coriolis and gyroscopic forces term `C(q, v)v` is zero.
966  /// Therefore the `i-th` column of `M(q)` can be obtained performing inverse
967  /// dynamics with an acceleration vector `v̇ = eᵢ`, with `eᵢ` the standard
968  /// (or natural) basis of `ℛⁿ` with n the number of generalized velocities.
969  /// We write this as: <pre>
970  /// H.ᵢ(q) = M(q) * e_i
971  /// </pre>
972  /// where `H.ᵢ(q)` (notice the dot for the rows index) denotes the `i-th`
973  /// column in M(q).
974  ///
975  /// @warning This is an O(n²) algorithm. Avoid the explicit computation of the
976  /// mass matrix whenever possible.
978  const systems::Context<T>& context, EigenPtr<MatrixX<T>> H) const;
979 
980  /// Computes the bias term `C(q, v)v` containing Coriolis and gyroscopic
981  /// effects of the multibody equations of motion: <pre>
982  /// M(q)v̇ + C(q, v)v = tau_app + ∑ J_WBᵀ(q) Fapp_Bo_W
983  /// </pre>
984  /// where `M(q)` is the multibody model's mass matrix and `tau_app` consists
985  /// of a vector applied generalized forces. The last term is a summation over
986  /// all bodies in the model where `Fapp_Bo_W` is an applied spatial force on
987  /// body B at `Bo` which gets projected into the space of generalized forces
988  /// with the geometric Jacobian `J_WB(q)` which maps generalized velocities
989  /// into body B spatial velocity as `V_WB = J_WB(q)v`.
990  ///
991  /// @param[in] context
992  /// The context containing the state of the %MultibodyTree model. It stores
993  /// the generalized positions q and the generalized velocities v.
994  /// @param[out] Cv
995  /// On output, `Cv` will contain the product `C(q, v)v`. It must be a valid
996  /// (non-null) pointer to a column vector in `ℛⁿ` with n the number of
997  /// generalized velocities (get_num_velocities()) of the model.
998  /// This method aborts if Cv is nullptr or if it does not have the
999  /// proper size.
1000  void CalcBiasTerm(
1001  const systems::Context<T>& context, EigenPtr<VectorX<T>> Cv) const;
1002 
1003  /// Computes the relative transform `X_AB(q)` from a frame B to a frame A, as
1004  /// a function of the generalized positions q of the model.
1005  /// That is, the position `p_AQ` of a point Q measured and expressed in
1006  /// frame A can be computed from the position `p_BQ` of this point measured
1007  /// and expressed in frame B using the transformation `p_AQ = X_AB⋅p_BQ`.
1008  ///
1009  /// @param[in] context
1010  /// The context containing the state of the %MultibodyTree model. It stores
1011  /// the generalized positions q of the model.
1012  /// @param[in] to_frame_A
1013  /// The target frame A in the computed relative transform `X_AB`.
1014  /// @param[in] from_frame_B
1015  /// The source frame B in the computed relative transform `X_AB`.
1016  /// @retval X_AB
1017  /// The relative transform from frame B to frame A, such that
1018  /// `p_AQ = X_AB⋅p_BQ`.
1020  const systems::Context<T>& context,
1021  const Frame<T>& to_frame_A, const Frame<T>& from_frame_B) const;
1022 
1023  /// Given the positions `p_BQi` for a set of points `Qi` measured and
1024  /// expressed in a frame B, this method computes the positions `p_AQi(q)` of
1025  /// each point `Qi` in the set as measured and expressed in another frame A,
1026  /// as a function of the generalized positions q of the model.
1027  ///
1028  /// @param[in] context
1029  /// The context containing the state of the %MultibodyTree model. It stores
1030  /// the generalized positions q of the model.
1031  /// @param[in] from_frame_B
1032  /// The frame B in which the positions `p_BQi` of a set of points `Qi` are
1033  /// given.
1034  /// @param[in] p_BQi
1035  /// The input positions of each point `Qi` in frame B. `p_BQi ∈ ℝ³ˣⁿᵖ` with
1036  /// `np` the number of points in the set. Each column of `p_BQi` corresponds
1037  /// to a vector in ℝ³ holding the position of one of the points in the set
1038  /// as measured and expressed in frame B.
1039  /// @param[in] to_frame_A
1040  /// The frame A in which it is desired to compute the positions `p_AQi` of
1041  /// each point `Qi` in the set.
1042  /// @param[out] p_AQi
1043  /// The output positions of each point `Qi` now computed as measured and
1044  /// expressed in frame A. The output `p_AQi` **must** have the same size as
1045  /// the input `p_BQi` or otherwise this method aborts. That is `p_AQi`
1046  /// **must** be in `ℝ³ˣⁿᵖ`.
1047  ///
1048  /// @note Both `p_BQi` and `p_AQi` must have three rows. Otherwise this
1049  /// method will throw a std::runtime_error exception. This method also throws
1050  /// a std::runtime_error exception if `p_BQi` and `p_AQi` differ in the number
1051  /// of columns.
1052  void CalcPointsPositions(
1053  const systems::Context<T>& context,
1054  const Frame<T>& from_frame_B,
1055  const Eigen::Ref<const MatrixX<T>>& p_BQi,
1056  const Frame<T>& to_frame_A,
1057  EigenPtr<MatrixX<T>> p_AQi) const;
1058 
1059  /// @name Methods to compute multibody Jacobians.
1060  /// @{
1061 
1062  /// Given a set of points `Qi` with fixed position vectors `p_BQi` in a frame
1063  /// B, (that is, their time derivative `ᴮd/dt(p_BQi)` in frame B is zero),
1064  /// this method computes the geometric Jacobian `J_WQi` defined by:
1065  /// <pre>
1066  /// J_WQi(q) = d(v_WQi(q, v))/dv
1067  /// </pre>
1068  /// where `p_WQi` is the position vector in the world frame for each point
1069  /// `Qi` in the input set, `v_WQi` is the translational velocity of point `Qi`
1070  /// in the world frame W and v is the vector of generalized velocities. Since
1071  /// the spatial velocity of each point `Qi` is linear in the generalized
1072  /// velocities, the geometric Jacobian `J_WQi` is a function of the
1073  /// generalized coordinates q only.
1074  ///
1075  /// @param[in] context
1076  /// The context containing the state of the %MultibodyTree model. It stores
1077  /// the generalized positions q.
1078  /// @param[in] frame_B
1079  /// The positions `p_BQi` of each point in the input set are measured and
1080  /// expressed in this frame B and are constant (fixed) in this frame.
1081  /// @param[in] p_BQi_set
1082  /// A matrix with the fixed position of a set of points `Qi` measured and
1083  /// expressed in `frame_B`.
1084  /// Each column of this matrix contains the position vector `p_BQi` for a
1085  /// point `Qi` measured and expressed in frame B. Therefore this input
1086  /// matrix lives in ℝ³ˣⁿᵖ with `np` the number of points in the set.
1087  /// @param[out] p_WQi_set
1088  /// The output positions of each point `Qi` now computed as measured and
1089  /// expressed in frame W. These positions are computed in the process of
1090  /// computing the geometric Jacobian `J_WQi` and therefore external storage
1091  /// must be provided.
1092  /// The output `p_WQi_set` **must** have the same size
1093  /// as the input set `p_BQi_set` or otherwise this method throws a
1094  /// std::runtime_error exception. That is `p_WQi_set` **must** be in
1095  /// `ℝ³ˣⁿᵖ`.
1096  /// @param[out] J_WQi
1097  /// The geometric Jacobian `J_WQi(q)`, function of the generalized positions
1098  /// q only. This Jacobian relates the translational velocity `v_WQi` of
1099  /// each point `Qi` in the input set by: <pre>
1100  /// `v_WQi(q, v) = J_WQi(q)⋅v`
1101  /// </pre>
1102  /// so that `v_WQi` is a column vector of size `3⋅np` concatenating the
1103  /// velocity of all points `Qi` in the same order they were given in the
1104  /// input set. Therefore `J_WQi` is a matrix of size `3⋅np x nv`, with `nv`
1105  /// the number of generalized velocities. On input, matrix `J_WQi` **must**
1106  /// have size `3⋅np x nv` or this method throws a std::runtime_error
1107  /// exception.
1109  const systems::Context<T>& context,
1110  const Frame<T>& frame_B, const Eigen::Ref<const MatrixX<T>>& p_BQi_set,
1111  EigenPtr<MatrixX<T>> p_WQi_set, EigenPtr<MatrixX<T>> J_WQi) const;
1112 
1113  /// @}
1114  // End of multibody Jacobian methods section.
1115 
1116  /// Transforms generalized velocities v to time derivatives `qdot` of the
1117  /// generalized positions vector `q` (stored in `context`). `v` and `qdot`
1118  /// are related linearly by `q̇ = N(q)⋅v`.
1119  /// Using the configuration `q` stored in the given `context` this method
1120  /// calculates `q̇ = N(q)⋅v`.
1121  ///
1122  /// @param[in] context
1123  /// The context containing the state of the %MultibodyTree model.
1124  /// @param[in] v
1125  /// A vector of of generalized velocities for `this` %MultibodyTree model.
1126  /// This method aborts if v is not of size get_num_velocities().
1127  /// @param[out] qdot
1128  /// A valid (non-null) pointer to a vector in `ℝⁿ` with n being the number
1129  /// of generalized positions in `this` %MultibodyTree model,
1130  /// given by `get_num_positions()`. This method aborts if `qdot` is nullptr
1131  /// or if it is not of size get_num_positions().
1132  ///
1133  /// @see MapQDotToVelocity()
1134  /// @see Mobilizer::MapVelocityToQDot()
1135  void MapVelocityToQDot(
1136  const systems::Context<T>& context,
1137  const Eigen::Ref<const VectorX<T>>& v,
1138  EigenPtr<VectorX<T>> qdot) const;
1139 
1140  /// Transforms the time derivative `qdot` of the generalized positions vector
1141  /// `q` (stored in `context`) to generalized velocities `v`. `v` and `q̇`
1142  /// are related linearly by `q̇ = N(q)⋅v`. Although `N(q)` is not
1143  /// necessarily square, its left pseudo-inverse `N⁺(q)` can be used to
1144  /// invert that relationship without residual error, provided that `qdot` is
1145  /// in the range space of `N(q)` (that is, if it *could* have been produced as
1146  /// `q̇ = N(q)⋅v` for some `v`).
1147  /// Using the configuration `q` stored in the given `context` this method
1148  /// calculates `v = N⁺(q)⋅q̇`.
1149  ///
1150  /// @param[in] context
1151  /// The context containing the state of the %MultibodyTree model.
1152  /// @param[in] qdot
1153  /// A vector containing the time derivatives of the generalized positions.
1154  /// This method aborts if `qdot` is not of size get_num_positions().
1155  /// @param[out] v
1156  /// A valid (non-null) pointer to a vector in `ℛⁿ` with n the number of
1157  /// generalized velocities. This method aborts if v is nullptr or if it
1158  /// is not of size get_num_velocities().
1159  ///
1160  /// @see MapVelocityToQDot()
1161  /// @see Mobilizer::MapQDotToVelocity()
1162  void MapQDotToVelocity(
1163  const systems::Context<T>& context,
1164  const Eigen::Ref<const VectorX<T>>& qdot,
1165  EigenPtr<VectorX<T>> v) const;
1166 
1167  /// @}
1168  // Closes "Computational methods" Doxygen section.
1169 
1170  /// @name Methods to retrieve multibody element variants
1171  ///
1172  /// Given two variants of the same %MultibodyTree, these methods map an
1173  /// element in one variant, to its corresponding element in the other variant.
1174  ///
1175  /// A concrete case is the call to ToAutoDiffXd() to obtain a
1176  /// %MultibodyTree variant templated on AutoDiffXd from a %MultibodyTree
1177  /// templated on `double`. Typically, a user holding a `Body<double>` (or any
1178  /// other multibody element in the original variant templated on `double`)
1179  /// would like to retrieve the corresponding `Body<AutoDiffXd>` variant from
1180  /// the new AutoDiffXd tree variant.
1181  ///
1182  /// Consider the following code example:
1183  /// @code
1184  /// // The user creates a model.
1185  /// MultibodyTree<double> model;
1186  /// // User adds a body and keeps a reference to it.
1187  /// const RigidBody<double>& body = model.AddBody<RigidBody>(...);
1188  /// // User creates an AutoDiffXd variant. Variants on other scalar types
1189  /// // can be created with a call to CloneToScalar().
1190  /// std::unique_ptr<MultibodyTree<Tvariant>> variant_model =
1191  /// model.ToAutoDiffXd();
1192  /// // User retrieves the AutoDiffXd variant corresponding to the original
1193  /// // body added above.
1194  /// const RigidBody<AutoDiffXd>&
1195  /// variant_body = variant_model.get_variant(body);
1196  /// @endcode
1197  ///
1198  /// MultibodyTree::get_variant() is templated on the multibody element
1199  /// type which is deduced from its only input argument. The returned element
1200  /// is templated on the scalar type T of the %MultibodyTree on which this
1201  /// method is invoked.
1202  /// @{
1203 
1204  /// SFINAE overload for Frame<T> elements.
1205  template <template <typename> class MultibodyElement, typename Scalar>
1206  std::enable_if_t<std::is_base_of<Frame<T>, MultibodyElement<T>>::value,
1207  const MultibodyElement<T>&> get_variant(
1208  const MultibodyElement<Scalar>& element) const {
1209  return get_frame_variant(element);
1210  }
1211 
1212  /// SFINAE overload for Body<T> elements.
1213  template <template <typename> class MultibodyElement, typename Scalar>
1214  std::enable_if_t<std::is_base_of<Body<T>, MultibodyElement<T>>::value,
1215  const MultibodyElement<T>&> get_variant(
1216  const MultibodyElement<Scalar>& element) const {
1217  return get_body_variant(element);
1218  }
1219 
1220  /// SFINAE overload for Mobilizer<T> elements.
1221  template <template <typename> class MultibodyElement, typename Scalar>
1222  std::enable_if_t<std::is_base_of<Mobilizer<T>, MultibodyElement<T>>::value,
1223  const MultibodyElement<T>&> get_variant(
1224  const MultibodyElement<Scalar>& element) const {
1225  return get_mobilizer_variant(element);
1226  }
1227 
1228  /// SFINAE overload for Joint<T> elements.
1229  template <template <typename> class MultibodyElement, typename Scalar>
1230  std::enable_if_t<std::is_base_of<Joint<T>, MultibodyElement<T>>::value,
1231  const MultibodyElement<T>&> get_variant(
1232  const MultibodyElement<Scalar>& element) const {
1233  return get_joint_variant(element);
1234  }
1235  /// @}
1236 
1237  /// Creates a deep copy of `this` %MultibodyTree templated on the same
1238  /// scalar type T as `this` tree.
1239  std::unique_ptr<MultibodyTree<T>> Clone() const {
1240  return CloneToScalar<T>();
1241  }
1242 
1243  /// Creates a deep copy of `this` %MultibodyTree templated on AutoDiffXd.
1244  std::unique_ptr<MultibodyTree<AutoDiffXd>> ToAutoDiffXd() const {
1245  return CloneToScalar<AutoDiffXd>();
1246  }
1247 
1248  /// Creates a deep copy of `this` %MultibodyTree templated on the scalar type
1249  /// `ToScalar`.
1250  /// The new deep copy is guaranteed to have exactly the same
1251  /// MultibodyTreeTopology as the original tree the method is called on.
1252  /// This method ensures the following cloning order:
1253  /// - Body objects, and their corresponding BodyFrame objects.
1254  /// - Frame objects.
1255  /// - If a Frame is attached to another frame, its parent frame is
1256  /// guaranteed to be created first.
1257  /// - Mobilizer objects are created last and therefore clones of the
1258  /// original Frame objects are guaranteed to already be part of the cloned
1259  /// tree.
1260  ///
1261  /// Consider the following code example:
1262  /// @code
1263  /// // The user creates a model.
1264  /// MultibodyTree<double> model;
1265  /// // User adds a body and keeps a reference to it.
1266  /// const RigidBody<double>& body = model.AddBody<RigidBody>(...);
1267  /// // User creates an AutoDiffXd variant, where ToScalar = AutoDiffXd.
1268  /// std::unique_ptr<MultibodyTree<AutoDiffXd>> model_autodiff =
1269  /// model.CloneToScalar<AutoDiffXd>();
1270  /// // User retrieves the AutoDiffXd variant corresponding to the original
1271  /// // body added above.
1272  /// const RigidBody<AutoDiffXd>&
1273  /// body_autodiff = model_autodiff.get_variant(body);
1274  /// @endcode
1275  ///
1276  /// MultibodyTree::get_variant() is templated on the multibody element
1277  /// type which is deduced from its only input argument. The returned element
1278  /// is templated on the scalar type T of the %MultibodyTree on which this
1279  /// method is invoked.
1280  /// In the example above, the user could have also invoked the method
1281  /// ToAutoDiffXd().
1282  ///
1283  /// @pre Finalize() must have already been called on this %MultibodyTree.
1284  template <typename ToScalar>
1285  std::unique_ptr<MultibodyTree<ToScalar>> CloneToScalar() const {
1286  if (!topology_is_valid()) {
1287  throw std::logic_error(
1288  "Attempting to clone a MultibodyTree with an invalid topology. "
1289  "MultibodyTree::Finalize() must be called before attempting to clone"
1290  " a MultibodyTree.");
1291  }
1292  auto tree_clone = std::make_unique<MultibodyTree<ToScalar>>();
1293 
1294  tree_clone->frames_.resize(get_num_frames());
1295  // Skipping the world body at body_index = 0.
1296  for (BodyIndex body_index(1); body_index < get_num_bodies(); ++body_index) {
1297  const Body<T>& body = get_body(body_index);
1298  tree_clone->CloneBodyAndAdd(body);
1299  }
1300 
1301  // Frames are cloned in their index order, that is, in the exact same order
1302  // they were added to the original tree. Since the Frame API enforces the
1303  // creation of the parent frame first, this traversal guarantees that parent
1304  // body frames are created before their child frames.
1305  for (const auto& frame : owned_frames_) {
1306  tree_clone->CloneFrameAndAdd(*frame);
1307  }
1308 
1309  for (const auto& mobilizer : owned_mobilizers_) {
1310  // This call assumes that tree_clone already contains all the cloned
1311  // frames.
1312  tree_clone->CloneMobilizerAndAdd(*mobilizer);
1313  }
1314 
1315  for (const auto& force_element : owned_force_elements_) {
1316  tree_clone->CloneForceElementAndAdd(*force_element);
1317  }
1318 
1319  // Since Joint<T> objects are implemented from basic element objects like
1320  // Body, Mobilizer, ForceElement and Constraint, they are cloned last so
1321  // that the clones of their dependencies are guaranteed to be available.
1322  // DO NOT change this order!!!
1323  for (const auto& joint : owned_joints_) {
1324  tree_clone->CloneJointAndAdd(*joint);
1325  }
1326 
1327  // We can safely make a deep copy here since the original multibody tree is
1328  // required to be finalized.
1329  tree_clone->topology_ = this->topology_;
1330  // All other internals templated on T are created with the following call to
1331  // FinalizeInternals().
1332  tree_clone->FinalizeInternals();
1333  return tree_clone;
1334  }
1335 
1336  private:
1337  // Make MultibodyTree templated on every other scalar type a friend of
1338  // MultibodyTree<T> so that CloneToScalar<ToAnyOtherScalar>() can access
1339  // private methods from MultibodyTree<T>.
1340  template <typename> friend class MultibodyTree;
1341 
1342  template <template<typename Scalar> class JointType>
1343  const JointType<T>& AddJoint(
1344  std::unique_ptr<JointType<T>> joint) {
1345  static_assert(std::is_convertible<JointType<T>*, Joint<T>*>::value,
1346  "JointType must be a sub-class of Joint<T>.");
1347  if (topology_is_valid()) {
1348  throw std::logic_error("This MultibodyTree is finalized already. "
1349  "Therefore adding more joints is not allowed. "
1350  "See documentation for Finalize() for details.");
1351  }
1352  if (joint == nullptr) {
1353  throw std::logic_error("Input joint is a nullptr.");
1354  }
1355  const JointIndex joint_index(owned_joints_.size());
1356  joint->set_parent_tree(this, joint_index);
1357  JointType<T>* raw_joint_ptr = joint.get();
1358  owned_joints_.push_back(std::move(joint));
1359  return *raw_joint_ptr;
1360  }
1361 
1362  // Finalizes the MultibodyTreeTopology of this tree.
1363  void FinalizeTopology();
1364 
1365  // At Finalize(), this method performs all other finalization that is not
1366  // topological (i.e. performed by FinalizeTopology()). This includes for
1367  // instance the creation of BodyNode objects.
1368  // This method will throw a std::logic_error if FinalizeTopology() was not
1369  // previously called on this tree.
1370  void FinalizeInternals();
1371 
1372  // Computes the cache entry associated with the geometric Jacobian H_PB_W for
1373  // each node.
1374  // The geometric Jacobian `H_PB_W` relates to the spatial velocity of B in P
1375  // by `V_PB_W = H_PB_W(q)⋅v_B`, where `v_B` corresponds to the generalized
1376  // velocities associated to body B. `H_PB_W` has size `6 x nm` with `nm` the
1377  // number of mobilities associated with body B.
1378  // `H_PB_W_cache` stores the Jacobian matrices for all nodes in the tree as a
1379  // vector of the columns of these matrices. Therefore `H_PB_W_cache` has as
1380  // many entries as number of generalized velocities in the tree.
1381  void CalcAcrossNodeGeometricJacobianExpressedInWorld(
1382  const systems::Context<T>& context,
1383  const PositionKinematicsCache<T>& pc,
1384  std::vector<Vector6<T>>* H_PB_W_cache) const;
1385 
1386  // Implementation for CalcMassMatrixViaInverseDynamics().
1387  // It assumes:
1388  // - The position kinematics cache object is already updated to be in sync
1389  // with `context`.
1390  // - H is not nullptr.
1391  // - H has storage for a square matrix of size get_num_velocities().
1392  void DoCalcMassMatrixViaInverseDynamics(
1393  const systems::Context<T>& context,
1394  const PositionKinematicsCache<T>& pc,
1395  EigenPtr<MatrixX<T>> H) const;
1396 
1397  // Implementation of CalcBiasTerm().
1398  // It assumes:
1399  // - The position kinematics cache object is already updated to be in sync
1400  // with `context`.
1401  // - The velocity kinematics cache object is already updated to be in sync
1402  // with `context`.
1403  // - Cv is not nullptr.
1404  // - Cv has storage for a vector of size get_num_velocities().
1405  void DoCalcBiasTerm(
1406  const systems::Context<T>& context,
1407  const PositionKinematicsCache<T>& pc,
1408  const VelocityKinematicsCache<T>& vc,
1409  EigenPtr<VectorX<T>> Cv) const;
1410 
1411  // Implementation of CalcPotentialEnergy().
1412  // It is assumed that the position kinematics cache pc is in sync with
1413  // context.
1414  T DoCalcPotentialEnergy(const systems::Context<T>& context,
1415  const PositionKinematicsCache<T>& pc) const;
1416 
1417  // Implementation of CalcConservativePower().
1418  // It is assumed that the position kinematics cache pc and the velocity
1419  // kinematics cache vc are in sync with context.
1420  T DoCalcConservativePower(const systems::Context<T>& context,
1421  const PositionKinematicsCache<T>& pc,
1422  const VelocityKinematicsCache<T>& vc) const;
1423 
1424  void CreateBodyNode(BodyNodeIndex body_node_index);
1425 
1426  // Helper method to create a clone of `frame` and add it to `this` tree.
1427  template <typename FromScalar>
1428  Frame<T>* CloneFrameAndAdd(const Frame<FromScalar>& frame) {
1429  FrameIndex frame_index = frame.get_index();
1430 
1431  auto frame_clone = frame.CloneToScalar(*this);
1432  frame_clone->set_parent_tree(this, frame_index);
1433 
1434  Frame<T>* raw_frame_clone_ptr = frame_clone.get();
1435  // The order in which frames are added into frames_ is important to keep the
1436  // topology invariant. Therefore we index new clones according to the
1437  // original frame_index.
1438  frames_[frame_index] = raw_frame_clone_ptr;
1439  // The order within owned_frames_ does not matter.
1440  owned_frames_.push_back(std::move(frame_clone));
1441  return raw_frame_clone_ptr;
1442  }
1443 
1444  // Helper method to create a clone of `body` and add it to `this` tree.
1445  // Because this method is only invoked in a controlled manner from within
1446  // CloneToScalar(), it is guaranteed that the cloned body in this variant's
1447  // `owned_bodies_` will occupy the same position as its corresponding Body
1448  // in the source variant `body`.
1449  template <typename FromScalar>
1450  Body<T>* CloneBodyAndAdd(const Body<FromScalar>& body) {
1451  const BodyIndex body_index = body.get_index();
1452  const FrameIndex body_frame_index = body.get_body_frame().get_index();
1453 
1454  auto body_clone = body.CloneToScalar(*this);
1455  body_clone->set_parent_tree(this, body_index);
1456  // MultibodyTree can access selected private methods in Body through its
1457  // BodyAttorney.
1458  Frame<T>* body_frame_clone =
1459  &internal::BodyAttorney<T>::get_mutable_body_frame(body_clone.get());
1460  body_frame_clone->set_parent_tree(this, body_frame_index);
1461 
1462  // The order in which frames are added into frames_ is important to keep the
1463  // topology invariant. Therefore we index new clones according to the
1464  // original body_frame_index.
1465  frames_[body_frame_index] = body_frame_clone;
1466  Body<T>* raw_body_clone_ptr = body_clone.get();
1467  // The order in which bodies are added into owned_bodies_ is important to
1468  // keep the topology invariant. Therefore this method is called from
1469  // MultibodyTree::CloneToScalar() within a loop by original body_index.
1470  DRAKE_DEMAND(static_cast<int>(owned_bodies_.size()) == body_index);
1471  owned_bodies_.push_back(std::move(body_clone));
1472  return raw_body_clone_ptr;
1473  }
1474 
1475  // Helper method to create a clone of `mobilizer` and add it to `this` tree.
1476  template <typename FromScalar>
1477  Mobilizer<T>* CloneMobilizerAndAdd(const Mobilizer<FromScalar>& mobilizer) {
1478  MobilizerIndex mobilizer_index = mobilizer.get_index();
1479  auto mobilizer_clone = mobilizer.CloneToScalar(*this);
1480  mobilizer_clone->set_parent_tree(this, mobilizer_index);
1481  Mobilizer<T>* raw_mobilizer_clone_ptr = mobilizer_clone.get();
1482  owned_mobilizers_.push_back(std::move(mobilizer_clone));
1483  return raw_mobilizer_clone_ptr;
1484  }
1485 
1486  // Helper method to create a clone of `force_element` and add it to `this`
1487  // tree.
1488  template <typename FromScalar>
1489  void CloneForceElementAndAdd(
1490  const ForceElement<FromScalar>& force_element) {
1491  ForceElementIndex force_element_index = force_element.get_index();
1492  auto force_element_clone = force_element.CloneToScalar(*this);
1493  force_element_clone->set_parent_tree(this, force_element_index);
1494  owned_force_elements_.push_back(std::move(force_element_clone));
1495  }
1496 
1497  // Helper method to create a clone of `joint` and add it to `this` tree.
1498  template <typename FromScalar>
1499  Joint<T>* CloneJointAndAdd(const Joint<FromScalar>& joint) {
1500  JointIndex joint_index = joint.get_index();
1501  auto joint_clone = joint.CloneToScalar(*this);
1502  joint_clone->set_parent_tree(this, joint_index);
1503  owned_joints_.push_back(std::move(joint_clone));
1504  return owned_joints_.back().get();
1505  }
1506 
1507  // Helper method to retrieve the corresponding Frame<T> variant to a Frame in
1508  // a MultibodyTree variant templated on Scalar.
1509  template <template <typename> class FrameType, typename Scalar>
1510  const FrameType<T>& get_frame_variant(const FrameType<Scalar>& frame) const {
1511  static_assert(std::is_base_of<Frame<T>, FrameType<T>>::value,
1512  "FrameType<T> must be a sub-class of Frame<T>.");
1513  // TODO(amcastro-tri):
1514  // DRAKE_DEMAND the parent tree of the variant is indeed a variant of this
1515  // MultibodyTree. That will require the tree to have some sort of id.
1516  FrameIndex frame_index = frame.get_index();
1517  DRAKE_DEMAND(frame_index < get_num_frames());
1518  const FrameType<T>* frame_variant =
1519  dynamic_cast<const FrameType<T>*>(frames_[frame_index]);
1520  DRAKE_DEMAND(frame_variant != nullptr);
1521  return *frame_variant;
1522  }
1523 
1524  // Helper method to retrieve the corresponding Body<T> variant to a Body in a
1525  // MultibodyTree variant templated on Scalar.
1526  template <template <typename> class BodyType, typename Scalar>
1527  const BodyType<T>& get_body_variant(const BodyType<Scalar>& body) const {
1528  static_assert(std::is_base_of<Body<T>, BodyType<T>>::value,
1529  "BodyType<T> must be a sub-class of Body<T>.");
1530  // TODO(amcastro-tri):
1531  // DRAKE_DEMAND the parent tree of the variant is indeed a variant of this
1532  // MultibodyTree. That will require the tree to have some sort of id.
1533  BodyIndex body_index = body.get_index();
1534  DRAKE_DEMAND(body_index < get_num_bodies());
1535  const BodyType<T>* body_variant =
1536  dynamic_cast<const BodyType<T>*>(
1537  owned_bodies_[body_index].get());
1538  DRAKE_DEMAND(body_variant != nullptr);
1539  return *body_variant;
1540  }
1541 
1542  // Helper method to retrieve the corresponding Mobilizer<T> variant to a Body
1543  // in a MultibodyTree variant templated on Scalar.
1544  template <template <typename> class MobilizerType, typename Scalar>
1545  const MobilizerType<T>& get_mobilizer_variant(
1546  const MobilizerType<Scalar>& mobilizer) const {
1547  static_assert(std::is_base_of<Mobilizer<T>, MobilizerType<T>>::value,
1548  "MobilizerType<T> must be a sub-class of Mobilizer<T>.");
1549  // TODO(amcastro-tri):
1550  // DRAKE_DEMAND the parent tree of the variant is indeed a variant of this
1551  // MultibodyTree. That will require the tree to have some sort of id.
1552  MobilizerIndex mobilizer_index = mobilizer.get_index();
1553  DRAKE_DEMAND(mobilizer_index < get_num_mobilizers());
1554  const MobilizerType<T>* mobilizer_variant =
1555  dynamic_cast<const MobilizerType<T>*>(
1556  owned_mobilizers_[mobilizer_index].get());
1557  DRAKE_DEMAND(mobilizer_variant != nullptr);
1558  return *mobilizer_variant;
1559  }
1560 
1561  // Helper method to retrieve the corresponding Joint<T> variant to a Joint
1562  // in a MultibodyTree variant templated on Scalar.
1563  template <template <typename> class JointType, typename Scalar>
1564  const JointType<T>& get_joint_variant(const JointType<Scalar>& joint) const {
1565  static_assert(std::is_base_of<Joint<T>, JointType<T>>::value,
1566  "JointType<T> must be a sub-class of Joint<T>.");
1567  // TODO(amcastro-tri):
1568  // DRAKE_DEMAND the parent tree of the variant is indeed a variant of this
1569  // MultibodyTree. That will require the tree to have some sort of id.
1570  JointIndex joint_index = joint.get_index();
1571  DRAKE_DEMAND(joint_index < get_num_joints());
1572  const JointType<T>* joint_variant =
1573  dynamic_cast<const JointType<T>*>(
1574  owned_joints_[joint_index].get());
1575  DRAKE_DEMAND(joint_variant != nullptr);
1576  return *joint_variant;
1577  }
1578 
1579  // TODO(amcastro-tri): In future PR's adding MBT computational methods, write
1580  // a method that verifies the state of the topology with a signature similar
1581  // to RoadGeometry::CheckInvariants().
1582 
1583  const RigidBody<T>* world_body_{nullptr};
1584  std::vector<std::unique_ptr<Body<T>>> owned_bodies_;
1585  std::vector<std::unique_ptr<Frame<T>>> owned_frames_;
1586  std::vector<std::unique_ptr<Mobilizer<T>>> owned_mobilizers_;
1587  std::vector<std::unique_ptr<ForceElement<T>>> owned_force_elements_;
1588  std::vector<std::unique_ptr<internal::BodyNode<T>>> body_nodes_;
1589 
1590  std::vector<std::unique_ptr<Joint<T>>> owned_joints_;
1591 
1592  // List of all frames in the system ordered by their FrameIndex.
1593  // This vector contains a pointer to all frames in owned_frames_ as well as a
1594  // pointer to each BodyFrame, which are owned by their corresponding Body.
1595  std::vector<const Frame<T>*> frames_;
1596 
1597  // Body node indexes ordered by level (a.k.a depth). Therefore for the
1598  // i-th level body_node_levels_[i] contains the list of all body node indexes
1599  // in that level.
1600  std::vector<std::vector<BodyNodeIndex>> body_node_levels_;
1601 
1602  MultibodyTreeTopology topology_;
1603 };
1604 
1605 } // namespace multibody
1606 } // namespace drake
std::unique_ptr< MultibodyTree< AutoDiffXd > > ToAutoDiffXd() const
Creates a deep copy of this MultibodyTree templated on AutoDiffXd.
Definition: multibody_tree.h:1244
Eigen::Transform< Scalar, 3, Eigen::Isometry > Isometry3
An Isometry templated on scalar type.
Definition: eigen_types.h:106
This class is used to represent a spatial acceleration that combines rotational (angular acceleration...
Definition: spatial_acceleration.h:52
const RigidBody< T > & get_world_body() const
Returns a constant reference to the world body.
Definition: multibody_tree.h:566
const Body< double > * world_body_
Definition: double_pendulum_test.cc:269
int get_num_force_elements() const
Returns the number of ForceElement objects in the MultibodyTree.
Definition: multibody_tree.h:535
int get_tree_height() const
Returns the number of tree levels in the topology.
Definition: multibody_tree_topology.h:429
This class is one of the cache entries in MultibodyTreeContext.
Definition: acceleration_kinematics_cache.h:34
int get_num_states() const
Returns the total size of the state vector in the model.
Definition: multibody_tree.h:550
bool topology_is_valid() const
Returns true if this MultibodyTree was finalized with Finalize() after all multibody elements were ad...
Definition: multibody_tree.h:608
void Finalize()
This method must be called after all elements in the tree (joints, bodies, force elements, constraints) were added and before any computations are performed.
Definition: multibody_tree.cc:106
Definition: automotive_demo.cc:89
std::unique_ptr< MultibodyTree< T > > Clone() const
Creates a deep copy of this MultibodyTree templated on the same scalar type T as this tree...
Definition: multibody_tree.h:1239
void MapQDotToVelocity(const systems::Context< T > &context, const Eigen::Ref< const VectorX< T >> &qdot, EigenPtr< VectorX< T >> v) const
Transforms the time derivative qdot of the generalized positions vector q (stored in context) to gene...
Definition: multibody_tree.cc:419
STL namespace.
Context is an abstract base class template that represents all the inputs to a System: time...
Definition: query_handle.h:10
const FrameType< T > & AddFrame(std::unique_ptr< FrameType< T >> frame)
Takes ownership of frame and adds it to this MultibodyTree.
Definition: multibody_tree.h:188
std::unique_ptr< systems::LeafContext< T > > CreateDefaultContext() const
Allocates a new context for this MultibodyTree uniquely identifying the state of the multibody system...
Definition: multibody_tree.cc:159
T CalcConservativePower(const systems::Context< T > &context) const
Computes and returns the power generated by conservative forces in the multibody model.
Definition: multibody_tree.cc:688
int get_num_velocities() const
Returns the number of generalized velocities of the model.
Definition: multibody_tree.h:545
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
A column vector of any size, templated on scalar type.
Definition: eigen_types.h:46
void CalcPointsPositions(const systems::Context< T > &context, const Frame< T > &from_frame_B, const Eigen::Ref< const MatrixX< T >> &p_BQi, const Frame< T > &to_frame_A, EigenPtr< MatrixX< T >> p_AQi) const
Given the positions p_BQi for a set of points Qi measured and expressed in a frame B...
Definition: multibody_tree.cc:546
void CalcMassMatrixViaInverseDynamics(const systems::Context< T > &context, EigenPtr< MatrixX< T >> H) const
Performs the computation of the mass matrix M(q) of the model using inverse dynamics, where the generalized positions q are stored in context.
Definition: multibody_tree.cc:457
Body provides the general abstraction of a body with an API that makes no assumption about whether a ...
Definition: body.h:19
int get_num_velocities() const
Returns the total number of generalized velocities in the model.
Definition: multibody_tree_topology.h:753
void SetDefaultContext(systems::Context< T > *context) const
Sets default values in the context.
Definition: multibody_tree.cc:172
MultibodyTree provides a representation for a physical system consisting of a collection of interconn...
Definition: body.h:104
const BodyFrame< T > & get_world_frame() const
Returns a constant reference to the world frame.
Definition: multibody_tree.h:574
void CalcForceElementsContribution(const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, const VelocityKinematicsCache< T > &vc, std::vector< SpatialForce< T >> *F_Bo_W_array, EigenPtr< VectorX< T >> tau_array) const
Computes the combined force contribution of ForceElement objects in the model.
Definition: multibody_tree.cc:393
ForceElementIndex add_force_element()
Creates and adds a new ForceElementTopology, associated with the given force_index, to this MultibodyTreeTopology.
Definition: multibody_tree_topology.h:585
std::unique_ptr< MultibodyTree< ToScalar > > CloneToScalar() const
Creates a deep copy of this MultibodyTree templated on the scalar type ToScalar.
Definition: multibody_tree.h:1285
std::enable_if_t< std::is_base_of< Joint< T >, MultibodyElement< T > >::value, const MultibodyElement< T > & > get_variant(const MultibodyElement< Scalar > &element) const
SFINAE overload for Joint<T> elements.
Definition: multibody_tree.h:1231
void CalcBiasTerm(const systems::Context< T > &context, EigenPtr< VectorX< T >> Cv) const
Computes the bias term C(q, v)v containing Coriolis and gyroscopic effects of the multibody equations...
Definition: multibody_tree.cc:497
A ForceElement allows modeling state and time dependent forces in a MultibodyTree model...
Definition: force_element.h:35
std::vector< double > vector
Definition: translator_test.cc:20
const MultibodyTreeTopology & get_topology() const
Returns the topology information for this multibody tree.
Definition: multibody_tree.h:614
const BodyFrame< T > & get_body_frame() const
Returns a const reference to the associated BodyFrame.
Definition: body.h:161
stx::optional< T > optional
Definition: drake_optional.h:14
int get_num_bodies() const
Returns the number of bodies in the MultibodyTree including the world body.
Definition: multibody_tree.h:519
A BodyFrame is a material Frame that serves as the unique reference frame for a Body.
Definition: body.h:57
vector< FrameId > frames_
Definition: geometry_state_test.cc:276
const Frame< T > & get_frame(FrameIndex frame_index) const
Returns a constant reference to the frame with unique index frame_index.
Definition: multibody_tree.h:589
std::enable_if_t< std::is_base_of< Body< T >, MultibodyElement< T > >::value, const MultibodyElement< T > & > get_variant(const MultibodyElement< Scalar > &element) const
SFINAE overload for Body<T> elements.
Definition: multibody_tree.h:1215
const BodyType< T > & AddBody(std::unique_ptr< BodyType< T >> body)
Takes ownership of body and adds it to this MultibodyTree.
Definition: multibody_tree.h:90
int get_num_frames() const
Returns the number of Frame objects in the MultibodyTree.
Definition: multibody_tree.h:513
std::pair< BodyIndex, FrameIndex > add_body()
Creates and adds a new BodyTopology to this MultibodyTreeTopology.
Definition: multibody_tree_topology.h:470
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
#define DRAKE_ASSERT(condition)
DRAKE_ASSERT(condition) is similar to the built-in assert(condition) from the C++ system header <cas...
Definition: drake_assert.h:37
int get_num_mobilizers() const
Returns the number of mobilizers in the MultibodyTree.
Definition: multibody_tree.h:530
int get_num_joints() const
Returns the number of joints added with AddJoint() to the MultibodyTree.
Definition: multibody_tree.h:522
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > MatrixX
A matrix of dynamic size, templated on scalar type.
Definition: eigen_types.h:87
const FrameType< T > & AddFrame(Args &&...args)
Constructs a new frame with type FrameType with the given args, and adds it to this MultibodyTree...
Definition: multibody_tree.h:248
int value
Definition: copyable_unique_ptr_test.cc:61
const MobilizerType< T > & AddMobilizer(Args &&...args)
Constructs a new mobilizer with type MobilizerType with the given args, and adds it to this Multibody...
Definition: multibody_tree.h:371
std::enable_if_t< std::is_base_of< Frame< T >, MultibodyElement< T > >::value, const MultibodyElement< T > & > get_variant(const MultibodyElement< Scalar > &element) const
SFINAE overload for Frame<T> elements.
Definition: multibody_tree.h:1207
#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
int get_num_positions() const
Returns the total number of generalized positions in the model.
Definition: multibody_tree_topology.h:750
std::unique_ptr< Mobilizer< ToScalar > > CloneToScalar(const MultibodyTree< ToScalar > &cloned_tree) const
NVI to DoCloneToScalar() templated on the scalar type of the new clone to be created.
Definition: mobilizer.h:550
State is a container for all the data comprising the complete state of a particular System at a parti...
Definition: state.h:27
Data structure to store the topological information associated with an entire MultibodyTree.
Definition: multibody_tree_topology.h:372
const ForceElementType< T > & AddForceElement(Args &&...args)
Definition: multibody_tree.h:411
MultibodyTree()
Creates a MultibodyTree containing only a world body.
Definition: multibody_tree.cc:44
const ForceElementType< T > & AddForceElement(std::unique_ptr< ForceElementType< T >> force_element)
Creates and adds to this MultibodyTree (which retains ownership) a new ForceElement member with the s...
Definition: multibody_tree.h:386
std::unique_ptr< Frame< ToScalar > > CloneToScalar(const MultibodyTree< ToScalar > &tree_clone) const
NVI to DoCloneToScalar() templated on the scalar type of the new clone to be created.
Definition: frame.h:80
This class is one of the cache entries in MultibodyTreeContext.
Definition: velocity_kinematics_cache.h:37
bool is_valid() const
Returns true if Finalize() was already called on this topology.
Definition: multibody_tree_topology.h:747
This wrapper class provides a way to write non-template functions taking raw pointers to Eigen object...
Definition: eigen_types.h:275
This class is one of the cache entries in MultibodyTreeContext.
Definition: position_kinematics_cache.h:33
int get_tree_height() const
Returns the height of the tree data structure of this MultibodyTree.
Definition: multibody_tree.h:561
void CalcSpatialAccelerationsFromVdot(const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, const VelocityKinematicsCache< T > &vc, const VectorX< T > &known_vdot, std::vector< SpatialAcceleration< T >> *A_WB_array) const
Given the state of this MultibodyTree in context and a known vector of generalized accelerations know...
Definition: multibody_tree.cc:259
void MapVelocityToQDot(const systems::Context< T > &context, const Eigen::Ref< const VectorX< T >> &v, EigenPtr< VectorX< T >> qdot) const
Transforms generalized velocities v to time derivatives qdot of the generalized positions vector q (s...
Definition: multibody_tree.cc:438
Isometry3< T > CalcRelativeTransform(const systems::Context< T > &context, const Frame< T > &to_frame_A, const Frame< T > &from_frame_B) const
Computes the relative transform X_AB(q) from a frame B to a frame A, as a function of the generalized...
Definition: multibody_tree.cc:532
const JointType< T > & AddJoint(const std::string &name, const Body< T > &parent, const optional< Isometry3< double >> &X_PF, const Body< T > &child, const optional< Isometry3< double >> &X_BM, Args &&...args)
This method helps to create a Joint of type JointType between two bodies.
Definition: multibody_tree.h:478
Mobilizer is a fundamental object within Drake&#39;s multibody engine used to specify the allowed motions...
Definition: mobilizer.h:223
T CalcPotentialEnergy(const systems::Context< T > &context) const
Computes and returns the total potential energy stored in this multibody model for the configuration ...
Definition: multibody_tree.cc:664
void CalcAccelerationKinematicsCache(const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, const VelocityKinematicsCache< T > &vc, const VectorX< T > &known_vdot, AccelerationKinematicsCache< T > *ac) const
Computes all the kinematic quantities that depend on the generalized accelerations that is...
Definition: multibody_tree.cc:296
const Body< T > & get_body(BodyIndex body_index) const
Returns a constant reference to the body with unique index body_index.
Definition: multibody_tree.h:581
The term rigid body implies that the deformations of the body under consideration are so small that t...
Definition: fixed_offset_frame.h:16
void CalcInverseDynamics(const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, const VelocityKinematicsCache< T > &vc, const VectorX< T > &known_vdot, const std::vector< SpatialForce< T >> &Fapplied_Bo_W_array, const Eigen::Ref< const VectorX< T >> &tau_applied_array, std::vector< SpatialAcceleration< T >> *A_WB_array, std::vector< SpatialForce< T >> *F_BMo_W_array, EigenPtr< VectorX< T >> tau_array) const
Given the state of this MultibodyTree in context and a known vector of generalized accelerations vdot...
Definition: multibody_tree.cc:314
const BodyType< T > & AddBody(Args &&...args)
Constructs a new body with type BodyType with the given args, and adds it to this MultibodyTree...
Definition: multibody_tree.h:156
MobilizerIndex add_mobilizer(FrameIndex in_frame, FrameIndex out_frame, int num_positions, int num_velocities)
Creates and adds a new MobilizerTopology connecting the inboard and outboard multibody frames identif...
Definition: multibody_tree_topology.h:515
void CalcVelocityKinematicsCache(const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, VelocityKinematicsCache< T > *vc) const
Computes all the kinematic quantities that depend on the generalized velocities and stores them in th...
Definition: multibody_tree.cc:218
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
void CalcPointsGeometricJacobianExpressedInWorld(const systems::Context< T > &context, const Frame< T > &frame_B, const Eigen::Ref< const MatrixX< T >> &p_BQi_set, EigenPtr< MatrixX< T >> p_WQi_set, EigenPtr< MatrixX< T >> J_WQi) const
Given a set of points Qi with fixed position vectors p_BQi in a frame B, (that is, their time derivative ᴮd/dt(p_BQi) in frame B is zero), this method computes the geometric Jacobian J_WQi defined by:
Definition: multibody_tree.cc:593
This class is used to represent a spatial force (also called a wrench) that combines both rotational ...
Definition: spatial_force.h:40
void CalcPositionKinematicsCache(const systems::Context< T > &context, PositionKinematicsCache< T > *pc) const
Computes into the position kinematics pc all the kinematic quantities that depend on the generalized ...
Definition: multibody_tree.cc:187
A Joint models the kinematical relationship which characterizes the possible relative motion between ...
Definition: joint.h:75
const MobilizerType< T > & AddMobilizer(std::unique_ptr< MobilizerType< T >> mobilizer)
Takes ownership of mobilizer and adds it to this MultibodyTree.
Definition: multibody_tree.h:295
#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
void SetDefaultState(const systems::Context< T > &context, systems::State< T > *state) const
Sets default values in the state.
Definition: multibody_tree.cc:179
int get_num_states() const
Returns the total size of the state vector in the model.
Definition: multibody_tree_topology.h:756
FrameIndex add_frame(BodyIndex body_index)
Creates and adds a new FrameTopology, associated with the given body_index, to this MultibodyTreeTopo...
Definition: multibody_tree_topology.h:489
Vector3d body_frame
Definition: model_test.cc:38
int get_num_positions() const
Returns the number of generalized positions of the model.
Definition: multibody_tree.h:540
const Mobilizer< T > & get_mobilizer(MobilizerIndex mobilizer_index) const
Returns a constant reference to the mobilizer with unique index mobilizer_index.
Definition: multibody_tree.h:598
BodyIndex world_index()
For every MultibodyTree the world body always has this unique index and it is always zero...
Definition: multibody_tree_indexes.h:32
std::enable_if_t< std::is_base_of< Mobilizer< T >, MultibodyElement< T > >::value, const MultibodyElement< T > & > get_variant(const MultibodyElement< Scalar > &element) const
SFINAE overload for Mobilizer<T> elements.
Definition: multibody_tree.h:1223