Add cloning functionality to StateSpace (#422)

* Fixing const-correctness in state space

* Fixing const-correctness of StateHandle

* Fix implicit conversion of StateHandle to State*

* Undo unnecessary changes

* Remove incorrect comment

* Add clone to StateSpace

* Format code

* Update changelog

* Update CHANGELOG.md

CHANGELOG.md

@@ -16,6 +16,7 @@
   * Fixed bug in the representation of SO(2) statespace: [#408](https://github.com/personalrobotics/aikido/pull/408)
   * Fixed const correctness of StateHandle::getState(): [#419](https://github.com/personalrobotics/aikido/pull/419)
   * Fixed hidden compose function (in-place version): [#421](https://github.com/personalrobotics/aikido/pull/421)
+  * Added clone functionality to StateSpace: [#422](https://github.com/personalrobotics/aikido/pull/422)
 
 * Constraint
 

include/aikido/statespace/CartesianProduct.hpp

@@ -35,6 +35,9 @@ class CartesianProduct : public std::enable_shared_from_this<CartesianProduct>,
   /// \return new \c ScopedState
   ScopedState createState() const;
 
+  /// Creates an identical clone of \c stateIn.
+  ScopedState cloneState(const StateSpace::State* stateIn) const;
+
   /// Gets number of subspaces.
   ///
   /// \return number of subspaces

include/aikido/statespace/Rn.hpp

@@ -74,6 +74,9 @@ class R : public virtual StateSpace
   /// \return new \c ScopedState
   ScopedState createState() const;
 
+  /// Creates an identical clone of \c stateIn.
+  ScopedState cloneState(const StateSpace::State* stateIn) const;
+
   /// Gets the real vector stored in a \c State.
   ///
   /// \param _state a \c State in this state space

include/aikido/statespace/SE2.hpp

@@ -68,6 +68,9 @@ class SE2 : public virtual StateSpace
   /// \return new \c ScopedState
   ScopedState createState() const;
 
+  /// Creates an identical clone of \c stateIn.
+  ScopedState cloneState(const StateSpace::State* stateIn) const;
+
   /// Gets value as an Eigen transformation object.
   ///
   /// \param _state a \c State in this state space

include/aikido/statespace/SE3.hpp

@@ -68,6 +68,9 @@ class SE3 : public virtual StateSpace
   /// \return new \c ScopedState
   ScopedState createState() const;
 
+  /// Creates an identical clone of \c stateIn.
+  ScopedState cloneState(const StateSpace::State* stateIn) const;
+
   /// Gets value as an Eigen transformation object.
   ///
   /// \param _state a \c State in this state space

include/aikido/statespace/SO2.hpp

@@ -36,6 +36,9 @@ class SO2 : virtual public StateSpace
   /// \return new \c ScopedState.
   ScopedState createState() const;
 
+  /// Creates an identical clone of \c stateIn.
+  ScopedState cloneState(const StateSpace::State* stateIn) const;
+
   /// Returns state as a rotation angle in (-pi, pi].
   ///
   /// \param[in] state State.

include/aikido/statespace/SO3.hpp

@@ -66,6 +66,9 @@ class SO3 : virtual public StateSpace
   /// \return new \c ScopedState
   ScopedState createState() const;
 
+  /// Creates an identical clone of \c stateIn.
+  ScopedState cloneState(const StateSpace::State* stateIn) const;
+
   /// Gets a state as a unit quaternion.
   ///
   /// \param _state input state

include/aikido/statespace/ScopedState.hpp

@@ -34,6 +34,9 @@ class ScopedState : public _Handle
   ScopedState(ScopedState&&) = default;
   ScopedState& operator=(ScopedState&&) = default;
 
+  /// Creates an identical clone of the state this ScopedState handles.
+  ScopedState clone() const;
+
 private:
   std::unique_ptr<char[]> mBuffer;
 };

include/aikido/statespace/StateSpace.hpp

@@ -47,6 +47,9 @@ class StateSpace
   /// \return new \c ScopedState
   ScopedState createState() const;
 
+  /// Creates an identical clone of \c stateIn.
+  ScopedState cloneState(const State* stateIn) const;
+
   /// Allocate a new state. This must be deleted with \c freeState. This is a
   /// helper function that allocates memory, uses \c allocateStateInBuffer to
   /// create a \c State, and returns that pointer.

include/aikido/statespace/detail/Rn-impl.hpp

@@ -121,6 +121,16 @@ auto R<N>::createState() const -> ScopedState
   return ScopedState(this);
 }
 
+//==============================================================================
+template <int N>
+auto R<N>::cloneState(const StateSpace::State* stateIn) const -> ScopedState
+{
+  auto newState = createState();
+  copyState(stateIn, newState);
+
+  return newState;
+}
+
 //==============================================================================
 template <int N>
 auto R<N>::getMutableValue(State* _state) const -> Eigen::Map<VectorNd>

include/aikido/statespace/detail/SO3-impl.hpp

@@ -18,6 +18,7 @@ class SO3StateHandle : public statespace::StateHandle<SO3, _QualifiedState>
   /// Construct and initialize to \c nullptr.
   SO3StateHandle()
   {
+    // Do nothing
   }
 
   /// Construct a handle for \c _state in \c _space.
@@ -27,6 +28,7 @@ class SO3StateHandle : public statespace::StateHandle<SO3, _QualifiedState>
   SO3StateHandle(const StateSpace* _space, QualifiedState* _state)
     : statespace::StateHandle<StateSpace, QualifiedState>(_space, _state)
   {
+    // Do nothing
   }
 
   /// Constructs a point in SO(3) from a unit quaternion.

include/aikido/statespace/detail/ScopedState-impl.hpp

@@ -20,5 +20,12 @@ ScopedState<Handle>::~ScopedState()
   this->mSpace->freeStateInBuffer(this->mState);
 }
 
+//==============================================================================
+template <class Handle>
+ScopedState<Handle> ScopedState<Handle>::clone() const
+{
+  return this->mSpace->cloneState(this->mState);
+}
+
 } // namespace statespace
 } // namespace aikido

