Adds Ability to Clone a RigidBodyTree. (#4958)

* Adds ability to clone RigidBodyTree.

Author: Chien-Liang Fok

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"],

drake/multibody/rigid_body_actuator.cc

@@ -1,5 +1,10 @@
 #include "drake/multibody/rigid_body_actuator.h"
 
+#include <string>
+
+#include "drake/common/text_logging.h"
+#include "drake/multibody/rigid_body.h"
+
 RigidBodyActuator::RigidBodyActuator(const std::string& name,
                                      const RigidBody<double>* body,
                                      double reduction,

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 <typename T>
 RigidBodyLoop<T>::RigidBodyLoop(std::shared_ptr<RigidBodyFrame<T>> frameA,

drake/multibody/rigid_body_loop.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <iostream>
 #include <memory>
 
 #include <Eigen/Dense>

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<T>::RigidBodyTree()
 template <typename T>
 RigidBodyTree<T>::~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<double>> RigidBodyTree<double>::Clone() const {
+  auto clone = make_unique<RigidBodyTree<double>>();
+  // 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<double>* cloned_body = clone->get_mutable_body(body_index);
+    DRAKE_DEMAND(cloned_body != nullptr);
+    DRAKE_DEMAND(cloned_body->get_body_index() == body_index);
+
+    const RigidBody<double>* 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<double>* 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<double>& 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<double>* cloned_frame_body =
+        clone->get_mutable_body(cloned_frame_body_index);
+    DRAKE_DEMAND(cloned_frame_body != nullptr);
+    std::shared_ptr<RigidBodyFrame<double>> cloned_frame =
+        original_frame->Clone(cloned_frame_body);
+    clone->frames.push_back(cloned_frame);
+  }
+
+  for (const auto& actuator : actuators) {
+    const RigidBody<double>& 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<RigidBodyFrame<double>> frame_a =
+        clone->findFrame(loop.frameA_->get_name(),
+            loop.frameA_->get_model_instance_id());
+    std::shared_ptr<RigidBodyFrame<double>> 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 <typename T>
 bool RigidBodyTree<T>::transformCollisionFrame(
     RigidBody<T>* body, const Eigen::Isometry3d& displace_transform) {
@@ -2760,11 +2846,16 @@ int RigidBodyTree<T>::FindIndexOfChildBodyOfJoint(const std::string& joint_name,
 
 template <typename T>
 const RigidBody<T>& RigidBodyTree<T>::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 <typename T>
+RigidBody<T>* RigidBodyTree<T>::get_mutable_body(int body_index) {
+  DRAKE_DEMAND(body_index >= 0 && body_index < get_num_bodies());
+  return bodies[body_index].get();
+}
+
 template <typename T>
 int RigidBodyTree<T>::get_num_bodies() const {
   return static_cast<int>(bodies.size());
@@ -2776,6 +2867,11 @@ int RigidBodyTree<T>::get_number_of_bodies() const {
   return get_num_bodies();
 }
 
+template <typename T>
+int RigidBodyTree<T>::get_num_frames() const {
+  return static_cast<int>(frames.size());
+}
+
 // TODO(liang.fok) Remove this method prior to Release 1.0.
 template <typename T>
 RigidBody<T>* RigidBodyTree<T>::findJoint(const std::string& joint_name,

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<double>::Clone()" to
+  // drake-distro/drake/bindings/swig/rbtree.i does not work.
+#ifndef SWIG
+  /**
+   * Returns a deep clone of this RigidBodyTree<double>. Currently, everything
+   * *except* for collision and visual elements are cloned.
+   */
+  std::unique_ptr<RigidBodyTree<double>> 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<T>& 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<T>* 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;

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)

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 <memory>
+
+#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

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