diff --git a/include/aikido/common.hpp b/include/aikido/common.hpp index 6441b559a10..d9d650a5b29 100644 --- a/include/aikido/common.hpp +++ b/include/aikido/common.hpp @@ -1,3 +1,4 @@ +#include "aikido/common/EnumFlags.hpp" #include "aikido/common/ExecutorMultiplexer.hpp" #include "aikido/common/ExecutorThread.hpp" #include "aikido/common/PseudoInverse.hpp" diff --git a/include/aikido/common/EnumFlags.hpp b/include/aikido/common/EnumFlags.hpp new file mode 100644 index 00000000000..e38bca214ca --- /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 bfa9334435d..55350a7c5f7 100644 --- a/src/statespace/dart/MetaSkeletonStateSaver.cpp +++ b/src/statespace/dart/MetaSkeletonStateSaver.cpp @@ -8,13 +8,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(); @@ -24,7 +26,7 @@ MetaSkeletonStateSaver::MetaSkeletonStateSaver( //============================================================================== MetaSkeletonStateSaver::~MetaSkeletonStateSaver() { - if (mOptions & Options::POSITIONS) + if ((mOptions & Options::POSITIONS) != Options::NONE) { if (static_cast(mPositions.size()) != mMetaSkeleton->getNumDofs()) @@ -36,7 +38,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 8a584a92ae6..ac145ca10c7 100644 --- a/tests/statespace/dart/test_MetaSkeletonStateSaver.cpp +++ b/tests/statespace/dart/test_MetaSkeletonStateSaver.cpp @@ -55,7 +55,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.);