Refactor DART StateSpaces

include/aikido/constraint/CollisionFree.hpp

@@ -24,11 +24,14 @@ class CollisionFree : public Testable
   /// for collision. You should call \c addPairWiseCheck and \c addSelfCheck
   /// to register collision checks before calling \c isSatisfied.
   ///
-  /// \param _statespace state space on which the constraint operates
+  /// \param _metaSkeletonStateSpace state space on which the constraint
+  /// operates
+  /// \param _metaskeleton MetaSkeleton to test with
   /// \param _collisionDetector collision detector used to test for collision
   /// \param _collisionOptions options passed to \c _collisionDetector
   CollisionFree(
-      statespace::dart::MetaSkeletonStateSpacePtr _statespace,
+      statespace::dart::MetaSkeletonStateSpacePtr _metaSkeletonStateSpace,
+      dart::dynamics::MetaSkeletonPtr _metaskeleton,
       std::shared_ptr<dart::collision::CollisionDetector> _collisionDetector,
       dart::collision::CollisionOption _collisionOptions
       = dart::collision::CollisionOption(
@@ -68,7 +71,8 @@ class CollisionFree : public Testable
 private:
   using CollisionGroup = dart::collision::CollisionGroup;
 
-  std::shared_ptr<aikido::statespace::dart::MetaSkeletonStateSpace> mStatespace;
+  aikido::statespace::dart::MetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
+  dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
   std::shared_ptr<dart::collision::CollisionDetector> mCollisionDetector;
   dart::collision::CollisionOption mCollisionOptions;
   std::vector<std::pair<std::shared_ptr<CollisionGroup>,

include/aikido/constraint/FrameDifferentiable.hpp

@@ -22,11 +22,13 @@ class FrameDifferentiable : public Differentiable
   /// Constructor.
   /// \param _metaSkeletonStateSpace StateSpace whose state
   ///        defines _jacobianNode's transform.
+  /// \param _metaskeleton MetaSkeleton to test with
   /// \param _jacobianNode The frame being constrained.
   /// \param _poseConstraint Constraint on _jacobian. This should be
   ///        in SE3.
   FrameDifferentiable(
       statespace::dart::MetaSkeletonStateSpacePtr _metaSkeletonStateSpace,
+      dart::dynamics::MetaSkeletonPtr _metaskeleton,
       dart::dynamics::ConstJacobianNodePtr _jacobianNode,
       DifferentiablePtr _poseConstraint);
 
@@ -62,10 +64,10 @@ class FrameDifferentiable : public Differentiable
   statespace::StateSpacePtr getStateSpace() const override;
 
 private:
-  dart::dynamics::ConstJacobianNodePtr mJacobianNode;
-  DifferentiablePtr mPoseConstraint;
   statespace::dart::MetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
   dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
+  dart::dynamics::ConstJacobianNodePtr mJacobianNode;
+  DifferentiablePtr mPoseConstraint;
 };
 
 } // namespace constraint

include/aikido/constraint/FramePairDifferentiable.hpp

@@ -21,13 +21,15 @@ class FramePairDifferentiable : public Differentiable
   /// Constructor.
   /// \param _metaSkeletonStateSpace StateSpace whose states define
   ///        _jacobianNodeTarget and _jacobianNodeBase's relative transform.
+  /// \param _metaskeleton MetaSkeleton to test with
   /// \param _jacobianNodeTarget The frame whose relative transform w.r.t.
   ///        _jacobianNodeBase is being constrained.
   /// \param _jacobianNodeBase The base frame for this constraint.
   /// \param _relPoseConstraint Relative pose constraint on _jacobianNodeTarget
   ///        w.r.t. _jacobianNodeBase.
   FramePairDifferentiable(
       statespace::dart::MetaSkeletonStateSpacePtr _metaSkeletonStateSpace,
+      dart::dynamics::MetaSkeletonPtr _metaskeleton,
       dart::dynamics::ConstJacobianNodePtr _jacobianNodeTarget,
       dart::dynamics::ConstJacobianNodePtr _jacobianNodeBase,
       DifferentiablePtr _relPoseConstraint);
@@ -57,11 +59,11 @@ class FramePairDifferentiable : public Differentiable
   statespace::StateSpacePtr getStateSpace() const override;
 
 private:
+  statespace::dart::MetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
+  dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
   dart::dynamics::ConstJacobianNodePtr mJacobianNode1;
   dart::dynamics::ConstJacobianNodePtr mJacobianNode2;
   DifferentiablePtr mRelPoseConstraint;
-  statespace::dart::MetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
-  dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
 };
 
 } // namespace constraint

include/aikido/constraint/FrameTestable.hpp

