Adds SE2BoxConstraint

CMakeLists.txt

@@ -82,7 +82,7 @@ file(GLOB_RECURSE headers_planner "${CMAKE_SOURCE_DIR}/include/aikido/planner/*.
 file(GLOB_RECURSE headers_util "${CMAKE_SOURCE_DIR}/include/aikido/util/*.hpp")
 file(GLOB_RECURSE headers_rviz "${CMAKE_SOURCE_DIR}/include/aikido/rviz/*.hpp")
 file(GLOB_RECURSE headers_distance "${CMAKE_SOURCE_DIR}/include/aikido/distance/*.hpp")
-file(GLOB_RECURSE headers_distance "${CMAKE_SOURCE_DIR}/include/aikido/control/*.hpp")
+file(GLOB_RECURSE headers_control "${CMAKE_SOURCE_DIR}/include/aikido/control/*.hpp")
 format_add_sources(${headers_statespace})
 format_add_sources(${headers_perception})
 format_add_sources(${headers_planner})

include/aikido/constraint.hpp

@@ -20,5 +20,6 @@
 #include "constraint/Testable.hpp"
 #include "constraint/TestableIntersection.hpp"
 #include "constraint/uniform/RnBoxConstraint.hpp"
+#include "constraint/uniform/SE2BoxConstraint.hpp"
 #include "constraint/uniform/SO2UniformSampler.hpp"
-#include "constraint/uniform/SO3UniformSampler.hpp"
+#include "constraint/uniform/SO3UniformSampler.hpp"

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

@@ -8,6 +8,7 @@
 #include "../../statespace/dart/WeldJoint.hpp"
 #include "../../util/metaprogramming.hpp"
 #include "../uniform/RnBoxConstraint.hpp"
+#include "../uniform/SE2BoxConstraint.hpp"
 #include "../uniform/SO2UniformSampler.hpp"
 #include "../uniform/SO3UniformSampler.hpp"
 #include "../uniform/RnConstantSampler.hpp"
@@ -90,12 +91,16 @@ std::unique_ptr<OutputConstraint> createBoxConstraint(
   const auto joint = _stateSpace->getJoint();
 
   if (isLimited(joint))
+  {
     return dart::common::make_unique<RBoxConstraint<N>>(
       std::move(_stateSpace), std::move(_rng),
       getPositionLowerLimits(joint), getPositionUpperLimits(joint));
+  }
   else
+  {
     return dart::common::make_unique<Satisfied>(
       std::move(_stateSpace));
+  }
 }
 
 //=============================================================================
@@ -301,6 +306,27 @@ struct createSampleableFor_impl<statespace::dart::SO3Joint>
 };
 
 //=============================================================================
+template <class OutputConstraint>
+std::unique_ptr<OutputConstraint> createBoxConstraint(
+  std::shared_ptr<statespace::dart::SE2Joint> _stateSpace,
+  std::unique_ptr<util::RNG> _rng)
+{
+  const auto joint = _stateSpace->getJoint();
+
+  if (isLimited(joint))
+  {
+    return dart::common::make_unique<SE2BoxConstraint>(
+      std::move(_stateSpace), std::move(_rng),
+      getPositionLowerLimits(joint).tail<2>(),
+      getPositionUpperLimits(joint).tail<2>());
+  }
+  else
+  {
+    return dart::common::make_unique<Satisfied>(
+      std::move(_stateSpace));
+  }
+}
+
 template <>
 struct createDifferentiableFor_impl<statespace::dart::SE2Joint>
 {
@@ -321,10 +347,17 @@ struct createTestableFor_impl<statespace::dart::SE2Joint>
   using StateSpace = statespace::dart::SE2Joint;
   using StateSpacePtr = std::shared_ptr<StateSpace>;
 
-  static std::unique_ptr<Testable> create(StateSpacePtr /*_stateSpace*/)
+  static std::unique_ptr<Testable> create(StateSpacePtr _stateSpace)
   {
-    throw std::runtime_error(
-      "No Testable is available for SE2Joint.");
+    auto joint = _stateSpace->getJoint();
+    if (joint->hasPositionLimit(0))
+    {
+      throw std::invalid_argument(
+        "Rotational component of SE2Joint must not have limits.");
+    }
+
+    return createBoxConstraint<Testable>(
+      std::move(_stateSpace), nullptr);
   }
 };
 
