diff --git a/include/aikido/statespace/dart/MetaSkeletonStateSpaceSaver.hpp b/include/aikido/statespace/dart/MetaSkeletonStateSpaceSaver.hpp new file mode 100644 index 0000000000..19013c3435 --- /dev/null +++ b/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_ diff --git a/include/aikido/statespace/dart/detail/MetaSkeletonStateSpaceSaver-impl.hpp b/include/aikido/statespace/dart/detail/MetaSkeletonStateSpaceSaver-impl.hpp new file mode 100644 index 0000000000..b9666c67ec --- /dev/null +++ b/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