Add flags to Saver classes to specify what to save

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)
 

include/aikido/planner/WorldStateSaver.hpp

@@ -1,6 +1,7 @@
 #ifndef AIKIDO_PLANNER_WORLDSTATESAVER_HPP_
 #define AIKIDO_PLANNER_WORLDSTATESAVER_HPP_
-#include "World.hpp"
+
+#include "aikido/planner/World.hpp"
 
 namespace aikido {
 namespace planner {
@@ -9,17 +10,29 @@ namespace planner {
 class WorldStateSaver
 {
 public:
-  /// Construct a WorldStateSaver and save the current state of the
-  /// \c World. This state will be restored when
-  /// WorldStateSaver is destructed.
+  /// 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.
   ///
-  /// \param _world World to save state from and restore to.
-  explicit WorldStateSaver(World* const _world);
+  /// \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);
 
   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;
 };
 

include/aikido/statespace/dart/MetaSkeletonStateSaver.hpp

@@ -12,13 +12,22 @@ namespace dart {
 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.
   ///
-  /// \param _metaskeleton MetaSkeleton to save/restore
+  /// \param[in] metaskeleton MetaSkeleton to save/restore
+  /// \param[in] 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;
 };
 

src/planner/WorldStateSaver.cpp

@@ -1,19 +1,22 @@
-#include <aikido/planner/WorldStateSaver.hpp>
+#include "aikido/planner/WorldStateSaver.hpp"
 
 namespace aikido {
 namespace planner {
 
-WorldStateSaver::WorldStateSaver(World* const world) : mWorld{world}
+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.");
 
-  mWorldState = mWorld->getState();
+  if (mOptions & Options::CONFIGURATIONS)
+    mWorldState = mWorld->getState();
 }
 
 WorldStateSaver::~WorldStateSaver()
 {
-  mWorld->setState(mWorldState);
+  if (mOptions & Options::CONFIGURATIONS)
+    mWorld->setState(mWorldState);
 }
 
 } // namespace planner

src/planner/vectorfield/VectorFieldPlanner.cpp

@@ -161,7 +161,8 @@ std::unique_ptr<aikido::trajectory::Spline> 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<MoveEndEffectorOffsetVectorField>(
@@ -212,7 +213,8 @@ std::unique_ptr<aikido::trajectory::Spline> 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<MoveEndEffectorPoseVectorField>(

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::dart::MetaSkeletonStateSpace>(
           statespace);
 
-  auto saver = statespace::dart::MetaSkeletonStateSaver(mSkeleton);
+  auto saver = MetaSkeletonStateSaver(
+      mSkeleton, MetaSkeletonStateSaver::Options::POSITIONS);
   DART_UNUSED(saver);
 
   auto state = metaSkeletonSs->createState();

src/statespace/dart/MetaSkeletonStateSaver.cpp

@@ -1,46 +1,58 @@
+#include "aikido/statespace/dart/MetaSkeletonStateSaver.hpp"
 #include <iostream>
-#include <aikido/statespace/dart/MetaSkeletonStateSaver.hpp>
 
 namespace aikido {
 namespace statespace {
 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 & Options::POSITIONS)
+    mPositions = mMetaSkeleton->getPositions();
+
+  if (mOptions & Options::POSITION_LIMITS)
+  {
+    mPositionLowerLimits = mMetaSkeleton->getPositionLowerLimits();
+    mPositionUpperLimits = mMetaSkeleton->getPositionUpperLimits();
+  }
 }
 
 //==============================================================================
 MetaSkeletonStateSaver::~MetaSkeletonStateSaver()
 {
-  if (static_cast<std::size_t>(mPositions.size())
-      != mMetaSkeleton->getNumDofs())
-  {
-    std::cerr << "[MetaSkeletonStateSaver] The number of DOFs in the "
-              << "MetaSkeleton does not match the saved position.";
-  }
-  if (static_cast<std::size_t>(mPositionLowerLimits.size())
-      != mMetaSkeleton->getNumDofs())
+  if (mOptions & Options::POSITIONS)
   {
-    std::cerr << "[MetaSkeletonStateSaver] The number of DOFs in the "
-              << "MetaSkeleton does not match the saved joint lower limits.";
+    if (static_cast<std::size_t>(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<std::size_t>(mPositionUpperLimits.size())
-      != mMetaSkeleton->getNumDofs())
+
+  if (mOptions & Options::POSITION_LIMITS)
   {
-    std::cerr << "[MetaSkeletonStateSaver] The number of DOFs in the "
-              << "MetaSkeleton does not match the saved joint upper limits.";
-  }
+    if (static_cast<std::size_t>(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<std::size_t>(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

tests/statespace/dart/test_MetaSkeletonStateSaver.cpp

@@ -1,37 +1,122 @@
 #include <dart/dynamics/dynamics.hpp>
 #include <gtest/gtest.h>
-#include <aikido/statespace/SO2.hpp>
 #include <aikido/statespace/dart/MetaSkeletonStateSaver.hpp>
-#include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
 
 using dart::dynamics::Skeleton;
 using dart::dynamics::RevoluteJoint;
-using aikido::statespace::dart::MetaSkeletonStateSpace;
 using aikido::statespace::dart::MetaSkeletonStateSaver;
-using aikido::statespace::SO2;
 
-TEST(MetaSkeletonStateSaver, MetaSkeletonStateSpaceReturnsToOriginal)
+class MetaSkeletonStateSaverTest : public testing::Test
 {
-  auto skeleton = Skeleton::create();
-  skeleton->createJointAndBodyNodePair<RevoluteJoint>();
+public:
+  void SetUp()
+  {
+    mSkeleton = Skeleton::create();
+    mSkeleton->createJointAndBodyNodePair<RevoluteJoint>();
+
+    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<MetaSkeletonStateSpace>(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<SO2>(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, MetaSkeletonStateSaver::Options::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, MetaSkeletonStateSaver::Options::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));
 }