Refactor VectorFieldPlanner to work with new MetaSkeletonStateSpaces.

One of the VectorFieldPlanner tests is currently failing, but I figured
it's not worth debugging since the improved VectorFieldPlanner will be
ready soon. This is in a separate commit so it can be easily reverted
when the improved implementation is ready.

include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp

@@ -44,11 +44,13 @@ class MoveEndEffectorOffsetVectorField
   /// Vectorfield callback function
   ///
   /// \param[in] _stateSpace MetaSkeleton state space
+  /// \param[in] _metaskeleton MetaSkeleton for planning
   /// \param[in] _t Current time being planned
   /// \param[out] _dq Joint velocities
   /// \return Whether joint velocities are successfully computed
   bool operator()(
       const aikido::statespace::dart::MetaSkeletonStateSpacePtr& _stateSpace,
+      dart::dynamics::MetaSkeletonPtr _metaSkeleton,
       double _t,
       Eigen::VectorXd& _dq);
 

include/aikido/planner/vectorfield/VectorFieldPlanner.hpp

@@ -22,12 +22,14 @@ enum class VectorFieldPlannerStatus
 /// MetaSkeleton.
 ///
 /// \param[in] _stateSpace MetaSkeleton state space
+/// \param[in] _metaskeleton MetaSkeleton to plan with
 /// \param[in] _t Planned time of a given duration
 /// \param[out] _dq Joint velocity calculated by a vector field and meta
 /// skeleton
 /// \return Whether vectorfield evaluation succeeds
 using VectorFieldCallback = std::function<bool(
     const aikido::statespace::dart::MetaSkeletonStateSpacePtr& _stateSpace,
+    dart::dynamics::MetaSkeletonPtr _metaskeleton,
     double _t,
     Eigen::VectorXd& _dq)>;
 
