From ce88092b80baf6b83c433798ddbf89d95c12f7c2 Mon Sep 17 00:00:00 2001 From: Alejandro Castro Date: Thu, 18 May 2017 09:39:58 -0400 Subject: [PATCH] MBT compile topological information (#6120) * Compiles BodyNodeTopology * Updates multibody_tree_creation_tests.cc * Adds unit tests for a complex tree topology * Adds unit tests doc * Adds more doc * Addresses reviews * Fixes gcc warning * Addresses reviews * Makes MBTTopology into a class * Refactors multibody_tree_creation_tests to stop accessing private members of topology * Makes MBTT::bodies_ private * Makes MBTT::body_nodes_ private * Makes MBTT::mobilizers_ private * Makes MBTT::frames_ private * Cleansup lint. * Addresses reviews * Get rid of copy-pasta --- drake/multibody/multibody_tree/body.h | 2 +- drake/multibody/multibody_tree/mobilizer.h | 2 +- .../multibody_tree/multibody_tree.cc | 7 +- .../multibody/multibody_tree/multibody_tree.h | 11 +- .../multibody_tree/multibody_tree_indexes.h | 3 + .../multibody_tree/multibody_tree_topology.h | 343 ++++++++++++++++-- .../test/multibody_tree_creation_tests.cc | 297 ++++++++++++--- 7 files changed, 564 insertions(+), 101 deletions(-) diff --git a/drake/multibody/multibody_tree/body.h b/drake/multibody/multibody_tree/body.h index dd8bd1da8389..fa80e0a1a1c4 100644 --- a/drake/multibody/multibody_tree/body.h +++ b/drake/multibody/multibody_tree/body.h @@ -137,7 +137,7 @@ class Body : public MultibodyTreeElement, BodyIndex> { // At MultibodyTree::Finalize() time, each body retrieves its topology // from the parent MultibodyTree. void DoSetTopology(const MultibodyTreeTopology& tree_topology) final { - topology_ = tree_topology.bodies[this->get_index()]; + topology_ = tree_topology.get_body(this->get_index()); body_frame_.SetTopology(tree_topology); } diff --git a/drake/multibody/multibody_tree/mobilizer.h b/drake/multibody/multibody_tree/mobilizer.h index 072293e0c006..213e98b06b74 100644 --- a/drake/multibody/multibody_tree/mobilizer.h +++ b/drake/multibody/multibody_tree/mobilizer.h @@ -183,7 +183,7 @@ class Mobilizer : public MultibodyTreeElement, MobilizerIndex> { // At MultibodyTree::Finalize() time, each mobilizer retrieves its topology // from the parent MultibodyTree. void DoSetTopology(const MultibodyTreeTopology& tree_topology) final { - topology_ = tree_topology.mobilizers[this->get_index()]; + topology_ = tree_topology.get_mobilizer(this->get_index()); } const Frame& inboard_frame_; diff --git a/drake/multibody/multibody_tree/multibody_tree.cc b/drake/multibody/multibody_tree/multibody_tree.cc index 486346e94b66..ab2e0f1f7f9b 100644 --- a/drake/multibody/multibody_tree/multibody_tree.cc +++ b/drake/multibody/multibody_tree/multibody_tree.cc @@ -28,9 +28,12 @@ void MultibodyTree::Finalize() { "MultibodyTree."); } + // Before performing any setup that depends on the scalar type , compile + // all the type-T independent topological information. + topology_.Finalize(); + // TODO(amcastro-tri): This is a brief list of operations to be added in // subsequent PR's: - // - Finalize non-T dependent topological information. // - Compute degrees of freedom, array sizes and any other information to // allocate a context and request the required cache entries. // - Setup computational structures (BodyNode based). @@ -49,8 +52,6 @@ void MultibodyTree::Finalize() { for (const auto& mobilizer : owned_mobilizers_) { mobilizer->SetTopology(topology_); } - - set_valid_topology(); } // Explicitly instantiates on the most common scalar types. diff --git a/drake/multibody/multibody_tree/multibody_tree.h b/drake/multibody/multibody_tree/multibody_tree.h index 09e61b1d7b27..9e739c898bf7 100644 --- a/drake/multibody/multibody_tree/multibody_tree.h +++ b/drake/multibody/multibody_tree/multibody_tree.h @@ -79,7 +79,7 @@ class MultibodyTree { const BodyType& AddBody(std::unique_ptr> body) { static_assert(std::is_convertible*, Body*>::value, "BodyType must be a sub-class of Body."); - if (topology_.is_valid) { + if (topology_is_valid()) { throw std::logic_error("This MultibodyTree is finalized already. " "Therefore adding more bodies is not allowed. " "See documentation for Finalize() for details."); @@ -177,7 +177,7 @@ class MultibodyTree { const FrameType& AddFrame(std::unique_ptr> frame) { static_assert(std::is_convertible*, Frame*>::value, "FrameType must be a sub-class of Frame."); - if (topology_.is_valid) { + if (topology_is_valid()) { throw std::logic_error("This MultibodyTree is finalized already. " "Therefore adding more frames is not allowed. " "See documentation for Finalize() for details."); @@ -285,7 +285,7 @@ class MultibodyTree { std::unique_ptr> mobilizer) { static_assert(std::is_convertible*, Mobilizer*>::value, "MobilizerType must be a sub-class of mobilizer."); - if (topology_.is_valid) { + if (topology_is_valid()) { throw std::logic_error("This MultibodyTree is finalized already. " "Therefore adding more bodies is not allowed. " "See documentation for Finalize() for details."); @@ -409,7 +409,7 @@ class MultibodyTree { /// When a %MultibodyTree is instantiated, its topology remains invalid until /// Finalize() is called, which validates the topology. /// @see Finalize(). - bool topology_is_valid() const { return topology_.is_valid; } + bool topology_is_valid() const { return topology_.is_valid(); } /// Returns the topology information for this multibody tree. Users should not /// need to call this method since MultibodyTreeTopology is an internal @@ -440,9 +440,6 @@ class MultibodyTree { // a method that verifies the state of the topology with a signature similar // to RoadGeometry::CheckInvariants(). - // Sets a flag indicate the topology is valid. - void set_valid_topology() { topology_.set_valid(); } - std::vector>> owned_bodies_; std::vector>> owned_frames_; std::vector>> owned_mobilizers_; diff --git a/drake/multibody/multibody_tree/multibody_tree_indexes.h b/drake/multibody/multibody_tree/multibody_tree_indexes.h index b4ab071da238..fd62af45231c 100644 --- a/drake/multibody/multibody_tree/multibody_tree_indexes.h +++ b/drake/multibody/multibody_tree/multibody_tree_indexes.h @@ -14,6 +14,9 @@ using BodyIndex = TypeSafeIndex; /// Type used to identify mobilizers by index in a multibody tree system. using MobilizerIndex = TypeSafeIndex; +/// Type used to identify tree nodes by index within a multibody tree system. +using BodyNodeIndex = TypeSafeIndex; + /// For every MultibodyTree the **world** body _always_ has this unique index /// and it is always zero. // Note: diff --git a/drake/multibody/multibody_tree/multibody_tree_topology.h b/drake/multibody/multibody_tree/multibody_tree_topology.h index 299346bea9ab..fe31264afda6 100644 --- a/drake/multibody/multibody_tree/multibody_tree_topology.h +++ b/drake/multibody/multibody_tree/multibody_tree_topology.h @@ -21,6 +21,8 @@ /// topology can be validated against the stored topology in debug builds. #include +#include +#include #include #include @@ -48,16 +50,32 @@ struct BodyTopology { BodyIndex index{0}; /// Unique index to the one and only inboard mobilizer a body can have. - /// By default this is left initialized to "invalid" so that we can detect + /// By default this is initialized to "invalid" so that we can detect /// graph loops within add_mobilizer(). + /// This will remain "invalid" for the world body. MobilizerIndex inboard_mobilizer{}; + /// Within the tree structure of a MultibodyTree, the immediate inboard (or + /// "parent") body connected by the Mobilizer indexed by `inboard_mobilizer`. + /// By default this is initialized to "invalid" so that we can assert + /// (from within add_mobilizer()) that each body can have only one parent + /// body. Also, this will remain "invalid" for the world body. + BodyIndex parent_body{}; + + /// Within the tree structure of a MultibodyTree, the immediate outboard (or + /// "child") bodies to this Body. + std::vector child_bodies; + /// Unique index to the frame associated with this body. FrameIndex body_frame{0}; /// Depth level in the MultibodyTree, level = 0 for the world. - /// Initialized to an invalid negative value. + /// Initialized to an invalid negative value so that we can detect at + /// Finalize() when a user forgets to connect a body with a mobilizer. int level{-1}; + + /// Index to the tree body node in the MultibodyTree. + BodyNodeIndex body_node; }; /// Data structure to store the topological information associated with a @@ -132,6 +150,10 @@ struct MobilizerTopology { BodyIndex inboard_body; /// Index to the outboard body. BodyIndex outboard_body; + /// Index to the tree node in the MultibodyTree responsible for this + /// mobilizer's computations. See the documentation for BodyNodeTopology for + /// further details on how these computations are organized. + BodyNodeIndex body_node; /// Mobilizer indexing info: Set at Finalize() time. // Number of generalized coordinates granted by this mobilizer. @@ -146,13 +168,71 @@ struct MobilizerTopology { int velocities_start{0}; }; +/// Data structure to store the topological information associated with a tree +/// node. A tree node essentially consists of a body and its inboard mobilizer. +/// A body node is in charge of the computations associated to that body and +/// mobilizer, especially within a base-to-tip or tip-to-base recursion. +/// As the topological entity associated with a tree node (and specifically a +/// MultibodyTree node), this struct contains information regarding parent and +/// child nodes, parent and child bodies, etc. +struct BodyNodeTopology { + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(BodyNodeTopology); + + /// Default construction to invalid configuration. + BodyNodeTopology() {} + + /// Constructor specifying the topological information for a tree node. + /// A tree node is instantiated for each body in the multibody system and it + /// contains, in addition to that particular body, the inboard mobilizer + /// connecting the body to the rest of the tree inwards (i.e. towards the + /// world or root of the tree) from the mobilizer. + /// + /// @param index The unique index for `this` body node. + /// @param level The level (depth or generation) in the tree structure. + /// @param parent_node The parent node, in a tree structure sense, of `this` + /// node. + /// @param body The index to the body associated with this node. + /// @param parent_body The parent body, in a tree structure sense, to `body`. + /// In other words, `parent_body` is the body associated + /// with node `parent_node`. + /// @param mobilizer The index to the mobilizer associated with this node. + BodyNodeTopology( + BodyNodeIndex index, int level, + BodyNodeIndex parent_node, + BodyIndex body, BodyIndex parent_body, MobilizerIndex mobilizer) : + index(index), level(level), + parent_body_node(parent_node), + body(body), parent_body(parent_body), mobilizer(mobilizer) {} + + /// Unique index of this node in the MultibodyTree. + BodyNodeIndex index{}; + + /// Depth level in the MultibodyTree, level = 0 for the world. + int level{-1}; + + /// The unique index to the parent BodyNode of this node. + BodyNodeIndex parent_body_node; + + BodyIndex body; // This node's body B. + BodyIndex parent_body; // This node's parent body P. + + MobilizerIndex mobilizer; // The mobilizer connecting bodies P and B. + + /// The list of child body nodes to this node. + std::vector child_nodes; + + /// Returns the number of children to this node. + int get_num_children() const { return static_cast(child_nodes.size());} +}; + /// Data structure to store the topological information associated with an /// entire MultibodyTree. -struct MultibodyTreeTopology { +class MultibodyTreeTopology { + public: DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(MultibodyTreeTopology); /// Default constructor creates an empty, invalid topology. The minimum valid - /// topology for a minimum valid MultibodyTree containts at least the + /// topology for a minimum valid MultibodyTree contains at least the /// BodyTopology for the world. The topology for the _world_ body does not get /// added until MultibodyTree construction, which creates a _world_ body /// and adds it to the tree. @@ -161,60 +241,118 @@ struct MultibodyTreeTopology { /// Returns the number of bodies in the multibody tree. This includes the /// "world" body and therefore the minimum number of bodies after /// MultibodyTree::Finalize() will always be one, not zero. - int get_num_bodies() const { return static_cast(bodies.size()); } + int get_num_bodies() const { return static_cast(bodies_.size()); } /// Returns the number of physical frames in the multibody tree. int get_num_frames() const { - return static_cast(frames.size()); + return static_cast(frames_.size()); } /// Returns the number of mobilizers in the multibody tree. Since the "world" /// body does not have a mobilizer, the number of mobilizers will always equal /// the number of bodies minus one. int get_num_mobilizers() const { - return static_cast(mobilizers.size()); + return static_cast(mobilizers_.size()); + } + + /// Returns the number of tree nodes. This must equal the number of bodies. + int get_num_body_nodes() const { + return static_cast(body_nodes_.size()); + } + + /// Returns the number of tree levels in the topology. + int get_num_levels() const { + return num_levels_; + } + + /// Returns a constant reference to the corresponding FrameTopology given the + /// FrameIndex. + const FrameTopology& get_frame(FrameIndex index) const { + DRAKE_ASSERT(index < get_num_frames()); + return frames_[index]; + } + + /// Returns a constant reference to the corresponding BodyTopology given a + /// BodyIndex. + const BodyTopology& get_body(BodyIndex index) const { + DRAKE_ASSERT(index < get_num_bodies()); + return bodies_[index]; + } + + /// Returns a constant reference to the corresponding BodyTopology given a + /// BodyIndex. + const MobilizerTopology& get_mobilizer(MobilizerIndex index) const { + DRAKE_ASSERT(index < get_num_mobilizers()); + return mobilizers_[index]; + } + + /// Returns a constant reference to the corresponding BodyNodeTopology given + /// a BodyNodeIndex. + const BodyNodeTopology& get_body_node(BodyNodeIndex index) const { + DRAKE_ASSERT(index < get_num_body_nodes()); + return body_nodes_[index]; } /// Creates and adds a new BodyTopology to this MultibodyTreeTopology. /// The BodyTopology will be assigned a new, unique BodyIndex and FrameIndex /// values. + /// + /// @throws std::logic_error if Finalize() was already called on `this` + /// topology. + /// /// @returns a std::pair containing the indexes /// assigned to the new BodyTopology. std::pair add_body() { + if (is_valid()) { + throw std::logic_error("This MultibodyTreeTopology is finalized already. " + "Therefore adding more bodies is not allowed. " + "See documentation for Finalize() for details."); + } BodyIndex body_index = BodyIndex(get_num_bodies()); FrameIndex body_frame_index = add_frame(body_index); - bodies.emplace_back(body_index, body_frame_index); + bodies_.emplace_back(body_index, body_frame_index); return std::make_pair(body_index, body_frame_index); } /// Creates and adds a new FrameTopology, associated with the given /// body_index, to this MultibodyTreeTopology. + /// + /// @throws std::logic_error if Finalize() was already called on `this` + /// topology. + /// /// @returns The FrameIndex assigned to the new FrameTopology. FrameIndex add_frame(BodyIndex body_index) { + if (is_valid()) { + throw std::logic_error("This MultibodyTreeTopology is finalized already. " + "Therefore adding more frames is not allowed. " + "See documentation for Finalize() for details."); + } FrameIndex frame_index(get_num_frames()); - frames.emplace_back(frame_index, body_index); + frames_.emplace_back(frame_index, body_index); return frame_index; } - /// Returns a constant reference to the corresponding FrameTopology given the - /// FrameIndex. - const FrameTopology& get_frame(FrameIndex index) const { - DRAKE_ASSERT(index < get_num_frames()); - return frames[index]; - } - /// Creates and adds a new MobilizerTopology connecting the inboard and /// outboard multibody frames identified by indexes `in_frame` and /// `out_frame`, respectively. - /// @returns The MobilizerIndex assigned to the new MobilizerTopology. + /// /// @throws std::runtime_error if either `in_frame` or `out_frame` do not /// index frame topologies in `this` %MultibodyTreeTopology. /// @throws a std::runtime_error if `in_frame == out_frame`. /// @throws a std::runtime_error if `in_frame` and `out_frame` already are /// connected by another mobilizer. More than one mobilizer between two frames /// is not allowed. + /// @throws std::logic_error if Finalize() was already called on `this` + /// topology. + /// + /// @returns The MobilizerIndex assigned to the new MobilizerTopology. MobilizerIndex add_mobilizer( FrameIndex in_frame, FrameIndex out_frame) { + if (is_valid()) { + throw std::logic_error("This MultibodyTreeTopology is finalized already. " + "Therefore adding more mobilizers is not allowed. " + "See documentation for Finalize() for details."); + } // Note: MultibodyTree double checks the mobilizer's frames belong to that // tree. Therefore the validity of in_frame and out_frame is already // guaranteed. We add the checks here for additional security. @@ -229,59 +367,188 @@ struct MultibodyTreeTopology { "This multibody tree already has a mobilizer connecting these two " "frames. More than one mobilizer between two frames is not allowed"); } - const BodyIndex inboard_body = frames[in_frame].body; - const BodyIndex outboard_body = frames[out_frame].body; + const BodyIndex inboard_body = frames_[in_frame].body; + const BodyIndex outboard_body = frames_[out_frame].body; if (IsThereAMobilizerBetweenBodies(inboard_body, outboard_body)) { throw std::runtime_error( "This multibody tree already has a mobilizer connecting these two " "bodies. More than one mobilizer between two bodies is not allowed"); } - // Checks for graph loops. Each body can have and only one inboard - // mobilizer. - if (bodies[outboard_body].inboard_mobilizer.is_valid()) { + // Checks for graph loops. Each body can have only one inboard mobilizer. + if (bodies_[outboard_body].inboard_mobilizer.is_valid()) { throw std::runtime_error( "This mobilizer is creating a closed loop since the outboard body " "already has an inboard mobilizer connected to it. " "If a physical loop is really needed, consider using a constraint " "instead."); } + + // The checks above guarantee that it is the first time we add an inboard + // mobilizer to `outboard_body`. The DRAKE_DEMANDs below double check our + // implementation. + // BodyTopology::inboard_mobilizer and BodyTopology::parent_body are both + // set within this method right after these checks. + DRAKE_DEMAND(!bodies_[outboard_body].inboard_mobilizer.is_valid()); + DRAKE_DEMAND(!bodies_[outboard_body].parent_body.is_valid()); MobilizerIndex mobilizer_index(get_num_mobilizers()); // Make note of the inboard mobilizer for the outboard body. - bodies[outboard_body].inboard_mobilizer = mobilizer_index; + bodies_[outboard_body].inboard_mobilizer = mobilizer_index; + // Similarly, record inboard_body as the parent of outboard_body. + bodies_[outboard_body].parent_body = inboard_body; + + // Records "child" bodies for bookkeeping in the context of the tree + // structure of MultibodyTree. + bodies_[inboard_body].child_bodies.push_back(outboard_body); - mobilizers.emplace_back(mobilizer_index, + mobilizers_.emplace_back(mobilizer_index, in_frame, out_frame, inboard_body, outboard_body); return mobilizer_index; } - bool is_valid{false}; + /// This method must be called by MultibodyTree::Finalize() after all + /// topological elements in the tree (corresponding to joints, bodies, force + /// elements, constraints) were added and before any computations are + /// performed. + /// It essentially compiles all the necessary "topological information", i.e. + /// how bodies, joints and, any other elements connect with each other, and + /// performs all the required pre-processing to perform computations at a + /// later stage. This preprocessing includes: + /// - sorting in BFT order for fast recursions through the tree, + /// - computation of state sizes and of pool sizes within cache entries, + /// - computation of index maps to retrieve either state or cache entries for + /// each multibody element. + /// + /// If the finalize stage is successful, the `this` topology is validated, + /// meaning it is up-to-date after this call. + /// No more multibody tree elements can be added after a call to Finalize(). + /// + /// @throws std::logic_error If users attempt to call this method on an + /// already finalized topology. + /// @see is_valid() + void Finalize() { + // If the topology is valid it means that it was already finalized. + // Re-compilation is not allowed. + if (is_valid()) { + throw std::logic_error( + "Attempting to call MultibodyTree::Finalize() on an already """ + "finalized MultibodyTree."); + } + + // Compute body levels in the tree. Root is the zero level. + // Breadth First Traversal (a.k.a. Level Order Traversal). + std::queue queue; + queue.push(BodyIndex(0)); // Starts at the root. + num_levels_ = 1; // At least one level with the world body at the root. + // While at it, create body nodes and index them in this BFT order for + // fast tree traversals of MultibodyTree recursive algorithms. + body_nodes_.reserve(get_num_bodies()); + while (!queue.empty()) { + const BodyNodeIndex node(get_num_body_nodes()); + const BodyIndex current = queue.front(); + const BodyIndex parent = bodies_[current].parent_body; + + bodies_[current].body_node = node; + + // Computes level. + int level = 0; // level = 0 for the world body. + if (current != 0) { // Not the world body. + level = bodies_[parent].level + 1; + const MobilizerIndex mobilizer = bodies_[current].inboard_mobilizer; + mobilizers_[mobilizer].body_node = node; + } + // Updates body levels. + bodies_[current].level = level; + // Keep track of the number of levels, the deepest (i.e. max) level. + num_levels_ = std::max(num_levels_, level + 1); + + // Since we are doing a BFT, it is valid to ask for the parent node, + // unless we are at the root. + BodyNodeIndex parent_node; + if (node != 0) { // If we are not at the root: + parent_node = bodies_[parent].body_node; + body_nodes_[parent_node].child_nodes.push_back(node); + } + + // Creates BodyNodeTopology. + body_nodes_.emplace_back( + node, level /* node index and level */, + parent_node /* This node's parent */, + current /* This node's body */, + bodies_[current].parent_body /* This node's parent body */, + bodies_[current].inboard_mobilizer /* This node's mobilizer */); + + // Pushes children to the back of the queue and pops current. + for (BodyIndex child : bodies_[current].child_bodies) { + queue.push(child); // Pushes at the back. + } + queue.pop(); // Pops front element. + } + + // Checks that all bodies were reached. We could have this situation if a + // user adds a body but forgets to add a mobilizer to it. + // Bodies that were not reached were not assigned a valid level. + // TODO(amcastro-tri): this will stop at the first body that is not + // connected to the tree. Add logic to emit a message with ALL bodies that + // are not properly connected to the tree. + for (BodyIndex body(0); body < get_num_bodies(); ++body) { + if (bodies_[body].level < 0) { + throw std::runtime_error("Body with index " + std::to_string(body) + + " was not assigned a mobilizer"); + } + } + + // After we checked all bodies were reached above, the number of tree nodes + // should equal the number of bodies in the tree. + DRAKE_DEMAND(get_num_bodies() == get_num_body_nodes()); + + // TODO(amcastro-tri): Compile topological information for BodyNode objects + // in a following PR. This will include: + // - Sizes (num dofs) + // - Indexing info. Start/end indexes into context (actually cache) pools. + + // We are done with a successful Finalize() and we mark it as so. + // Do not add any more code after this! + is_valid_ = true; + } - /// Topology gets validated by MultibodyTree::Finalize(). - void set_valid() { is_valid = true; } + /// Returns `true` if Finalize() was already called on `this` topology. + /// @see Finalize() + bool is_valid() const { return is_valid_; } - /// Returns `true` if there is _any_ mobilizer in the multibody tree - /// connecting the frames with indexes `frame` and `frame2`. - bool IsThereAMobilizerBetweenFrames(FrameIndex frame1, FrameIndex frame2) { - for (const auto& mobilizer_topology : mobilizers) { + private: + // Returns `true` if there is _any_ mobilizer in the multibody tree + // connecting the frames with indexes `frame` and `frame2`. + bool IsThereAMobilizerBetweenFrames( + FrameIndex frame1, FrameIndex frame2) const { + for (const auto& mobilizer_topology : mobilizers_) { if (mobilizer_topology.connects_frames(frame1, frame2)) return true; } return false; } - /// Returns `true` if there is _any_ mobilizer in the multibody tree - /// connecting the bodies with indexes `body2` and `body2`. - bool IsThereAMobilizerBetweenBodies(BodyIndex body1, BodyIndex body2) { - for (const auto& mobilizer_topology : mobilizers) { + // Returns `true` if there is _any_ mobilizer in the multibody tree + // connecting the bodies with indexes `body2` and `body2`. + bool IsThereAMobilizerBetweenBodies( + BodyIndex body1, BodyIndex body2) const { + for (const auto& mobilizer_topology : mobilizers_) { if (mobilizer_topology.connects_bodies(body1, body2)) return true; } return false; } - std::vector bodies; - std::vector frames; - std::vector mobilizers; + // is_valid is set to `true` after a successful Finalize(). + bool is_valid_{false}; + // Number of levels (or generations) in the tree topology. After Finalize() + // there will be at least one level (level = 0) with the world body. + int num_levels_{-1}; + + // Topological elements: + std::vector frames_; + std::vector bodies_; + std::vector mobilizers_; + std::vector body_nodes_; }; } // namespace multibody diff --git a/drake/multibody/multibody_tree/test/multibody_tree_creation_tests.cc b/drake/multibody/multibody_tree/test/multibody_tree_creation_tests.cc index d23759967f8b..7336b081a335 100644 --- a/drake/multibody/multibody_tree/test/multibody_tree_creation_tests.cc +++ b/drake/multibody/multibody_tree/test/multibody_tree_creation_tests.cc @@ -1,6 +1,7 @@ #include "drake/multibody/multibody_tree/multibody_tree.h" #include +#include #include #include @@ -16,12 +17,13 @@ namespace { using Eigen::Vector3d; using std::make_unique; +using std::set; using std::unique_ptr; -// Test the basic MultibodyTree API to create and add bodies. -GTEST_TEST(MultibodyTree, AddBodies) { - auto owned_model = std::make_unique>(); - MultibodyTree* model = owned_model.get(); +// Tests the basic MultibodyTree API to add bodies and mobilizers. +// Tests we cannot create graph loops. +GTEST_TEST(MultibodyTree, BasicAPIToAddBodiesAndMobilizers) { + auto model = std::make_unique>(); // Initially there is only one body, the world. EXPECT_EQ(model->get_num_bodies(), 1); @@ -39,51 +41,6 @@ GTEST_TEST(MultibodyTree, AddBodies) { // Adds a new body to the world. const RigidBody& pendulum = model->AddBody(M_Bo_B); - // Topology is invalid before MultibodyTree::Finalize(). - EXPECT_FALSE(model->topology_is_valid()); - // Verifies that the topology of this model gets validated at finalize stage. - model->Finalize(); - EXPECT_TRUE(model->topology_is_valid()); - - // Body identifiers are unique and are assigned by MultibodyTree in increasing - // order starting with index = 0 (world_index()) for the "world" body. - EXPECT_EQ(world_body.get_index(), world_index()); - EXPECT_EQ(pendulum.get_index(), BodyIndex(1)); - - // Tests API to access bodies. - EXPECT_EQ(model->get_body(BodyIndex(1)).get_index(), pendulum.get_index()); - EXPECT_EQ(model->get_body(BodyIndex(1)).get_index(), pendulum.get_index()); - - // Rigid bodies have no generalized coordinates. - EXPECT_EQ(pendulum.get_num_flexible_positions(), 0); - EXPECT_EQ(pendulum.get_num_flexible_velocities(), 0); - - // Verifies that an exception is throw if a call to Finalize() is attempted to - // an already finalized MultibodyTree. - EXPECT_THROW(model->Finalize(), std::logic_error); - - // Verifies that after compilation no more bodies can be added. - EXPECT_THROW(model->AddBody(M_Bo_B), std::logic_error); -} - -// Tests the basic MultibodyTree API to create and add bodies. -// Tests we cannot create graph loops. -GTEST_TEST(MultibodyTree, AddMobilizers) { - auto model = std::make_unique>(); - - // Retrieves the world body. - const Body& world_body = model->get_world_body(); - - // Creates a NaN SpatialInertia to instantiate the RigidBody links of the - // pendulum. Using a NaN spatial inertia is ok so far since we are still - // not performing any numerical computations. This is only to test API. - // M_Bo_B is the spatial inertia about the body frame's origin Bo and - // expressed in the body frame B. - SpatialInertia M_Bo_B; - - // Adds a new body to the world. - const RigidBody& pendulum = model->AddBody(M_Bo_B); - // Adds a revolute mobilizer. EXPECT_NO_THROW((model->AddMobilizer( world_body.get_body_frame(), pendulum.get_body_frame(), @@ -121,6 +78,33 @@ GTEST_TEST(MultibodyTree, AddMobilizers) { // (failed) call. EXPECT_EQ(model->get_num_bodies(), 3); EXPECT_EQ(model->get_num_mobilizers(), 2); + + // Topology is invalid before MultibodyTree::Finalize(). + EXPECT_FALSE(model->topology_is_valid()); + // Verifies that the topology of this model gets validated at finalize stage. + EXPECT_NO_THROW(model->Finalize()); + EXPECT_TRUE(model->topology_is_valid()); + + // Body identifiers are unique and are assigned by MultibodyTree in increasing + // order starting with index = 0 (world_index()) for the "world" body. + EXPECT_EQ(world_body.get_index(), world_index()); + EXPECT_EQ(pendulum.get_index(), BodyIndex(1)); + EXPECT_EQ(pendulum2.get_index(), BodyIndex(2)); + + // Tests API to access bodies. + EXPECT_EQ(model->get_body(BodyIndex(1)).get_index(), pendulum.get_index()); + EXPECT_EQ(model->get_body(BodyIndex(2)).get_index(), pendulum2.get_index()); + + // Rigid bodies have no generalized coordinates. + EXPECT_EQ(pendulum.get_num_flexible_positions(), 0); + EXPECT_EQ(pendulum.get_num_flexible_velocities(), 0); + + // Verifies that an exception is throw if a call to Finalize() is attempted to + // an already finalized MultibodyTree. + EXPECT_THROW(model->Finalize(), std::logic_error); + + // Verifies that after compilation no more bodies can be added. + EXPECT_THROW(model->AddBody(M_Bo_B), std::logic_error); } // Tests the correctness of MultibodyTreeElement checks to verify one or more @@ -153,8 +137,19 @@ GTEST_TEST(MultibodyTree, MultibodyTreeElementChecks) { body2.get_body_frame() /*body2 belongs to model2, not model1!!!*/, Vector3d::UnitZ() /*axis of rotation*/)), std::logic_error); - model1->Finalize(); - model2->Finalize(); + // model1 is complete. Expect no-throw. + EXPECT_NO_THROW(model1->Finalize()); + + // model2->Finalize() is expected to throw an exception since there is no + // mobilizer for body2. + try { + model2->Finalize(); + GTEST_FAIL(); + } catch (std::runtime_error& e) { + std::string expected_msg = + "Body with index 1 was not assigned a mobilizer"; + EXPECT_EQ(e.what(), expected_msg); + } // Tests that the created multibody elements indeed do have a parent // MultibodyTree. @@ -172,6 +167,206 @@ GTEST_TEST(MultibodyTree, MultibodyTreeElementChecks) { EXPECT_NO_THROW(body2.HasThisParentTreeOrThrow(model2.get())); } +// This unit test builds a MultibodyTree as shown in the schematic below, where +// the number inside the boxes corresponds to each of the bodies' indexes ( +// assigned by MultibodyTree in the order bodies are created) and "m?" next to +// each connection denotes a mobilizer with the number corresponding to the +// mobilizer index (assigned by MultibodyTree in the order mobilizers are +// created). +// At Finalize(), a body node is created per (body, inboard_mobilizer) pair. +// Body node indexes are assigned according to a BFT order. +// Tests below make reference to the indexes in this schematic. The following +// points are important: +// - Body nodes are ordered by a BFT sort however, +// - There is no guarantee on which body node is assigned to which body, +// - Body nodes at a particular level are not guaranteed to be in any particular +// order, only that they belong to the right level is important. +// +// +---+ +// | 0 | Level 0 (root, world) +// +-----------+-+-+-----------+ +// | | | +// | m6 | m2 | m4 +// | | | +// +-v-+ +-v-+ +-v-+ +// | 4 | | 7 | | 5 | Level 1 +// +--+---+--+ +---+ +-+-+ +// | | | +// | m1 | m5 | m3 +// | | | +// +-v-+ +-v-+ +-v-+ +// | 2 | | 1 | | 3 | Level 2 +// +---+ +-+-+ +---+ +// | +// | m0 +// | +// +-v-+ +// | 6 | Level 3 +// +---+ +// +class TreeTopologyTests : public ::testing::Test { + public: + // Creates MultibodyTree according to the schematic above. + void SetUp() override { + model_ = std::make_unique>(); + + const int kNumBodies = 8; + bodies_.push_back(&model_->get_world_body()); + for (int i =1; i < kNumBodies; ++i) + AddTestBody(); + + // Adds mobilizers to connect bodies according to the following diagram: + ConnectBodies(*bodies_[1], *bodies_[6]); // mob. 0 + ConnectBodies(*bodies_[4], *bodies_[2]); // mob. 1 + ConnectBodies(*bodies_[0], *bodies_[7]); // mob. 2 + ConnectBodies(*bodies_[5], *bodies_[3]); // mob. 3 + ConnectBodies(*bodies_[0], *bodies_[5]); // mob. 4 + ConnectBodies(*bodies_[4], *bodies_[1]); // mob. 5 + ConnectBodies(*bodies_[0], *bodies_[4]); // mob. 6 + } + + const RigidBody* AddTestBody() { + // NaN SpatialInertia to instantiate the RigidBody objects. + // It is safe here since this tests only focus on topological information. + const SpatialInertia M_Bo_B; + const RigidBody* body = &model_->AddBody(M_Bo_B); + bodies_.push_back(body); + return body; + } + + const Mobilizer* ConnectBodies( + const Body& inboard, const Body& outboard) { + const Mobilizer* mobilizer = + &model_->AddMobilizer( + inboard.get_body_frame(), outboard.get_body_frame(), + Vector3d::UnitZ()); + mobilizers_.push_back(mobilizer); + return mobilizer; + } + + // Performs a number of tests on the BodyNodeTopology corresponding to the + // body indexed by `body`. + void TestBodyNode(BodyIndex body) const { + const MultibodyTreeTopology& topology = model_->get_topology(); + const BodyNodeIndex node = get_body_topology(body).body_node; + + // Verify that the corresponding Body and BodyNode reference each other + // correctly. + EXPECT_EQ(get_body_topology(body).body_node, + get_body_node_topology(node).index); + EXPECT_EQ(get_body_node_topology(node).body, get_body_topology(body).index); + + // They should belong to the same level. + EXPECT_EQ(get_body_topology(body).level, + get_body_node_topology(node).level); + + const BodyNodeIndex parent_node = + get_body_node_topology(node).parent_body_node; + // Either (and thus the exclusive or): + // 1. `body` is the world, and thus `parent_node` is invalid, XOR + // 2. `body` is not the world, and thus we have a valid `parent_node`. + EXPECT_TRUE(parent_node.is_valid() ^ (body == world_index())); + + if (body != world_index()) { + // Verifies BodyNode has the parent node to the correct body. + const BodyIndex parent_body = get_body_node_topology(parent_node).body; + EXPECT_TRUE(parent_body.is_valid()); + EXPECT_EQ(parent_body, get_body_topology(body).parent_body); + EXPECT_EQ(get_body_node_topology(parent_node).index, + get_body_topology(parent_body).body_node); + + // Verifies that BodyNode makes reference to the proper mobilizer index. + const MobilizerIndex mobilizer = get_body_node_topology(node).mobilizer; + EXPECT_EQ(mobilizer, get_body_topology(body).inboard_mobilizer); + + // Verifies the mobilizer makes reference to the appropriate node. + EXPECT_EQ(topology.get_mobilizer(mobilizer).body_node, node); + + // Helper lambda to check if this "node" effectively is a child of + // "parent_node". + auto is_child_of_parent = [&]() { + const auto& children = get_body_node_topology(parent_node).child_nodes; + return + std::find(children.begin(), children.end(), node) != children.end(); + }; + EXPECT_TRUE(is_child_of_parent()); + } + } + + const BodyTopology& get_body_topology(int body_index) const { + const MultibodyTreeTopology& topology = model_->get_topology(); + return topology.get_body(BodyIndex(body_index)); + } + + const BodyNodeTopology& get_body_node_topology(int body_node_index) const { + const MultibodyTreeTopology& topology = model_->get_topology(); + return topology.get_body_node(BodyNodeIndex(body_node_index)); + } + + protected: + std::unique_ptr> model_; + // Bodies: + std::vector*> bodies_; + // Mobilizers: + std::vector*> mobilizers_; +}; + +// This unit tests verifies that the multibody topology is properly compiled. +TEST_F(TreeTopologyTests, Finalize) { + model_->Finalize(); + EXPECT_EQ(model_->get_num_bodies(), 8); + EXPECT_EQ(model_->get_num_mobilizers(), 7); + + const MultibodyTreeTopology& topology = model_->get_topology(); + EXPECT_EQ(topology.get_num_body_nodes(), model_->get_num_bodies()); + EXPECT_EQ(topology.get_num_levels(), 4); + + // These sets contain the indexes of the bodies in each tree level. + // The order of these indexes in each set is not important, but only the fact + // that they belong to the appropriate set. + set expected_level0 = {BodyIndex(0)}; + set expected_level1 = {BodyIndex(4), BodyIndex(7), BodyIndex(5)}; + set expected_level2 = {BodyIndex(2), BodyIndex(1), BodyIndex(3)}; + set expected_level3 = {BodyIndex(6)}; + + set level0 = {get_body_node_topology(0).body}; + set level1 = {get_body_node_topology(1).body, + get_body_node_topology(2).body, + get_body_node_topology(3).body}; + set level2 = {get_body_node_topology(4).body, + get_body_node_topology(5).body, + get_body_node_topology(6).body}; + set level3 = {get_body_node_topology(7).body}; + + // Comparison of sets. The order of the elements is not important. + EXPECT_EQ(level0, expected_level0); + EXPECT_EQ(level1, expected_level1); + EXPECT_EQ(level2, expected_level2); + EXPECT_EQ(level3, expected_level3); + + // Verifies the expected number of child nodes. + EXPECT_EQ(get_body_node_topology(0).get_num_children(), 3); + EXPECT_EQ(get_body_node_topology( + get_body_topology(4).body_node).get_num_children(), 2); + EXPECT_EQ(get_body_node_topology( + get_body_topology(7).body_node).get_num_children(), 0); + EXPECT_EQ(get_body_node_topology( + get_body_topology(5).body_node).get_num_children(), 1); + EXPECT_EQ(get_body_node_topology( + get_body_topology(2).body_node).get_num_children(), 0); + EXPECT_EQ(get_body_node_topology( + get_body_topology(1).body_node).get_num_children(), 1); + EXPECT_EQ(get_body_node_topology( + get_body_topology(3).body_node).get_num_children(), 0); + EXPECT_EQ(get_body_node_topology( + get_body_topology(6).body_node).get_num_children(), 0); + + // Checks the correctness of each BodyNode associated with a body. + for (BodyIndex body(0); body < model_->get_num_bodies(); ++body) { + TestBodyNode(body); + } +} + } // namespace } // namespace multibody } // namespace drake