Adds ability to clone RigidBodyTree.

Author: Chien-Liang Fok

drake/multibody/BUILD

@@ -176,6 +176,16 @@ cc_googletest(
     ],
 )
 
+cc_googletest(
+    name = "rigid_body_tree_clone_test",
+    srcs = ["test/rigid_body_tree/rigid_body_tree_clone_test.cc"],
+    data = glob(["test/rigid_body_tree/*.urdf"]),
+    deps = [
+        ":rigid_body_tree",
+        "//drake/multibody/parsers",
+    ],
+)
+
 cc_googletest(
     name = "rigid_body_tree_creation_test",
     srcs = ["test/rigid_body_tree/rigid_body_tree_creation_test.cc"],

drake/multibody/joints/drake_joint.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <memory>
 #include <random>
 #include <string>
 
@@ -101,6 +102,11 @@ class DrakeJoint {
    */
   virtual ~DrakeJoint();
 
+  /**
+   * Returns a clone of this DrakeJoint.
+   */
+  virtual std::unique_ptr<DrakeJoint> Clone() const = 0;
+
   /**
    * Returns the transform `X_PF` giving the pose of the joint's "fixed" frame
    * `F` in its parent body frame `P`. Frame `F` is the joint frame that is

drake/multibody/joints/fixed_joint.cc

@@ -1,5 +1,14 @@
 #include "drake/multibody/joints/fixed_joint.h"
 
+#include <memory>
+#include <utility>
+
+std::unique_ptr<DrakeJoint> FixedJoint::Clone() const {
+  auto joint = std::make_unique<FixedJoint>(get_name(),
+                                            get_transform_to_parent_body());
+  return std::move(joint);
+}
+
 std::string FixedJoint::get_position_name(int index) const {
   throw std::runtime_error("bad index");
 }

drake/multibody/joints/fixed_joint.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <memory>
 #include <string>
 
 #include "drake/common/eigen_types.h"
@@ -15,6 +16,8 @@ class FixedJoint : public DrakeJointImpl<FixedJoint> {
 
   virtual ~FixedJoint() {}
 
+  std::unique_ptr<DrakeJoint> Clone() const override;
+
   template <typename DerivedQ>
   Eigen::Transform<typename DerivedQ::Scalar, 3, Eigen::Isometry>
   jointTransform(const Eigen::MatrixBase<DerivedQ>& q) const {

drake/multibody/joints/helical_joint.cc

@@ -1,7 +1,17 @@
 #include "drake/multibody/joints/helical_joint.h"
 
+#include <memory>
+#include <utility>
+
 using Eigen::Vector3d;
 
+std::unique_ptr<DrakeJoint> HelicalJoint::Clone() const {
+  auto joint = std::make_unique<HelicalJoint>(get_name(),
+                                              get_transform_to_parent_body(),
+                                              axis_, pitch_);
+  return std::move(joint);
+}
+
 drake::TwistVector<double> HelicalJoint::spatialJointAxis(const Vector3d& axis,
                                                           double pitch) {
   drake::TwistVector<double> ret;

drake/multibody/joints/helical_joint.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <memory>
 #include <string>
 
 #include "drake/common/eigen_types.h"
@@ -28,6 +29,8 @@ class HelicalJoint : public FixedAxisOneDoFJoint<HelicalJoint> {
 
   virtual ~HelicalJoint() {}
 
+  std::unique_ptr<DrakeJoint> Clone() const override;
+
   template <typename DerivedQ>
   Eigen::Transform<typename DerivedQ::Scalar, 3, Eigen::Isometry>
   jointTransform(const Eigen::MatrixBase<DerivedQ>& q) const {

drake/multibody/joints/prismatic_joint.cc

@@ -1,7 +1,17 @@
 #include "drake/multibody/joints/prismatic_joint.h"
 
+#include <memory>
+#include <utility>
+
 using Eigen::Vector3d;
 
+std::unique_ptr<DrakeJoint> PrismaticJoint::Clone() const {
+  auto joint = std::make_unique<PrismaticJoint>(get_name(),
+                                                get_transform_to_parent_body(),
+                                                translation_axis_);
+  return std::move(joint);
+}
+
 drake::TwistVector<double> PrismaticJoint::spatialJointAxis(
     const Vector3d& translation_axis) {
   drake::TwistVector<double> ret;

drake/multibody/joints/prismatic_joint.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <memory>
 #include <string>
 
 #include "drake/common/drake_assert.h"
@@ -51,6 +52,8 @@ class PrismaticJoint : public FixedAxisOneDoFJoint<PrismaticJoint> {
 
   virtual ~PrismaticJoint() {}
 
+  std::unique_ptr<DrakeJoint> Clone() const override;
+
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  private:

drake/multibody/joints/quaternion_floating_joint.cc

@@ -1,13 +1,21 @@
 #include "drake/multibody/joints/quaternion_floating_joint.h"
 
+#include <memory>
 #include <random>
+#include <utility>
 
 #include "drake/math/random_rotation.h"
 
 using Eigen::Vector4d;
 using Eigen::VectorXd;
 using std::normal_distribution;
 
+std::unique_ptr<DrakeJoint> QuaternionFloatingJoint::Clone() const {
+  auto joint = std::make_unique<QuaternionFloatingJoint>(get_name(),
+                                                get_transform_to_parent_body());
+  return std::move(joint);
+}
+
 std::string QuaternionFloatingJoint::get_position_name(int index) const {
   switch (index) {
     case 0:

drake/multibody/joints/quaternion_floating_joint.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <memory>
 #include <string>
 
 #include "drake/common/constants.h"
@@ -80,6 +81,8 @@ class QuaternionFloatingJoint : public DrakeJointImpl<QuaternionFloatingJoint> {
 
   virtual ~QuaternionFloatingJoint() {}
 
+  std::unique_ptr<DrakeJoint> Clone() const override;
+
   /** Returns the transform `X_PB(q)` where P is the parent body and B the
    * child body connected by this joint.
    */

drake/multibody/joints/revolute_joint.cc

@@ -1,7 +1,17 @@
 #include "drake/multibody/joints/revolute_joint.h"
 
+#include <memory>
+#include <utility>
+
 using Eigen::Vector3d;
 
+std::unique_ptr<DrakeJoint> RevoluteJoint::Clone() const {
+  auto joint = std::make_unique<RevoluteJoint>(get_name(),
+                                               get_transform_to_parent_body(),
+                                               rotation_axis);
+  return std::move(joint);
+}
+
 drake::TwistVector<double> RevoluteJoint::spatialJointAxis(
     const Vector3d& rotation_axis) {
   drake::TwistVector<double> ret;

drake/multibody/joints/revolute_joint.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <memory>
 #include <string>
 
 #include <Eigen/Geometry>
@@ -11,13 +12,6 @@
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Woverloaded-virtual"
 class RevoluteJoint : public FixedAxisOneDoFJoint<RevoluteJoint> {
-  // disable copy construction and assignment
-  // RevoluteJoint(const RevoluteJoint&) = delete;
-  // RevoluteJoint& operator=(const RevoluteJoint&) = delete;
-
- private:
-  Eigen::Vector3d rotation_axis;
-
  public:
   RevoluteJoint(const std::string& name,
                 const Eigen::Isometry3d& transform_to_parent_body,
@@ -31,6 +25,8 @@ class RevoluteJoint : public FixedAxisOneDoFJoint<RevoluteJoint> {
 
   virtual ~RevoluteJoint() {}
 
+  std::unique_ptr<DrakeJoint> Clone() const override;
+
   template <typename DerivedQ>
   Eigen::Transform<typename DerivedQ::Scalar, 3, Eigen::Isometry>
   jointTransform(const Eigen::MatrixBase<DerivedQ>& q) const {
@@ -47,5 +43,8 @@ class RevoluteJoint : public FixedAxisOneDoFJoint<RevoluteJoint> {
 
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ private:
+  Eigen::Vector3d rotation_axis;
 };
 #pragma GCC diagnostic pop  // pop -Wno-overloaded-virtual

drake/multibody/joints/roll_pitch_yaw_floating_joint.cc

@@ -1,6 +1,8 @@
 #include "drake/multibody/joints/roll_pitch_yaw_floating_joint.h"
 
+#include <memory>
 #include <random>
+#include <utility>
 
 #include <Eigen/Dense>
 
@@ -10,6 +12,13 @@ using Eigen::Map;
 using Eigen::Vector3d;
 using Eigen::VectorXd;
 
+std::unique_ptr<DrakeJoint> RollPitchYawFloatingJoint::Clone() const {
+  auto joint = std::make_unique<RollPitchYawFloatingJoint>(
+      get_name(),
+      get_transform_to_parent_body());
+  return std::move(joint);
+}
+
 std::string RollPitchYawFloatingJoint::get_position_name(int index) const {
   switch (index) {
     case 0:

drake/multibody/joints/roll_pitch_yaw_floating_joint.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <memory>
 #include <string>
 
 #include "drake/common/constants.h"
@@ -26,6 +27,8 @@ class RollPitchYawFloatingJoint
 
   virtual ~RollPitchYawFloatingJoint() {}
 
+  std::unique_ptr<DrakeJoint> Clone() const override;
+
   template <typename DerivedQ>
   Eigen::Transform<typename DerivedQ::Scalar, 3, Eigen::Isometry>
   jointTransform(const Eigen::MatrixBase<DerivedQ>& q) const {

drake/multibody/rigid_body.cc

@@ -9,6 +9,8 @@ using Eigen::Isometry3d;
 using Eigen::Matrix;
 using Eigen::Vector3d;
 
+using std::make_unique;
+using std::move;
 using std::ostream;
 using std::runtime_error;
 using std::string;
@@ -21,6 +23,35 @@ RigidBody<T>::RigidBody() {
   spatial_inertia_ << drake::SquareTwistMatrix<double>::Zero();
 }
 
+// TODO(liang.fok) For an explanation of why these SWIG preprocessor commands
+// are needed, see the comment immediately above the declaration of
+// RigidBody::Clone() in rigid_body.h.
+#ifndef SWIG
+template <typename T>
+std::unique_ptr<RigidBody<T>> RigidBody<T>::Clone() const {
+  auto body = make_unique<RigidBody<T>>();
+  body->set_name(get_name());
+  body->set_model_name(get_model_name());
+  body->set_model_instance_id(get_model_instance_id());
+  body->set_body_index(body_index_);
+  body->set_position_start_index(position_start_index_);
+  body->set_velocity_start_index(velocity_start_index_);
+
+  // TODO(liang.fok) Add cloning of visual elements.
+  // body::visual_elements_ = ;
+
+  // TODO(liang.fok Add cloning of collision elements.)
+  // body::collision_element_ids_ = ;
+  // body::collision_element_groups_ = ;
+
+  body->set_contact_points(contact_points_);
+  body->set_mass(mass_);
+  body->set_center_of_mass(center_of_mass_);
+  body->set_spatial_inertia(spatial_inertia_);
+  return move(body);
+}
+#endif
+
 template <typename T>
 const std::string& RigidBody<T>::get_name() const { return name_; }
 

drake/multibody/rigid_body.h

@@ -21,6 +21,19 @@ class RigidBody {
  public:
   RigidBody();
 
+  // TODO(liang.fok) The following preprocessor condition is necessary because
+  // wrapping method Clone() in SWIG caused a build error to the effect of "call
+  // to implicitly-deleted copy constructor". Unfortunately, adding
+  // "%ignore RigidBodyTree<double>::Clone()" to
+  // drake-distro/drake/bindings/swig/rbtree.i does not work for some unknown
+  // reason.
+#ifndef SWIG
+  /**
+   * Returns a clone of this RigidBody.
+   */
+  virtual std::unique_ptr<RigidBody<T>> Clone() const;
+#endif
+
   /**
    * Returns the name of this rigid body.
    */

drake/multibody/rigid_body_frame.cc

@@ -1,5 +1,7 @@
 #include "drake/multibody/rigid_body_frame.h"
 
+#include <memory>
+
 #include "drake/common/eigen_autodiff_types.h"
 #include "drake/math/roll_pitch_yaw.h"
 
@@ -16,6 +18,20 @@ RigidBodyFrame<T>::RigidBodyFrame(const std::string& name, RigidBody<T>* body,
   transform_to_body_.matrix() << drake::math::rpy2rotmat(rpy), xyz, 0, 0, 0, 1;
 }
 
+// TODO(liang.fok) For an explanation of why these SWIG preprocessor commands
+// are needed, see the comment immediately above the declaration of
+// RigidBodyFrame::Clone() in rigid_body_frame.h.
+#ifndef SWIG
+template <typename T>
+std::shared_ptr<RigidBodyFrame<T>> RigidBodyFrame<T>::Clone() const {
+  auto frame = std::make_shared<RigidBodyFrame<T>>();
+  frame->set_name(name_);
+  frame->set_transform_to_body(transform_to_body_);
+  frame->set_frame_index(frame_index_);
+  return move(frame);
+}
+#endif
+
 template <typename T>
 int RigidBodyFrame<T>::get_model_instance_id() const {
   return body_->get_model_instance_id();

drake/multibody/rigid_body_frame.h

@@ -57,6 +57,21 @@ class RigidBodyFrame {
   RigidBodyFrame()
       : RigidBodyFrame("", nullptr, Eigen::Isometry3d::Identity()) {}
 
+  // TODO(liang.fok) The following preprocessor condition is necessary because
+  // wrapping method Clone() in SWIG caused a build error to the effect of "call
+  // to implicitly-deleted copy constructor". Unfortunately, adding
+  // "%ignore RigidBodyTree<double>::Clone()" to
+  // drake-distro/drake/bindings/swig/rbtree.i does not work for some unknown
+  // reason.
+#ifndef SWIG
+  // TODO(liang.fok) Update this to return a unique_ptr. This is related to
+  // #3093.
+  /**
+   * Returns a clone of this RigidBodyFrame.
+   */
+  virtual std::shared_ptr<RigidBodyFrame<T>> Clone() const;
+#endif
+
   /**
    * Returns the ID of the model instance to which this rigid body frame
    * belongs.