Adding WeldJoint and ConstantSampler (#146)

* WeldJoint

* Removing empty lines.

* Changing header name

* Adding to CMakeList

* Remove unnecessary operations in WeldJoint

* Add prefix 'Rn-' to ConstantSampler

* Add unit tests for WeldJoint and RnConstantSampler

* Remove unnecessary static variables

* Add RnConstantSampler::getConstantValue()

* Return const reference

include/aikido/constraint/Sampleable.hpp

@@ -59,7 +59,6 @@ class SampleGenerator {
 
 using SampleablePtr = std::shared_ptr<Sampleable>;
 
-
 } // namespace constraint
 } // namespace aikido
 

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

@@ -5,10 +5,12 @@
 #include "../../statespace/dart/SO3Joint.hpp"
 #include "../../statespace/dart/SE2Joint.hpp"
 #include "../../statespace/dart/SE3Joint.hpp"
+#include "../../statespace/dart/WeldJoint.hpp"
 #include "../../util/metaprogramming.hpp"
 #include "../uniform/RnBoxConstraint.hpp"
 #include "../uniform/SO2UniformSampler.hpp"
 #include "../uniform/SO3UniformSampler.hpp"
+#include "../uniform/RnConstantSampler.hpp"
 #include "../Satisfied.hpp"
 
 namespace aikido {
@@ -58,7 +60,8 @@ using JointStateSpaceTypeList = util::type_list<
   statespace::dart::SO2Joint,
   statespace::dart::SO3Joint,
   statespace::dart::SE2Joint,
-  statespace::dart::SE3Joint
+  statespace::dart::SE3Joint,
+  statespace::dart::WeldJoint
 >;
 
 //=============================================================================
@@ -91,6 +94,7 @@ std::unique_ptr<OutputConstraint> createBoxConstraint(
       std::move(_stateSpace));
 }
 
+//=============================================================================
 template <>
 struct createDifferentiableFor_impl<statespace::dart::RnJoint>
 {
@@ -103,6 +107,7 @@ struct createDifferentiableFor_impl<statespace::dart::RnJoint>
   }
 };
 
+//=============================================================================
 template <>
 struct createTestableFor_impl<statespace::dart::RnJoint>
 {
@@ -116,6 +121,7 @@ struct createTestableFor_impl<statespace::dart::RnJoint>
   }
 };
 
+//=============================================================================
 template <>
 struct createProjectableFor_impl<statespace::dart::RnJoint>
 {
@@ -129,6 +135,7 @@ struct createProjectableFor_impl<statespace::dart::RnJoint>
   }
 };
 
+//=============================================================================
 template <>
 struct createSampleableFor_impl<statespace::dart::RnJoint>
 {
@@ -167,6 +174,7 @@ struct createDifferentiableFor_impl<statespace::dart::SO2Joint>
   }
 };
 
+//=============================================================================
 template <>
 struct createTestableFor_impl<statespace::dart::SO2Joint>
 {
@@ -183,6 +191,7 @@ struct createTestableFor_impl<statespace::dart::SO2Joint>
   }
 };
 
+//=============================================================================
 template <>
 struct createProjectableFor_impl<statespace::dart::SO2Joint>
 {
@@ -199,6 +208,7 @@ struct createProjectableFor_impl<statespace::dart::SO2Joint>
   }
 };
 
+//=============================================================================
 template <>
 struct createSampleableFor_impl<statespace::dart::SO2Joint>
 {
@@ -234,6 +244,7 @@ struct createDifferentiableFor_impl<statespace::dart::SO3Joint>
   }
 };
 
+//=============================================================================
 template <>
 struct createTestableFor_impl<statespace::dart::SO3Joint>
 {
@@ -250,6 +261,7 @@ struct createTestableFor_impl<statespace::dart::SO3Joint>
   }
 };
 
+//=============================================================================
 template <>
 struct createProjectableFor_impl<statespace::dart::SO3Joint>
 {
@@ -266,6 +278,7 @@ struct createProjectableFor_impl<statespace::dart::SO3Joint>
   }
 };
 
+//=============================================================================
 template <>
 struct createSampleableFor_impl<statespace::dart::SO3Joint>
 {
@@ -297,6 +310,7 @@ struct createDifferentiableFor_impl<statespace::dart::SE2Joint>
   }
 };
 
+//=============================================================================
 template <>
 struct createTestableFor_impl<statespace::dart::SE2Joint>
 {
@@ -310,6 +324,7 @@ struct createTestableFor_impl<statespace::dart::SE2Joint>
   }
 };
 
+//=============================================================================
 template <>
 struct createProjectableFor_impl<statespace::dart::SE2Joint>
 {
@@ -323,6 +338,7 @@ struct createProjectableFor_impl<statespace::dart::SE2Joint>
   }
 };
 
