From ce5372cb9a7493b13b9f85da3c65ac6b674c8e46 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Fri, 21 Apr 2017 17:23:37 -0400 Subject: [PATCH 1/4] Add RAII StateSaver class to save position. --- .../statespace/StateSpaceStateSaver.hpp | 40 +++++++++++++++++++ .../detail/StateSpaceStateSaver-impl.hpp | 16 ++++++++ 2 files changed, 56 insertions(+) create mode 100644 include/aikido/statespace/StateSpaceStateSaver.hpp create mode 100644 include/aikido/statespace/detail/StateSpaceStateSaver-impl.hpp diff --git a/include/aikido/statespace/StateSpaceStateSaver.hpp b/include/aikido/statespace/StateSpaceStateSaver.hpp new file mode 100644 index 0000000000..a432fbba3c --- /dev/null +++ b/include/aikido/statespace/StateSpaceStateSaver.hpp @@ -0,0 +1,40 @@ +#ifndef AIKIDO_STATESPACE_STATESPACESTATESAVER_HPP_ +#define AIKIDO_STATESPACE_STATESPACESTATESAVER_HPP_ + +namespace aikido { +namespace statespace { + +/// RAII class to save and restore a MetaSkeletonStateSpace's state. +/// FIXME: currently only saves position. +class StateSpaceStateSaver +{ +public: + using MetaSkeletonStateSpacePtr = dart::MetaSkeletonStateSpacePtr; + + /// Construct a StateSpaceStateSaver and save the current state of the + /// \c MetaSkeletonStateSpace. This state will be restored when + /// StateSpaceStateSaver is destructed. + /// + /// \param _space MetaSkeletonStateSpace to save/restore + explicit StateSpaceStateSaver(MetaSkeletonStateSpacePtr _space); + + virtual ~StateSpaceStateSaver(); + + // StateSpaceStateSaver is uncopyable, must use std::move + StateSpaceStateSaver(const StateSpaceStateSaver&) = delete; + StateSpaceStateSaver& operator =(const StateSpaceStateSaver&) = delete; + + StateSpaceStateSaver(StateSpaceStateSaver&&) = default; + StateSpaceStateSaver& operator =(StateSpaceStateSaver&&) = default; + +private: + MetaSkeletonStateSpacePtr mSpace; + Eigen::VectorXd mPositions; +}; + +} // namespace statespace +} // namespace aikido + +#include "detail/StateSpaceStateSaver-impl.hpp" + +#endif // ifndef AIKIDO_STATESPACE_STATESPACESTATESAVER_HPP_ diff --git a/include/aikido/statespace/detail/StateSpaceStateSaver-impl.hpp b/include/aikido/statespace/detail/StateSpaceStateSaver-impl.hpp new file mode 100644 index 0000000000..fdedb49cb3 --- /dev/null +++ b/include/aikido/statespace/detail/StateSpaceStateSaver-impl.hpp @@ -0,0 +1,16 @@ +namespace aikido { +namespace statespace { + +StateSpaceStateSaver::StateSpaceStateSaver(MetaSkeletonStateSpacePtr _space) +: mSpace(std::move(_space)) +, mPositions(mSpace->getMetaSkeleton()->getPositions()) +{ +} + +StateSpaceStateSaver::~StateSpaceStateSaver() +{ + mSpace->getMetaSkeleton()->setPositions(mPositions); +} + +} // namespace statespace +} // namespace aikido From 20680612877ff5c786bbc41fab639d3368137410 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Fri, 21 Apr 2017 17:39:59 -0400 Subject: [PATCH 2/4] Rename and move class to aikido::statespace::dart. --- .../statespace/StateSpaceStateSaver.hpp | 40 ------------------ .../dart/MetaSkeletonStateSpaceSaver.hpp | 42 +++++++++++++++++++ .../MetaSkeletonStateSpaceSaver-impl.hpp} | 6 ++- 3 files changed, 46 insertions(+), 42 deletions(-) delete mode 100644 include/aikido/statespace/StateSpaceStateSaver.hpp create mode 100644 include/aikido/statespace/dart/MetaSkeletonStateSpaceSaver.hpp rename include/aikido/statespace/{detail/StateSpaceStateSaver-impl.hpp => dart/detail/MetaSkeletonStateSpaceSaver-impl.hpp} (56%) diff --git a/include/aikido/statespace/StateSpaceStateSaver.hpp b/include/aikido/statespace/StateSpaceStateSaver.hpp deleted file mode 100644 index a432fbba3c..0000000000 --- a/include/aikido/statespace/StateSpaceStateSaver.hpp +++ /dev/null @@ -1,40 +0,0 @@ -#ifndef AIKIDO_STATESPACE_STATESPACESTATESAVER_HPP_ -#define AIKIDO_STATESPACE_STATESPACESTATESAVER_HPP_ - -namespace aikido { -namespace statespace { - -/// RAII class to save and restore a MetaSkeletonStateSpace's state. -/// FIXME: currently only saves position. -class StateSpaceStateSaver -{ -public: - using MetaSkeletonStateSpacePtr = dart::MetaSkeletonStateSpacePtr; - - /// Construct a StateSpaceStateSaver and save the current state of the - /// \c MetaSkeletonStateSpace. This state will be restored when - /// StateSpaceStateSaver is destructed. - /// - /// \param _space MetaSkeletonStateSpace to save/restore - explicit StateSpaceStateSaver(MetaSkeletonStateSpacePtr _space); - - virtual ~StateSpaceStateSaver(); - - // StateSpaceStateSaver is uncopyable, must use std::move - StateSpaceStateSaver(const StateSpaceStateSaver&) = delete; - StateSpaceStateSaver& operator =(const StateSpaceStateSaver&) = delete; - - StateSpaceStateSaver(StateSpaceStateSaver&&) = default; - StateSpaceStateSaver& operator =(StateSpaceStateSaver&&) = default; - -private: - MetaSkeletonStateSpacePtr mSpace; - Eigen::VectorXd mPositions; -}; - -} // namespace statespace -} // namespace aikido - -#include "detail/StateSpaceStateSaver-impl.hpp" - -#endif // ifndef AIKIDO_STATESPACE_STATESPACESTATESAVER_HPP_ 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/detail/StateSpaceStateSaver-impl.hpp b/include/aikido/statespace/dart/detail/MetaSkeletonStateSpaceSaver-impl.hpp similarity index 56% rename from include/aikido/statespace/detail/StateSpaceStateSaver-impl.hpp rename to include/aikido/statespace/dart/detail/MetaSkeletonStateSpaceSaver-impl.hpp index fdedb49cb3..047f5b8be5 100644 --- a/include/aikido/statespace/detail/StateSpaceStateSaver-impl.hpp +++ b/include/aikido/statespace/dart/detail/MetaSkeletonStateSpaceSaver-impl.hpp @@ -1,16 +1,18 @@ namespace aikido { namespace statespace { +namespace dart { -StateSpaceStateSaver::StateSpaceStateSaver(MetaSkeletonStateSpacePtr _space) +MetaSkeletonStateSpaceSaver::MetaSkeletonStateSpaceSaver(MetaSkeletonStateSpacePtr _space) : mSpace(std::move(_space)) , mPositions(mSpace->getMetaSkeleton()->getPositions()) { } -StateSpaceStateSaver::~StateSpaceStateSaver() +MetaSkeletonStateSpaceSaver::~MetaSkeletonStateSpaceSaver() { mSpace->getMetaSkeleton()->setPositions(mPositions); } +} // namespace dart } // namespace statespace } // namespace aikido From acae85d97704f798415134fd9a5d5a637b0cbccd Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Sat, 22 Apr 2017 20:28:38 -0400 Subject: [PATCH 3/4] Add some error-checking for StateSaver. --- .../dart/detail/MetaSkeletonStateSpaceSaver-impl.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/include/aikido/statespace/dart/detail/MetaSkeletonStateSpaceSaver-impl.hpp b/include/aikido/statespace/dart/detail/MetaSkeletonStateSpaceSaver-impl.hpp index 047f5b8be5..59798a1fe4 100644 --- a/include/aikido/statespace/dart/detail/MetaSkeletonStateSpaceSaver-impl.hpp +++ b/include/aikido/statespace/dart/detail/MetaSkeletonStateSpaceSaver-impl.hpp @@ -10,6 +10,11 @@ MetaSkeletonStateSpaceSaver::MetaSkeletonStateSpaceSaver(MetaSkeletonStateSpaceP MetaSkeletonStateSpaceSaver::~MetaSkeletonStateSpaceSaver() { + if (mPositions.size() != mSpace->getMetaSkeleton().getNumDofs()) + { + dtwarn << "[MetaSkeletonStateSpaceSaver] The number of DOFs in the " + << "MetaSkeleton does not match the saved state."; + } mSpace->getMetaSkeleton()->setPositions(mPositions); } From 7ec31944cd69e1fccf42ff5385c27de161a8ded4 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Sat, 22 Apr 2017 21:20:38 -0400 Subject: [PATCH 4/4] Fix bugs. --- .../dart/detail/MetaSkeletonStateSpaceSaver-impl.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/aikido/statespace/dart/detail/MetaSkeletonStateSpaceSaver-impl.hpp b/include/aikido/statespace/dart/detail/MetaSkeletonStateSpaceSaver-impl.hpp index 59798a1fe4..b9666c67ec 100644 --- a/include/aikido/statespace/dart/detail/MetaSkeletonStateSpaceSaver-impl.hpp +++ b/include/aikido/statespace/dart/detail/MetaSkeletonStateSpaceSaver-impl.hpp @@ -10,10 +10,10 @@ MetaSkeletonStateSpaceSaver::MetaSkeletonStateSpaceSaver(MetaSkeletonStateSpaceP MetaSkeletonStateSpaceSaver::~MetaSkeletonStateSpaceSaver() { - if (mPositions.size() != mSpace->getMetaSkeleton().getNumDofs()) + if (mPositions.size() != mSpace->getMetaSkeleton()->getNumDofs()) { - dtwarn << "[MetaSkeletonStateSpaceSaver] The number of DOFs in the " - << "MetaSkeleton does not match the saved state."; + std::cerr << "[MetaSkeletonStateSpaceSaver] The number of DOFs in the " + << "MetaSkeleton does not match the saved state."; } mSpace->getMetaSkeleton()->setPositions(mPositions); }