@@ -335,10 +368,17 @@ struct createProjectableFor_impl<statespace::dart::SE2Joint>
   using StateSpace = statespace::dart::SE2Joint;
   using StateSpacePtr = std::shared_ptr<StateSpace>;
 
-  static std::unique_ptr<Projectable> create(StateSpacePtr /*_stateSpace*/)
+  static std::unique_ptr<Projectable> create(StateSpacePtr _stateSpace)
   {
-    throw std::runtime_error(
-      "No Projectable is available for SE2Joint.");
+    auto joint = _stateSpace->getJoint();
+    if (joint->hasPositionLimit(0))
+    {
+      throw std::invalid_argument(
+        "Rotational component of SE2Joint must not have limits.");
+    }
+
+    return createBoxConstraint<Projectable>(
+      std::move(_stateSpace), nullptr);
   }
 };
 
@@ -350,10 +390,26 @@ struct createSampleableFor_impl<statespace::dart::SE2Joint>
   using StateSpacePtr = std::shared_ptr<StateSpace>;
 
   static std::unique_ptr<Sampleable> create(
-    StateSpacePtr /*_stateSpace*/, std::unique_ptr<util::RNG> /*_rng*/)
+    StateSpacePtr stateSpace, std::unique_ptr<util::RNG> rng)
   {
-    throw std::runtime_error(
-      "No Sampleable is available for SE2Joint.");
+    auto joint = stateSpace->getJoint();
+    if (joint->hasPositionLimit(0))
+    {
+      throw std::invalid_argument(
+        "Rotational component of SE2Joint must not have limits.");
+    }
+    else if (!(joint->hasPositionLimit(1) && joint->hasPositionLimit(2)))
+    {
+      throw std::runtime_error(
+        "Unable to create Sampleable for unbounded SE2.");
+    }
+    else
+    {
+      return dart::common::make_unique<SE2BoxConstraint>(
+        std::move(stateSpace), std::move(rng),
+        getPositionLowerLimits(joint).tail<2>(),
+        getPositionUpperLimits(joint).tail<2>());
+    }
   }
 };
 

include/aikido/constraint/uniform/SE2BoxConstraint.hpp

@@ -0,0 +1,78 @@
+#ifndef AIKIDO_STATESPACE_SE2BOXCONSTRAINT_HPP_
+#define AIKIDO_STATESPACE_SE2BOXCONSTRAINT_HPP_
+
+#include "../../statespace/SE2.hpp"
+#include "../Differentiable.hpp"
+#include "../Projectable.hpp"
+#include "../Sampleable.hpp"
+#include "../Testable.hpp"
+
+namespace aikido {
+namespace constraint {
+
+/// A BoxConstraint on SE2.
+/// This class does *not* allow constraint on rotation.
+/// For each dimension x and y, this constraint has lowerLimit and upperLimit.
+class SE2BoxConstraint
+  : public constraint::Projectable
+  , public constraint::Sampleable
+  , public constraint::Testable
+{
+public:
+  using constraint::Projectable::project;
+
+  /// Constructor.
+  /// \param _space Space in which this constraint operates.
+  /// \param _rng Random number generator to be used for sampling.
+  /// \param _lowerLimits Lower limits on the state, only on x and y.
+  /// \param _upperLimits Upper limits on the state, only on x and y.
+  SE2BoxConstraint(
+    std::shared_ptr<statespace::SE2> _space,
+    std::unique_ptr<util::RNG> _rng,
+    const Eigen::Vector2d& _lowerLimits,
+    const Eigen::Vector2d& _upperLimits);
+
+  // Documentation inherited.
+  statespace::StateSpacePtr getStateSpace() const override;
+
+  // Documentation inherited.
+  bool isSatisfied(const statespace::StateSpace::State* state) const override;
+
+  // Documentation inherited.
+  bool project(
+    const statespace::StateSpace::State* _s,
+    statespace::StateSpace::State* _out) const override;
+
+  // Documentation inherited.
+  std::unique_ptr<constraint::SampleGenerator>
+    createSampleGenerator() const override;
+
+  /// Returns lower limits of this constraint.
+  Eigen::Vector2d getLowerLimits() const;
+
+  /// Returns upper limits of this constraint.
+  Eigen::Vector2d getUpperLimits() const;
+
+private:
+  std::shared_ptr<statespace::SE2> mSpace;
+  std::unique_ptr<util::RNG> mRng;
+
+  /// Lower limits on the state. The first element encodes the rotational limit
+  /// and the last two elements encode the translational limits.
+  Eigen::Vector3d mLowerLimits;
+
+  /// Upper limits on the state. The first element encodes the rotational limit
+  /// and the last two elements encode the translational limits.
+  Eigen::Vector3d mUpperLimits;
+
+  // DOFs of joint that have limits, in this case translational DOFs.
+  // TODO: Confirm this with Gilwoo
+  size_t mRnDimension;
+  // DOF of the joint
+  size_t mDimension;
+};
+
+} // namespace constraint
+} // namespace aikido
+
+#endif // AIKIDO_STATESPACE_SE2BOXCONSTRAINT_HPP_