src/statespace/CartesianProduct.cpp

@@ -31,6 +31,16 @@ auto CartesianProduct::createState() const -> ScopedState
   return ScopedState(this);
 }
 
+//==============================================================================
+CartesianProduct::ScopedState CartesianProduct::cloneState(
+    const StateSpace::State* stateIn) const
+{
+  auto newState = createState();
+  copyState(stateIn, newState);
+
+  return newState;
+}
+
 //==============================================================================
 std::size_t CartesianProduct::getNumSubspaces() const
 {

src/statespace/SE2.cpp

@@ -31,6 +31,15 @@ auto SE2::createState() const -> ScopedState
   return ScopedState(this);
 }
 
+//==============================================================================
+SE2::ScopedState SE2::cloneState(const StateSpace::State* stateIn) const
+{
+  auto newState = createState();
+  copyState(stateIn, newState);
+
+  return newState;
+}
+
 //==============================================================================
 auto SE2::getIsometry(const State* _state) const -> const Isometry2d&
 {

src/statespace/SE3.cpp

@@ -31,6 +31,15 @@ auto SE3::createState() const -> ScopedState
   return ScopedState(this);
 }
 
+//==============================================================================
+SE3::ScopedState SE3::cloneState(const StateSpace::State* stateIn) const
+{
+  auto newState = createState();
+  copyState(stateIn, newState);
+
+  return newState;
+}
+
 //==============================================================================
 auto SE3::getIsometry(const State* _state) const -> const Isometry3d&
 {

src/statespace/SO2.cpp

@@ -1,7 +1,8 @@
-#include <aikido/statespace/SO2.hpp>
+#include "aikido/statespace/SO2.hpp"
 
 namespace aikido {
 namespace statespace {
+
 //==============================================================================
 SO2::State::State(double angle)
 {
@@ -43,6 +44,15 @@ auto SO2::createState() const -> ScopedState
   return ScopedState(this);
 }
 
+//==============================================================================
+SO2::ScopedState SO2::cloneState(const StateSpace::State* stateIn) const
+{
+  auto newState = createState();
+  copyState(stateIn, newState);
+
+  return newState;
+}
+
 //==============================================================================
 double SO2::toAngle(const State* state) const
 {

src/statespace/SO3.cpp

@@ -34,6 +34,15 @@ auto SO3::createState() const -> ScopedState
   return ScopedState(this);
 }
 
+//==============================================================================
+auto SO3::cloneState(const StateSpace::State* stateIn) const -> ScopedState
+{
+  auto newState = createState();
+  copyState(stateIn, newState);
+
+  return newState;
+}
+
 //==============================================================================
 auto SO3::getQuaternion(const State* _state) const -> const Quaternion&
 {

src/statespace/StateSpace.cpp

@@ -9,6 +9,16 @@ auto StateSpace::createState() const -> ScopedState
   return ScopedState(this);
 }
 
+//==============================================================================
+StateSpace::ScopedState StateSpace::cloneState(
+    const StateSpace::State* stateIn) const
+{
+  auto newState = createState();
+  copyState(stateIn, newState);
+
+  return newState;
+}
+
 //==============================================================================
 void StateSpace::compose(State* _state1, const State* _state2) const
 {

tests/statespace/test_CartesianProduct.cpp

@@ -1,3 +1,4 @@
+#include <dart/math/Helpers.hpp>
 #include <gtest/gtest.h>
 #include <aikido/statespace/CartesianProduct.hpp>
 #include <aikido/statespace/Rn.hpp>
@@ -12,6 +13,28 @@ using aikido::statespace::SO2;
 using aikido::statespace::SO3;
 using aikido::statespace::SE2;
 
+TEST(CartesianProduct, Clone)
+{
+  CartesianProduct space({std::make_shared<SO2>(), std::make_shared<R2>()});
+
+  for (auto i = 0u; i < 5u; ++i)
+  {
+    auto s1 = space.createState();
+    const auto angle = dart::math::random(-M_PI, M_PI);
+    s1.getSubStateHandle<SO2>(0).fromAngle(angle);
+    s1.getSubStateHandle<R2>(1).setValue(Eigen::Vector2d::Random());
+
+    auto s2 = s1.clone();
+
+    EXPECT_DOUBLE_EQ(
+        s1.getSubStateHandle<SO2>(0).toAngle(),
+        s2.getSubStateHandle<SO2>(0).toAngle());
+    EXPECT_TRUE(
+        s1.getSubStateHandle<R2>(1).getValue().isApprox(
+            s2.getSubStateHandle<R2>(1).getValue()));
+  }
+}
+
 TEST(CartesianProduct, Compose)
 {
   using Eigen::Vector2d;

tests/statespace/test_Rn.cpp

@@ -5,6 +5,42 @@ using aikido::statespace::Rn;
 using aikido::statespace::R3;
 using R4 = aikido::statespace::R<4>;
 
+//==============================================================================
+TEST(Rn, CloneR3)
+{
+  R3 rvss;
+
+  for (auto i = 0u; i < 5u; ++i)
+  {
+    const Eigen::Vector3d pos = Eigen::Vector3d::Random();
+
+    auto s1 = rvss.createState();
+    s1.setValue(pos);
+
+    auto s2 = s1.clone();
+
+    EXPECT_TRUE(s1.getValue().isApprox(s2.getValue()));
+  }
+}
+
+//==============================================================================
+TEST(Rn, CloneRx)
+{
+  Rn rvss(3);
+
+  for (auto i = 0u; i < 5u; ++i)
+  {
+    const Eigen::Vector3d pos = Eigen::Vector3d::Random();
+
+    auto s1 = rvss.createState();
+    s1.setValue(pos);
+
+    auto s2 = s1.clone();
+
+    EXPECT_TRUE(s1.getValue().isApprox(s2.getValue()));
+  }
+}
+
 //==============================================================================
 TEST(Rn, ComposeR3)
 {

tests/statespace/test_SE2.cpp

@@ -1,8 +1,29 @@
+#include <dart/math/Helpers.hpp>
 #include <gtest/gtest.h>
 #include <aikido/statespace/SE2.hpp>
 
 using aikido::statespace::SE2;
 
+TEST(SE2, Clone)
+{
+  SE2 se2;
+
+  for (auto i = 0u; i < 5u; ++i)
+  {
+    const auto angle = dart::math::random(-M_PI, M_PI);
+    Eigen::Isometry2d pose = Eigen::Isometry2d::Identity();
+    pose.rotate(Eigen::Rotation2Dd(angle));
+    pose.translation() = Eigen::Vector2d::Random();
+
+    auto s1 = se2.createState();
+    s1.setIsometry(pose);
+
+    auto s2 = s1.clone();
+
+    EXPECT_TRUE(s1.getIsometry().isApprox(s2.getIsometry()));
+  }
+}
+
 TEST(SE2, Compose)
 {
   SE2 space;

tests/statespace/test_SE3.cpp

@@ -1,9 +1,32 @@
+#include <dart/math/Helpers.hpp>
 #include <gtest/gtest.h>
 #include <aikido/statespace/SE3.hpp>
 
 using aikido::statespace::SE3;
 using Vector6d = Eigen::Matrix<double, 6, 1>;
 
+TEST(SE3, Clone)
+{
+  SE3 se3;
+
+  for (auto i = 0u; i < 5u; ++i)
+  {
+    Eigen::Isometry3d pose = Eigen::Isometry3d::Identity();
+    const auto angle = dart::math::random(-M_PI, M_PI);
+    const auto axis = Eigen::Vector3d::Random().normalized();
+    const auto angleAxis = Eigen::AngleAxisd(angle, axis);
+    pose.linear() = angleAxis.toRotationMatrix();
+    pose.translation() = Eigen::Vector3d::Random();
+
+    auto s1 = se3.createState();
+    s1.setIsometry(pose);
+
+    auto s2 = s1.clone();
+
+    EXPECT_TRUE(s1.getIsometry().isApprox(s2.getIsometry()));
+  }
+}
+
 TEST(SE3, Compose)
 {
   SE3 space;