Add StateSaver class.

include/aikido/statespace/dart/MetaSkeletonStateSpaceSaver.hpp

@@ -0,0 +1,42 @@
+#ifndef AIKIDO_STATESPACE_DART_METASKELETONSTATESPACESAVER_HPP_
+#define AIKIDO_STATESPACE_DART_METASKELETONSTATESPACESAVER_HPP_
+#include "MetaSkeletonStateSpace.hpp"
+
+namespace aikido {
+namespace statespace {
+namespace dart {
+
+/// RAII class to save and restore a MetaSkeletonStateSpace's state.
+/// FIXME: currently only saves position.
+class MetaSkeletonStateSpaceSaver
+{
+public:
+
+  /// Construct a MetaSkeletonStateSpaceSaver and save the current state of the
+  /// \c MetaSkeletonStateSpace. This state will be restored when
+  /// MetaSkeletonStateSpaceSaver is destructed.
+  ///
+  /// \param _space MetaSkeletonStateSpace to save/restore
+  explicit MetaSkeletonStateSpaceSaver(MetaSkeletonStateSpacePtr _space);
+
+  virtual ~MetaSkeletonStateSpaceSaver();
+
+  // MetaSkeletonStateSpaceSaver is uncopyable, must use std::move
+  MetaSkeletonStateSpaceSaver(const MetaSkeletonStateSpaceSaver&) = delete;
+  MetaSkeletonStateSpaceSaver& operator =(const MetaSkeletonStateSpaceSaver&) = delete;
+
+  MetaSkeletonStateSpaceSaver(MetaSkeletonStateSpaceSaver&&) = default;
+  MetaSkeletonStateSpaceSaver& operator =(MetaSkeletonStateSpaceSaver&&) = default;
+
+private:
+  MetaSkeletonStateSpacePtr mSpace;
+  Eigen::VectorXd mPositions;
+};
+
+} // namespace dart
+} // namespace statespace
+} // namespace aikido
+
+#include "detail/MetaSkeletonStateSpaceSaver-impl.hpp"
+
+#endif // ifndef AIKIDO_STATESPACE_DART_METASKELETONSTATESPACESAVER_HPP_

include/aikido/statespace/dart/detail/MetaSkeletonStateSpaceSaver-impl.hpp

@@ -0,0 +1,23 @@
+namespace aikido {
+namespace statespace {
+namespace dart {
+
+MetaSkeletonStateSpaceSaver::MetaSkeletonStateSpaceSaver(MetaSkeletonStateSpacePtr _space)
+: mSpace(std::move(_space))
+, mPositions(mSpace->getMetaSkeleton()->getPositions())
+{
+}
+
+MetaSkeletonStateSpaceSaver::~MetaSkeletonStateSpaceSaver()
+{
+  if (mPositions.size() != mSpace->getMetaSkeleton()->getNumDofs())
+  {
+    std::cerr << "[MetaSkeletonStateSpaceSaver] The number of DOFs in the "
+              << "MetaSkeleton does not match the saved state.";
+  }
+  mSpace->getMetaSkeleton()->setPositions(mPositions);
+}
+
+} // namespace dart
+} // namespace statespace
+} // namespace aikido