@@ -43,13 +45,15 @@ using VectorFieldStatusCallback = std::function<VectorFieldPlannerStatus(
 /// Plan to a trajectory by a given vector field.
 ///
 /// \param[in] _stateSpace MetaSkeleton state space
+/// \param[in] _metaskeleton MetaSkeleton to plan with
 /// \param[in] _constraint Trajectory-wide constraint that must be satisfied
 /// \param[in] _timestep How long an evaluation step is
 /// \param[in] _vectorField Callback of vector field calculation
 /// \param[in] _statusCb Callback of planning status
 /// \return Trajectory or \c nullptr if planning failed
 std::unique_ptr<aikido::trajectory::Spline> planPathByVectorField(
     const aikido::statespace::dart::MetaSkeletonStateSpacePtr& _stateSpace,
+    dart::dynamics::MetaSkeletonPtr _metaskeleton,
     const aikido::constraint::TestablePtr& _constraint,
     double _timestep,
     const VectorFieldCallback& _vectorFieldCb,
@@ -59,6 +63,7 @@ std::unique_ptr<aikido::trajectory::Spline> planPathByVectorField(
 /// distance.
 ///
 /// \param[in] _stateSpace MetaSkeleton state space
+/// \param[in] _metaskeleton MetaSkeleton to plan with
 /// \param[in] _bn Body node of the end-effector
 /// \param[in] _constraint Trajectory-wide constraint that must be satisfied
 /// \param[in] _direction Direction of moving the end-effector
@@ -78,6 +83,7 @@ std::unique_ptr<aikido::trajectory::Spline> planPathByVectorField(
 /// \return Trajectory or \c nullptr if planning failed
 std::unique_ptr<aikido::trajectory::Spline> planToEndEffectorOffset(
     const aikido::statespace::dart::MetaSkeletonStateSpacePtr& _stateSpace,
+    dart::dynamics::MetaSkeletonPtr _metaskeleton,
     const dart::dynamics::BodyNodePtr& _bn,
     const aikido::constraint::TestablePtr& _constraint,
     const Eigen::Vector3d& _direction,

include/aikido/planner/vectorfield/VectorFieldPlannerExceptions.hpp

@@ -25,18 +25,17 @@ class DofLimitError : public VectorFieldTerminated
 public:
   /// Constructor
   ///
-  /// \param[in] _dof Degree of freedom
+  /// \param[in] _dof Degree of freedom name
   /// \param[in] _whatArg Error string
-  DofLimitError(
-      const dart::dynamics::DegreeOfFreedom* _dof, const std::string& _whatArg);
+  DofLimitError(const std::string& _dof, const std::string& _whatArg);
 
-  /// Get degree of freedom
+  /// Get degree of freedom name
   ///
-  /// \return Degree of freedom
-  const dart::dynamics::DegreeOfFreedom* dof() const;
+  /// \return Degree of freedom name
+  const std::string dof() const;
 
 private:
-  const dart::dynamics::DegreeOfFreedom* mDof;
+  const std::string mDof;
 };
 
 } // namespace vectorfield

include/aikido/planner/vectorfield/VectorFieldUtil.hpp

@@ -69,6 +69,7 @@ class DesiredTwistFunction : public dart::optimizer::Function
 /// \param[in] _desiredTwist Desired twist, which consists of angular velocity
 /// and linear velocity
 /// \param[in] _stateSpace MetaSkeleton state space
+/// \param[in] _metaskeleton MetaSkeleton to read position and velocity from
 /// \param[in]  _bodyNode Body node of the end-effector
 /// \param[in] _optimizationTolerance Callback of vector field calculation
 /// \param[in] _timestep How long will the computed joint velocities be executed
@@ -77,6 +78,7 @@ class DesiredTwistFunction : public dart::optimizer::Function
 bool computeJointVelocityFromTwist(
     const Eigen::Vector6d& _desiredTwist,
     const aikido::statespace::dart::MetaSkeletonStateSpacePtr _stateSpace,
+    const dart::dynamics::MetaSkeletonPtr _metaskeleton,
     const dart::dynamics::BodyNodePtr _bodyNode,
     double _optimizationTolerance,
     double _timestep,

src/planner/vectorfield/MoveEndEffectorOffsetVectorField.cpp

@@ -61,6 +61,7 @@ MoveEndEffectorOffsetVectorField::MoveEndEffectorOffsetVectorField(
 //==============================================================================
 bool MoveEndEffectorOffsetVectorField::operator()(
     const aikido::statespace::dart::MetaSkeletonStateSpacePtr& _stateSpace,
+    dart::dynamics::MetaSkeletonPtr _metaskeleton,
     double,
     Eigen::VectorXd& _dq)
 {
@@ -95,6 +96,7 @@ bool MoveEndEffectorOffsetVectorField::operator()(
   bool result = computeJointVelocityFromTwist(
       desiredTwist,
       _stateSpace,
+      _metaskeleton,
       mBodynode,
       mOptimizationTolerance,
       mTimestep,

src/planner/vectorfield/VectorFieldPlanner.cpp

@@ -18,47 +18,49 @@ static void checkDofLimits(
     Eigen::VectorXd const& q,
     Eigen::VectorXd const& qd)
 {
-  using dart::dynamics::DegreeOfFreedom;
+  const auto properties = stateSpace->getProperties();
+  const auto dofNames = properties.getDofNames();
+  const auto positionLowerLimits = properties.getPositionLowerLimits();
+  const auto positionUpperLimits = properties.getPositionUpperLimits();
+  const auto velocityLowerLimits = properties.getVelocityLowerLimits();
+  const auto velocityUpperLimits = properties.getVelocityUpperLimits();
+
   std::stringstream ss;
 
-  for (std::size_t i = 0; i < stateSpace->getMetaSkeleton()->getNumDofs(); ++i)
+  for (std::size_t i = 0; i < properties.getNumDofs(); ++i)
   {
-    DegreeOfFreedom* const dof = stateSpace->getMetaSkeleton()->getDof(i);
-
-    if (q[i] < dof->getPositionLowerLimit())
+    if (q[i] < positionLowerLimits[i])
     {
-      ss << "DOF " << dof->getName() << " exceeds lower position limit: ";
-      ss << q[i] << " < " << dof->getPositionLowerLimit();
-      throw DofLimitError(dof, ss.str());
+      ss << "DOF " << dofNames[i] << " exceeds lower position limit: ";
+      ss << q[i] << " < " << positionLowerLimits[i];
+      throw DofLimitError(dofNames[i], ss.str());
     }
-    else if (q[i] > dof->getPositionUpperLimit())
+    else if (q[i] > positionUpperLimits[i])
     {
-      ss << "DOF " << dof->getName() << " exceeds upper position limit: ";
-      ss << q[i] << " > " << dof->getPositionUpperLimit();
-      throw DofLimitError(dof, ss.str());
+      ss << "DOF " << dofNames[i] << " exceeds upper position limit: ";
+      ss << q[i] << " > " << positionUpperLimits[i];
+      throw DofLimitError(dofNames[i], ss.str());
     }
-    else if (qd[i] < dof->getVelocityLowerLimit())
+    else if (qd[i] < velocityLowerLimits[i])
     {
-      ss << "DOF " << dof->getName() << " exceeds lower velocity limit: ";
-      ss << qd[i] << " < " << dof->getVelocityLowerLimit();
-      throw DofLimitError(dof, ss.str());
+      ss << "DOF " << dofNames[i] << " exceeds lower velocity limit: ";
+      ss << qd[i] << " < " << velocityLowerLimits[i];
+      throw DofLimitError(dofNames[i], ss.str());
     }
-    else if (qd[i] > dof->getVelocityUpperLimit())
+    else if (qd[i] > velocityUpperLimits[i])
     {
-      ss << "DOF " << dof->getName() << " exceeds upper velocity limit: ";
-      ss << qd[i] << " > " << dof->getVelocityUpperLimit();
-      throw DofLimitError(dof, ss.str());
+      ss << "DOF " << dofNames[i] << " exceeds upper velocity limit: ";
+      ss << qd[i] << " > " << velocityUpperLimits[i];
+      throw DofLimitError(dofNames[i], ss.str());
     }
   }
 }
 
 //==============================================================================
 static void checkCollision(
-    const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace,
+    const aikido::statespace::dart::MetaSkeletonStateSpace::State* state,
     const aikido::constraint::TestablePtr& constraint)
 {
-  // Get current position
-  auto state = stateSpace->getScopedStateFromMetaSkeleton();
   // Throw a termination if in collision
   if (!constraint->isSatisfied(state))
   {
@@ -69,42 +71,50 @@ static void checkCollision(
 //==============================================================================
 std::unique_ptr<aikido::trajectory::Spline> planPathByVectorField(
     const aikido::statespace::dart::MetaSkeletonStateSpacePtr& _stateSpace,
+    dart::dynamics::MetaSkeletonPtr _metaskeleton,
     const aikido::constraint::TestablePtr& _constraint,
     double _dt,
-    const VectorFieldCallback& _vectorFiledCb,
+    const VectorFieldCallback& _vectorFieldCb,
     const VectorFieldStatusCallback& _statusCb)
 {
-  auto saver = MetaSkeletonStateSaver(_stateSpace->getMetaSkeleton());
+  auto saver = MetaSkeletonStateSaver(_metaskeleton);
   DART_UNUSED(saver);
 
-  dart::dynamics::MetaSkeletonPtr skeleton = _stateSpace->getMetaSkeleton();
-
-  if (skeleton->getPositionUpperLimits() == skeleton->getPositionLowerLimits())
+  const auto metaSkeletonProperties = _stateSpace->getProperties();
+  const std::size_t numDof = metaSkeletonProperties.getNumDofs();
+  const auto positionLowerLimits
+      = metaSkeletonProperties.getPositionLowerLimits();
+  const auto positionUpperLimits
+      = metaSkeletonProperties.getPositionUpperLimits();
+  const auto velocityLowerLimits
+      = metaSkeletonProperties.getVelocityLowerLimits();
+  const auto velocityUpperLimits
+      = metaSkeletonProperties.getVelocityUpperLimits();
+
+  if (positionUpperLimits == positionLowerLimits)
   {
     throw std::invalid_argument("State space volume zero");
   }
-  if (skeleton->getVelocityUpperLimits() == skeleton->getVelocityLowerLimits())
+  if (velocityUpperLimits == velocityLowerLimits)
   {
     throw std::invalid_argument("Velocity space volume zero");
   }
 
-  for (std::size_t i = 0; i < skeleton->getNumDofs(); ++i)
+  for (std::size_t i = 0; i < numDof; ++i)
   {
     std::stringstream ss;
-    if (skeleton->getPositionLowerLimit(i) > skeleton->getPositionUpperLimit(i))
+    if (positionLowerLimits[i] > positionUpperLimits[i])
     {
       ss << "Position lower limit is larger than upper limit at DOF " << i;
       throw std::invalid_argument(ss.str());
     }
-    if (skeleton->getVelocityLowerLimit(i) > skeleton->getVelocityUpperLimit(i))
+    if (velocityLowerLimits[i] > velocityUpperLimits[i])
     {
       ss << "Velocity lower limit is larger than upper limit at DOF " << i;
       throw std::invalid_argument(ss.str());
     }
   }
 
-  const std::size_t numDof = _stateSpace->getDimension();
-
   std::vector<Knot> knots;
   VectorFieldPlannerStatus terminationStatus
       = VectorFieldPlannerStatus::CONTINUE;
@@ -113,10 +123,7 @@ std::unique_ptr<aikido::trajectory::Spline> planPathByVectorField(
   int cacheIndex = -1;
   int index = 0;
   double t = 0;
-  Eigen::VectorXd q = _stateSpace->getMetaSkeleton()->getPositions();
-
-  auto startState = _stateSpace->createState();
-  _stateSpace->convertPositionsToState(q, startState);
+  Eigen::VectorXd q = _metaskeleton->getPositions();
   Eigen::VectorXd dq(numDof);
   assert(static_cast<std::size_t>(q.size()) == numDof);
 
@@ -125,7 +132,7 @@ std::unique_ptr<aikido::trajectory::Spline> planPathByVectorField(
     // Evaluate the vector field.
     try
     {
-      if (!_vectorFiledCb(_stateSpace, t, dq))
+      if (!_vectorFieldCb(_stateSpace, _metaskeleton, t, dq))
       {
         throw VectorFieldTerminated("Failed evaluating VectorField.");
       }
@@ -150,11 +157,13 @@ std::unique_ptr<aikido::trajectory::Spline> planPathByVectorField(
     // Compute the number of collision checks we need.
     for (std::size_t istep = 0; istep < numSteps; ++istep)
     {
+      auto currentState = _stateSpace->createState();
+
       try
       {
         checkDofLimits(_stateSpace, q, dq);
-        _stateSpace->getMetaSkeleton()->setPositions(q);
-        checkCollision(_stateSpace, _constraint);
+        _stateSpace->convertPositionsToState(q, currentState);
+        checkCollision(currentState, _constraint);
       }
       catch (const VectorFieldTerminated& e)
       {
@@ -174,12 +183,12 @@ std::unique_ptr<aikido::trajectory::Spline> planPathByVectorField(
       knots.push_back(knot);
 
       // Take a step.
-      auto currentState = _stateSpace->createState();
-      _stateSpace->convertPositionsToState(q, currentState);
       auto deltaState = _stateSpace->createState();
       _stateSpace->convertPositionsToState(_dt * dq, deltaState);
+
       auto nextState = _stateSpace->createState();
       _stateSpace->compose(currentState, deltaState, nextState);
+
       _stateSpace->convertStateToPositions(nextState, q);
 
       t += _dt;
@@ -227,6 +236,7 @@ std::unique_ptr<aikido::trajectory::Spline> planPathByVectorField(
 //==============================================================================
 std::unique_ptr<aikido::trajectory::Spline> planToEndEffectorOffset(
     const aikido::statespace::dart::MetaSkeletonStateSpacePtr& _stateSpace,
+    dart::dynamics::MetaSkeletonPtr _metaskeleton,
     const dart::dynamics::BodyNodePtr& _bn,
     const aikido::constraint::TestablePtr& _constraint,
     const Eigen::Vector3d& _direction,
@@ -272,7 +282,12 @@ std::unique_ptr<aikido::trajectory::Spline> planToEndEffectorOffset(
       _angularTolerance);
 
   return planPathByVectorField(
-      _stateSpace, _constraint, _timestep, vectorfield, vectorfield);
+      _stateSpace,
+      _metaskeleton,
+      _constraint,
+      _timestep,
+      vectorfield,
+      vectorfield);
 }
 
 } // namespace vectorfield

src/planner/vectorfield/VectorFieldPlannerExceptions.cpp

@@ -12,14 +12,14 @@ VectorFieldTerminated::VectorFieldTerminated(const std::string& _whatArg)
 
 //==============================================================================
 DofLimitError::DofLimitError(
-    const dart::dynamics::DegreeOfFreedom* _dof, const std::string& _whatArg)
+    const std::string& _dof, const std::string& _whatArg)
   : VectorFieldTerminated(_whatArg), mDof(_dof)
 {
   // Do nothing
 }
 
 //==============================================================================
-const dart::dynamics::DegreeOfFreedom* DofLimitError::dof() const
+const std::string DofLimitError::dof() const
 {
   return mDof;
 }

src/planner/vectorfield/VectorFieldUtil.cpp

@@ -13,7 +13,7 @@ std::unique_ptr<aikido::trajectory::Spline> convertToSpline(
 {
   using dart::common::make_unique;
 
-  std::size_t numDof = _stateSpace->getMetaSkeleton()->getNumDofs();
+  std::size_t numDof = _stateSpace->getProperties().getNumDofs();
   // Construct the output spline.
   Eigen::VectorXd times(_cacheIndex);
   std::transform(
@@ -78,6 +78,7 @@ void DesiredTwistFunction::evalGradient(
 bool computeJointVelocityFromTwist(
     const Eigen::Vector6d& _desiredTwist,
     const aikido::statespace::dart::MetaSkeletonStateSpacePtr _stateSpace,
+    const dart::dynamics::MetaSkeletonPtr _metaskeleton,
     const dart::dynamics::BodyNodePtr _bodyNode,
     double _optimizationTolerance,
     double _timestep,
@@ -89,21 +90,21 @@ bool computeJointVelocityFromTwist(
   using dart::optimizer::Solver;
   using Eigen::VectorXd;
 
-  const dart::dynamics::MetaSkeletonPtr skeleton
-      = _stateSpace->getMetaSkeleton();
   // Use LBFGS to find joint angles that won't violate the joint limits.
-  const Jacobian jacobian = skeleton->getWorldJacobian(_bodyNode);
-
-  const std::size_t numDofs = skeleton->getNumDofs();
-  const VectorXd positions = skeleton->getPositions();
-  VectorXd initialGuess = skeleton->getVelocities();
-  const VectorXd positionLowerLimits = skeleton->getPositionLowerLimits();
-  const VectorXd positionUpperLimits = skeleton->getPositionUpperLimits();
-  VectorXd velocityLowerLimits = skeleton->getVelocityLowerLimits();
-  VectorXd velocityUpperLimits = skeleton->getVelocityUpperLimits();
-
-  auto currentState = _stateSpace->createState();
-  _stateSpace->convertPositionsToState(positions, currentState);
+  const Jacobian jacobian = _metaskeleton->getWorldJacobian(_bodyNode);
+  const auto properties = _stateSpace->getProperties();
+
+  const std::size_t numDofs = properties.getNumDofs();
+  const VectorXd positionLowerLimits = properties.getPositionLowerLimits();
+  const VectorXd positionUpperLimits = properties.getPositionUpperLimits();
+  VectorXd velocityLowerLimits = properties.getVelocityLowerLimits();
+  VectorXd velocityUpperLimits = properties.getVelocityUpperLimits();
+
+  auto currentState
+      = _stateSpace->getScopedStateFromMetaSkeleton(_metaskeleton.get());
+  VectorXd positions(numDofs);
+  _stateSpace->convertStateToPositions(currentState, positions);
+  VectorXd initialGuess = _metaskeleton->getVelocities();
 
   for (std::size_t i = 0; i < numDofs; ++i)
   {