Move MetaSkeletonStateSpaceSaver implementation to a cpp file.

CHANGELOG.md

@@ -2,6 +2,9 @@
 
 ### 0.2.0 (201X-XX-XX)
 
+* State Space
+  * Moved MetaSkeletonStateSpaceSaver implementation to src: [#273](https://github.com/personalrobotics/aikido/pull/273)
+
 * Constraint
 
   * Added methods for removing groups from NonColliding constraints: [#247](https://github.com/personalrobotics/aikido/pull/247)

include/aikido/statespace/dart/MetaSkeletonStateSpaceSaver.hpp

@@ -40,6 +40,4 @@ class MetaSkeletonStateSpaceSaver
 } // namespace statespace
 } // namespace aikido
 
-#include "detail/MetaSkeletonStateSpaceSaver-impl.hpp"
-
 #endif // ifndef AIKIDO_STATESPACE_DART_METASKELETONSTATESPACESAVER_HPP_

src/statespace/CMakeLists.txt

@@ -10,6 +10,7 @@ set(sources
   dart/JointStateSpace.cpp
   dart/JointStateSpaceHelpers.cpp
   dart/MetaSkeletonStateSpace.cpp
+  dart/MetaSkeletonStateSpaceSaver.cpp
   dart/SE2Joint.cpp
   dart/SE3Joint.cpp
   dart/SO2Joint.cpp

include/aikido/statespace/dart/detail/MetaSkeletonStateSpaceSaver-impl.hpp → src/statespace/dart/MetaSkeletonStateSpaceSaver.cpp

@@ -1,16 +1,21 @@
+#include <aikido/statespace/dart/MetaSkeletonStateSpaceSaver.hpp>
+
 namespace aikido {
 namespace statespace {
 namespace dart {
 
+//==============================================================================
 MetaSkeletonStateSpaceSaver::MetaSkeletonStateSpaceSaver(
     MetaSkeletonStateSpacePtr _space)
   : mSpace(std::move(_space))
   , mPositions(mSpace->getMetaSkeleton()->getPositions())
   , mPositionLowerLimits(mSpace->getMetaSkeleton()->getPositionLowerLimits())
   , mPositionUpperLimits(mSpace->getMetaSkeleton()->getPositionUpperLimits())
 {
+  // Do nothing
 }
 
+//==============================================================================
 MetaSkeletonStateSpaceSaver::~MetaSkeletonStateSpaceSaver()
 {
   if (static_cast<std::size_t>(mPositions.size())

tests/statespace/CMakeLists.txt

@@ -21,5 +21,10 @@ aikido_add_test(test_MetaSkeletonStateSpace
 target_link_libraries(test_MetaSkeletonStateSpace
   "${PROJECT_NAME}_statespace")
 
+aikido_add_test(test_MetaSkeletonStateSpaceSaver
+  dart/test_MetaSkeletonStateSpaceSaver.cpp)
+target_link_libraries(test_MetaSkeletonStateSpaceSaver
+  "${PROJECT_NAME}_statespace")
+
 aikido_add_test(test_DartJointStateSpaces dart/test_DartJointStateSpaces.cpp)
 target_link_libraries(test_DartJointStateSpaces "${PROJECT_NAME}_statespace")

tests/statespace/dart/test_MetaSkeletonStateSpaceSaver.cpp

@@ -0,0 +1,37 @@
+#include <dart/dynamics/dynamics.hpp>
+#include <gtest/gtest.h>
+#include <aikido/statespace/SO2.hpp>
+#include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
+#include <aikido/statespace/dart/MetaSkeletonStateSpaceSaver.hpp>
+
+using dart::dynamics::Skeleton;
+using dart::dynamics::RevoluteJoint;
+using aikido::statespace::dart::MetaSkeletonStateSpace;
+using aikido::statespace::dart::MetaSkeletonStateSpaceSaver;
+using aikido::statespace::SO2;
+
+TEST(MetaSkeletonStateSpaceSaver, MetaSkeletonStateSpaceReturnsToOriginal)
+{
+  auto skeleton = Skeleton::create();
+  skeleton->createJointAndBodyNodePair<RevoluteJoint>();
+
+  auto space = std::make_shared<MetaSkeletonStateSpace>(skeleton);
+  ASSERT_EQ(1, space->getNumSubspaces());
+
+  auto state = space->createState();
+  auto substate = state.getSubStateHandle<SO2>(0);
+
+  substate.setAngle(1.);
+  space->setState(state);
+  EXPECT_DOUBLE_EQ(1., skeleton->getPosition(0));
+
+  {
+    auto saver = MetaSkeletonStateSpaceSaver(space);
+    DART_UNUSED(saver);
+
+    substate.setAngle(6.);
+    space->setState(state);
+    EXPECT_DOUBLE_EQ(6., skeleton->getPosition(0));
+  }
+  EXPECT_DOUBLE_EQ(1., skeleton->getPosition(0));
+}