+//=============================================================================
 template <>
 struct createSampleableFor_impl<statespace::dart::SE2Joint>
 {
@@ -351,6 +367,7 @@ struct createDifferentiableFor_impl<statespace::dart::SE3Joint>
   }
 };
 
+//=============================================================================
 template <>
 struct createTestableFor_impl<statespace::dart::SE3Joint>
 {
@@ -364,6 +381,7 @@ struct createTestableFor_impl<statespace::dart::SE3Joint>
   }
 };
 
+//=============================================================================
 template <>
 struct createProjectableFor_impl<statespace::dart::SE3Joint>
 {
@@ -377,6 +395,7 @@ struct createProjectableFor_impl<statespace::dart::SE3Joint>
   }
 };
 
+//=============================================================================
 template <>
 struct createSampleableFor_impl<statespace::dart::SE3Joint>
 {
@@ -391,6 +410,72 @@ struct createSampleableFor_impl<statespace::dart::SE3Joint>
   }
 };
 
+//=============================================================================
+template <class OutputConstraint>
+std::unique_ptr<OutputConstraint> createBoxConstraint(
+  std::shared_ptr<statespace::dart::WeldJoint> _stateSpace,
+  std::unique_ptr<util::RNG> /*_rng*/)
+{
+  return dart::common::make_unique<Satisfied>(std::move(_stateSpace));
+}
+
+//=============================================================================
+template <>
+struct createDifferentiableFor_impl<statespace::dart::WeldJoint>
+{
+  using StateSpace = statespace::dart::WeldJoint;
+  using StateSpacePtr = std::shared_ptr<StateSpace>;
+
+  static std::unique_ptr<Differentiable> create(StateSpacePtr _stateSpace)
+  {
+    return createBoxConstraint<Differentiable>(std::move(_stateSpace), nullptr);
+  }
+};
+
+//=============================================================================
+template <>
+struct createTestableFor_impl<statespace::dart::WeldJoint>
+{
+  using StateSpace = statespace::dart::WeldJoint;
+  using StateSpacePtr = std::shared_ptr<StateSpace>;
+
+  static std::unique_ptr<Testable> create(StateSpacePtr _stateSpace)
+  {
+    return createBoxConstraint<Testable>(std::move(_stateSpace), nullptr);
+  }
+};
+
+//=============================================================================
+template <>
+struct createProjectableFor_impl<statespace::dart::WeldJoint>
+{
+  using StateSpace = statespace::dart::WeldJoint;
+  using StateSpacePtr = std::shared_ptr<StateSpace>;
+
+  static std::unique_ptr<Projectable> create(StateSpacePtr _stateSpace)
+  {
+    return createBoxConstraint<Projectable>(std::move(_stateSpace), nullptr);
+  }
+};
+
+//=============================================================================
+template <>
+struct createSampleableFor_impl<statespace::dart::WeldJoint>
+{
+  using StateSpace = statespace::dart::WeldJoint;
+  using StateSpacePtr = std::shared_ptr<StateSpace>;
+
+  static std::unique_ptr<Sampleable> create(
+    StateSpacePtr _stateSpace, std::unique_ptr<util::RNG> /*_rng*/)
+  {
+    const auto joint = _stateSpace->getJoint();
+    Eigen::VectorXd positions = joint->getPositions();
+
+    return dart::common::make_unique<RnConstantSampler>(
+        std::move(_stateSpace), positions);
+  }
+};
+
 } // namespace detail
 
 //=============================================================================

include/aikido/constraint/uniform/RnBoxConstraint.hpp

@@ -65,11 +65,11 @@ class RnBoxConstraint
   std::unique_ptr<constraint::SampleGenerator>
     createSampleGenerator() const override;
 
-  // Returns lower limits of this constraint.
-  Eigen::VectorXd getLowerLimits();
+  /// Returns lower limits of this constraint.
+  const Eigen::VectorXd& getLowerLimits() const;
 
-  // Returns upper limits of this constraint.
-  Eigen::VectorXd getUpperLimits();
+  /// Returns upper limits of this constraint.
+  const Eigen::VectorXd& getUpperLimits() const;
 
 private:
   std::shared_ptr<statespace::Rn> mSpace;

include/aikido/constraint/uniform/RnConstantSampler.hpp

@@ -0,0 +1,39 @@
+#ifndef AIKIDO_CONSTRAINT_RNCONSTANTSAMPLER_HPP_
+#define AIKIDO_CONSTRAINT_RNCONSTANTSAMPLER_HPP_
+#include "../../statespace/Rn.hpp"
+#include "../Sampleable.hpp"
+
+namespace aikido {
+namespace constraint {
+
+/// ConstantSampler for RealVectorStates.
+/// Stub sampler for WeldJoint or any fixed constant state space.
+class RnConstantSampler : public constraint::Sampleable
+{
+public:
+  /// 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);
+
+  // Documentation inherited.
+  statespace::StateSpacePtr getStateSpace() const override;
+
+  // Documentation inherited.
+  std::unique_ptr<constraint::SampleGenerator>
+    createSampleGenerator() const override;
+
+  /// Returns constant value to be generated by this sampler.
+  const Eigen::VectorXd& getConstantValue() const;
+
+private:
+  std::shared_ptr<statespace::Rn> mSpace;
+  Eigen::VectorXd mValue;
+};
+
+} // namespace constraint
+} // namespace aikido
+
+#endif // AIKIDO_CONSTRAINT_RNCONSTANTSAMPLER_HPP_

