Drake
multibody_tree.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <tuple>
5 #include <type_traits>
6 #include <utility>
7 #include <vector>
8 
20 
21 namespace drake {
22 namespace multibody {
23 
38 template <typename T>
39 class MultibodyTree {
40  public:
42 
43 
44  MultibodyTree();
45 
54  // TODO(amcastro-tri): add at least one example of a method that requires a
55  // valid topology in this documentation.
56  // See this Reviewable comment:
57  // https://reviewable.io/reviews/robotlocomotion/drake/5583#-KgGqGisnX9uMuYDkHpx
58 
84  template <template<typename Scalar> class BodyType>
85  const BodyType<T>& AddBody(std::unique_ptr<BodyType<T>> body) {
86  static_assert(std::is_convertible<BodyType<T>*, Body<T>*>::value,
87  "BodyType must be a sub-class of Body<T>.");
88  if (topology_is_valid()) {
89  throw std::logic_error("This MultibodyTree is finalized already. "
90  "Therefore adding more bodies is not allowed. "
91  "See documentation for Finalize() for details.");
92  }
93  if (body == nullptr) {
94  throw std::logic_error("Input body is a nullptr.");
95  }
96  BodyIndex body_index(0);
97  FrameIndex body_frame_index(0);
98  std::tie(body_index, body_frame_index) = topology_.add_body();
99  // These tests MUST be performed BEFORE frames_.push_back() and
100  // owned_bodies_.push_back() below. Do not move them around!
101  DRAKE_ASSERT(body_index == get_num_bodies());
102  DRAKE_ASSERT(body_frame_index == get_num_frames());
103 
104  // TODO(amcastro-tri): consider not depending on setting this pointer at
105  // all. Consider also removing MultibodyTreeElement altogether.
106  body->set_parent_tree(this, body_index);
107  // MultibodyTree can access selected private methods in Body through its
108  // BodyAttorney.
110  &internal::BodyAttorney<T>::get_mutable_body_frame(body.get());
111  body_frame->set_parent_tree(this, body_frame_index);
112  frames_.push_back(body_frame);
113  BodyType<T>* raw_body_ptr = body.get();
114  owned_bodies_.push_back(std::move(body));
115  return *raw_body_ptr;
116  }
117 
150  template<template<typename Scalar> class BodyType, typename... Args>
151  const BodyType<T>& AddBody(Args&&... args) {
152  static_assert(std::is_convertible<BodyType<T>*, Body<T>*>::value,
153  "BodyType must be a sub-class of Body<T>.");
154  return AddBody(std::make_unique<BodyType<T>>(std::forward<Args>(args)...));
155  }
156 
182  template <template<typename Scalar> class FrameType>
183  const FrameType<T>& AddFrame(std::unique_ptr<FrameType<T>> frame) {
184  static_assert(std::is_convertible<FrameType<T>*, Frame<T>*>::value,
185  "FrameType must be a sub-class of Frame<T>.");
186  if (topology_is_valid()) {
187  throw std::logic_error("This MultibodyTree is finalized already. "
188  "Therefore adding more frames is not allowed. "
189  "See documentation for Finalize() for details.");
190  }
191  if (frame == nullptr) {
192  throw std::logic_error("Input frame is a nullptr.");
193  }
194  FrameIndex frame_index = topology_.add_frame(frame->get_body().get_index());
195  // This test MUST be performed BEFORE frames_.push_back() and
196  // owned_frames_.push_back() below. Do not move it around!
197  DRAKE_ASSERT(frame_index == get_num_frames());
198  // TODO(amcastro-tri): consider not depending on setting this pointer at
199  // all. Consider also removing MultibodyTreeElement altogether.
200  frame->set_parent_tree(this, frame_index);
201  FrameType<T>* raw_frame_ptr = frame.get();
202  frames_.push_back(raw_frame_ptr);
203  owned_frames_.push_back(std::move(frame));
204  return *raw_frame_ptr;
205  }
206 
242  template<template<typename Scalar> class FrameType, typename... Args>
243  const FrameType<T>& AddFrame(Args&&... args) {
244  static_assert(std::is_convertible<FrameType<T>*, Frame<T>*>::value,
245  "FrameType must be a sub-class of Frame<T>.");
246  return AddFrame(
247  std::make_unique<FrameType<T>>(std::forward<Args>(args)...));
248  }
249 
289  template <template<typename Scalar> class MobilizerType>
290  const MobilizerType<T>& AddMobilizer(
291  std::unique_ptr<MobilizerType<T>> mobilizer) {
292  static_assert(std::is_convertible<MobilizerType<T>*, Mobilizer<T>*>::value,
293  "MobilizerType must be a sub-class of mobilizer<T>.");
294  if (topology_is_valid()) {
295  throw std::logic_error("This MultibodyTree is finalized already. "
296  "Therefore adding more bodies is not allowed. "
297  "See documentation for Finalize() for details.");
298  }
299  if (mobilizer == nullptr) {
300  throw std::logic_error("Input mobilizer is a nullptr.");
301  }
302  // Verifies that the inboard/outboard frames provided by the user do belong
303  // to this tree. This is a pathological case, but in theory nothing
304  // (but this test) stops a user from adding frames to a tree1 and attempting
305  // later to define mobilizers between those frames in a second tree2.
306  mobilizer->get_inboard_frame().HasThisParentTreeOrThrow(this);
307  mobilizer->get_outboard_frame().HasThisParentTreeOrThrow(this);
308  const int num_positions = mobilizer->get_num_positions();
309  const int num_velocities = mobilizer->get_num_velocities();
310  MobilizerIndex mobilizer_index = topology_.add_mobilizer(
311  mobilizer->get_inboard_frame().get_index(),
312  mobilizer->get_outboard_frame().get_index(),
313  num_positions, num_velocities);
314 
315  // This DRAKE_ASSERT MUST be performed BEFORE owned_mobilizers_.push_back()
316  // below. Do not move it around!
317  DRAKE_ASSERT(mobilizer_index == get_num_mobilizers());
318 
319  // TODO(amcastro-tri): consider not depending on setting this pointer at
320  // all. Consider also removing MultibodyTreeElement altogether.
321  mobilizer->set_parent_tree(this, mobilizer_index);
322 
323  MobilizerType<T>* raw_mobilizer_ptr = mobilizer.get();
324  owned_mobilizers_.push_back(std::move(mobilizer));
325  return *raw_mobilizer_ptr;
326  }
327 
365  template<template<typename Scalar> class MobilizerType, typename... Args>
366  const MobilizerType<T>& AddMobilizer(Args&&... args) {
367  static_assert(std::is_base_of<Mobilizer<T>, MobilizerType<T>>::value,
368  "MobilizerType must be a sub-class of Mobilizer<T>.");
369  return AddMobilizer(
370  std::make_unique<MobilizerType<T>>(std::forward<Args>(args)...));
371  }
372 
374  // Closes Doxygen section.
375 
380  int get_num_frames() const {
381  return static_cast<int>(frames_.size());
382  }
383 
386  int get_num_bodies() const { return static_cast<int>(owned_bodies_.size()); }
387 
391  // TODO(amcastro-tri): Consider adding a WorldMobilizer (0-dofs) for the world
392  // body. This could be useful to query for reaction forces of the entire
393  // model.
394  int get_num_mobilizers() const {
395  return static_cast<int>(owned_mobilizers_.size());
396  }
397 
399  int get_num_positions() const {
400  return topology_.get_num_positions();
401  }
402 
404  int get_num_velocities() const {
405  return topology_.get_num_velocities();
406  }
407 
415  int get_tree_height() const {
416  return topology_.get_tree_height();
417  }
418 
420  const Body<T>& get_world_body() const {
421  return *owned_bodies_[world_index()];
422  }
423 
425  const BodyFrame<T>& get_world_frame() const {
426  return owned_bodies_[world_index()]->get_body_frame();
427  }
428 
432  const Body<T>& get_body(BodyIndex body_index) const {
433  DRAKE_ASSERT(body_index < get_num_bodies());
434  return *owned_bodies_[body_index];
435  }
436 
442  bool topology_is_valid() const { return topology_.is_valid(); }
443 
448  const MultibodyTreeTopology& get_topology() const { return topology_; }
449 
464  // TODO(amcastro-tri): Consider making this method private and calling it
465  // automatically when CreateDefaultContext() is called.
466  void Finalize();
467 
477  // TODO(amcastro-tri): Split this method into implementations to be used by
478  // System::AllocateContext() so that MultibodyPlant() can make use of it
479  // within the system's infrastructure. This will require at least the
480  // introduction of system's methods to:
481  // - Create a context different from LeafContext, in this case MBTContext.
482  // - Create or request cache entries.
483  std::unique_ptr<systems::Context<T>> CreateDefaultContext() const;
484 
488  void SetDefaults(systems::Context<T>* context) const;
489 
502  const systems::Context<T>& context,
503  PositionKinematicsCache<T>* pc) const;
504 
519  const systems::Context<T>& context,
520  const PositionKinematicsCache<T>& pc,
521  VelocityKinematicsCache<T>* vc) const;
522 
552  const systems::Context<T>& context,
553  const PositionKinematicsCache<T>& pc,
554  const VelocityKinematicsCache<T>& vc,
555  const VectorX<T>& known_vdot,
557 
590  const systems::Context<T>& context,
591  const PositionKinematicsCache<T>& pc,
592  const VelocityKinematicsCache<T>& vc,
593  const VectorX<T>& known_vdot,
594  std::vector<SpatialAcceleration<T>>* A_WB_array) const;
595 
657  // TODO(amcastro-tri): add provision for an array of applied spatial forces
658  // per body. These could contain both external as well as constraint forces.
659  void CalcInverseDynamics(
660  const systems::Context<T>& context,
661  const PositionKinematicsCache<T>& pc,
662  const VelocityKinematicsCache<T>& vc,
663  const VectorX<T>& known_vdot,
664  std::vector<SpatialAcceleration<T>>* A_WB_array,
665  std::vector<SpatialForce<T>>* F_BMo_W_array,
666  VectorX<T>* tau_array) const;
667 
668  private:
669  void CreateBodyNode(BodyNodeIndex body_node_index);
670 
671  // TODO(amcastro-tri): In future PR's adding MBT computational methods, write
672  // a method that verifies the state of the topology with a signature similar
673  // to RoadGeometry::CheckInvariants().
674 
675  std::vector<std::unique_ptr<Body<T>>> owned_bodies_;
676  std::vector<std::unique_ptr<Frame<T>>> owned_frames_;
677  std::vector<std::unique_ptr<Mobilizer<T>>> owned_mobilizers_;
678  std::vector<std::unique_ptr<internal::BodyNode<T>>> body_nodes_;
679 
680  // List of all frames in the system ordered by their FrameIndex.
681  // This vector contains a pointer to all frames in owned_frames_ as well as a
682  // pointer to each BodyFrame, which are owned by their corresponding Body.
683  std::vector<const Frame<T>*> frames_;
684 
685  // Body node indexes ordered by level (a.k.a depth). Therefore for the
686  // i-th level body_node_levels_[i] contains the list of all body node indexes
687  // in that level.
688  std::vector<std::vector<BodyNodeIndex>> body_node_levels_;
689 
690  MultibodyTreeTopology topology_;
691 };
692 
693 } // namespace multibody
694 } // namespace drake
This class is used to represent a spatial acceleration that combines rotational (angular acceleration...
Definition: spatial_acceleration.h:52
int get_tree_height() const
Returns the number of tree levels in the topology.
Definition: multibody_tree_topology.h:293
This class is one of the cache entries in MultibodyTreeContext.
Definition: acceleration_kinematics_cache.h:34
bool topology_is_valid() const
Returns true if this MultibodyTree was finalized with Finalize() after all multibody elements were ad...
Definition: multibody_tree.h:442
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:27
NOTE: The contents of this class are for the most part direct ports of drake/systems/plants//inverseK...
Definition: automotive_demo.cc:88
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:183
int get_num_velocities() const
Returns the number of generalized velocities of the model.
Definition: multibody_tree.h:404
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
A column vector of any size, templated on scalar type.
Definition: eigen_types.h:43
Body provides the general abstraction of a body with an API that makes no assumption about whether a ...
Definition: body.h:17
int get_num_velocities() const
Returns the total number of generalized velocities in the model.
Definition: multibody_tree_topology.h:598
const BodyFrame< T > & get_world_frame() const
Returns a constant reference to the world frame.
Definition: multibody_tree.h:425
void SetDefaults(systems::Context< T > *context) const
Sets default values in the context.
Definition: multibody_tree.cc:125
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:448
int get_num_bodies() const
Returns the number of bodies in the MultibodyTree including the world body.
Definition: multibody_tree.h:386
A BodyFrame is a material Frame that serves as the unique reference frame for a Body.
Definition: body.h:55
const BodyType< T > & AddBody(std::unique_ptr< BodyType< T >> body)
Takes ownership of body and adds it to this MultibodyTree.
Definition: multibody_tree.h:85
int get_num_frames() const
Returns the number of Frame objects in the MultibodyTree.
Definition: multibody_tree.h:380
std::pair< BodyIndex, FrameIndex > add_body()
Creates and adds a new BodyTopology to this MultibodyTreeTopology.
Definition: multibody_tree_topology.h:334
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:38
#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:39
int get_num_mobilizers() const
Returns the number of mobilizers in the MultibodyTree.
Definition: multibody_tree.h:394
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:243
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:366
int get_num_positions() const
Returns the total number of generalized positions in the model.
Definition: multibody_tree_topology.h:595
const Body< T > & get_world_body() const
Returns a constant reference to the world body.
Definition: multibody_tree.h:420
Data structure to store the topological information associated with an entire MultibodyTree.
Definition: multibody_tree_topology.h:259
MultibodyTree()
Creates a MultibodyTree containing only a world body.
Definition: multibody_tree.cc:21
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:592
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:415
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:190
Mobilizer is a fundamental object within Drake&#39;s multibody engine used to specify the allowed motions...
Definition: mobilizer.h:215
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:227
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:432
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:151
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:379
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:163
This class is used to represent a spatial force (also called a wrench) that combines both rotational ...
Definition: spatial_force.h:40
std::unique_ptr< systems::Context< T > > CreateDefaultContext() const
Allocates a new context for this MultibodyTree uniquely identifying the state of the multibody system...
Definition: multibody_tree.cc:112
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:132
const MobilizerType< T > & AddMobilizer(std::unique_ptr< MobilizerType< T >> mobilizer)
Takes ownership of mobilizer and adds it to this MultibodyTree.
Definition: multibody_tree.h:290
#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:35
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:353
Vector3d body_frame
Definition: model_test.cc:36
void CalcInverseDynamics(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, std::vector< SpatialForce< T >> *F_BMo_W_array, 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:245
int get_num_positions() const
Returns the number of generalized positions of the model.
Definition: multibody_tree.h:399
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:25