From 0bcee069defa21450e2102064760ee58a8743799 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Sat, 17 Feb 2018 23:35:44 -0800 Subject: [PATCH 1/8] Add flags to select what should be saved by Saver classes. --- include/aikido/planner/WorldStateSaver.hpp | 25 ++++++-- .../dart/MetaSkeletonStateSaver.hpp | 23 ++++++- src/planner/WorldStateSaver.cpp | 11 ++-- .../dart/MetaSkeletonStateSaver.cpp | 62 +++++++++++-------- 4 files changed, 84 insertions(+), 37 deletions(-) diff --git a/include/aikido/planner/WorldStateSaver.hpp b/include/aikido/planner/WorldStateSaver.hpp index 15d7f974590..610a5048492 100644 --- a/include/aikido/planner/WorldStateSaver.hpp +++ b/include/aikido/planner/WorldStateSaver.hpp @@ -1,25 +1,38 @@ #ifndef AIKIDO_PLANNER_WORLDSTATESAVER_HPP_ #define AIKIDO_PLANNER_WORLDSTATESAVER_HPP_ -#include "World.hpp" + +#include "aikido/planner/World.hpp" namespace aikido { namespace planner { +/// Options to specify what WorldStateSaver should save. +enum WorldStateSaverOptions +{ + CONFIGURATIONS = 1 << 0, +}; + /// RAII class to save and restore a World's state. class WorldStateSaver { public: - /// Construct a WorldStateSaver and save the current state of the - /// \c World. This state will be restored when - /// WorldStateSaver is destructed. + /// Construct a WorldStateSaver and save the current state of the \c World. + /// This state will be restored when WorldStateSaver is destructed. /// - /// \param _world World to save state from and restore to. - explicit WorldStateSaver(World* const _world); + /// \param world World to save state from and restore to. + /// \param options Options to specify what should be saved + explicit WorldStateSaver(World* const world, int options = CONFIGURATIONS); virtual ~WorldStateSaver(); private: + /// World to save the state of World* mWorld; + + /// Options to specify what should be saved + int mOptions; + + /// Saved state World::State mWorldState; }; diff --git a/include/aikido/statespace/dart/MetaSkeletonStateSaver.hpp b/include/aikido/statespace/dart/MetaSkeletonStateSaver.hpp index 71589b3c9ec..24b40540ee5 100644 --- a/include/aikido/statespace/dart/MetaSkeletonStateSaver.hpp +++ b/include/aikido/statespace/dart/MetaSkeletonStateSaver.hpp @@ -7,6 +7,13 @@ namespace aikido { namespace statespace { namespace dart { +/// Options to specify what MetaSkeletonStateSaver should save. +enum MetaSkeletonStateSaverOptions +{ + POSITIONS = 1 << 0, + POSITION_LIMITS = 1 << 1, +}; + /// RAII class to save and restore a MetaSkeleton's state. /// FIXME: currently only saves position and joint limits. class MetaSkeletonStateSaver @@ -16,9 +23,11 @@ class MetaSkeletonStateSaver /// MetaSkeleton. This state will be restored when MetaSkeletonStateSaver is /// destructed. /// - /// \param _metaskeleton MetaSkeleton to save/restore + /// \param metaskeleton MetaSkeleton to save/restore + /// \param options Options to specify what should be saved explicit MetaSkeletonStateSaver( - ::dart::dynamics::MetaSkeletonPtr _metaskeleton); + ::dart::dynamics::MetaSkeletonPtr metaskeleton, + int options = POSITIONS | POSITION_LIMITS); virtual ~MetaSkeletonStateSaver(); @@ -30,9 +39,19 @@ class MetaSkeletonStateSaver MetaSkeletonStateSaver& operator=(MetaSkeletonStateSaver&&) = default; private: + /// MetaSkeleton to save the state of ::dart::dynamics::MetaSkeletonPtr mMetaSkeleton; + + /// Options to specify what should be saved + int mOptions; + + /// Saved positions Eigen::VectorXd mPositions; + + /// Saved position lower limits Eigen::VectorXd mPositionLowerLimits; + + /// Saved position upper limits Eigen::VectorXd mPositionUpperLimits; }; diff --git a/src/planner/WorldStateSaver.cpp b/src/planner/WorldStateSaver.cpp index a3128008304..69013b0108c 100644 --- a/src/planner/WorldStateSaver.cpp +++ b/src/planner/WorldStateSaver.cpp @@ -1,19 +1,22 @@ -#include +#include "aikido/planner/WorldStateSaver.hpp" namespace aikido { namespace planner { -WorldStateSaver::WorldStateSaver(World* const world) : mWorld{world} +WorldStateSaver::WorldStateSaver(World* const world, int options) + : mWorld{world}, mOptions{options} { if (!world) throw std::invalid_argument("World must not be nullptr."); - mWorldState = mWorld->getState(); + if (mOptions & WorldStateSaverOptions::CONFIGURATIONS) + mWorldState = mWorld->getState(); } WorldStateSaver::~WorldStateSaver() { - mWorld->setState(mWorldState); + if (mOptions & WorldStateSaverOptions::CONFIGURATIONS) + mWorld->setState(mWorldState); } } // namespace planner diff --git a/src/statespace/dart/MetaSkeletonStateSaver.cpp b/src/statespace/dart/MetaSkeletonStateSaver.cpp index 97b8cd11955..57aa666d27c 100644 --- a/src/statespace/dart/MetaSkeletonStateSaver.cpp +++ b/src/statespace/dart/MetaSkeletonStateSaver.cpp @@ -1,5 +1,5 @@ +#include "aikido/statespace/dart/MetaSkeletonStateSaver.hpp" #include -#include namespace aikido { namespace statespace { @@ -7,40 +7,52 @@ namespace dart { //============================================================================== MetaSkeletonStateSaver::MetaSkeletonStateSaver( - ::dart::dynamics::MetaSkeletonPtr _metaskeleton) - : mMetaSkeleton(std::move(_metaskeleton)) - , mPositions(mMetaSkeleton->getPositions()) - , mPositionLowerLimits(mMetaSkeleton->getPositionLowerLimits()) - , mPositionUpperLimits(mMetaSkeleton->getPositionUpperLimits()) + ::dart::dynamics::MetaSkeletonPtr metaskeleton, int options) + : mMetaSkeleton(std::move(metaskeleton)), mOptions(std::move(options)) { - // Do nothing + if (mOptions & MetaSkeletonStateSaverOptions::POSITIONS) + mPositions = mMetaSkeleton->getPositions(); + + if (mOptions & MetaSkeletonStateSaverOptions::POSITION_LIMITS) + { + mPositionLowerLimits = mMetaSkeleton->getPositionLowerLimits(); + mPositionUpperLimits = mMetaSkeleton->getPositionUpperLimits(); + } } //============================================================================== MetaSkeletonStateSaver::~MetaSkeletonStateSaver() { - if (static_cast(mPositions.size()) - != mMetaSkeleton->getNumDofs()) - { - std::cerr << "[MetaSkeletonStateSaver] The number of DOFs in the " - << "MetaSkeleton does not match the saved position."; - } - if (static_cast(mPositionLowerLimits.size()) - != mMetaSkeleton->getNumDofs()) + if (mOptions & MetaSkeletonStateSaverOptions::POSITIONS) { - std::cerr << "[MetaSkeletonStateSaver] The number of DOFs in the " - << "MetaSkeleton does not match the saved joint lower limits."; + if (static_cast(mPositions.size()) + != mMetaSkeleton->getNumDofs()) + { + std::cerr << "[MetaSkeletonStateSaver] The number of DOFs in the " + << "MetaSkeleton does not match the saved position."; + } + + mMetaSkeleton->setPositions(mPositions); } - if (static_cast(mPositionUpperLimits.size()) - != mMetaSkeleton->getNumDofs()) + + if (mOptions & MetaSkeletonStateSaverOptions::POSITION_LIMITS) { - std::cerr << "[MetaSkeletonStateSaver] The number of DOFs in the " - << "MetaSkeleton does not match the saved joint upper limits."; - } + if (static_cast(mPositionLowerLimits.size()) + != mMetaSkeleton->getNumDofs()) + { + std::cerr << "[MetaSkeletonStateSaver] The number of DOFs in the " + << "MetaSkeleton does not match the saved joint lower limits."; + } + if (static_cast(mPositionUpperLimits.size()) + != mMetaSkeleton->getNumDofs()) + { + std::cerr << "[MetaSkeletonStateSaver] The number of DOFs in the " + << "MetaSkeleton does not match the saved joint upper limits."; + } - mMetaSkeleton->setPositions(mPositions); - mMetaSkeleton->setPositionLowerLimits(mPositionLowerLimits); - mMetaSkeleton->setPositionUpperLimits(mPositionUpperLimits); + mMetaSkeleton->setPositionLowerLimits(mPositionLowerLimits); + mMetaSkeleton->setPositionUpperLimits(mPositionUpperLimits); + } } } // namespace dart From 1620118c59460efb3528c0ee2fe1cacfe3a585f3 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Sun, 18 Feb 2018 00:00:29 -0800 Subject: [PATCH 2/8] Add more tests. --- .../dart/test_MetaSkeletonStateSaver.cpp | 124 +++++++++++++++--- 1 file changed, 105 insertions(+), 19 deletions(-) diff --git a/tests/statespace/dart/test_MetaSkeletonStateSaver.cpp b/tests/statespace/dart/test_MetaSkeletonStateSaver.cpp index cfdb9b6a8af..c5950009958 100644 --- a/tests/statespace/dart/test_MetaSkeletonStateSaver.cpp +++ b/tests/statespace/dart/test_MetaSkeletonStateSaver.cpp @@ -1,37 +1,123 @@ #include #include -#include #include -#include using dart::dynamics::Skeleton; using dart::dynamics::RevoluteJoint; -using aikido::statespace::dart::MetaSkeletonStateSpace; using aikido::statespace::dart::MetaSkeletonStateSaver; -using aikido::statespace::SO2; +using aikido::statespace::dart::MetaSkeletonStateSaverOptions; -TEST(MetaSkeletonStateSaver, MetaSkeletonStateSpaceReturnsToOriginal) +class MetaSkeletonStateSaverTest : public testing::Test { - auto skeleton = Skeleton::create(); - skeleton->createJointAndBodyNodePair(); +public: + void SetUp() + { + mSkeleton = Skeleton::create(); + mSkeleton->createJointAndBodyNodePair(); + + mSkeleton->setPosition(0, 1.); + mSkeleton->setPositionLowerLimit(0, 0.); + mSkeleton->setPositionUpperLimit(0, 3.); + } + +protected: + ::dart::dynamics::SkeletonPtr mSkeleton; +}; + +TEST_F(MetaSkeletonStateSaverTest, MetaSkeletonStateSpaceReturnsToOriginal) +{ + EXPECT_DOUBLE_EQ(0., mSkeleton->getPositionLowerLimit(0)); + EXPECT_DOUBLE_EQ(1., mSkeleton->getPosition(0)); + EXPECT_DOUBLE_EQ(3., mSkeleton->getPositionUpperLimit(0)); + + { + auto saver = MetaSkeletonStateSaver(mSkeleton); + DART_UNUSED(saver); - auto space = std::make_shared(skeleton.get()); - ASSERT_EQ(1, space->getNumSubspaces()); + mSkeleton->setPositionLowerLimit(0, 1.); + mSkeleton->setPositionUpperLimit(0, 5.); + mSkeleton->setPosition(0, 4.); - auto state = space->createState(); - auto substate = state.getSubStateHandle(0); + EXPECT_DOUBLE_EQ(1., mSkeleton->getPositionLowerLimit(0)); + EXPECT_DOUBLE_EQ(4., mSkeleton->getPosition(0)); + EXPECT_DOUBLE_EQ(5., mSkeleton->getPositionUpperLimit(0)); + } - substate.setAngle(1.); - space->setState(skeleton.get(), state); - EXPECT_DOUBLE_EQ(1., skeleton->getPosition(0)); + EXPECT_DOUBLE_EQ(0., mSkeleton->getPositionLowerLimit(0)); + EXPECT_DOUBLE_EQ(1., mSkeleton->getPosition(0)); + EXPECT_DOUBLE_EQ(3., mSkeleton->getPositionUpperLimit(0)); +} + +TEST_F(MetaSkeletonStateSaverTest, Flags_None) +{ + EXPECT_DOUBLE_EQ(0., mSkeleton->getPositionLowerLimit(0)); + EXPECT_DOUBLE_EQ(1., mSkeleton->getPosition(0)); + EXPECT_DOUBLE_EQ(3., mSkeleton->getPositionUpperLimit(0)); { - auto saver = MetaSkeletonStateSaver(skeleton); + auto saver = MetaSkeletonStateSaver(mSkeleton, 0); DART_UNUSED(saver); - substate.setAngle(6.); - space->setState(skeleton.get(), state); - EXPECT_DOUBLE_EQ(6., skeleton->getPosition(0)); + mSkeleton->setPositionLowerLimit(0, 1.); + mSkeleton->setPositionUpperLimit(0, 5.); + mSkeleton->setPosition(0, 4.); + + EXPECT_DOUBLE_EQ(1., mSkeleton->getPositionLowerLimit(0)); + EXPECT_DOUBLE_EQ(4., mSkeleton->getPosition(0)); + EXPECT_DOUBLE_EQ(5., mSkeleton->getPositionUpperLimit(0)); } - EXPECT_DOUBLE_EQ(1., skeleton->getPosition(0)); + + EXPECT_DOUBLE_EQ(1., mSkeleton->getPositionLowerLimit(0)); + EXPECT_DOUBLE_EQ(4., mSkeleton->getPosition(0)); + EXPECT_DOUBLE_EQ(5., mSkeleton->getPositionUpperLimit(0)); +} + +TEST_F(MetaSkeletonStateSaverTest, Flags_PositionsOnly) +{ + EXPECT_DOUBLE_EQ(0., mSkeleton->getPositionLowerLimit(0)); + EXPECT_DOUBLE_EQ(1., mSkeleton->getPosition(0)); + EXPECT_DOUBLE_EQ(3., mSkeleton->getPositionUpperLimit(0)); + + { + auto saver = MetaSkeletonStateSaver( + mSkeleton, MetaSkeletonStateSaverOptions::POSITIONS); + DART_UNUSED(saver); + + mSkeleton->setPositionLowerLimit(0, 1.); + mSkeleton->setPositionUpperLimit(0, 5.); + mSkeleton->setPosition(0, 4.); + + EXPECT_DOUBLE_EQ(1., mSkeleton->getPositionLowerLimit(0)); + EXPECT_DOUBLE_EQ(4., mSkeleton->getPosition(0)); + EXPECT_DOUBLE_EQ(5., mSkeleton->getPositionUpperLimit(0)); + } + + EXPECT_DOUBLE_EQ(1., mSkeleton->getPositionLowerLimit(0)); + EXPECT_DOUBLE_EQ(1., mSkeleton->getPosition(0)); + EXPECT_DOUBLE_EQ(5., mSkeleton->getPositionUpperLimit(0)); +} + +TEST_F(MetaSkeletonStateSaverTest, Flags_PositionLimitsOnly) +{ + EXPECT_DOUBLE_EQ(0., mSkeleton->getPositionLowerLimit(0)); + EXPECT_DOUBLE_EQ(1., mSkeleton->getPosition(0)); + EXPECT_DOUBLE_EQ(3., mSkeleton->getPositionUpperLimit(0)); + + { + auto saver = MetaSkeletonStateSaver( + mSkeleton, MetaSkeletonStateSaverOptions::POSITION_LIMITS); + DART_UNUSED(saver); + + mSkeleton->setPositionLowerLimit(0, 1.); + mSkeleton->setPositionUpperLimit(0, 5.); + mSkeleton->setPosition(0, 2.); + + EXPECT_DOUBLE_EQ(1., mSkeleton->getPositionLowerLimit(0)); + EXPECT_DOUBLE_EQ(2., mSkeleton->getPosition(0)); + EXPECT_DOUBLE_EQ(5., mSkeleton->getPositionUpperLimit(0)); + } + + EXPECT_DOUBLE_EQ(0., mSkeleton->getPositionLowerLimit(0)); + EXPECT_DOUBLE_EQ(2., mSkeleton->getPosition(0)); + EXPECT_DOUBLE_EQ(3., mSkeleton->getPositionUpperLimit(0)); } From a47a6935f85cb5e1f07e99a127e4bb58f688e680 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Sun, 18 Feb 2018 00:15:15 -0800 Subject: [PATCH 3/8] Restructure SaverOptions. --- include/aikido/planner/WorldStateSaver.hpp | 12 ++++++------ .../statespace/dart/MetaSkeletonStateSaver.hpp | 14 +++++++------- src/planner/WorldStateSaver.cpp | 4 ++-- src/planner/vectorfield/VectorFieldPlanner.cpp | 6 ++++-- src/rviz/TrajectoryMarker.cpp | 4 +++- src/statespace/dart/MetaSkeletonStateSaver.cpp | 8 ++++---- .../dart/test_MetaSkeletonStateSaver.cpp | 5 ++--- 7 files changed, 28 insertions(+), 25 deletions(-) diff --git a/include/aikido/planner/WorldStateSaver.hpp b/include/aikido/planner/WorldStateSaver.hpp index 610a5048492..78b88aa3a24 100644 --- a/include/aikido/planner/WorldStateSaver.hpp +++ b/include/aikido/planner/WorldStateSaver.hpp @@ -6,16 +6,16 @@ namespace aikido { namespace planner { -/// Options to specify what WorldStateSaver should save. -enum WorldStateSaverOptions -{ - CONFIGURATIONS = 1 << 0, -}; - /// RAII class to save and restore a World's state. class WorldStateSaver { public: + /// Options to specify what WorldStateSaver should save. + enum Options + { + CONFIGURATIONS = 1 << 0, + }; + /// Construct a WorldStateSaver and save the current state of the \c World. /// This state will be restored when WorldStateSaver is destructed. /// diff --git a/include/aikido/statespace/dart/MetaSkeletonStateSaver.hpp b/include/aikido/statespace/dart/MetaSkeletonStateSaver.hpp index 24b40540ee5..298f7b4ef42 100644 --- a/include/aikido/statespace/dart/MetaSkeletonStateSaver.hpp +++ b/include/aikido/statespace/dart/MetaSkeletonStateSaver.hpp @@ -7,18 +7,18 @@ namespace aikido { namespace statespace { namespace dart { -/// Options to specify what MetaSkeletonStateSaver should save. -enum MetaSkeletonStateSaverOptions -{ - POSITIONS = 1 << 0, - POSITION_LIMITS = 1 << 1, -}; - /// RAII class to save and restore a MetaSkeleton's state. /// FIXME: currently only saves position and joint limits. class MetaSkeletonStateSaver { public: + /// Options to specify what MetaSkeletonStateSaver should save. + enum Options + { + POSITIONS = 1 << 0, + POSITION_LIMITS = 1 << 1, + }; + /// Construct a MetaSkeletonStateSaver and save the current state of the \c /// MetaSkeleton. This state will be restored when MetaSkeletonStateSaver is /// destructed. diff --git a/src/planner/WorldStateSaver.cpp b/src/planner/WorldStateSaver.cpp index 69013b0108c..e519018ac4f 100644 --- a/src/planner/WorldStateSaver.cpp +++ b/src/planner/WorldStateSaver.cpp @@ -9,13 +9,13 @@ WorldStateSaver::WorldStateSaver(World* const world, int options) if (!world) throw std::invalid_argument("World must not be nullptr."); - if (mOptions & WorldStateSaverOptions::CONFIGURATIONS) + if (mOptions & Options::CONFIGURATIONS) mWorldState = mWorld->getState(); } WorldStateSaver::~WorldStateSaver() { - if (mOptions & WorldStateSaverOptions::CONFIGURATIONS) + if (mOptions & Options::CONFIGURATIONS) mWorld->setState(mWorldState); } diff --git a/src/planner/vectorfield/VectorFieldPlanner.cpp b/src/planner/vectorfield/VectorFieldPlanner.cpp index ec3cec30588..d2dfe7b8deb 100644 --- a/src/planner/vectorfield/VectorFieldPlanner.cpp +++ b/src/planner/vectorfield/VectorFieldPlanner.cpp @@ -161,7 +161,8 @@ std::unique_ptr planToEndEffectorOffset( // TODO: Check compatibility between MetaSkeleton and MetaSkeletonStateSpace // Save the current state of the space - auto saver = MetaSkeletonStateSaver(metaskeleton); + auto saver = MetaSkeletonStateSaver( + metaskeleton, MetaSkeletonStateSaver::Options::POSITIONS); DART_UNUSED(saver); auto vectorfield = std::make_shared( @@ -212,7 +213,8 @@ std::unique_ptr planToEndEffectorPose( // TODO: Check compatibility between MetaSkeleton and MetaSkeletonStateSpace // Save the current state of the space - auto saver = MetaSkeletonStateSaver(metaskeleton); + auto saver = MetaSkeletonStateSaver( + metaskeleton, MetaSkeletonStateSaver::Options::POSITIONS); DART_UNUSED(saver); auto vectorfield = std::make_shared( diff --git a/src/rviz/TrajectoryMarker.cpp b/src/rviz/TrajectoryMarker.cpp index 24158fb1a99..61d2b49ca37 100644 --- a/src/rviz/TrajectoryMarker.cpp +++ b/src/rviz/TrajectoryMarker.cpp @@ -198,6 +198,7 @@ void TrajectoryMarker::update() void TrajectoryMarker::updatePoints() { using visualization_msgs::Marker; + using aikido::statespace::dart::MetaSkeletonStateSaver; if (!mNeedPointsUpdate) return; @@ -218,7 +219,8 @@ void TrajectoryMarker::updatePoints() = std::dynamic_pointer_cast( statespace); - auto saver = statespace::dart::MetaSkeletonStateSaver(mSkeleton); + auto saver = MetaSkeletonStateSaver( + mSkeleton, MetaSkeletonStateSaver::Options::POSITIONS); DART_UNUSED(saver); auto state = metaSkeletonSs->createState(); diff --git a/src/statespace/dart/MetaSkeletonStateSaver.cpp b/src/statespace/dart/MetaSkeletonStateSaver.cpp index 57aa666d27c..1546f63ad6a 100644 --- a/src/statespace/dart/MetaSkeletonStateSaver.cpp +++ b/src/statespace/dart/MetaSkeletonStateSaver.cpp @@ -10,10 +10,10 @@ MetaSkeletonStateSaver::MetaSkeletonStateSaver( ::dart::dynamics::MetaSkeletonPtr metaskeleton, int options) : mMetaSkeleton(std::move(metaskeleton)), mOptions(std::move(options)) { - if (mOptions & MetaSkeletonStateSaverOptions::POSITIONS) + if (mOptions & Options::POSITIONS) mPositions = mMetaSkeleton->getPositions(); - if (mOptions & MetaSkeletonStateSaverOptions::POSITION_LIMITS) + if (mOptions & Options::POSITION_LIMITS) { mPositionLowerLimits = mMetaSkeleton->getPositionLowerLimits(); mPositionUpperLimits = mMetaSkeleton->getPositionUpperLimits(); @@ -23,7 +23,7 @@ MetaSkeletonStateSaver::MetaSkeletonStateSaver( //============================================================================== MetaSkeletonStateSaver::~MetaSkeletonStateSaver() { - if (mOptions & MetaSkeletonStateSaverOptions::POSITIONS) + if (mOptions & Options::POSITIONS) { if (static_cast(mPositions.size()) != mMetaSkeleton->getNumDofs()) @@ -35,7 +35,7 @@ MetaSkeletonStateSaver::~MetaSkeletonStateSaver() mMetaSkeleton->setPositions(mPositions); } - if (mOptions & MetaSkeletonStateSaverOptions::POSITION_LIMITS) + if (mOptions & Options::POSITION_LIMITS) { if (static_cast(mPositionLowerLimits.size()) != mMetaSkeleton->getNumDofs()) diff --git a/tests/statespace/dart/test_MetaSkeletonStateSaver.cpp b/tests/statespace/dart/test_MetaSkeletonStateSaver.cpp index c5950009958..56b4d085346 100644 --- a/tests/statespace/dart/test_MetaSkeletonStateSaver.cpp +++ b/tests/statespace/dart/test_MetaSkeletonStateSaver.cpp @@ -5,7 +5,6 @@ using dart::dynamics::Skeleton; using dart::dynamics::RevoluteJoint; using aikido::statespace::dart::MetaSkeletonStateSaver; -using aikido::statespace::dart::MetaSkeletonStateSaverOptions; class MetaSkeletonStateSaverTest : public testing::Test { @@ -80,7 +79,7 @@ TEST_F(MetaSkeletonStateSaverTest, Flags_PositionsOnly) { auto saver = MetaSkeletonStateSaver( - mSkeleton, MetaSkeletonStateSaverOptions::POSITIONS); + mSkeleton, MetaSkeletonStateSaver::Options::POSITIONS); DART_UNUSED(saver); mSkeleton->setPositionLowerLimit(0, 1.); @@ -105,7 +104,7 @@ TEST_F(MetaSkeletonStateSaverTest, Flags_PositionLimitsOnly) { auto saver = MetaSkeletonStateSaver( - mSkeleton, MetaSkeletonStateSaverOptions::POSITION_LIMITS); + mSkeleton, MetaSkeletonStateSaver::Options::POSITION_LIMITS); DART_UNUSED(saver); mSkeleton->setPositionLowerLimit(0, 1.); From 0ab8008d9a0d313c0cf623fdad6c087c64bb0261 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Sun, 18 Feb 2018 23:17:33 -0800 Subject: [PATCH 4/8] Address @jslee02's comments. --- include/aikido/planner/WorldStateSaver.hpp | 6 +++--- include/aikido/statespace/dart/MetaSkeletonStateSaver.hpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/include/aikido/planner/WorldStateSaver.hpp b/include/aikido/planner/WorldStateSaver.hpp index 78b88aa3a24..4d85a472113 100644 --- a/include/aikido/planner/WorldStateSaver.hpp +++ b/include/aikido/planner/WorldStateSaver.hpp @@ -19,9 +19,9 @@ class WorldStateSaver /// Construct a WorldStateSaver and save the current state of the \c World. /// This state will be restored when WorldStateSaver is destructed. /// - /// \param world World to save state from and restore to. - /// \param options Options to specify what should be saved - explicit WorldStateSaver(World* const world, int options = CONFIGURATIONS); + /// \param[in] world World to save state from and restore to. + /// \param[in] options Options to specify what should be saved + explicit WorldStateSaver(const World* world, int options = CONFIGURATIONS); virtual ~WorldStateSaver(); diff --git a/include/aikido/statespace/dart/MetaSkeletonStateSaver.hpp b/include/aikido/statespace/dart/MetaSkeletonStateSaver.hpp index 298f7b4ef42..c9002286bea 100644 --- a/include/aikido/statespace/dart/MetaSkeletonStateSaver.hpp +++ b/include/aikido/statespace/dart/MetaSkeletonStateSaver.hpp @@ -23,8 +23,8 @@ class MetaSkeletonStateSaver /// MetaSkeleton. This state will be restored when MetaSkeletonStateSaver is /// destructed. /// - /// \param metaskeleton MetaSkeleton to save/restore - /// \param options Options to specify what should be saved + /// \param[in] metaskeleton MetaSkeleton to save/restore + /// \param[in] options Options to specify what should be saved explicit MetaSkeletonStateSaver( ::dart::dynamics::MetaSkeletonPtr metaskeleton, int options = POSITIONS | POSITION_LIMITS); From f08fa35559e2e489bb0522d71824cb4bdc4ac02a Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Mon, 19 Feb 2018 00:50:41 -0800 Subject: [PATCH 5/8] Remove incorrect const specifier. --- include/aikido/planner/WorldStateSaver.hpp | 2 +- src/planner/WorldStateSaver.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/aikido/planner/WorldStateSaver.hpp b/include/aikido/planner/WorldStateSaver.hpp index 4d85a472113..9faa15ad5b4 100644 --- a/include/aikido/planner/WorldStateSaver.hpp +++ b/include/aikido/planner/WorldStateSaver.hpp @@ -21,7 +21,7 @@ class WorldStateSaver /// /// \param[in] world World to save state from and restore to. /// \param[in] options Options to specify what should be saved - explicit WorldStateSaver(const World* world, int options = CONFIGURATIONS); + explicit WorldStateSaver(World* world, int options = CONFIGURATIONS); virtual ~WorldStateSaver(); diff --git a/src/planner/WorldStateSaver.cpp b/src/planner/WorldStateSaver.cpp index e519018ac4f..73ee9f6d48b 100644 --- a/src/planner/WorldStateSaver.cpp +++ b/src/planner/WorldStateSaver.cpp @@ -3,10 +3,10 @@ namespace aikido { namespace planner { -WorldStateSaver::WorldStateSaver(World* const world, int options) - : mWorld{world}, mOptions{options} +WorldStateSaver::WorldStateSaver(World* world, int options) + : mWorld{std::move(world)}, mOptions{options} { - if (!world) + if (!mWorld) throw std::invalid_argument("World must not be nullptr."); if (mOptions & Options::CONFIGURATIONS) From d7e1b3e3c354e4f6eff14f82468a4707b5dd12d1 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Mon, 19 Feb 2018 14:03:42 -0800 Subject: [PATCH 6/8] Update CHANGELOG.md. --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 070dde165ea..0501efea1a3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,6 +11,7 @@ * Refactored JointStateSpace and MetaSkeletonStateSpace: [#278](https://github.com/personalrobotics/aikido/pull/278) * Added methods for checking compatibility between DART objects and state spaces: [#315](https://github.com/personalrobotics/aikido/pull/315) + * Added flags to MetaSkeletonStateSaver to specify what to save: [#339](https://github.com/personalrobotics/aikido/pull/339) * Control @@ -21,6 +22,7 @@ * Added parabolic timing for linear spline [#302](https://github.com/personalrobotics/aikido/pull/302), [#324](https://github.com/personalrobotics/aikido/pull/324) * Fixed step sequence iteration in VPF: [#303](https://github.com/personalrobotics/aikido/pull/303) + * Added flags to WorldStateSaver to specify what to save: [#339](https://github.com/personalrobotics/aikido/pull/339) ### 0.2.0 (2018-01-09) From 7d4ff7f1a05e5996f98f68ed3fbdf19d073e3322 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Mon, 19 Feb 2018 15:36:26 -0800 Subject: [PATCH 7/8] Use strongly-typed enum flags. --- include/aikido/common.hpp | 1 + include/aikido/common/EnumFlags.hpp | 44 +++++++++++++++++++ include/aikido/planner/WorldStateSaver.hpp | 21 ++++++--- .../dart/MetaSkeletonStateSaver.hpp | 22 ++++++---- src/planner/WorldStateSaver.cpp | 6 +-- .../dart/MetaSkeletonStateSaver.cpp | 12 ++--- .../dart/test_MetaSkeletonStateSaver.cpp | 3 +- 7 files changed, 85 insertions(+), 24 deletions(-) create mode 100644 include/aikido/common/EnumFlags.hpp diff --git a/include/aikido/common.hpp b/include/aikido/common.hpp index 182178d694b..6d509d95900 100644 --- a/include/aikido/common.hpp +++ b/include/aikido/common.hpp @@ -1,3 +1,4 @@ +#include "common/EnumFlags.hpp" #include "common/ExecutorMultiplexer.hpp" #include "common/ExecutorThread.hpp" #include "common/PseudoInverse.hpp" diff --git a/include/aikido/common/EnumFlags.hpp b/include/aikido/common/EnumFlags.hpp new file mode 100644 index 00000000000..0591835698e --- /dev/null +++ b/include/aikido/common/EnumFlags.hpp @@ -0,0 +1,44 @@ +#ifndef AIKIDO_COMMON_ENUMFLAGS_HPP_ +#define AIKIDO_COMMON_ENUMFLAGS_HPP_ + +#include + +/// Enable bitwise operators for strongly-typed enums. +/// +/// Adapted from +/// http://blog.bitwigglers.org/using-enum-classes-as-type-safe-bitmasks/ + +// clang-format off + +#define AIKIDO_ENABLE_BITWISE_OPERATORS(X) \ +template<> \ +struct EnableBitwiseOperators< X > \ +{ \ + static const bool enable = true; \ +}; + +template +struct EnableBitwiseOperators +{ + static const bool enable = false; +}; + +template +typename std::enable_if::enable, Enum>::type +operator&(Enum lhs, Enum rhs) +{ + using T = typename std::underlying_type::type; + return static_cast(static_cast(lhs) & static_cast(rhs)); +} + +template +typename std::enable_if::enable, Enum>::type +operator|(Enum lhs, Enum rhs) +{ + using T = typename std::underlying_type::type; + return static_cast(static_cast(lhs) | static_cast(rhs)); +} + +// clang-format on + +#endif // AIKIDO_COMMON_ENUMFLAGS_HPP_ diff --git a/include/aikido/planner/WorldStateSaver.hpp b/include/aikido/planner/WorldStateSaver.hpp index 9faa15ad5b4..3353f8c71f7 100644 --- a/include/aikido/planner/WorldStateSaver.hpp +++ b/include/aikido/planner/WorldStateSaver.hpp @@ -1,8 +1,18 @@ #ifndef AIKIDO_PLANNER_WORLDSTATESAVER_HPP_ #define AIKIDO_PLANNER_WORLDSTATESAVER_HPP_ +#include "aikido/common/EnumFlags.hpp" #include "aikido/planner/World.hpp" +/// Options to specify what WorldStateSaver should save. +enum class WorldStateSaverOptions +{ + NONE = 0, + CONFIGURATIONS = 1 << 0, +}; + +AIKIDO_ENABLE_BITWISE_OPERATORS(WorldStateSaverOptions) + namespace aikido { namespace planner { @@ -10,18 +20,15 @@ namespace planner { class WorldStateSaver { public: - /// Options to specify what WorldStateSaver should save. - enum Options - { - CONFIGURATIONS = 1 << 0, - }; + using Options = WorldStateSaverOptions; /// Construct a WorldStateSaver and save the current state of the \c World. /// This state will be restored when WorldStateSaver is destructed. /// /// \param[in] world World to save state from and restore to. /// \param[in] options Options to specify what should be saved - explicit WorldStateSaver(World* world, int options = CONFIGURATIONS); + explicit WorldStateSaver( + World* world, Options options = Options::CONFIGURATIONS); virtual ~WorldStateSaver(); @@ -30,7 +37,7 @@ class WorldStateSaver World* mWorld; /// Options to specify what should be saved - int mOptions; + Options mOptions; /// Saved state World::State mWorldState; diff --git a/include/aikido/statespace/dart/MetaSkeletonStateSaver.hpp b/include/aikido/statespace/dart/MetaSkeletonStateSaver.hpp index c9002286bea..133532a0d95 100644 --- a/include/aikido/statespace/dart/MetaSkeletonStateSaver.hpp +++ b/include/aikido/statespace/dart/MetaSkeletonStateSaver.hpp @@ -2,6 +2,17 @@ #define AIKIDO_STATESPACE_DART_METASKELETONSTATESAVER_HPP_ #include +#include "aikido/common/EnumFlags.hpp" + +/// Options to specify what MetaSkeletonStateSaver should save. +enum class MetaSkeletonStateSaverOptions +{ + NONE = 0, + POSITIONS = 1 << 0, + POSITION_LIMITS = 1 << 1, +}; + +AIKIDO_ENABLE_BITWISE_OPERATORS(MetaSkeletonStateSaverOptions) namespace aikido { namespace statespace { @@ -12,12 +23,7 @@ namespace dart { class MetaSkeletonStateSaver { public: - /// Options to specify what MetaSkeletonStateSaver should save. - enum Options - { - POSITIONS = 1 << 0, - POSITION_LIMITS = 1 << 1, - }; + using Options = MetaSkeletonStateSaverOptions; /// Construct a MetaSkeletonStateSaver and save the current state of the \c /// MetaSkeleton. This state will be restored when MetaSkeletonStateSaver is @@ -27,7 +33,7 @@ class MetaSkeletonStateSaver /// \param[in] options Options to specify what should be saved explicit MetaSkeletonStateSaver( ::dart::dynamics::MetaSkeletonPtr metaskeleton, - int options = POSITIONS | POSITION_LIMITS); + Options options = Options::POSITIONS | Options::POSITION_LIMITS); virtual ~MetaSkeletonStateSaver(); @@ -43,7 +49,7 @@ class MetaSkeletonStateSaver ::dart::dynamics::MetaSkeletonPtr mMetaSkeleton; /// Options to specify what should be saved - int mOptions; + Options mOptions; /// Saved positions Eigen::VectorXd mPositions; diff --git a/src/planner/WorldStateSaver.cpp b/src/planner/WorldStateSaver.cpp index 73ee9f6d48b..c44cad135c5 100644 --- a/src/planner/WorldStateSaver.cpp +++ b/src/planner/WorldStateSaver.cpp @@ -3,19 +3,19 @@ namespace aikido { namespace planner { -WorldStateSaver::WorldStateSaver(World* world, int options) +WorldStateSaver::WorldStateSaver(World* world, Options options) : mWorld{std::move(world)}, mOptions{options} { if (!mWorld) throw std::invalid_argument("World must not be nullptr."); - if (mOptions & Options::CONFIGURATIONS) + if ((mOptions & Options::CONFIGURATIONS) != Options::NONE) mWorldState = mWorld->getState(); } WorldStateSaver::~WorldStateSaver() { - if (mOptions & Options::CONFIGURATIONS) + if ((mOptions & Options::CONFIGURATIONS) != Options::NONE) mWorld->setState(mWorldState); } diff --git a/src/statespace/dart/MetaSkeletonStateSaver.cpp b/src/statespace/dart/MetaSkeletonStateSaver.cpp index 1546f63ad6a..beca0d7fa6b 100644 --- a/src/statespace/dart/MetaSkeletonStateSaver.cpp +++ b/src/statespace/dart/MetaSkeletonStateSaver.cpp @@ -7,13 +7,15 @@ namespace dart { //============================================================================== MetaSkeletonStateSaver::MetaSkeletonStateSaver( - ::dart::dynamics::MetaSkeletonPtr metaskeleton, int options) + ::dart::dynamics::MetaSkeletonPtr metaskeleton, Options options) : mMetaSkeleton(std::move(metaskeleton)), mOptions(std::move(options)) { - if (mOptions & Options::POSITIONS) + if ((mOptions & Options::POSITIONS) != Options::NONE) + { mPositions = mMetaSkeleton->getPositions(); + } - if (mOptions & Options::POSITION_LIMITS) + if ((mOptions & Options::POSITION_LIMITS) != Options::NONE) { mPositionLowerLimits = mMetaSkeleton->getPositionLowerLimits(); mPositionUpperLimits = mMetaSkeleton->getPositionUpperLimits(); @@ -23,7 +25,7 @@ MetaSkeletonStateSaver::MetaSkeletonStateSaver( //============================================================================== MetaSkeletonStateSaver::~MetaSkeletonStateSaver() { - if (mOptions & Options::POSITIONS) + if ((mOptions & Options::POSITIONS) != Options::NONE) { if (static_cast(mPositions.size()) != mMetaSkeleton->getNumDofs()) @@ -35,7 +37,7 @@ MetaSkeletonStateSaver::~MetaSkeletonStateSaver() mMetaSkeleton->setPositions(mPositions); } - if (mOptions & Options::POSITION_LIMITS) + if ((mOptions & Options::POSITION_LIMITS) != Options::NONE) { if (static_cast(mPositionLowerLimits.size()) != mMetaSkeleton->getNumDofs()) diff --git a/tests/statespace/dart/test_MetaSkeletonStateSaver.cpp b/tests/statespace/dart/test_MetaSkeletonStateSaver.cpp index 56b4d085346..0f88e6d92bd 100644 --- a/tests/statespace/dart/test_MetaSkeletonStateSaver.cpp +++ b/tests/statespace/dart/test_MetaSkeletonStateSaver.cpp @@ -54,7 +54,8 @@ TEST_F(MetaSkeletonStateSaverTest, Flags_None) EXPECT_DOUBLE_EQ(3., mSkeleton->getPositionUpperLimit(0)); { - auto saver = MetaSkeletonStateSaver(mSkeleton, 0); + auto saver = MetaSkeletonStateSaver( + mSkeleton, MetaSkeletonStateSaver::Options::NONE); DART_UNUSED(saver); mSkeleton->setPositionLowerLimit(0, 1.); From 2c681adad54306ecf7deacb15aecf482f243c859 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Sat, 2 Feb 2019 22:53:10 -0800 Subject: [PATCH 8/8] Address review comments --- include/aikido/common/EnumFlags.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/aikido/common/EnumFlags.hpp b/include/aikido/common/EnumFlags.hpp index 0591835698e..e38bca214ca 100644 --- a/include/aikido/common/EnumFlags.hpp +++ b/include/aikido/common/EnumFlags.hpp @@ -4,14 +4,14 @@ #include /// Enable bitwise operators for strongly-typed enums. -/// -/// Adapted from -/// http://blog.bitwigglers.org/using-enum-classes-as-type-safe-bitmasks/ + +// Adapted from +// http://blog.bitwigglers.org/using-enum-classes-as-type-safe-bitmasks/ // clang-format off #define AIKIDO_ENABLE_BITWISE_OPERATORS(X) \ -template<> \ +template <> \ struct EnableBitwiseOperators< X > \ { \ static const bool enable = true; \