Template Rn for dimension to exploit performance of fixed-size Eigen …

…objects (#159)

.travis.yml

@@ -38,3 +38,7 @@ script:
 
 after_script:
   - ./scripts/view-all-results.sh test_results
+
+after_failure: 
+  - cat ./build/aikido/Testing/Temporary/LastTest.log
+  - cat ./build/aikido/Testing/Temporary/LastTestsFailed.log

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

@@ -56,7 +56,11 @@ inline Eigen::VectorXd getPositionUpperLimits(
 
 //=============================================================================
 using JointStateSpaceTypeList = util::type_list<
-  statespace::dart::RnJoint,
+  statespace::dart::R0Joint,
+  statespace::dart::R1Joint,
+  statespace::dart::R2Joint,
+  statespace::dart::R3Joint,
+  statespace::dart::R6Joint,
   statespace::dart::SO2Joint,
   statespace::dart::SO3Joint,
   statespace::dart::SE2Joint,
@@ -78,15 +82,15 @@ template <class T>
 struct createSampleableFor_impl { };
 
 //=============================================================================
-template <class OutputConstraint>
+template <int N, class OutputConstraint>
 std::unique_ptr<OutputConstraint> createBoxConstraint(
-  std::shared_ptr<statespace::dart::RnJoint> _stateSpace,
+  std::shared_ptr<statespace::dart::RJoint<N>> _stateSpace,
   std::unique_ptr<util::RNG> _rng)
 {
   const auto joint = _stateSpace->getJoint();
 
   if (isLimited(joint))
-    return dart::common::make_unique<RnBoxConstraint>(
+    return dart::common::make_unique<RBoxConstraint<N>>(
       std::move(_stateSpace), std::move(_rng),
       getPositionLowerLimits(joint), getPositionUpperLimits(joint));
   else
@@ -95,51 +99,51 @@ std::unique_ptr<OutputConstraint> createBoxConstraint(
 }
 
 //=============================================================================
-template <>
-struct createDifferentiableFor_impl<statespace::dart::RnJoint>
+template <int N>
+struct createDifferentiableFor_impl<statespace::dart::RJoint<N>>
 {
-  using StateSpace = statespace::dart::RnJoint;
+  using StateSpace = statespace::dart::RJoint<N>;
   using StateSpacePtr = std::shared_ptr<StateSpace>;
 
   static std::unique_ptr<Differentiable> create(StateSpacePtr _stateSpace)
   {
-    return createBoxConstraint<Differentiable>(std::move(_stateSpace), nullptr);
+    return createBoxConstraint<N, Differentiable>(std::move(_stateSpace), nullptr);
   }
 };
 
 //=============================================================================
-template <>
-struct createTestableFor_impl<statespace::dart::RnJoint>
+template <int N>
+struct createTestableFor_impl<statespace::dart::RJoint<N>>
 {
-  using StateSpace = statespace::dart::RnJoint;
+  using StateSpace = statespace::dart::RJoint<N>;
   using StateSpacePtr = std::shared_ptr<StateSpace>;
 
   static std::unique_ptr<Testable> create(StateSpacePtr _stateSpace)
   {
-    return createBoxConstraint<Testable>(
+    return createBoxConstraint<N, Testable>(
       std::move(_stateSpace), nullptr);
   }
 };
 
 //=============================================================================
-template <>
-struct createProjectableFor_impl<statespace::dart::RnJoint>
+template <int N>
+struct createProjectableFor_impl<statespace::dart::RJoint<N>>
 {
-  using StateSpace = statespace::dart::RnJoint;
+  using StateSpace = statespace::dart::RJoint<N>;
   using StateSpacePtr = std::shared_ptr<StateSpace>;
 
   static std::unique_ptr<Projectable> create(StateSpacePtr _stateSpace)
   {
-    return createBoxConstraint<Projectable>(
+    return createBoxConstraint<N, Projectable>(
       std::move(_stateSpace), nullptr);
   }
 };
 
 //=============================================================================
-template <>
-struct createSampleableFor_impl<statespace::dart::RnJoint>
+template <int N>
+struct createSampleableFor_impl<statespace::dart::RJoint<N>>
 {
-  using StateSpace = statespace::dart::RnJoint;
+  using StateSpace = statespace::dart::RJoint<N>;
   using StateSpacePtr = std::shared_ptr<StateSpace>;
 
   static std::unique_ptr<Sampleable> create(
@@ -148,7 +152,7 @@ struct createSampleableFor_impl<statespace::dart::RnJoint>
     const auto joint = _stateSpace->getJoint();
 
     if (isLimited(joint))
-      return dart::common::make_unique<RnBoxConstraint>(
+      return dart::common::make_unique<RBoxConstraint<N>>(
         std::move(_stateSpace), std::move(_rng),
         getPositionLowerLimits(joint), getPositionUpperLimits(joint));
     else
@@ -471,7 +475,7 @@ struct createSampleableFor_impl<statespace::dart::WeldJoint>
     const auto joint = _stateSpace->getJoint();
     Eigen::VectorXd positions = joint->getPositions();
 
-    return dart::common::make_unique<RnConstantSampler>(
+    return dart::common::make_unique<R0ConstantSampler>(
         std::move(_stateSpace), positions);
   }
 };

include/aikido/constraint/uniform/RnBoxConstraint.hpp

@@ -11,7 +11,8 @@ namespace constraint {
 
 /// A BoxConstraint on RealVectorStates.
 /// For each dimension, this constraint has lowerLimit and upperLimit. 
-class RnBoxConstraint
+template <int N>
+class RBoxConstraint
   : public constraint::Differentiable
   , public constraint::Projectable
   , public constraint::Sampleable
@@ -21,18 +22,20 @@ class RnBoxConstraint
   using constraint::Projectable::project;
   using constraint::Differentiable::getValueAndJacobian;
 
+  using Vectord = Eigen::Matrix<double, N, 1>;
+
   /// 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 states.
   ///        The length of this vector should match the dimension of _space.
   /// \param _upperLimits Upper limits.
   ///        The length of this vector should match the dimension of _space. 
-  RnBoxConstraint(
-    std::shared_ptr<statespace::Rn> _space,
+  RBoxConstraint(
+    std::shared_ptr<statespace::R<N>> _space,
     std::unique_ptr<util::RNG> _rng,
-    const Eigen::VectorXd& _lowerLimits,
-    const Eigen::VectorXd& _upperLimits);
+    const Vectord& _lowerLimits,
+    const Vectord& _upperLimits);
 
   // Documentation inherited.
   statespace::StateSpacePtr getStateSpace() const override;
@@ -66,20 +69,28 @@ class RnBoxConstraint
     createSampleGenerator() const override;
 
   /// Returns lower limits of this constraint.
-  const Eigen::VectorXd& getLowerLimits() const;
+  auto getLowerLimits() const -> const Vectord&;
 
   /// Returns upper limits of this constraint.
-  const Eigen::VectorXd& getUpperLimits() const;
+  auto getUpperLimits() const -> const Vectord&;
 
 private:
-  std::shared_ptr<statespace::Rn> mSpace;
+  std::shared_ptr<statespace::R<N>> mSpace;
   std::unique_ptr<util::RNG> mRng;
-  Eigen::VectorXd mLowerLimits;
-  Eigen::VectorXd mUpperLimits;
+  Vectord mLowerLimits;
+  Vectord mUpperLimits;
 };
 
+using R0BoxConstraint = RBoxConstraint<0>;
+using R1BoxConstraint = RBoxConstraint<1>;
+using R2BoxConstraint = RBoxConstraint<2>;
+using R3BoxConstraint = RBoxConstraint<3>;
+using R6BoxConstraint = RBoxConstraint<6>;
+using RnBoxConstraint = RBoxConstraint<Eigen::Dynamic>;
 
 } // namespace constraint
 } // namespace aikido
 
+#include "aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp"
+
 #endif // AIKIDO_STATESPACE_REALVECTORSTATESPACESAMPLEABLECONSTRAINT_H_

include/aikido/constraint/uniform/RnConstantSampler.hpp

@@ -8,15 +8,17 @@ namespace constraint {
 
 /// ConstantSampler for RealVectorStates.
 /// Stub sampler for WeldJoint or any fixed constant state space.
-class RnConstantSampler : public constraint::Sampleable
+template <int N>
+class RConstantSampler : public constraint::Sampleable
 {
 public:
+  using Vectord = Eigen::Matrix<double, N, 1>;
+
   /// Constructor.
   /// \param _space Space in which this constraint operates.
   /// \param _value Value to return when sampled.
-  RnConstantSampler(
-    std::shared_ptr<statespace::Rn> _space,
-    const Eigen::VectorXd& _value);
+  RConstantSampler(
+    std::shared_ptr<statespace::R<N>> _space, const Vectord& _value);
 
   // Documentation inherited.
   statespace::StateSpacePtr getStateSpace() const override;
@@ -26,14 +28,23 @@ class RnConstantSampler : public constraint::Sampleable
     createSampleGenerator() const override;
 
   /// Returns constant value to be generated by this sampler.
-  const Eigen::VectorXd& getConstantValue() const;
+  const Vectord& getConstantValue() const;
 
 private:
-  std::shared_ptr<statespace::Rn> mSpace;
-  Eigen::VectorXd mValue;
+  std::shared_ptr<statespace::R<N>> mSpace;
+  Vectord mValue;
 };
 
+using R0ConstantSampler = RConstantSampler<0>;
+using R1ConstantSampler = RConstantSampler<1>;
+using R2ConstantSampler = RConstantSampler<2>;
+using R3ConstantSampler = RConstantSampler<3>;
+using R6ConstantSampler = RConstantSampler<6>;
+using RnConstantSampler = RConstantSampler<Eigen::Dynamic>;
+
 } // namespace constraint
 } // namespace aikido
 
+#include "aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp"
+
 #endif // AIKIDO_CONSTRAINT_RNCONSTANTSAMPLER_HPP_