include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp

@@ -145,8 +145,8 @@ RBoxConstraint<N>::RBoxConstraint(std::shared_ptr<statespace::R<N>> _space,
     if (mLowerLimits[i] > mUpperLimits[i])
     {
       std::stringstream msg;
-      msg << "Unable to sample from StateSpace because lower limit exeeds"
-             " upper limit on dimension " << i << ": "
+      msg << "Unable to sample from StateSpace because lower limit exceeds"
+          << " upper limit on dimension " << i << ": "
           << mLowerLimits[i] << " > " << mUpperLimits[i] << ".";
       throw std::invalid_argument(msg.str());
     }

include/aikido/distance/detail/defaults-impl.hpp

@@ -1,10 +1,13 @@
 #include <dart/common/StlHelpers.hpp>
+
 #include "../../statespace/CartesianProduct.hpp"
 #include "../../statespace/Rn.hpp"
+#include "../../statespace/SE2.hpp"
 #include "../../statespace/SO2.hpp"
 #include "../../statespace/SO3.hpp"
 #include "../../util/metaprogramming.hpp"
 #include "../RnEuclidean.hpp"
+#include "../SE2.hpp"
 #include "../SO2Angular.hpp"
 #include "../SO3Angular.hpp"
 #include "../Weighted.hpp"
@@ -20,6 +23,7 @@ using Ptr = std::unique_ptr<DistanceMetric>;
 template <class Space>
 struct createDistanceMetricFor_impl
 {
+  // Nothing defined
 };
 
 //=============================================================================
@@ -116,6 +120,16 @@ struct createDistanceMetricFor_impl<statespace::CartesianProduct>
   }
 };
 
+//=============================================================================
+template <>
+struct createDistanceMetricFor_impl<statespace::SE2>
+{
+  static Ptr create(std::shared_ptr<statespace::SE2> _sspace)
+  {
+    return make_unique<SE2>(std::move(_sspace));
+  }
+};
+
 //=============================================================================
 using SupportedStateSpaces = util::type_list<statespace::CartesianProduct,
                                              statespace::R0,
@@ -124,7 +138,8 @@ using SupportedStateSpaces = util::type_list<statespace::CartesianProduct,
                                              statespace::R3,
                                              statespace::R6,
                                              statespace::SO2,
-                                             statespace::SO3>;
+                                             statespace::SO3,
+                                             statespace::SE2>;
 
 } // namespace detail
 

src/constraint/CMakeLists.txt

@@ -3,6 +3,7 @@ set(sources
   uniform/RnConstantSampler.cpp
   uniform/SO2UniformSampler.cpp
   uniform/SO3UniformSampler.cpp
+  uniform/SE2BoxConstraint.cpp
   CartesianProductProjectable.cpp
   CartesianProductSampleable.cpp
   CartesianProductTestable.cpp