Drake
multibody_tree.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <string>
5 #include <tuple>
6 #include <type_traits>
7 #include <utility>
8 #include <vector>
9 
10 #include "drake/common/autodiff.h"
25 
26 namespace drake {
27 namespace multibody {
28 
43 template <typename T>
44 class MultibodyTree {
45  public:
47 
48 
49  MultibodyTree();
50 
59  // TODO(amcastro-tri): add at least one example of a method that requires a
60  // valid topology in this documentation.
61  // See this Reviewable comment:
62  // https://reviewable.io/reviews/robotlocomotion/drake/5583#-KgGqGisnX9uMuYDkHpx
63 
89  template <template<typename Scalar> class BodyType>
90  const BodyType<T>& AddBody(std::unique_ptr<BodyType<T>> body) {
91  static_assert(std::is_convertible<BodyType<T>*, Body<T>*>::value,
92  "BodyType must be a sub-class of Body<T>.");
93  if (topology_is_valid()) {
94  throw std::logic_error("This MultibodyTree is finalized already. "
95  "Therefore adding more bodies is not allowed. "
96  "See documentation for Finalize() for details.");
97  }
98  if (body == nullptr) {
99  throw std::logic_error("Input body is a nullptr.");
100  }
101  BodyIndex body_index(0);
102  FrameIndex body_frame_index(0);
103  std::tie(body_index, body_frame_index) = topology_.add_body();
104  // These tests MUST be performed BEFORE frames_.push_back() and
105  // owned_bodies_.push_back() below. Do not move them around!
106  DRAKE_ASSERT(body_index == get_num_bodies());
107  DRAKE_ASSERT(body_frame_index == get_num_frames());
108 
109  // TODO(amcastro-tri): consider not depending on setting this pointer at
110  // all. Consider also removing MultibodyTreeElement altogether.
111  body->set_parent_tree(this, body_index);
112  // MultibodyTree can access selected private methods in Body through its
113  // BodyAttorney.
115  &internal::BodyAttorney<T>::get_mutable_body_frame(body.get());
116  body_frame->set_parent_tree(this, body_frame_index);
117  frames_.push_back(body_frame);
118  BodyType<T>* raw_body_ptr = body.get();
119  owned_bodies_.push_back(std::move(body));
120  return *raw_body_ptr;
121  }
122 
155  template<template<typename Scalar> class BodyType, typename... Args>
156  const BodyType<T>& AddBody(Args&&... args) {
157  static_assert(std::is_convertible<BodyType<T>*, Body<T>*>::value,
158  "BodyType must be a sub-class of Body<T>.");
159  return AddBody(std::make_unique<BodyType<T>>(std::forward<Args>(args)...));
160  }
161 
187  template <template<typename Scalar> class FrameType>
188  const FrameType<T>& AddFrame(std::unique_ptr<FrameType<T>> frame) {
189  static_assert(std::is_convertible<FrameType<T>*, Frame<T>*>::value,
190  "FrameType must be a sub-class of Frame<T>.");
191  if (topology_is_valid()) {
192  throw std::logic_error("This MultibodyTree is finalized already. "
193  "Therefore adding more frames is not allowed. "
194  "See documentation for Finalize() for details.");
195  }
196  if (frame == nullptr) {
197  throw std::logic_error("Input frame is a nullptr.");
198  }
199  FrameIndex frame_index = topology_.add_frame(frame->get_body().get_index());
200  // This test MUST be performed BEFORE frames_.push_back() and
201  // owned_frames_.push_back() below. Do not move it around!
202  DRAKE_ASSERT(frame_index == get_num_frames());
203  // TODO(amcastro-tri): consider not depending on setting this pointer at
204  // all. Consider also removing MultibodyTreeElement altogether.
205  frame->set_parent_tree(this, frame_index);
206  FrameType<T>* raw_frame_ptr = frame.get();
207  frames_.push_back(raw_frame_ptr);
208  owned_frames_.push_back(std::move(frame));
209  return *raw_frame_ptr;
210  }
211 
247  template<template<typename Scalar> class FrameType, typename... Args>
248  const FrameType<T>& AddFrame(Args&&... args) {
249  static_assert(std::is_convertible<FrameType<T>*, Frame<T>*>::value,
250  "FrameType must be a sub-class of Frame<T>.");
251  return AddFrame(
252  std::make_unique<FrameType<T>>(std::forward<Args>(args)...));
253  }
254 
294  template <template<typename Scalar> class MobilizerType>
295  const MobilizerType<T>& AddMobilizer(
296  std::unique_ptr<MobilizerType<T>> mobilizer) {
297  static_assert(std::is_convertible<MobilizerType<T>*, Mobilizer<T>*>::value,
298  "MobilizerType must be a sub-class of mobilizer<T>.");
299  if (topology_is_valid()) {
300  throw std::logic_error("This MultibodyTree is finalized already. "
301  "Therefore adding more mobilizers is not allowed. "
302  "See documentation for Finalize() for details.");
303  }
304  if (mobilizer == nullptr) {
305  throw std::logic_error("Input mobilizer is a nullptr.");
306  }
307  // Verifies that the inboard/outboard frames provided by the user do belong
308  // to this tree. This is a pathological case, but in theory nothing
309  // (but this test) stops a user from adding frames to a tree1 and attempting
310  // later to define mobilizers between those frames in a second tree2.
311  mobilizer->get_inboard_frame().HasThisParentTreeOrThrow(this);
312  mobilizer->get_outboard_frame().HasThisParentTreeOrThrow(this);
313  const int num_positions = mobilizer->get_num_positions();
314  const int num_velocities = mobilizer->get_num_velocities();
315  MobilizerIndex mobilizer_index = topology_.add_mobilizer(
316  mobilizer->get_inboard_frame().get_index(),
317  mobilizer->get_outboard_frame().get_index(),
318  num_positions, num_velocities);
319 
320  // This DRAKE_ASSERT MUST be performed BEFORE owned_mobilizers_.push_back()
321  // below. Do not move it around!
322  DRAKE_ASSERT(mobilizer_index == get_num_mobilizers());
323 
324  // TODO(amcastro-tri): consider not depending on setting this pointer at
325  // all. Consider also removing MultibodyTreeElement altogether.
326  mobilizer->set_parent_tree(this, mobilizer_index);
327 
328  MobilizerType<T>* raw_mobilizer_ptr = mobilizer.get();
329  owned_mobilizers_.push_back(std::move(mobilizer));
330  return *raw_mobilizer_ptr;
331  }
332 
370  template<template<typename Scalar> class MobilizerType, typename... Args>
371  const MobilizerType<T>& AddMobilizer(Args&&... args) {
372  static_assert(std::is_base_of<Mobilizer<T>, MobilizerType<T>>::value,
373  "MobilizerType must be a sub-class of Mobilizer<T>.");
374  return AddMobilizer(
375  std::make_unique<MobilizerType<T>>(std::forward<Args>(args)...));
376  }
377 
385  template <template<typename Scalar> class ForceElementType>
386  const ForceElementType<T>& AddForceElement(
387  std::unique_ptr<ForceElementType<T>> force_element) {
388  static_assert(
389  std::is_convertible<ForceElementType<T>*, ForceElement<T>*>::value,
390  "ForceElementType<T> must be a sub-class of ForceElement<T>.");
391  if (topology_is_valid()) {
392  throw std::logic_error(
393  "This MultibodyTree is finalized already. Therefore adding more "
394  "force elements is not allowed. "
395  "See documentation for Finalize() for details.");
396  }
397  if (force_element == nullptr) {
398  throw std::logic_error("Input force element is a nullptr.");
399  }
400  ForceElementIndex force_element_index = topology_.add_force_element();
401  // This test MUST be performed BEFORE owned_force_elements_.push_back()
402  // below. Do not move it around!
403  DRAKE_ASSERT(force_element_index == get_num_force_elements());
404  force_element->set_parent_tree(this, force_element_index);
405  ForceElementType<T>* raw_force_element_ptr = force_element.get();
406  owned_force_elements_.push_back(std::move(force_element));
407  return *raw_force_element_ptr;
408  }
409 
410  template<template<typename Scalar> class ForceElementType, typename... Args>
411  const ForceElementType<T>& AddForceElement(Args&&... args) {
412  static_assert(std::is_base_of<ForceElement<T>, ForceElementType<T>>::value,
413  "ForceElementType<T> must be a sub-class of "
414  "ForceElement<T>.");
415  return AddForceElement(
416  std::make_unique<ForceElementType<T>>(std::forward<Args>(args)...));
417  }
418 
477  template<template<typename> class JointType, typename... Args>
478  const JointType<T>& AddJoint(
479  const std::string& name,
480  const Body<T>& parent, const optional<Isometry3<double>>& X_PF,
481  const Body<T>& child, const optional<Isometry3<double>>& X_BM,
482  Args&&... args) {
483  static_assert(std::is_base_of<Joint<T>, JointType<T>>::value,
484  "JointType<T> must be a sub-class of Joint<T>.");
485 
486  const Frame<T>* frame_on_parent;
487  if (X_PF) {
488  frame_on_parent = &this->AddFrame<FixedOffsetFrame>(parent, *X_PF);
489  } else {
490  frame_on_parent = &parent.get_body_frame();
491  }
492 
493  const Frame<T>* frame_on_child;
494  if (X_BM) {
495  frame_on_child = &this->AddFrame<FixedOffsetFrame>(child, *X_BM);
496  } else {
497  frame_on_child = &child.get_body_frame();
498  }
499 
500  return AddJoint(
501  std::make_unique<JointType<T>>(
502  name,
503  *frame_on_parent, *frame_on_child,
504  std::forward<Args>(args)...));
505  }
507  // Closes Doxygen section.
508 
513  int get_num_frames() const {
514  return static_cast<int>(frames_.size());
515  }
516 
519  int get_num_bodies() const { return static_cast<int>(owned_bodies_.size()); }
520 
522  int get_num_joints() const { return static_cast<int>(owned_joints_.size()); }
523 
527  // TODO(amcastro-tri): Consider adding a WorldMobilizer (0-dofs) for the world
528  // body. This could be useful to query for reaction forces of the entire
529  // model.
530  int get_num_mobilizers() const {
531  return static_cast<int>(owned_mobilizers_.size());
532  }
533 
536  return static_cast<int>(owned_force_elements_.size());
537  }
538 
540  int get_num_positions() const {
541  return topology_.get_num_positions();
542  }
543 
545  int get_num_velocities() const {
546  return topology_.get_num_velocities();
547  }
548 
550  int get_num_states() const {
551  return topology_.get_num_states();
552  }
553 
561  int get_tree_height() const {
562  return topology_.get_tree_height();
563  }
564 
566  const RigidBody<T>& get_world_body() const {
567  // world_body_ is set in the constructor. So this assert is here only to
568  // verify future constructors do not mess that up.
569  DRAKE_ASSERT(world_body_ != nullptr);
570  return *world_body_;
571  }
572 
574  const BodyFrame<T>& get_world_frame() const {
575  return owned_bodies_[world_index()]->get_body_frame();
576  }
577 
581  const Body<T>& get_body(BodyIndex body_index) const {
582  DRAKE_ASSERT(body_index < get_num_bodies());
583  return *owned_bodies_[body_index];
584  }
585 
589  const Frame<T>& get_frame(FrameIndex frame_index) const {
590  DRAKE_ASSERT(frame_index < get_num_frames());
591  return *frames_[frame_index];
592  }
593 
598  const Mobilizer<T>& get_mobilizer(MobilizerIndex mobilizer_index) const {
599  DRAKE_ASSERT(mobilizer_index < get_num_mobilizers());
600  return *owned_mobilizers_[mobilizer_index];
601  }
602 
608  bool topology_is_valid() const { return topology_.is_valid(); }
609 
614  const MultibodyTreeTopology& get_topology() const { return topology_; }
615 
630  // TODO(amcastro-tri): Consider making this method private and calling it
631  // automatically when CreateDefaultContext() is called.
632  void Finalize();
633 
643  // TODO(amcastro-tri): Split this method into implementations to be used by
644  // System::AllocateContext() so that MultibodyPlant() can make use of it
645  // within the system's infrastructure. This will require at least the
646  // introduction of system's methods to:
647  // - Create a context different from LeafContext, in this case MBTContext.
648  // - Create or request cache entries.
649  std::unique_ptr<systems::LeafContext<T>> CreateDefaultContext() const;
650 
654  void SetDefaultContext(systems::Context<T>* context) const;
655 
668  const systems::Context<T>& context,
669  PositionKinematicsCache<T>* pc) const;
670 
685  const systems::Context<T>& context,
686  const PositionKinematicsCache<T>& pc,
687  VelocityKinematicsCache<T>* vc) const;
688 
718  const systems::Context<T>& context,
719  const PositionKinematicsCache<T>& pc,
720  const VelocityKinematicsCache<T>& vc,
721  const VectorX<T>& known_vdot,
723 
756  const systems::Context<T>& context,
757  const PositionKinematicsCache<T>& pc,
758  const VelocityKinematicsCache<T>& vc,
759  const VectorX<T>& known_vdot,
760  std::vector<SpatialAcceleration<T>>* A_WB_array) const;
761 
869  void CalcInverseDynamics(
870  const systems::Context<T>& context,
871  const PositionKinematicsCache<T>& pc,
872  const VelocityKinematicsCache<T>& vc,
873  const VectorX<T>& known_vdot,
874  const std::vector<SpatialForce<T>>& Fapplied_Bo_W_array,
875  const Eigen::Ref<const VectorX<T>>& tau_applied_array,
876  std::vector<SpatialAcceleration<T>>* A_WB_array,
877  std::vector<SpatialForce<T>>* F_BMo_W_array,
878  EigenPtr<VectorX<T>> tau_array) const;
879 
923  const systems::Context<T>& context,
924  const PositionKinematicsCache<T>& pc,
925  const VelocityKinematicsCache<T>& vc,
926  std::vector<SpatialForce<T>>* F_Bo_W_array,
927  EigenPtr<VectorX<T>> tau_array) const;
928 
934  T CalcPotentialEnergy(const systems::Context<T>& context) const;
935 
943  T CalcConservativePower(const systems::Context<T>& context) const;
944 
976  const systems::Context<T>& context, EigenPtr<MatrixX<T>> H) const;
977 
998  void CalcBiasTerm(
999  const systems::Context<T>& context, EigenPtr<VectorX<T>> Cv) const;
1000 
1034 
1036  template <template <typename> class MultibodyElement, typename Scalar>
1037  std::enable_if_t<std::is_base_of<Frame<T>, MultibodyElement<T>>::value,
1038  const MultibodyElement<T>&> get_variant(
1039  const MultibodyElement<Scalar>& element) const {
1040  return get_frame_variant(element);
1041  }
1042 
1044  template <template <typename> class MultibodyElement, typename Scalar>
1045  std::enable_if_t<std::is_base_of<Body<T>, MultibodyElement<T>>::value,
1046  const MultibodyElement<T>&> get_variant(
1047  const MultibodyElement<Scalar>& element) const {
1048  return get_body_variant(element);
1049  }
1050 
1052  template <template <typename> class MultibodyElement, typename Scalar>
1053  std::enable_if_t<std::is_base_of<Mobilizer<T>, MultibodyElement<T>>::value,
1054  const MultibodyElement<T>&> get_variant(
1055  const MultibodyElement<Scalar>& element) const {
1056  return get_mobilizer_variant(element);
1057  }
1058 
1060  template <template <typename> class MultibodyElement, typename Scalar>
1061  std::enable_if_t<std::is_base_of<Joint<T>, MultibodyElement<T>>::value,
1062  const MultibodyElement<T>&> get_variant(
1063  const MultibodyElement<Scalar>& element) const {
1064  return get_joint_variant(element);
1065  }
1067 
1070  std::unique_ptr<MultibodyTree<T>> Clone() const {
1071  return CloneToScalar<T>();
1072  }
1073 
1075  std::unique_ptr<MultibodyTree<AutoDiffXd>> ToAutoDiffXd() const {
1076  return CloneToScalar<AutoDiffXd>();
1077  }
1078 
1115  template <typename ToScalar>
1116  std::unique_ptr<MultibodyTree<ToScalar>> CloneToScalar() const {
1117  if (!topology_is_valid()) {
1118  throw std::logic_error(
1119  "Attempting to clone a MultibodyTree with an invalid topology. "
1120  "MultibodyTree::Finalize() must be called before attempting to clone"
1121  " a MultibodyTree.");
1122  }
1123  auto tree_clone = std::make_unique<MultibodyTree<ToScalar>>();
1124 
1125  tree_clone->frames_.resize(get_num_frames());
1126  // Skipping the world body at body_index = 0.
1127  for (BodyIndex body_index(1); body_index < get_num_bodies(); ++body_index) {
1128  const Body<T>& body = get_body(body_index);
1129  tree_clone->CloneBodyAndAdd(body);
1130  }
1131 
1132  // Frames are cloned in their index order, that is, in the exact same order
1133  // they were added to the original tree. Since the Frame API enforces the
1134  // creation of the parent frame first, this traversal guarantees that parent
1135  // body frames are created before their child frames.
1136  for (const auto& frame : owned_frames_) {
1137  tree_clone->CloneFrameAndAdd(*frame);
1138  }
1139 
1140  for (const auto& mobilizer : owned_mobilizers_) {
1141  // This call assumes that tree_clone already contains all the cloned
1142  // frames.
1143  tree_clone->CloneMobilizerAndAdd(*mobilizer);
1144  }
1145 
1146  for (const auto& force_element : owned_force_elements_) {
1147  tree_clone->CloneForceElementAndAdd(*force_element);
1148  }
1149 
1150  // Since Joint<T> objects are implemented from basic element objects like
1151  // Body, Mobilizer, ForceElement and Constraint, they are cloned last so
1152  // that the clones of their dependencies are guaranteed to be available.
1153  // DO NOT change this order!!!
1154  for (const auto& joint : owned_joints_) {
1155  tree_clone->CloneJointAndAdd(*joint);
1156  }
1157 
1158  // We can safely make a deep copy here since the original multibody tree is
1159  // required to be finalized.
1160  tree_clone->topology_ = this->topology_;
1161  // All other internals templated on T are created with the following call to
1162  // FinalizeInternals().
1163  tree_clone->FinalizeInternals();
1164  return tree_clone;
1165  }
1166 
1167  private:
1168  // Make MultibodyTree templated on every other scalar type a friend of
1169  // MultibodyTree<T> so that CloneToScalar<ToAnyOtherScalar>() can access
1170  // private methods from MultibodyTree<T>.
1171  template <typename> friend class MultibodyTree;
1172 
1173  template <template<typename Scalar> class JointType>
1174  const JointType<T>& AddJoint(
1175  std::unique_ptr<JointType<T>> joint) {
1176  static_assert(std::is_convertible<JointType<T>*, Joint<T>*>::value,
1177  "JointType must be a sub-class of Joint<T>.");
1178  if (topology_is_valid()) {
1179  throw std::logic_error("This MultibodyTree is finalized already. "
1180  "Therefore adding more joints is not allowed. "
1181  "See documentation for Finalize() for details.");
1182  }
1183  if (joint == nullptr) {
1184  throw std::logic_error("Input joint is a nullptr.");
1185  }
1186  const JointIndex joint_index(owned_joints_.size());
1187  joint->set_parent_tree(this, joint_index);
1188  JointType<T>* raw_joint_ptr = joint.get();
1189  owned_joints_.push_back(std::move(joint));
1190  return *raw_joint_ptr;
1191  }
1192 
1193  // Finalizes the MultibodyTreeTopology of this tree.
1194  void FinalizeTopology();
1195 
1196  // At Finalize(), this method performs all other finalization that is not
1197  // topological (i.e. performed by FinalizeTopology()). This includes for
1198  // instance the creation of BodyNode objects.
1199  // This method will throw a std::logic_error if FinalizeTopology() was not
1200  // previously called on this tree.
1201  void FinalizeInternals();
1202 
1203  // Implementation for CalcMassMatrixViaInverseDynamics().
1204  // It assumes:
1205  // - The position kinematics cache object is already updated to be in sync
1206  // with `context`.
1207  // - H is not nullptr.
1208  // - H has storage for a square matrix of size get_num_velocities().
1209  void DoCalcMassMatrixViaInverseDynamics(
1210  const systems::Context<T>& context,
1211  const PositionKinematicsCache<T>& pc,
1212  EigenPtr<MatrixX<T>> H) const;
1213 
1214  // Implementation of CalcBiasTerm().
1215  // It assumes:
1216  // - The position kinematics cache object is already updated to be in sync
1217  // with `context`.
1218  // - The velocity kinematics cache object is already updated to be in sync
1219  // with `context`.
1220  // - Cv is not nullptr.
1221  // - Cv has storage for a vector of size get_num_velocities().
1222  void DoCalcBiasTerm(
1223  const systems::Context<T>& context,
1224  const PositionKinematicsCache<T>& pc,
1225  const VelocityKinematicsCache<T>& vc,
1226  EigenPtr<VectorX<T>> Cv) const;
1227 
1228  // Implementation of CalcPotentialEnergy().
1229  // It is assumed that the position kinematics cache pc is in sync with
1230  // context.
1231  T DoCalcPotentialEnergy(const systems::Context<T>& context,
1232  const PositionKinematicsCache<T>& pc) const;
1233 
1234  // Implementation of CalcConservativePower().
1235  // It is assumed that the position kinematics cache pc and the velocity
1236  // kinematics cache vc are in sync with context.
1237  T DoCalcConservativePower(const systems::Context<T>& context,
1238  const PositionKinematicsCache<T>& pc,
1239  const VelocityKinematicsCache<T>& vc) const;
1240 
1241  void CreateBodyNode(BodyNodeIndex body_node_index);
1242 
1243  // Helper method to create a clone of `frame` and add it to `this` tree.
1244  template <typename FromScalar>
1245  Frame<T>* CloneFrameAndAdd(const Frame<FromScalar>& frame) {
1246  FrameIndex frame_index = frame.get_index();
1247 
1248  auto frame_clone = frame.CloneToScalar(*this);
1249  frame_clone->set_parent_tree(this, frame_index);
1250 
1251  Frame<T>* raw_frame_clone_ptr = frame_clone.get();
1252  // The order in which frames are added into frames_ is important to keep the
1253  // topology invariant. Therefore we index new clones according to the
1254  // original frame_index.
1255  frames_[frame_index] = raw_frame_clone_ptr;
1256  // The order within owned_frames_ does not matter.
1257  owned_frames_.push_back(std::move(frame_clone));
1258  return raw_frame_clone_ptr;
1259  }
1260 
1261  // Helper method to create a clone of `body` and add it to `this` tree.
1262  // Because this method is only invoked in a controlled manner from within
1263  // CloneToScalar(), it is guaranteed that the cloned body in this variant's
1264  // `owned_bodies_` will occupy the same position as its corresponding Body
1265  // in the source variant `body`.
1266  template <typename FromScalar>
1267  Body<T>* CloneBodyAndAdd(const Body<FromScalar>& body) {
1268  const BodyIndex body_index = body.get_index();
1269  const FrameIndex body_frame_index = body.get_body_frame().get_index();
1270 
1271  auto body_clone = body.CloneToScalar(*this);
1272  body_clone->set_parent_tree(this, body_index);
1273  // MultibodyTree can access selected private methods in Body through its
1274  // BodyAttorney.
1275  Frame<T>* body_frame_clone =
1276  &internal::BodyAttorney<T>::get_mutable_body_frame(body_clone.get());
1277  body_frame_clone->set_parent_tree(this, body_frame_index);
1278 
1279  // The order in which frames are added into frames_ is important to keep the
1280  // topology invariant. Therefore we index new clones according to the
1281  // original body_frame_index.
1282  frames_[body_frame_index] = body_frame_clone;
1283  Body<T>* raw_body_clone_ptr = body_clone.get();
1284  // The order in which bodies are added into owned_bodies_ is important to
1285  // keep the topology invariant. Therefore this method is called from
1286  // MultibodyTree::CloneToScalar() within a loop by original body_index.
1287  DRAKE_DEMAND(static_cast<int>(owned_bodies_.size()) == body_index);
1288  owned_bodies_.push_back(std::move(body_clone));
1289  return raw_body_clone_ptr;
1290  }
1291 
1292  // Helper method to create a clone of `mobilizer` and add it to `this` tree.
1293  template <typename FromScalar>
1294  Mobilizer<T>* CloneMobilizerAndAdd(const Mobilizer<FromScalar>& mobilizer) {
1295  MobilizerIndex mobilizer_index = mobilizer.get_index();
1296  auto mobilizer_clone = mobilizer.CloneToScalar(*this);
1297  mobilizer_clone->set_parent_tree(this, mobilizer_index);
1298  Mobilizer<T>* raw_mobilizer_clone_ptr = mobilizer_clone.get();
1299  owned_mobilizers_.push_back(std::move(mobilizer_clone));
1300  return raw_mobilizer_clone_ptr;
1301  }
1302 
1303  // Helper method to create a clone of `force_element` and add it to `this`
1304  // tree.
1305  template <typename FromScalar>
1306  void CloneForceElementAndAdd(
1307  const ForceElement<FromScalar>& force_element) {
1308  ForceElementIndex force_element_index = force_element.get_index();
1309  auto force_element_clone = force_element.CloneToScalar(*this);
1310  force_element_clone->set_parent_tree(this, force_element_index);
1311  owned_force_elements_.push_back(std::move(force_element_clone));
1312  }
1313 
1314  // Helper method to create a clone of `joint` and add it to `this` tree.
1315  template <typename FromScalar>
1316  Joint<T>* CloneJointAndAdd(const Joint<FromScalar>& joint) {
1317  JointIndex joint_index = joint.get_index();
1318  auto joint_clone = joint.CloneToScalar(*this);
1319  joint_clone->set_parent_tree(this, joint_index);
1320  owned_joints_.push_back(std::move(joint_clone));
1321  return owned_joints_.back().get();
1322  }
1323 
1324  // Helper method to retrieve the corresponding Frame<T> variant to a Frame in
1325  // a MultibodyTree variant templated on Scalar.
1326  template <template <typename> class FrameType, typename Scalar>
1327  const FrameType<T>& get_frame_variant(const FrameType<Scalar>& frame) const {
1328  static_assert(std::is_base_of<Frame<T>, FrameType<T>>::value,
1329  "FrameType<T> must be a sub-class of Frame<T>.");
1330  // TODO(amcastro-tri):
1331  // DRAKE_DEMAND the parent tree of the variant is indeed a variant of this
1332  // MultibodyTree. That will require the tree to have some sort of id.
1333  FrameIndex frame_index = frame.get_index();
1334  DRAKE_DEMAND(frame_index < get_num_frames());
1335  const FrameType<T>* frame_variant =
1336  dynamic_cast<const FrameType<T>*>(frames_[frame_index]);
1337  DRAKE_DEMAND(frame_variant != nullptr);
1338  return *frame_variant;
1339  }
1340 
1341  // Helper method to retrieve the corresponding Body<T> variant to a Body in a
1342  // MultibodyTree variant templated on Scalar.
1343  template <template <typename> class BodyType, typename Scalar>
1344  const BodyType<T>& get_body_variant(const BodyType<Scalar>& body) const {
1345  static_assert(std::is_base_of<Body<T>, BodyType<T>>::value,
1346  "BodyType<T> must be a sub-class of Body<T>.");
1347  // TODO(amcastro-tri):
1348  // DRAKE_DEMAND the parent tree of the variant is indeed a variant of this
1349  // MultibodyTree. That will require the tree to have some sort of id.
1350  BodyIndex body_index = body.get_index();
1351  DRAKE_DEMAND(body_index < get_num_bodies());
1352  const BodyType<T>* body_variant =
1353  dynamic_cast<const BodyType<T>*>(
1354  owned_bodies_[body_index].get());
1355  DRAKE_DEMAND(body_variant != nullptr);
1356  return *body_variant;
1357  }
1358 
1359  // Helper method to retrieve the corresponding Mobilizer<T> variant to a Body
1360  // in a MultibodyTree variant templated on Scalar.
1361  template <template <typename> class MobilizerType, typename Scalar>
1362  const MobilizerType<T>& get_mobilizer_variant(
1363  const MobilizerType<Scalar>& mobilizer) const {
1364  static_assert(std::is_base_of<Mobilizer<T>, MobilizerType<T>>::value,
1365  "MobilizerType<T> must be a sub-class of Mobilizer<T>.");
1366  // TODO(amcastro-tri):
1367  // DRAKE_DEMAND the parent tree of the variant is indeed a variant of this
1368  // MultibodyTree. That will require the tree to have some sort of id.
1369  MobilizerIndex mobilizer_index = mobilizer.get_index();
1370  DRAKE_DEMAND(mobilizer_index < get_num_mobilizers());
1371  const MobilizerType<T>* mobilizer_variant =
1372  dynamic_cast<const MobilizerType<T>*>(
1373  owned_mobilizers_[mobilizer_index].get());
1374  DRAKE_DEMAND(mobilizer_variant != nullptr);
1375  return *mobilizer_variant;
1376  }
1377 
1378  // Helper method to retrieve the corresponding Joint<T> variant to a Joint
1379  // in a MultibodyTree variant templated on Scalar.
1380  template <template <typename> class JointType, typename Scalar>
1381  const JointType<T>& get_joint_variant(const JointType<Scalar>& joint) const {
1382  static_assert(std::is_base_of<Joint<T>, JointType<T>>::value,
1383  "JointType<T> must be a sub-class of Joint<T>.");
1384  // TODO(amcastro-tri):
1385  // DRAKE_DEMAND the parent tree of the variant is indeed a variant of this
1386  // MultibodyTree. That will require the tree to have some sort of id.
1387  JointIndex joint_index = joint.get_index();
1388  DRAKE_DEMAND(joint_index < get_num_joints());
1389  const JointType<T>* joint_variant =
1390  dynamic_cast<const JointType<T>*>(
1391  owned_joints_[joint_index].get());
1392  DRAKE_DEMAND(joint_variant != nullptr);
1393  return *joint_variant;
1394  }
1395 
1396  // TODO(amcastro-tri): In future PR's adding MBT computational methods, write
1397  // a method that verifies the state of the topology with a signature similar
1398  // to RoadGeometry::CheckInvariants().
1399 
1400  const RigidBody<T>* world_body_{nullptr};
1401  std::vector<std::unique_ptr<Body<T>>> owned_bodies_;
1402  std::vector<std::unique_ptr<Frame<T>>> owned_frames_;
1403  std::vector<std::unique_ptr<Mobilizer<T>>> owned_mobilizers_;
1404  std::vector<std::unique_ptr<ForceElement<T>>> owned_force_elements_;
1405  std::vector<std::unique_ptr<internal::BodyNode<T>>> body_nodes_;
1406 
1407  std::vector<std::unique_ptr<Joint<T>>> owned_joints_;
1408 
1409  // List of all frames in the system ordered by their FrameIndex.
1410  // This vector contains a pointer to all frames in owned_frames_ as well as a
1411  // pointer to each BodyFrame, which are owned by their corresponding Body.
1412  std::vector<const Frame<T>*> frames_;
1413 
1414  // Body node indexes ordered by level (a.k.a depth). Therefore for the
1415  // i-th level body_node_levels_[i] contains the list of all body node indexes
1416  // in that level.
1417  std::vector<std::vector<BodyNodeIndex>> body_node_levels_;
1418 
1419  MultibodyTreeTopology topology_;
1420 };
1421 
1422 } // namespace multibody
1423 } // namespace drake
std::unique_ptr< MultibodyTree< AutoDiffXd > > ToAutoDiffXd() const
Creates a deep copy of this MultibodyTree templated on AutoDiffXd.
Definition: multibody_tree.h:1075
Eigen::Transform< Scalar, 3, Eigen::Isometry > Isometry3
An Isometry templated on scalar type.
Definition: eigen_types.h:106
This class is used to represent a spatial acceleration that combines rotational (angular acceleration...
Definition: spatial_acceleration.h:52
const RigidBody< T > & get_world_body() const
Returns a constant reference to the world body.
Definition: multibody_tree.h:566
const Body< double > * world_body_
Definition: double_pendulum_test.cc:269
int get_num_force_elements() const
Returns the number of ForceElement objects in the MultibodyTree.
Definition: multibody_tree.h:535
int get_tree_height() const
Returns the number of tree levels in the topology.
Definition: multibody_tree_topology.h:429
This class is one of the cache entries in MultibodyTreeContext.
Definition: acceleration_kinematics_cache.h:34
int get_num_states() const
Returns the total size of the state vector in the model.
Definition: multibody_tree.h:550
bool topology_is_valid() const
Returns true if this MultibodyTree was finalized with Finalize() after all multibody elements were ad...
Definition: multibody_tree.h:608
void Finalize()
This method must be called after all elements in the tree (joints, bodies, force elements, constraints) were added and before any computations are performed.
Definition: multibody_tree.cc:105
Definition: automotive_demo.cc:88
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:1070
STL namespace.
Context is an abstract base class template that represents all the inputs to a System: time...
Definition: query_handle.h:10
const FrameType< T > & AddFrame(std::unique_ptr< FrameType< T >> frame)
Takes ownership of frame and adds it to this MultibodyTree.
Definition: multibody_tree.h:188
std::unique_ptr< systems::LeafContext< T > > CreateDefaultContext() const
Allocates a new context for this MultibodyTree uniquely identifying the state of the multibody system...
Definition: multibody_tree.cc:158
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:495
int get_num_velocities() const
Returns the number of generalized velocities of the model.
Definition: multibody_tree.h:545
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
A column vector of any size, templated on scalar type.
Definition: eigen_types.h:46
void 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:396
Body provides the general abstraction of a body with an API that makes no assumption about whether a ...
Definition: body.h:19
int get_num_velocities() const
Returns the total number of generalized velocities in the model.
Definition: multibody_tree_topology.h:753
void SetDefaultContext(systems::Context< T > *context) const
Sets default values in the context.
Definition: multibody_tree.cc:171
MultibodyTree provides a representation for a physical system consisting of a collection of interconn...
Definition: body.h:104
const BodyFrame< T > & get_world_frame() const
Returns a constant reference to the world frame.
Definition: multibody_tree.h:574
void CalcForceElementsContribution(const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, const VelocityKinematicsCache< T > &vc, std::vector< SpatialForce< T >> *F_Bo_W_array, EigenPtr< VectorX< T >> tau_array) const
Computes the combined force contribution of ForceElement objects in the model.
Definition: multibody_tree.cc:370
ForceElementIndex add_force_element()
Creates and adds a new ForceElementTopology, associated with the given force_index, to this MultibodyTreeTopology.
Definition: multibody_tree_topology.h:585
std::unique_ptr< MultibodyTree< ToScalar > > CloneToScalar() const
Creates a deep copy of this MultibodyTree templated on the scalar type ToScalar.
Definition: multibody_tree.h:1116
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:1062
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:436
A ForceElement allows modeling state and time dependent forces in a MultibodyTree model...
Definition: force_element.h:35
std::vector< double > vector
Definition: translator_test.cc:20
const MultibodyTreeTopology & get_topology() const
Returns the topology information for this multibody tree.
Definition: multibody_tree.h:614
const BodyFrame< T > & get_body_frame() const
Returns a const reference to the associated BodyFrame.
Definition: body.h:161
stx::optional< T > optional
Definition: drake_optional.h:14
int get_num_bodies() const
Returns the number of bodies in the MultibodyTree including the world body.
Definition: multibody_tree.h:519
A BodyFrame is a material Frame that serves as the unique reference frame for a Body.
Definition: body.h:57
vector< FrameId > frames_
Definition: geometry_state_test.cc:257
const Frame< T > & get_frame(FrameIndex frame_index) const
Returns a constant reference to the frame with unique index frame_index.
Definition: multibody_tree.h:589
std::enable_if_t< std::is_base_of< Body< T >, MultibodyElement< T > >::value, const MultibodyElement< T > & > get_variant(const MultibodyElement< Scalar > &element) const
SFINAE overload for Body<T> elements.
Definition: multibody_tree.h:1046
const BodyType< T > & AddBody(std::unique_ptr< BodyType< T >> body)
Takes ownership of body and adds it to this MultibodyTree.
Definition: multibody_tree.h:90
int get_num_frames() const
Returns the number of Frame objects in the MultibodyTree.
Definition: multibody_tree.h:513
std::pair< BodyIndex, FrameIndex > add_body()
Creates and adds a new BodyTopology to this MultibodyTreeTopology.
Definition: multibody_tree_topology.h:470
Frame is an abstract class representing a material frame (also called a physical frame), meaning that it is associated with a material point of a Body.
Definition: frame.h:41
#define DRAKE_ASSERT(condition)
DRAKE_ASSERT(condition) is similar to the built-in assert(condition) from the C++ system header <cas...
Definition: drake_assert.h:37
int get_num_mobilizers() const
Returns the number of mobilizers in the MultibodyTree.
Definition: multibody_tree.h:530
int get_num_joints() const
Returns the number of joints added with AddJoint() to the MultibodyTree.
Definition: multibody_tree.h:522
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > MatrixX
A matrix of dynamic size, templated on scalar type.
Definition: eigen_types.h:87
const FrameType< T > & AddFrame(Args &&...args)
Constructs a new frame with type FrameType with the given args, and adds it to this MultibodyTree...
Definition: multibody_tree.h:248
int value
Definition: copyable_unique_ptr_test.cc:61
const MobilizerType< T > & AddMobilizer(Args &&...args)
Constructs a new mobilizer with type MobilizerType with the given args, and adds it to this Multibody...
Definition: multibody_tree.h:371
std::enable_if_t< std::is_base_of< Frame< T >, MultibodyElement< T > >::value, const MultibodyElement< T > & > get_variant(const MultibodyElement< Scalar > &element) const
SFINAE overload for Frame<T> elements.
Definition: multibody_tree.h:1038
#define DRAKE_DEMAND(condition)
Evaluates condition and iff the value is false will trigger an assertion failure with a message showi...
Definition: drake_assert.h:45
int get_num_positions() const
Returns the total number of generalized positions in the model.
Definition: multibody_tree_topology.h:750
Provides drake::optional as an alias for the appropriate implementation of std::optional or std::expe...
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:475
Data structure to store the topological information associated with an entire MultibodyTree.
Definition: multibody_tree_topology.h:372
const ForceElementType< T > & AddForceElement(Args &&...args)
Definition: multibody_tree.h:411
MultibodyTree()
Creates a MultibodyTree containing only a world body.
Definition: multibody_tree.cc:43
const ForceElementType< T > & AddForceElement(std::unique_ptr< ForceElementType< T >> force_element)
Creates and adds to this MultibodyTree (which retains ownership) a new ForceElement member with the s...
Definition: multibody_tree.h:386
std::unique_ptr< Frame< ToScalar > > CloneToScalar(const MultibodyTree< ToScalar > &tree_clone) const
NVI to DoCloneToScalar() templated on the scalar type of the new clone to be created.
Definition: frame.h:80
This class is one of the cache entries in MultibodyTreeContext.
Definition: velocity_kinematics_cache.h:37
bool is_valid() const
Returns true if Finalize() was already called on this topology.
Definition: multibody_tree_topology.h:747
This wrapper class provides a way to write non-template functions taking raw pointers to Eigen object...
Definition: eigen_types.h:275
This class is one of the cache entries in MultibodyTreeContext.
Definition: position_kinematics_cache.h:33
int get_tree_height() const
Returns the height of the tree data structure of this MultibodyTree.
Definition: multibody_tree.h:561
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:236
const JointType< T > & AddJoint(const std::string &name, const Body< T > &parent, const optional< Isometry3< double >> &X_PF, const Body< T > &child, const optional< Isometry3< double >> &X_BM, Args &&...args)
This method helps to create a Joint of type JointType between two bodies.
Definition: multibody_tree.h:478
Mobilizer is a fundamental object within Drake&#39;s multibody engine used to specify the allowed motions...
Definition: mobilizer.h:216
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:471
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:273
const Body< T > & get_body(BodyIndex body_index) const
Returns a constant reference to the body with unique index body_index.
Definition: multibody_tree.h:581
The term rigid body implies that the deformations of the body under consideration are so small that t...
Definition: fixed_offset_frame.h:16
void CalcInverseDynamics(const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, const VelocityKinematicsCache< T > &vc, const VectorX< T > &known_vdot, const std::vector< SpatialForce< T >> &Fapplied_Bo_W_array, const Eigen::Ref< const VectorX< T >> &tau_applied_array, std::vector< SpatialAcceleration< T >> *A_WB_array, std::vector< SpatialForce< T >> *F_BMo_W_array, EigenPtr< VectorX< T >> tau_array) const
Given the state of this MultibodyTree in context and a known vector of generalized accelerations vdot...
Definition: multibody_tree.cc:291
const BodyType< T > & AddBody(Args &&...args)
Constructs a new body with type BodyType with the given args, and adds it to this MultibodyTree...
Definition: multibody_tree.h:156
MobilizerIndex add_mobilizer(FrameIndex in_frame, FrameIndex out_frame, int num_positions, int num_velocities)
Creates and adds a new MobilizerTopology connecting the inboard and outboard multibody frames identif...
Definition: multibody_tree_topology.h:515
void CalcVelocityKinematicsCache(const systems::Context< T > &context, const PositionKinematicsCache< T > &pc, VelocityKinematicsCache< T > *vc) const
Computes all the kinematic quantities that depend on the generalized velocities and stores them in th...
Definition: multibody_tree.cc:209
std::unique_ptr< Body< ToScalar > > CloneToScalar(const MultibodyTree< ToScalar > &tree_clone) const
NVI (Non-Virtual Interface) to DoCloneToScalar() templated on the scalar type of the new clone to be ...
Definition: body.h:198
This class is used to represent a spatial force (also called a wrench) that combines both rotational ...
Definition: spatial_force.h:40
void CalcPositionKinematicsCache(const systems::Context< T > &context, PositionKinematicsCache< T > *pc) const
Computes into the position kinematics pc all the kinematic quantities that depend on the generalized ...
Definition: multibody_tree.cc:178
A Joint models the kinematical relationship which characterizes the possible relative motion between ...
Definition: joint.h:75
const MobilizerType< T > & AddMobilizer(std::unique_ptr< MobilizerType< T >> mobilizer)
Takes ownership of mobilizer and adds it to this MultibodyTree.
Definition: multibody_tree.h:295
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33
int get_num_states() const
Returns the total size of the state vector in the model.
Definition: multibody_tree_topology.h:756
FrameIndex add_frame(BodyIndex body_index)
Creates and adds a new FrameTopology, associated with the given body_index, to this MultibodyTreeTopo...
Definition: multibody_tree_topology.h:489
Vector3d body_frame
Definition: model_test.cc:38
int get_num_positions() const
Returns the number of generalized positions of the model.
Definition: multibody_tree.h:540
const Mobilizer< T > & get_mobilizer(MobilizerIndex mobilizer_index) const
Returns a constant reference to the mobilizer with unique index mobilizer_index.
Definition: multibody_tree.h:598
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:32
std::enable_if_t< std::is_base_of< Mobilizer< T >, MultibodyElement< T > >::value, const MultibodyElement< T > & > get_variant(const MultibodyElement< Scalar > &element) const
SFINAE overload for Mobilizer<T> elements.
Definition: multibody_tree.h:1054