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