Adds ability to clone RigidBodyTree.

Author: Chien-Liang Fok

drake/multibody/BUILD

@@ -98,6 +98,16 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "rigid_body_tree_compare_to_clone",
+    srcs = ["test/rigid_body_tree/rigid_body_tree_compare_to_clone.cc"],
+    hdrs = ["test/rigid_body_tree/rigid_body_tree_compare_to_clone.h"],
+    linkstatic = 1,
+    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",
@@ -181,6 +191,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,
@@ -10,3 +15,44 @@ RigidBodyActuator::RigidBodyActuator(const std::string& name,
       reduction_(reduction),
       effort_limit_min_(effort_limit_min),
       effort_limit_max_(effort_limit_max) {}
+
+bool RigidBodyActuator::CompareToClone(const RigidBodyActuator& other) const {
+  if (this->name_ != other.name_) {
+    drake::log()->debug(
+        "RigidBodyActuator::CompareToClone(): Names mismatch:\n"
+        "  - this: {}\n"
+        "  - other: {}",
+        this->name_, other.name_);
+    return false;
+  }
+  if (!this->body_->CompareToClone(*other.body_)) {
+    drake::log()->debug(
+        "RigidBodyActuator::CompareToClone(): Bodies mismatch.");
+    return false;
+  }
+  if (this->reduction_ != other.reduction_) {
+    drake::log()->debug(
+        "RigidBodyActuator::CompareToClone(): Reduction mismatch:\n"
+        "  - this: {}\n"
+        "  - other: {}",
+        this->reduction_, other.reduction_);
+    return false;
+  }
+  if (this->effort_limit_min_ != other.effort_limit_min_) {
+    drake::log()->debug(
+        "RigidBodyActuator::CompareToClone(): Minimum effort limits mismatch:\n"
+        "  - this: {}\n"
+        "  - other: {}",
+        this->effort_limit_min_, other.effort_limit_min_);
+    return false;
+  }
+  if (this->effort_limit_max_ != other.effort_limit_max_) {
+    drake::log()->debug(
+        "RigidBodyActuator::CompareToClone(): Maximum effort limits mismatch:\n"
+        "  - this: {}\n"
+        "  - other: {}",
+        this->effort_limit_max_, other.effort_limit_max_);
+    return false;
+  }
+  return true;
+}

drake/multibody/rigid_body_actuator.h

@@ -36,6 +36,14 @@ class RigidBodyActuator {
       double effort_limit_min = -std::numeric_limits<double>::infinity(),
       double effort_limit_max = std::numeric_limits<double>::infinity());
 
+  /**
+   * Compares this %RigidBodyActuator with a clone. Since this method is
+   * intended to compare a clone, an *exact* match is performed. This method
+   * will only return `true` if the provided `other` %RigidBodyActuator is
+   * exactly the same as this %RigidBodyActuator.
+   */
+  bool CompareToClone(const RigidBodyActuator& other) const;
+
   const std::string name_;
   const RigidBody<double>* const body_;
   const double reduction_;

drake/multibody/rigid_body_loop.cc

@@ -1,13 +1,36 @@
 #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,
                                 std::shared_ptr<RigidBodyFrame<T>> frameB,
                                 const Eigen::Vector3d& axis)
     : frameA_(frameA), frameB_(frameB), axis_(axis) {}
 
+template <>
+bool RigidBodyLoop<double>::CompareToClone(const RigidBodyLoop<double>& other)
+    const {
+  if (!this->frameA_->CompareToClone(*other.frameA_)) {
+    drake::log()->debug("RigidBodyLoop::CompareToClone(): FrameA mismatch.");
+    return false;
+  }
+  if (!this->frameB_->CompareToClone(*other.frameB_)) {
+    drake::log()->debug("RigidBodyLoop::CompareToClone(): FrameB mismatch.");
+    return false;
+  }
+  if (this->axis_ != other.axis_) {
+    drake::log()->debug(
+        "RigidBodyLoop::CompareToClone(): Axes mismatch:\n"
+        "  - this: {}\n"
+        "  - other: {}",
+        this->axis_.transpose(), other.axis_.transpose());
+    return false;
+  }
+  return true;
+}
+
 std::ostream& operator<<(std::ostream& os, const RigidBodyLoop<double>& obj) {
   os << "loop connects pt "
      << obj.frameA_->get_transform_to_body().matrix().topRightCorner(3, 1)

drake/multibody/rigid_body_loop.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <iostream>
 #include <memory>
 
 #include <Eigen/Dense>
@@ -44,6 +45,14 @@ class RigidBodyLoop {
                 std::shared_ptr<RigidBodyFrame<T>> frameB,
                 const Eigen::Vector3d& axis);
 
+  /**
+   * Compares this %RigidBodyLoop with a clone. Since this method is intended to
+   * compare a clone, an *exact* match is performed. This method will only
+   * return `true` if the provided `other` %RigidBodyLoop is exactly the same as
+   * this %RigidBodyLoop.
+   */
+  bool CompareToClone(const RigidBodyLoop<double>& other) const;
+
   const std::shared_ptr<RigidBodyFrame<T>> frameA_, frameB_;
   const Eigen::Vector3d axis_;
 

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
@@ -1268,17 +1283,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;
+
 #ifndef SWIG
   DRAKE_DEPRECATED("Please use get_num_bodies().")
 #endif
@@ -1458,6 +1484,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/rigid_body_tree/CMakeLists.txt

@@ -23,8 +23,21 @@ 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)
+
 drake_add_cc_test(NAME rigid_body_tree_creation_test SIZE medium)
 target_link_libraries(rigid_body_tree_creation_test
     drakeMultibodyParsers