Add satisfiable tolerance parameter to TSR constructor (#180)

* add satisfiable tolerance parameter to TSR constructor

* address JS's comments

* address JS's comment

* address JS's comment

* address Gilwoo's comments

* address Mike's comments

include/aikido/constraint/TSR.hpp

@@ -40,11 +40,13 @@ class TSR : public Sampleable,
   ///        bound rotation following Roll-Pitch-Yaw convention.
   ///        _Bw(i, 0) should be less than or equal to _Bw(i, 1).
   /// \param _Tw_e end-effector offset transform in the coordinates of w
+  /// \param _testableTolerance tolerance used in isSatisfiable as testable
   TSR(std::unique_ptr<util::RNG> _rng,
       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(),
+      double _testableTolerance = 1e-6);
 
 
   /// Constructor with default random seed generator.
@@ -54,10 +56,12 @@ class TSR : public Sampleable,
   ///        bound rotation following Roll-Pitch-Yaw convention.
   ///        _Bw(i, 0) should be less than or equal to _Bw(i, 1).
   /// \param _Tw_e end-effector offset transform in the coordinates of w
+  /// \param _testableTolerance tolerance used in isSatisfiable as testable
   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(),
+      double _testableTolerance = 1e-6);
 
   TSR(const TSR& other);
   TSR(TSR&& other);
@@ -112,6 +116,14 @@ class TSR : public Sampleable,
   bool project(const statespace::StateSpace::State* _s,
       statespace::StateSpace::State* _out) const override;
 
+  /// Get the testable tolerance used in isSatisfiable.
+  /// \param[out] _out Testable tolerance, double.
+  double getTestableTolerance();
+
+  /// Set the testable tolerance used in isSatisfiable.
+  /// \param _testableTolerance Testable tolerance to set.
+  void setTestableTolerance(double _testableTolerance);
+
   /// 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.
@@ -125,6 +137,8 @@ class TSR : public Sampleable,
   Eigen::Isometry3d mTw_e;
 
 private:
+  /// Tolerance used in isSatisfied as a testable
+  double mTestableTolerance;
   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,
+         double _testableTolerance)
     : mT0_w(_T0_w)
     , mBw(_Bw)
     , mTw_e(_Tw_e)
+    , mTestableTolerance(_testableTolerance)
     , 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, double _testableTolerance)
     : mT0_w(_T0_w)
     , mBw(_Bw)
     , mTw_e(_Tw_e)
+    , mTestableTolerance(_testableTolerance)
     , 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)
+    , mTestableTolerance(other.mTestableTolerance)
     , 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)
+    , mTestableTolerance(other.mTestableTolerance)
     , 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;
+  mTestableTolerance = other.mTestableTolerance;
   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);
+  mTestableTolerance = other.mTestableTolerance;
   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() < mTestableTolerance;
 }
 
 //=============================================================================
@@ -409,6 +415,17 @@ bool TSRSampleGenerator::sample(statespace::StateSpace::State* _state)
   return true;
 }
 
+//=============================================================================
+double TSR::getTestableTolerance()
+{
+  return mTestableTolerance;
+}
+
+//=============================================================================
+void TSR::setTestableTolerance(double _testableTolerance)
+{
+  mTestableTolerance = _testableTolerance;
+}
 
 //=============================================================================
 bool TSRSampleGenerator::canSample() const
@@ -431,6 +448,7 @@ int TSRSampleGenerator::getNumSamples() const
 
   return NO_LIMIT;
 }
+
 } // namespace constraint
 } // namespace aikido