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