@@ -15,12 +15,14 @@ namespace constraint {
 class FrameTestable : public Testable
 {
 public:
-  /// Creat a Testable for the MetaSkeleton.
-  /// \param _stateSpace Configuration space of the metaskeleton.
+  /// Create a Testable for the MetaSkeleton.
+  /// \param _metaSkeletonStateSpace Configuration space of the metaskeleton.
+  /// \param _metaskeleton MetaSkeleton to test with
   /// \param _frame Frame constrained by _poseConstraint.
   /// \param _poseConstraint A testable constraint on _frame.
   FrameTestable(
-      statespace::dart::MetaSkeletonStateSpacePtr _stateSpace,
+      statespace::dart::MetaSkeletonStateSpacePtr _metaSkeletonStateSpace,
+      dart::dynamics::MetaSkeletonPtr _metaskeleton,
       dart::dynamics::ConstJacobianNodePtr _frame,
       TestablePtr _poseConstraint);
 
@@ -31,11 +33,12 @@ class FrameTestable : public Testable
   ///        StateSpace returend by getStateSpace().
   bool isSatisfied(const statespace::StateSpace::State* _state) const override;
 
-  // Documentation inhereted
+  // Documentation inherited
   std::shared_ptr<statespace::StateSpace> getStateSpace() const override;
 
 private:
-  statespace::dart::MetaSkeletonStateSpacePtr mStateSpace;
+  statespace::dart::MetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
+  dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
   dart::dynamics::ConstJacobianNodePtr mFrame;
   TestablePtr mPoseConstraint;
   std::shared_ptr<statespace::SE3> mPoseStateSpace;

include/aikido/constraint/InverseKinematicsSampleable.hpp

@@ -19,8 +19,9 @@ class InverseKinematicsSampleable : public Sampleable
 {
 public:
   /// Constructor.
-  /// \param _stateSpace StateSpace of a skeleton
+  /// \param _metaSkeletonStateSpace StateSpace of a skeleton
   ///        one of whose frame is being constrained by _poseConstraint.
+  /// \param _metaskeleton MetaSkeleton to test with
   /// \param _poseConstraint This samples poses for a frame in the skeleton.
   /// \param _seedConstraint This samples configurations for the skeleton.
   ///        These samples are used as seeds when solving inverse kinematics.
@@ -29,7 +30,8 @@ class InverseKinematicsSampleable : public Sampleable
   /// \param _maxNumTrials Max number of trials for its sample generator
   ///        to retry sampling and finding an inverse kinematics solution.
   InverseKinematicsSampleable(
-      statespace::dart::MetaSkeletonStateSpacePtr _stateSpace,
+      statespace::dart::MetaSkeletonStateSpacePtr _metaSkeletonStateSpace,
+      dart::dynamics::MetaSkeletonPtr _metaskeleton,
       SampleablePtr _poseConstraint,
       SampleablePtr _seedConstraint,
       dart::dynamics::InverseKinematicsPtr _inverseKinematics,
@@ -44,7 +46,8 @@ class InverseKinematicsSampleable : public Sampleable
   std::unique_ptr<SampleGenerator> createSampleGenerator() const override;
 
 private:
-  statespace::dart::MetaSkeletonStateSpacePtr mStateSpace;
+  statespace::dart::MetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
+  dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
   SampleablePtr mPoseConstraint;
   SampleablePtr mSeedConstraint;
   dart::dynamics::InverseKinematicsPtr mInverseKinematics;

include/aikido/constraint/detail/JointStateSpaceHelpers-impl.hpp

@@ -18,43 +18,6 @@ namespace aikido {
 namespace constraint {
 namespace detail {
 
-//==============================================================================
-inline bool isLimited(const dart::dynamics::Joint* _joint)
-{
-  for (std::size_t i = 0; i < _joint->getNumDofs(); ++i)
-  {
-    if (_joint->hasPositionLimit(i))
-      return true;
-  }
-  return false;
-}
-
-//==============================================================================
-inline Eigen::VectorXd getPositionLowerLimits(
-    const dart::dynamics::Joint* _joint)
-{
-  const auto dimension = _joint->getNumDofs();
-  Eigen::VectorXd bounds(dimension);
-
-  for (std::size_t i = 0; i < dimension; ++i)
-    bounds[i] = _joint->getPositionLowerLimit(i);
-
-  return bounds;
-}
-
-//==============================================================================
-inline Eigen::VectorXd getPositionUpperLimits(
-    const dart::dynamics::Joint* _joint)
-{
-  const auto dimension = _joint->getNumDofs();
-  Eigen::VectorXd bounds(dimension);
-
-  for (std::size_t i = 0; i < dimension; ++i)
-    bounds[i] = _joint->getPositionUpperLimit(i);
-
-  return bounds;
-}
-
 //==============================================================================
 using JointStateSpaceTypeList = common::type_list<statespace::dart::R0Joint,
                                                   statespace::dart::R1Joint,
@@ -98,15 +61,15 @@ std::unique_ptr<OutputConstraint> createBoxConstraint(
     std::shared_ptr<statespace::dart::RJoint<N>> _stateSpace,
     std::unique_ptr<common::RNG> _rng)
 {
-  const auto joint = _stateSpace->getJoint();
+  const auto properties = _stateSpace->getProperties();
 
-  if (isLimited(joint))
+  if (properties.isPositionLimited())
   {
     return dart::common::make_unique<RBoxConstraint<N>>(
         std::move(_stateSpace),
         std::move(_rng),
-        getPositionLowerLimits(joint),
-        getPositionUpperLimits(joint));
+        properties.getPositionLowerLimits(),
+        properties.getPositionUpperLimits());
   }
   else
   {
@@ -164,15 +127,15 @@ struct createSampleableFor_impl<statespace::dart::RJoint<N>>
   static std::unique_ptr<Sampleable> create(
       StateSpacePtr _stateSpace, std::unique_ptr<common::RNG> _rng)
   {
-    const auto joint = _stateSpace->getJoint();
+    const auto properties = _stateSpace->getProperties();
 
-    if (isLimited(joint))
+    if (properties.isPositionLimited())
     {
       return dart::common::make_unique<RBoxConstraint<N>>(
           std::move(_stateSpace),
           std::move(_rng),
-          getPositionLowerLimits(joint),
-          getPositionUpperLimits(joint));
+          properties.getPositionLowerLimits(),
+          properties.getPositionUpperLimits());
     }
     else
     {
@@ -190,7 +153,7 @@ struct createDifferentiableFor_impl<statespace::dart::SO2Joint>
 
   static std::unique_ptr<Differentiable> create(StateSpacePtr _stateSpace)
   {
-    if (isLimited(_stateSpace->getJoint()))
+    if (_stateSpace->getProperties().isPositionLimited())
       throw std::invalid_argument("SO2Joint must not have limits.");
 
     return dart::common::make_unique<Satisfied>(std::move(_stateSpace));
@@ -206,7 +169,7 @@ struct createTestableFor_impl<statespace::dart::SO2Joint>
 
   static std::unique_ptr<Testable> create(StateSpacePtr _stateSpace)
   {
-    if (isLimited(_stateSpace->getJoint()))
+    if (_stateSpace->getProperties().isPositionLimited())
       throw std::invalid_argument("SO2Joint must not have limits.");
 
     return dart::common::make_unique<Satisfied>(std::move(_stateSpace));
@@ -222,7 +185,7 @@ struct createProjectableFor_impl<statespace::dart::SO2Joint>
 
   static std::unique_ptr<Projectable> create(StateSpacePtr _stateSpace)
   {
-    if (isLimited(_stateSpace->getJoint()))
+    if (_stateSpace->getProperties().isPositionLimited())
       throw std::invalid_argument("SO2Joint must not have limits.");
 
     return dart::common::make_unique<Satisfied>(std::move(_stateSpace));
@@ -239,7 +202,7 @@ struct createSampleableFor_impl<statespace::dart::SO2Joint>
   static std::unique_ptr<Sampleable> create(
       StateSpacePtr _stateSpace, std::unique_ptr<common::RNG> _rng)
   {
-    if (isLimited(_stateSpace->getJoint()))
+    if (_stateSpace->getProperties().isPositionLimited())
       throw std::invalid_argument("SO2Joint must not have limits.");
 
     return dart::common::make_unique<SO2UniformSampler>(
@@ -256,7 +219,7 @@ struct createDifferentiableFor_impl<statespace::dart::SO3Joint>
 
   static std::unique_ptr<Differentiable> create(StateSpacePtr _stateSpace)
   {
-    if (isLimited(_stateSpace->getJoint()))
+    if (_stateSpace->getProperties().isPositionLimited())
       throw std::invalid_argument("SO3Joint must not have limits.");
 
     return dart::common::make_unique<Satisfied>(std::move(_stateSpace));
@@ -272,7 +235,7 @@ struct createTestableFor_impl<statespace::dart::SO3Joint>
 
   static std::unique_ptr<Testable> create(StateSpacePtr _stateSpace)
   {
-    if (isLimited(_stateSpace->getJoint()))
+    if (_stateSpace->getProperties().isPositionLimited())
       throw std::invalid_argument("SO3Joint must not have limits.");
 
     return dart::common::make_unique<Satisfied>(std::move(_stateSpace));
@@ -288,7 +251,7 @@ struct createProjectableFor_impl<statespace::dart::SO3Joint>
 
   static std::unique_ptr<Projectable> create(StateSpacePtr _stateSpace)
   {
-    if (isLimited(_stateSpace->getJoint()))
+    if (_stateSpace->getProperties().isPositionLimited())
       throw std::invalid_argument("SO3Joint must not have limits.");
 
     return dart::common::make_unique<Satisfied>(std::move(_stateSpace));
@@ -305,7 +268,7 @@ struct createSampleableFor_impl<statespace::dart::SO3Joint>
   static std::unique_ptr<Sampleable> create(
       StateSpacePtr _stateSpace, std::unique_ptr<common::RNG> _rng)
   {
-    if (isLimited(_stateSpace->getJoint()))
+    if (_stateSpace->getProperties().isPositionLimited())
       throw std::invalid_argument("SO3Joint must not have limits.");
 
     return dart::common::make_unique<SO3UniformSampler>(
@@ -319,15 +282,15 @@ std::unique_ptr<OutputConstraint> createBoxConstraint(
     std::shared_ptr<statespace::dart::SE2Joint> _stateSpace,
     std::unique_ptr<common::RNG> _rng)
 {
-  const auto joint = _stateSpace->getJoint();
+  const auto properties = _stateSpace->getProperties();
 
-  if (isLimited(joint))
+  if (properties.isPositionLimited())
   {
     return dart::common::make_unique<SE2BoxConstraint>(
         std::move(_stateSpace),
         std::move(_rng),
-        getPositionLowerLimits(joint).tail<2>(),
-        getPositionUpperLimits(joint).tail<2>());
+        properties.getPositionLowerLimits().tail<2>(),
+        properties.getPositionUpperLimits().tail<2>());
   }
   else
   {
@@ -358,8 +321,8 @@ struct createTestableFor_impl<statespace::dart::SE2Joint>
 
   static std::unique_ptr<Testable> create(StateSpacePtr _stateSpace)
   {
-    auto joint = _stateSpace->getJoint();
-    if (joint->hasPositionLimit(0))
+    const auto properties = _stateSpace->getProperties();
+    if (properties.hasPositionLimit(0))
     {
       throw std::invalid_argument(
           "Rotational component of SE2Joint must not have limits.");
@@ -378,8 +341,8 @@ struct createProjectableFor_impl<statespace::dart::SE2Joint>
 
   static std::unique_ptr<Projectable> create(StateSpacePtr _stateSpace)
   {
-    auto joint = _stateSpace->getJoint();
-    if (joint->hasPositionLimit(0))
+    const auto properties = _stateSpace->getProperties();
+    if (properties.hasPositionLimit(0))
     {
       throw std::invalid_argument(
           "Rotational component of SE2Joint must not have limits.");
@@ -399,13 +362,14 @@ struct createSampleableFor_impl<statespace::dart::SE2Joint>
   static std::unique_ptr<Sampleable> create(
       StateSpacePtr stateSpace, std::unique_ptr<common::RNG> rng)
   {
-    auto joint = stateSpace->getJoint();
-    if (joint->hasPositionLimit(0))
+    const auto properties = stateSpace->getProperties();
+    if (properties.hasPositionLimit(0))
     {
       throw std::invalid_argument(
           "Rotational component of SE2Joint must not have limits.");
     }
-    else if (!(joint->hasPositionLimit(1) && joint->hasPositionLimit(2)))
+    else if (
+        !(properties.hasPositionLimit(1) && properties.hasPositionLimit(2)))
     {
       throw std::runtime_error(
           "Unable to create Sampleable for unbounded SE2.");
@@ -415,8 +379,8 @@ struct createSampleableFor_impl<statespace::dart::SE2Joint>
       return dart::common::make_unique<SE2BoxConstraint>(
           std::move(stateSpace),
           std::move(rng),
-          getPositionLowerLimits(joint).tail<2>(),
-          getPositionUpperLimits(joint).tail<2>());
+          properties.getPositionLowerLimits().tail<2>(),
+          properties.getPositionUpperLimits().tail<2>());
     }
   }
 };
@@ -533,8 +497,8 @@ struct createSampleableFor_impl<statespace::dart::WeldJoint>
   static std::unique_ptr<Sampleable> create(
       StateSpacePtr _stateSpace, std::unique_ptr<common::RNG> /*_rng*/)
   {
-    const auto joint = _stateSpace->getJoint();
-    Eigen::VectorXd positions = joint->getPositions();
+    // A WeldJoint has zero DOFs
+    Eigen::VectorXd positions = Eigen::Matrix<double, 0, 1>();
 
     return dart::common::make_unique<R0ConstantSampler>(
         std::move(_stateSpace), positions);

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);