Add satisfiable tolerance parameter to TSR constructor

include/aikido/constraint/TSR.hpp

@@ -44,7 +44,8 @@ class TSR : public Sampleable,
       const Eigen::Isometry3d& _T0_w = Eigen::Isometry3d::Identity(),
       const Eigen::Matrix<double, 6, 2>& _Bw =
           Eigen::Matrix<double, 6, 2>::Zero(),
-      const Eigen::Isometry3d& _Tw_e = Eigen::Isometry3d::Identity());
+      const Eigen::Isometry3d& _Tw_e = Eigen::Isometry3d::Identity(),
+      const double _satisfiableTolerance = 1e-6);
 
 
   /// Constructor with default random seed generator.
@@ -57,7 +58,8 @@ class TSR : public Sampleable,
   TSR(const Eigen::Isometry3d& _T0_w = Eigen::Isometry3d::Identity(),
       const Eigen::Matrix<double, 6, 2>& _Bw =
           Eigen::Matrix<double, 6, 2>::Zero(),
-      const Eigen::Isometry3d& _Tw_e = Eigen::Isometry3d::Identity());
+      const Eigen::Isometry3d& _Tw_e = Eigen::Isometry3d::Identity(),
+      const double _satisfiableTolerance = 1e-6);
 
   TSR(const TSR& other);
   TSR(TSR&& other);
@@ -112,6 +114,14 @@ class TSR : public Sampleable,
   bool project(const statespace::StateSpace::State* _s,
       statespace::StateSpace::State* _out) const override;
 
+  /// Get the staisfiable tolerance for isSatisfiable.
+  /// \param[out] _out Satisfialble tolerance, double.
+  double getSatisfiableTolerance();
+
+  /// Set the staisfiable tolerance for isSatisfiable.
+  /// \param _satisfiableTolerance Satisfiable tolerance to set.
+  void setSatisfiableTolerance(const double _satisfiableTolerance);
+
   /// Transformation from origin frame into the TSR frame "w".
   /// "w" is usually centered at the origin of an object held by the hand
   ///  or at a location on an object that is useful for grasping.
@@ -124,7 +134,10 @@ class TSR : public Sampleable,
   /// This often represent an offset from "w" to the origin of the end-effector.
   Eigen::Isometry3d mTw_e;
 
+
 private:
+  /// Tolerance for checking satisfiability
+  double mSatisfiableTolerance;
   std::unique_ptr<util::RNG> mRng;
   std::shared_ptr<statespace::SE3> mStateSpace;
 };

src/constraint/TSR.cpp

@@ -79,10 +79,12 @@ class TSRSampleGenerator : public SampleGenerator
 
 //=============================================================================
 TSR::TSR(std::unique_ptr<util::RNG> _rng, const Eigen::Isometry3d& _T0_w,
-         const Eigen::Matrix<double, 6, 2>& _Bw, const Eigen::Isometry3d& _Tw_e)
+         const Eigen::Matrix<double, 6, 2>& _Bw, const Eigen::Isometry3d& _Tw_e,
+         const double _satisfiableTolerance)
     : mT0_w(_T0_w)
     , mBw(_Bw)
     , mTw_e(_Tw_e)
+    , mSatisfiableTolerance(_satisfiableTolerance)
     , mRng(std::move(_rng))
     , mStateSpace(std::make_shared<SE3>())
 {
@@ -91,10 +93,11 @@ TSR::TSR(std::unique_ptr<util::RNG> _rng, const Eigen::Isometry3d& _T0_w,
 
 //=============================================================================
 TSR::TSR(const Eigen::Isometry3d& _T0_w, const Eigen::Matrix<double, 6, 2>& _Bw,
-         const Eigen::Isometry3d& _Tw_e)
+         const Eigen::Isometry3d& _Tw_e, const double _satisfiableTolerance)
     : mT0_w(_T0_w)
     , mBw(_Bw)
     , mTw_e(_Tw_e)
+    , mSatisfiableTolerance(_satisfiableTolerance)
     , mRng(std::unique_ptr<util::RNG>(
           new util::RNGWrapper<std::default_random_engine>(0)))
     , mStateSpace(std::make_shared<SE3>())
@@ -107,6 +110,7 @@ TSR::TSR(const TSR& other)
     : mT0_w(other.mT0_w)
     , mBw(other.mBw)
     , mTw_e(other.mTw_e)
+    , mSatisfiableTolerance(other.mSatisfiableTolerance)
     , mRng(std::move(other.mRng->clone()))
     , mStateSpace(std::make_shared<SE3>())
 {
@@ -118,6 +122,7 @@ TSR::TSR(TSR&& other)
     : mT0_w(other.mT0_w)
     , mBw(other.mBw)
     , mTw_e(other.mTw_e)
+    , mSatisfiableTolerance(other.mSatisfiableTolerance)
     , mRng(std::move(other.mRng))
     , mStateSpace(std::make_shared<SE3>())
 {
@@ -131,6 +136,7 @@ TSR& TSR::operator=(const TSR& other)
   mT0_w = other.mT0_w;
   mBw = other.mBw;
   mTw_e = other.mTw_e;
+  mSatisfiableTolerance = other.mSatisfiableTolerance;
   mRng = std::move(other.mRng->clone());
 
   // Intentionally don't assign StateSpace.
@@ -144,6 +150,7 @@ TSR& TSR::operator=(TSR&& other)
   mT0_w = std::move(other.mT0_w);
   mBw = std::move(other.mBw);
   mTw_e = std::move(other.mTw_e);
+  mSatisfiableTolerance = other.mSatisfiableTolerance;
   mRng = std::move(other.mRng);
   mStateSpace = std::move(other.mStateSpace);
 
@@ -190,10 +197,9 @@ std::unique_ptr<SampleGenerator> TSR::createSampleGenerator() const
 //=============================================================================
 bool TSR::isSatisfied(const statespace::StateSpace::State* _s) const
 {
-  static constexpr double eps = 1e-6;
   Eigen::VectorXd dist;
   getValue(_s, dist);
-  return dist.norm() < eps;
+  return dist.norm() < mSatisfiableTolerance;
 }
 
 //=============================================================================
@@ -409,6 +415,17 @@ bool TSRSampleGenerator::sample(statespace::StateSpace::State* _state)
   return true;
 }
 
+//=============================================================================
+double TSR::getSatisfiableTolerance()
+{
+  return mSatisfiableTolerance;
+}
+
+//=============================================================================
+void TSR::setSatisfiableTolerance(const double _satisfiableTolerance)
+{
+  mSatisfiableTolerance = _satisfiableTolerance;
+}
 
 //=============================================================================
 bool TSRSampleGenerator::canSample() const
@@ -431,6 +448,7 @@ int TSRSampleGenerator::getNumSamples() const
 
   return NO_LIMIT;
 }
+
 } // namespace constraint
 } // namespace aikido