include/aikido/statespace/dart/WeldJoint.hpp

@@ -0,0 +1,40 @@
+#ifndef AIKIDO_STATESPACE_DART_WELDJOINTSTATESPACE_HPP_
+#define AIKIDO_STATESPACE_DART_WELDJOINTSTATESPACE_HPP_
+#include "../Rn.hpp"
+#include "JointStateSpace.hpp"
+
+namespace aikido {
+namespace statespace {
+namespace dart {
+
+/// \c Weld for DART \c WeldJoint. This class treats the
+/// joint's positions as a real vector space.
+class WeldJoint
+  : public Rn
+  , public JointStateSpace
+  , public std::enable_shared_from_this<WeldJoint>
+{
+public:
+  using Rn::State;
+
+  /// Create a real vector state space for \c _joint.
+  ///
+  /// \param _joint joint to create a state space for
+  explicit WeldJoint(::dart::dynamics::WeldJoint* _joint);
+
+  // Documentation inherited.
+  void convertPositionsToState(
+    const Eigen::VectorXd& _positions,
+    StateSpace::State* _state) const override;
+
+  // Documentation inherited.
+  void convertStateToPositions(
+    const StateSpace::State* _state,
+    Eigen::VectorXd& _positions) const override;
+};
+
+} // namespace dart
+} // namespace statespace
+} // namespace aikido
+
+#endif // ifndef AIKIDO_STATESPACE_WELDJOINTSTATESPACE_HPP_

include/aikido/statespace/dart/detail/JointStateSpaceHelpers-impl.hpp

@@ -5,6 +5,7 @@
 #include "../SO3Joint.hpp"
 #include "../SE2Joint.hpp"
 #include "../SE3Joint.hpp"
+#include "../WeldJoint.hpp"
 #include "../../../util/metaprogramming.hpp"
 
 namespace aikido {
@@ -82,16 +83,26 @@ struct createJointStateSpaceFor_impl<::dart::dynamics::FreeJoint>
   }
 };
 
+//=============================================================================
+template <>
+struct createJointStateSpaceFor_impl<::dart::dynamics::WeldJoint>
+{
+  static Ptr create(::dart::dynamics::WeldJoint* _joint)
+  {
+    return make_unique<WeldJoint>(_joint);
+  }
+};
+
 //=============================================================================
 using SupportedJoints = util::type_list<
   ::dart::dynamics::BallJoint,
   ::dart::dynamics::FreeJoint,
   ::dart::dynamics::PlanarJoint,
   ::dart::dynamics::PrismaticJoint,
   ::dart::dynamics::RevoluteJoint,
-  ::dart::dynamics::TranslationalJoint
+  ::dart::dynamics::TranslationalJoint,
+  ::dart::dynamics::WeldJoint
   // TODO: Support ScrewJoint.
-  // TODO: Support WeldJoint.
   // TODO: Support UniversalJoint.
   // TODO: Support EulerJoint.
 >;

src/constraint/CMakeLists.txt

@@ -1,5 +1,6 @@
 set(sources
   uniform/RnBoxConstraint.cpp
+  uniform/RnConstantSampler.cpp
   uniform/SO2UniformSampler.cpp
   uniform/SO3UniformSampler.cpp
   CartesianProductProjectable.cpp

src/constraint/uniform/RnBoxConstraint.cpp

@@ -7,8 +7,7 @@ namespace constraint {
 using constraint::ConstraintType;
 
 //=============================================================================
-class RnBoxConstraintSampleGenerator
-  : public constraint::SampleGenerator
+class RnBoxConstraintSampleGenerator : public constraint::SampleGenerator
 {
 public:
   statespace::StateSpacePtr getStateSpace() const override;
@@ -253,13 +252,13 @@ std::unique_ptr<constraint::SampleGenerator>
 }
 
 //=============================================================================
-Eigen::VectorXd RnBoxConstraint::getLowerLimits()
+const Eigen::VectorXd& RnBoxConstraint::getLowerLimits() const
 {
   return mLowerLimits;
 }
 
 //=============================================================================
-Eigen::VectorXd RnBoxConstraint::getUpperLimits()
+const Eigen::VectorXd& RnBoxConstraint::getUpperLimits() const
 {
   return mUpperLimits;
 }