From ecb94f8e3a41b9fb1209bde591198421d294ec3a Mon Sep 17 00:00:00 2001 From: Chien-Liang Fok Date: Wed, 15 Feb 2017 11:42:46 -0500 Subject: [PATCH] Adds Ability to Clone a RigidBodyTree. (#4958) * Adds ability to clone RigidBodyTree. --- drake/multibody/BUILD | 53 +++++ drake/multibody/rigid_body_actuator.cc | 5 + drake/multibody/rigid_body_loop.cc | 1 + drake/multibody/rigid_body_loop.h | 1 + drake/multibody/rigid_body_tree.cc | 100 ++++++++- drake/multibody/rigid_body_tree.h | 36 ++- drake/multibody/test/CMakeLists.txt | 8 + .../rigid_body_actuator_compare_to_clone.cc | 58 +++++ .../rigid_body_actuator_compare_to_clone.h | 23 ++ .../test/rigid_body_loop_compare_to_clone.cc | 36 +++ .../test/rigid_body_loop_compare_to_clone.h | 23 ++ .../test/rigid_body_tree/CMakeLists.txt | 15 ++ .../rigid_body_tree_clone_test.cc | 209 ++++++++++++++++++ .../rigid_body_tree_compare_to_clone.cc | 134 +++++++++++ .../rigid_body_tree_compare_to_clone.h | 21 ++ 15 files changed, 719 insertions(+), 4 deletions(-) create mode 100644 drake/multibody/test/rigid_body_actuator_compare_to_clone.cc create mode 100644 drake/multibody/test/rigid_body_actuator_compare_to_clone.h create mode 100644 drake/multibody/test/rigid_body_loop_compare_to_clone.cc create mode 100644 drake/multibody/test/rigid_body_loop_compare_to_clone.h create mode 100644 drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc create mode 100644 drake/multibody/test/rigid_body_tree/rigid_body_tree_compare_to_clone.cc create mode 100644 drake/multibody/test/rigid_body_tree/rigid_body_tree_compare_to_clone.h diff --git a/drake/multibody/BUILD b/drake/multibody/BUILD index e8cf1154ef89..df43e99c315c 100644 --- a/drake/multibody/BUILD +++ b/drake/multibody/BUILD @@ -94,6 +94,38 @@ drake_cc_library( ], ) +drake_cc_library( + name = "rigid_body_tree_compare_to_clone", + testonly = 1, + srcs = ["test/rigid_body_tree/rigid_body_tree_compare_to_clone.cc"], + hdrs = ["test/rigid_body_tree/rigid_body_tree_compare_to_clone.h"], + deps = [ + ":rigid_body_actuator_compare_to_clone", + ":rigid_body_loop_compare_to_clone", + ":rigid_body_tree", + ], +) + +drake_cc_library( + name = "rigid_body_actuator_compare_to_clone", + testonly = 1, + srcs = ["test/rigid_body_actuator_compare_to_clone.cc"], + hdrs = ["test/rigid_body_actuator_compare_to_clone.h"], + deps = [ + ":rigid_body_tree", + ], +) + +drake_cc_library( + name = "rigid_body_loop_compare_to_clone", + testonly = 1, + srcs = ["test/rigid_body_loop_compare_to_clone.cc"], + hdrs = ["test/rigid_body_loop_compare_to_clone.h"], + deps = [ + ":rigid_body_tree", + ], +) + # TODO(jwnimmer-tri) This is just some random program. Do we want to keep it? drake_cc_binary( name = "benchmark_rigid_body_tree", @@ -175,6 +207,27 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "rigid_body_tree_clone_test", + srcs = ["test/rigid_body_tree/rigid_body_tree_clone_test.cc"], + data = [ + ":test_models", + "//drake/automotive:models", + "//drake/examples/Atlas:models", + "//drake/examples/Pendulum:models", + "//drake/examples/Valkyrie:models", + ], + deps = [ + ":rigid_body_tree_compare_to_clone", + "//drake/common:eigen_matrix_compare", + "//drake/multibody/parsers", + "//drake/multibody/rigid_body_plant", + "//drake/systems/analysis", + "//drake/systems/primitives:constant_vector_source", + "//drake/systems/primitives:signal_logger", + ], +) + drake_cc_googletest( name = "rigid_body_tree_creation_test", srcs = ["test/rigid_body_tree/rigid_body_tree_creation_test.cc"], diff --git a/drake/multibody/rigid_body_actuator.cc b/drake/multibody/rigid_body_actuator.cc index eae740215fd4..17b3e6760e36 100644 --- a/drake/multibody/rigid_body_actuator.cc +++ b/drake/multibody/rigid_body_actuator.cc @@ -1,5 +1,10 @@ #include "drake/multibody/rigid_body_actuator.h" +#include + +#include "drake/common/text_logging.h" +#include "drake/multibody/rigid_body.h" + RigidBodyActuator::RigidBodyActuator(const std::string& name, const RigidBody* body, double reduction, diff --git a/drake/multibody/rigid_body_loop.cc b/drake/multibody/rigid_body_loop.cc index e51464f1d2aa..ddd8960d588f 100644 --- a/drake/multibody/rigid_body_loop.cc +++ b/drake/multibody/rigid_body_loop.cc @@ -1,6 +1,7 @@ #include "drake/multibody/rigid_body_loop.h" #include "drake/common/eigen_autodiff_types.h" +#include "drake/common/text_logging.h" template RigidBodyLoop::RigidBodyLoop(std::shared_ptr> frameA, diff --git a/drake/multibody/rigid_body_loop.h b/drake/multibody/rigid_body_loop.h index b43e9d2e8739..1d5a5c51c5e0 100644 --- a/drake/multibody/rigid_body_loop.h +++ b/drake/multibody/rigid_body_loop.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include diff --git a/drake/multibody/rigid_body_tree.cc b/drake/multibody/rigid_body_tree.cc index 4ce18b88a0bd..80a15b8b9c83 100644 --- a/drake/multibody/rigid_body_tree.cc +++ b/drake/multibody/rigid_body_tree.cc @@ -13,6 +13,7 @@ #include "drake/common/constants.h" #include "drake/common/eigen_autodiff_types.h" #include "drake/common/eigen_types.h" +#include "drake/common/text_logging.h" #include "drake/math/autodiff.h" #include "drake/math/autodiff_gradient.h" #include "drake/math/gradient.h" @@ -124,6 +125,91 @@ RigidBodyTree::RigidBodyTree() template RigidBodyTree::~RigidBodyTree() {} +// For an explanation of why these SWIG preprocessor commands are needed, see +// the comment immediately above the declaration of RigidBodyTree::Clone() in +// rigid_body_tree.h. +#ifndef SWIG +template <> +unique_ptr> RigidBodyTree::Clone() const { + auto clone = make_unique>(); + // The following is necessary to remove the world link from the clone. The + // world link will be re-added when the bodies are cloned below. + clone->bodies.clear(); + + clone->joint_limit_min = this->joint_limit_min; + clone->joint_limit_max = this->joint_limit_max; + clone->a_grav = this->a_grav; + clone->B = this->B; + clone->num_positions_ = this->num_positions_; + clone->num_velocities_ = this->num_velocities_; + clone->num_model_instances_ = this->num_model_instances_; + clone->initialized_ = this->initialized_; + + // Clones the rigid bodies. + for (const auto& body : bodies) { + clone->bodies.push_back(body->Clone()); + } + + // Clones the joints and adds them to the cloned RigidBody objects. + for (const auto& original_body : bodies) { + const int body_index = original_body->get_body_index(); + if (body_index == RigidBodyTreeConstants::kWorldBodyIndex) { + continue; + } + + RigidBody* cloned_body = clone->get_mutable_body(body_index); + DRAKE_DEMAND(cloned_body != nullptr); + DRAKE_DEMAND(cloned_body->get_body_index() == body_index); + + const RigidBody* original_body_parent = original_body->get_parent(); + DRAKE_DEMAND(original_body_parent != nullptr); + + const int parent_body_index = original_body_parent->get_body_index(); + + RigidBody* cloned_body_parent = + clone->get_mutable_body(parent_body_index); + DRAKE_DEMAND(cloned_body_parent != nullptr); + + cloned_body->add_joint(cloned_body_parent, + original_body->getJoint().Clone()); + } + + for (const auto& original_frame : frames) { + const RigidBody& original_frame_body = + original_frame->get_rigid_body(); + const int cloned_frame_body_index = + clone->FindBodyIndex(original_frame_body.get_name(), + original_frame_body.get_model_instance_id()); + RigidBody* cloned_frame_body = + clone->get_mutable_body(cloned_frame_body_index); + DRAKE_DEMAND(cloned_frame_body != nullptr); + std::shared_ptr> cloned_frame = + original_frame->Clone(cloned_frame_body); + clone->frames.push_back(cloned_frame); + } + + for (const auto& actuator : actuators) { + const RigidBody& cloned_body = + clone->get_body(actuator.body_->get_body_index()); + clone->actuators.emplace_back( + actuator.name_, &cloned_body, actuator.reduction_, + actuator.effort_limit_min_, actuator.effort_limit_max_); + } + + for (const auto& loop : loops) { + std::shared_ptr> frame_a = + clone->findFrame(loop.frameA_->get_name(), + loop.frameA_->get_model_instance_id()); + std::shared_ptr> frame_b = + clone->findFrame(loop.frameB_->get_name(), + loop.frameB_->get_model_instance_id()); + clone->loops.emplace_back(frame_a, frame_b, loop.axis_); + } + + return clone; +} +#endif + template bool RigidBodyTree::transformCollisionFrame( RigidBody* body, const Eigen::Isometry3d& displace_transform) { @@ -2760,11 +2846,16 @@ int RigidBodyTree::FindIndexOfChildBodyOfJoint(const std::string& joint_name, template const RigidBody& RigidBodyTree::get_body(int body_index) const { - DRAKE_DEMAND(body_index >= 0 && - body_index < get_num_bodies()); + DRAKE_DEMAND(body_index >= 0 && body_index < get_num_bodies()); return *bodies[body_index].get(); } +template +RigidBody* RigidBodyTree::get_mutable_body(int body_index) { + DRAKE_DEMAND(body_index >= 0 && body_index < get_num_bodies()); + return bodies[body_index].get(); +} + template int RigidBodyTree::get_num_bodies() const { return static_cast(bodies.size()); @@ -2776,6 +2867,11 @@ int RigidBodyTree::get_number_of_bodies() const { return get_num_bodies(); } +template +int RigidBodyTree::get_num_frames() const { + return static_cast(frames.size()); +} + // TODO(liang.fok) Remove this method prior to Release 1.0. template RigidBody* RigidBodyTree::findJoint(const std::string& joint_name, diff --git a/drake/multibody/rigid_body_tree.h b/drake/multibody/rigid_body_tree.h index 9b4d6c61e95f..2347c9a2bc8e 100644 --- a/drake/multibody/rigid_body_tree.h +++ b/drake/multibody/rigid_body_tree.h @@ -113,6 +113,21 @@ class RigidBodyTree { virtual ~RigidBodyTree(); + // The following preprocessor condition is necessary because wrapping method + // Clone() in SWIG causes the following build error to occur: + // + // "call to implicitly-deleted copy constructor" + // + // Unfortunately, adding "%ignore RigidBodyTree::Clone()" to + // drake-distro/drake/bindings/swig/rbtree.i does not work. +#ifndef SWIG + /** + * Returns a deep clone of this RigidBodyTree. Currently, everything + * *except* for collision and visual elements are cloned. + */ + std::unique_ptr> Clone() const; +#endif + /** * Adds a new model instance to this `RigidBodyTree`. The model instance is * identified by a unique model instance ID, which is the return value of @@ -1250,17 +1265,28 @@ class RigidBodyTree { /** * Returns the body at index @p body_index. Parameter @p body_index must be * between zero and the number of bodies in this tree, which can be determined - * by calling RigidBodyTree::get_num_bodies(). Note that the body at - * index 0 represents the world. + * by calling RigidBodyTree::get_num_bodies(). */ const RigidBody& get_body(int body_index) const; + /** + * Returns the body at index @p body_index. Parameter @p body_index must be + * between zero and the number of bodies in this tree, which can be determined + * by calling RigidBodyTree::get_num_bodies(). + */ + RigidBody* get_mutable_body(int body_index); + /** * Returns the number of bodies in this tree. This includes the one body that * represents the world. */ int get_num_bodies() const; + /** + * Returns the number of frames in this tree. + */ + int get_num_frames() const; + DRAKE_DEPRECATED("Please use get_num_bodies().") int get_number_of_bodies() const; @@ -1434,6 +1460,12 @@ class RigidBodyTree { */ int get_num_actuators() const; + /** + * Returns whether this %RigidBodyTree is initialized. It is initialized after + * compile() is called. + */ + bool initialized() const { return initialized_; } + public: Eigen::VectorXd joint_limit_min; Eigen::VectorXd joint_limit_max; diff --git a/drake/multibody/test/CMakeLists.txt b/drake/multibody/test/CMakeLists.txt index 81f78099e704..2e674cc7f1d8 100644 --- a/drake/multibody/test/CMakeLists.txt +++ b/drake/multibody/test/CMakeLists.txt @@ -50,4 +50,12 @@ if (snopt_FOUND) add_ik_gtest(test_ik_traj) endif () +add_library_with_exports(LIB_NAME drakeRigidBodyActuatorCompareToClone + SOURCE_FILES rigid_body_actuator_compare_to_clone.cc) +target_link_libraries(drakeRigidBodyActuatorCompareToClone drakeRBM) + +add_library_with_exports(LIB_NAME drakeRigidBodyLoopCompareToClone + SOURCE_FILES rigid_body_loop_compare_to_clone.cc) +target_link_libraries(drakeRigidBodyLoopCompareToClone drakeRBM) + add_subdirectory(rigid_body_tree) diff --git a/drake/multibody/test/rigid_body_actuator_compare_to_clone.cc b/drake/multibody/test/rigid_body_actuator_compare_to_clone.cc new file mode 100644 index 000000000000..335b9767e157 --- /dev/null +++ b/drake/multibody/test/rigid_body_actuator_compare_to_clone.cc @@ -0,0 +1,58 @@ +#include "drake/multibody/test/rigid_body_actuator_compare_to_clone.h" + +#include + +#include "drake/common/text_logging.h" +#include "drake/multibody/rigid_body.h" + +namespace drake { +namespace multibody { +namespace test { +namespace rigid_body_actuator { + +bool CompareToClone(const RigidBodyActuator& original, + const RigidBodyActuator& clone) { + if (original.name_ != clone.name_) { + drake::log()->debug( + "CompareToClone(RigidBodyActuator): Names mismatch:\n" + " - original: {}\n" + " - clone: {}", + original.name_, clone.name_); + return false; + } + if (!original.body_->CompareToClone(*clone.body_)) { + drake::log()->debug( + "CompareToClone(RigidBodyActuator): Bodies mismatch."); + return false; + } + if (original.reduction_ != clone.reduction_) { + drake::log()->debug( + "CompareToClone(RigidBodyActuator): Reduction mismatch:\n" + " - original: {}\n" + " - clone: {}", + original.reduction_, clone.reduction_); + return false; + } + if (original.effort_limit_min_ != clone.effort_limit_min_) { + drake::log()->debug( + "CompareToClone(RigidBodyActuator): Minimum effort limits mismatch:\n" + " - original: {}\n" + " - clone: {}", + original.effort_limit_min_, clone.effort_limit_min_); + return false; + } + if (original.effort_limit_max_ != clone.effort_limit_max_) { + drake::log()->debug( + "CompareToClone(RigidBodyActuator): Maximum effort limits mismatch:\n" + " - original: {}\n" + " - clone: {}", + original.effort_limit_max_, clone.effort_limit_max_); + return false; + } + return true; +} + +} // namespace rigid_body_actuator +} // namespace test +} // namespace multibody +} // namespace drake diff --git a/drake/multibody/test/rigid_body_actuator_compare_to_clone.h b/drake/multibody/test/rigid_body_actuator_compare_to_clone.h new file mode 100644 index 000000000000..1f9a3eb8ffd1 --- /dev/null +++ b/drake/multibody/test/rigid_body_actuator_compare_to_clone.h @@ -0,0 +1,23 @@ +#pragma once + +#include "drake/multibody/rigid_body_actuator.h" + +namespace drake { +namespace multibody { +namespace test { +namespace rigid_body_actuator { + +/** + * This method compares the provided @p original and @p clone RigidBodyActuator + * objects, which are clones, and verifies their correctness. Since this method + * is intended to compare a clone, an *exact* match is performed. This method + * will only return `true` if the provided %RigidBodyActuator is exactly the + * same as its clone. + */ +bool CompareToClone(const RigidBodyActuator& original, + const RigidBodyActuator& clone); + +} // namespace rigid_body_actuator +} // namespace test +} // namespace multibody +} // namespace drake diff --git a/drake/multibody/test/rigid_body_loop_compare_to_clone.cc b/drake/multibody/test/rigid_body_loop_compare_to_clone.cc new file mode 100644 index 000000000000..038c61d5e7c0 --- /dev/null +++ b/drake/multibody/test/rigid_body_loop_compare_to_clone.cc @@ -0,0 +1,36 @@ +#include "drake/multibody/test/rigid_body_loop_compare_to_clone.h" + +#include + +#include "drake/common/text_logging.h" + +namespace drake { +namespace multibody { +namespace test { +namespace rigid_body_loop { + +bool CompareToClone(const RigidBodyLoop& original, + const RigidBodyLoop& other) { + if (!original.frameA_->CompareToClone(*other.frameA_)) { + drake::log()->debug("CompareToClone(RigidBodyLoop): FrameA mismatch."); + return false; + } + if (!original.frameB_->CompareToClone(*other.frameB_)) { + drake::log()->debug("CompareToClone(RigidBodyLoop): FrameB mismatch."); + return false; + } + if (original.axis_ != other.axis_) { + drake::log()->debug( + "CompareToClone(RigidBodyLoop): Axes mismatch:\n" + " - this: {}\n" + " - other: {}", + original.axis_.transpose(), other.axis_.transpose()); + return false; + } + return true; +} + +} // namespace rigid_body_loop +} // namespace test +} // namespace multibody +} // namespace drake diff --git a/drake/multibody/test/rigid_body_loop_compare_to_clone.h b/drake/multibody/test/rigid_body_loop_compare_to_clone.h new file mode 100644 index 000000000000..ba25d04e3a28 --- /dev/null +++ b/drake/multibody/test/rigid_body_loop_compare_to_clone.h @@ -0,0 +1,23 @@ +#pragma once + +#include "drake/multibody/rigid_body_loop.h" + +namespace drake { +namespace multibody { +namespace test { +namespace rigid_body_loop { + +/** + * This method compares the provided @p original and @p other RigidBodyLoop + * objects, which are clones, and verifies their correctness. Since this method + * is intended to compare a clone, an *exact* match is performed. This method + * will only return `true` if the provided %RigidBodyLoop is exactly the + * same as its clone. + */ +bool CompareToClone(const RigidBodyLoop& original, + const RigidBodyLoop& other); + +} // namespace rigid_body_loop +} // namespace test +} // namespace multibody +} // namespace drake diff --git a/drake/multibody/test/rigid_body_tree/CMakeLists.txt b/drake/multibody/test/rigid_body_tree/CMakeLists.txt index e04bc8b28648..87696f9716f3 100644 --- a/drake/multibody/test/rigid_body_tree/CMakeLists.txt +++ b/drake/multibody/test/rigid_body_tree/CMakeLists.txt @@ -23,8 +23,23 @@ if(Bullet_FOUND) target_link_libraries(rigid_body_collision_clique_test drakeMultibodyParsers drakeRBM) + + drake_add_cc_test(NAME rigid_body_tree_clone_test SIZE small) + target_link_libraries(rigid_body_tree_clone_test + drakeMultibodyParsers + drakeRigidBodyPlant + drakeRigidBodyTreeCompareToClone + drakeSystemAnalysis + drakeSystemPrimitives) endif() +add_library_with_exports(LIB_NAME drakeRigidBodyTreeCompareToClone SOURCE_FILES + rigid_body_tree_compare_to_clone.cc) +target_link_libraries(drakeRigidBodyTreeCompareToClone + drakeRBM + drakeRigidBodyActuatorCompareToClone + drakeRigidBodyLoopCompareToClone) + drake_add_cc_test(NAME rigid_body_tree_creation_test SIZE medium) target_link_libraries(rigid_body_tree_creation_test drakeMultibodyParsers diff --git a/drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc b/drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc new file mode 100644 index 000000000000..27bdd21c815b --- /dev/null +++ b/drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc @@ -0,0 +1,209 @@ +#include "drake/multibody/rigid_body_tree.h" + +#include +#include +#include + +#include + +#include "drake/common/drake_path.h" +#include "drake/common/eigen_matrix_compare.h" +#include "drake/common/eigen_types.h" +#include "drake/multibody/joints/floating_base_types.h" +#include "drake/multibody/parsers/sdf_parser.h" +#include "drake/multibody/parsers/urdf_parser.h" +#include "drake/multibody/rigid_body_plant/rigid_body_plant.h" +#include "drake/multibody/test/rigid_body_tree/rigid_body_tree_compare_to_clone.h" +#include "drake/systems/analysis/explicit_euler_integrator.h" +#include "drake/systems/analysis/simulator.h" +#include "drake/systems/framework/context.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/framework/diagram.h" +#include "drake/systems/primitives/constant_vector_source.h" +#include "drake/systems/primitives/signal_logger.h" + +using std::unique_ptr; + +namespace drake { + +using multibody::test::rigid_body_tree::CompareToClone; +using parsers::ModelInstanceIdTable; +using parsers::sdf::AddModelInstancesFromSdfFileToWorld; +using parsers::urdf::AddModelInstanceFromUrdfFileWithRpyJointToWorld; +using parsers::urdf::AddModelInstanceFromUrdfFileToWorld; + +namespace systems { +namespace plants { +namespace test { +namespace { + +class RigidBodyTreeCloneTest : public ::testing::Test { + protected: + virtual void SetUp() { + tree_ = std::make_unique>(); + } + unique_ptr> tree_; +}; + +// Tests RigidBodyTree::Clone() using a simple two DOF robot. +TEST_F(RigidBodyTreeCloneTest, CloneTwoDofRobot) { + std::string filename = drake::GetDrakePath() + + "/multibody/test/rigid_body_tree/two_dof_robot.urdf"; + AddModelInstanceFromUrdfFileWithRpyJointToWorld(filename, tree_.get()); + EXPECT_TRUE(CompareToClone(*tree_)); +} + +// Tests RigidBodyTree::Clone() using Atlas. +TEST_F(RigidBodyTreeCloneTest, CloneAtlas) { + std::string filename = drake::GetDrakePath() + + "/examples/Atlas/urdf/atlas_convex_hull.urdf"; + AddModelInstanceFromUrdfFileToWorld(filename, multibody::joints::kQuaternion, + tree_.get()); + EXPECT_TRUE(CompareToClone(*tree_)); +} + +// Tests RigidBodyTree::Clone() using a Prius with LIDAR sensors. +TEST_F(RigidBodyTreeCloneTest, ClonePrius) { + std::string filename = drake::GetDrakePath() + + "/automotive/models/prius/prius_with_lidar.sdf"; + AddModelInstancesFromSdfFileToWorld(filename, multibody::joints::kQuaternion, + tree_.get()); + EXPECT_TRUE(CompareToClone(*tree_)); +} + +// Tests RigidBodyTree::Clone() using Valkyrie. +TEST_F(RigidBodyTreeCloneTest, CloneValkyrie) { + std::string filename = drake::GetDrakePath() + + "/examples/Valkyrie/urdf/urdf/" + "valkyrie_A_sim_drake_one_neck_dof_wide_ankle_rom.urdf"; + // While it may seem odd to use a fixed floating joint with Valkyrie, it is + // used in this case just to confirm that RigidBodyTree::Clone() works with + // this type of joint. Previous unit tests already cover the quaternion + // floating joint type. + AddModelInstanceFromUrdfFileToWorld(filename, multibody::joints::kFixed, + tree_.get()); + EXPECT_TRUE(CompareToClone(*tree_)); +} + +class TestRbtCloneDiagram : public Diagram { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(TestRbtCloneDiagram); + + explicit TestRbtCloneDiagram(unique_ptr> tree) { + tree_ = tree.get(); + DiagramBuilder builder; + + plant_ = + builder.template AddSystem>(move(tree)); + + const int num_actuators = plant_->actuator_command_input_port().size(); + auto constant_zero_source = + builder.template AddSystem>( + VectorX::Zero(num_actuators)); + + logger_ = builder.template AddSystem>( + plant_->get_num_states()); + + builder.Connect(constant_zero_source->get_output_port(), + plant_->actuator_command_input_port()); + builder.Connect(plant_->state_output_port(), + logger_->get_input_port(0)); + builder.BuildInto(this); + } + + /// Sets the initial state of the pendulum. + /// + /// @param[in] context This Diagram's context. + /// @param[in] q The initial position of the pendulum's pivot joint. + /// @param[in] v The initial velocity of the pendulum's pivot joint. + void SetInitialState(Context* context, double q, double v) { + Context* plant_context = + GetMutableSubsystemContext(context, plant_); + DRAKE_DEMAND(plant_context != nullptr); + ContinuousState* plant_state = + plant_context->get_mutable_continuous_state(); + DRAKE_DEMAND(plant_state != nullptr); + DRAKE_DEMAND(plant_state->size() == 2); + (*plant_state)[0] = q; + (*plant_state)[1] = v; + } + + const SignalLogger& get_logger() const { return *logger_; } + + private: + RigidBodyTree* tree_{nullptr}; + RigidBodyPlant* plant_{nullptr}; + SignalLogger* logger_{nullptr}; +}; + +// Tests RigidBodyTree::Clone() by running two simulation, one using the +// original and another using a clone and verifying that the two behave +// identically. +TEST_F(RigidBodyTreeCloneTest, PendulumDynamicsTest) { + const std::string model_file_name = + GetDrakePath() + "/examples/Pendulum/Pendulum.urdf"; + const std::string model_name = "Pendulum"; + auto tree = std::make_unique>(); + AddModelInstanceFromUrdfFileToWorld( + model_file_name, drake::multibody::joints::kFixed, tree.get()); + TestRbtCloneDiagram cloned_diagram(tree->Clone()); + TestRbtCloneDiagram original_diagram(std::move(tree)); + + // Instantiates the contexts. + unique_ptr> original_context = + original_diagram.AllocateContext(); + original_diagram.SetDefaultState( + *original_context, original_context->get_mutable_state()); + original_diagram.SetInitialState(original_context.get(), 1.57, 0); + + unique_ptr> cloned_context = + cloned_diagram.AllocateContext(); + cloned_diagram.SetDefaultState( + *cloned_context, cloned_context->get_mutable_state()); + cloned_diagram.SetInitialState(cloned_context.get(), 1.57, 0); + + // Instantiates the simulators. + Simulator original_simulator( + original_diagram, std::move(original_context)); + Simulator cloned_simulator( + cloned_diagram, std::move(cloned_context)); + + // TODO(liang.fok) Consider switching to a variable step size integrator so + // the integrator step size need not be computed in the following manner. + // + // Computes a reasonable integrator step size. To do this, we use estimate the + // swing period of the pendulum, which is approximated by the equation below: + // + // swing_period = 2 * pi * sqrt(L / g) + // + // where L is the pendulum's center of mass, and g is the acceleration due to + // gravity. In this case, L = 0.5 m and g = 9.81 m/s^2. + // + const double swing_period = 2 * M_PI * std::sqrt(0.5 / 9.81); + + // We arbitrarily compute 100 steps per pendulum swing period. This should be + // sufficient to test the original RigidBodyTree against its clone. + const double integrator_step_size = swing_period / 100; + original_simulator.reset_integrator>( + original_diagram, integrator_step_size, + original_simulator.get_mutable_context()); + cloned_simulator.reset_integrator>( + cloned_diagram, integrator_step_size, + cloned_simulator.get_mutable_context()); + + original_simulator.Initialize(); + cloned_simulator.Initialize(); + + const SignalLogger& original_logger = original_diagram.get_logger(); + const SignalLogger& cloned_logger = cloned_diagram.get_logger(); + + original_simulator.StepTo(swing_period); + cloned_simulator.StepTo(swing_period); + ASSERT_TRUE(CompareMatrices(original_logger.data(), cloned_logger.data())); +} + +} // namespace +} // namespace test +} // namespace plants +} // namespace systems +} // namespace drake diff --git a/drake/multibody/test/rigid_body_tree/rigid_body_tree_compare_to_clone.cc b/drake/multibody/test/rigid_body_tree/rigid_body_tree_compare_to_clone.cc new file mode 100644 index 000000000000..f392cc966608 --- /dev/null +++ b/drake/multibody/test/rigid_body_tree/rigid_body_tree_compare_to_clone.cc @@ -0,0 +1,134 @@ +#include "drake/multibody/test/rigid_body_tree/rigid_body_tree_compare_to_clone.h" + +#include + +#include "drake/common/text_logging.h" +#include "drake/multibody/test/rigid_body_actuator_compare_to_clone.h" +#include "drake/multibody/test/rigid_body_loop_compare_to_clone.h" + +namespace drake { +namespace multibody { +namespace test { +namespace rigid_body_tree { + +bool CompareToClone(const RigidBodyTree& tree) { + std::unique_ptr> clone = tree.Clone(); + if (tree.get_num_model_instances() != clone->get_num_model_instances()) { + drake::log()->debug( + "CompareToClone(RigidBodyTree): num model instances mismatch:\n" + " - this: {}\n" + " - clone: {}", + tree.get_num_model_instances(), + clone->get_num_model_instances()); + return false; + } + if (tree.get_num_bodies() != clone->get_num_bodies()) { + drake::log()->debug( + "CompareToClone(RigidBodyTree): num bodies mismatch:\n" + " - this: {}\n" + " - clone: {}", + tree.get_num_bodies(), + clone->get_num_bodies()); + return false; + } + if (tree.get_num_frames() != clone->get_num_frames()) { + drake::log()->debug( + "CompareToClone(RigidBodyTree): num frames mismatch:\n" + " - this: {}\n" + " - clone: {}", + tree.get_num_frames(), + clone->get_num_frames()); + return false; + } + if (tree.get_num_positions() != clone->get_num_positions()) { + drake::log()->debug( + "CompareToClone(RigidBodyTree): num positions mismatch:\n" + " - this: {}\n" + " - clone: {}", + tree.get_num_positions(), + clone->get_num_positions()); + return false; + } + if (tree.get_num_velocities() != clone->get_num_velocities()) { + drake::log()->debug( + "CompareToClone(RigidBodyTree): num velocities mismatch:\n" + " - this: {}\n" + " - clone: {}", + tree.get_num_velocities(), + clone->get_num_velocities()); + return false; + } + if (tree.get_num_actuators() != clone->get_num_actuators()) { + drake::log()->debug( + "CompareToClone(RigidBodyTree): num actuators mismatch:\n" + " - this: {}\n" + " - clone: {}", + tree.get_num_actuators(), + clone->get_num_actuators()); + return false; + } + for (int i = 0; i < tree.get_num_bodies(); ++i) { + if (!tree.bodies.at(i)->CompareToClone(*clone->bodies.at(i))) { + drake::log()->debug( + "CompareToClone(RigidBodyTree): bodies mismatch at index {}.", i); + return false; + } + } + for (int i = 0; i < tree.get_num_frames(); ++i) { + if (!tree.frames.at(i)->CompareToClone(*clone->frames.at(i))) { + drake::log()->debug( + "CompareToClone(RigidBodyTree): frames mismatch at index {}.", i); + return false; + } + } + for (int i = 0; i < tree.get_num_actuators(); ++i) { + if (!rigid_body_actuator::CompareToClone( + tree.actuators.at(i), clone->actuators.at(i))) { + drake::log()->debug( + "CompareToClone(RigidBodyTree): actuators mismatch at index {}.", i); + return false; + } + } + for (int i = 0; i < static_cast(tree.loops.size()); ++i) { + const RigidBodyLoop* this_loop = &tree.loops.at(i); + const RigidBodyLoop* other_loop = &clone->loops.at(i); + if (!rigid_body_loop::CompareToClone(*this_loop, *other_loop)) { + drake::log()->debug( + "CompareToClone(RigidBodyTree): loops mismatch at index {}.", i); + return false; + } + } + if (tree.a_grav != clone->a_grav) { + drake::log()->debug( + "CompareToClone(RigidBodyTree): gravity vector mismatch:\n" + " - this: {}\n" + " - clone: {}", + tree.a_grav, + clone->a_grav); + return false; + } + if (tree.B != clone->B) { + drake::log()->debug( + "CompareToClone(RigidBodyTree): B matrix mismatch:\n" + " - this:\n{}\n" + " - clone:\n{}", + tree.B, + clone->B); + return false; + } + if (tree.initialized() != clone->initialized()) { + drake::log()->debug( + "CompareToClone(RigidBodyTree): initialized_ mismatch:\n" + " - this:\n{}\n" + " - clone:\n{}", + tree.initialized(), + clone->initialized()); + return false; + } + return true; +} + +} // namespace rigid_body_tree +} // namespace test +} // namespace multibody +} // namespace drake diff --git a/drake/multibody/test/rigid_body_tree/rigid_body_tree_compare_to_clone.h b/drake/multibody/test/rigid_body_tree/rigid_body_tree_compare_to_clone.h new file mode 100644 index 000000000000..bee859366e92 --- /dev/null +++ b/drake/multibody/test/rigid_body_tree/rigid_body_tree_compare_to_clone.h @@ -0,0 +1,21 @@ +#pragma once + +#include "drake/multibody/rigid_body_tree.h" + +namespace drake { +namespace multibody { +namespace test { +namespace rigid_body_tree { + +/** + * This method clones the provided @p tree and verifies its correctness against + * the original RigidBodyTree. Since this method is intended to compare a clone, + * an *exact* match is performed. This method will only return `true` if the + * provided %RigidBodyTree is exactly the same as its clone. + */ +bool CompareToClone(const RigidBodyTree& tree); + +} // namespace rigid_body_tree +} // namespace test +} // namespace multibody +} // namespace drake