add boolean testReachability and quasiStatic to the python API of interpolate

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -343,7 +343,9 @@ module hpp
     /// \param path path computed.
     /// \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
     /// \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
-    floatSeqSeq interpolate(in double timestep, in double path, in double robustnessTreshold, in unsigned short filterStates) raises (Error);
+    /// \param testReachability : if true, check each contact transition with our reachability criterion
+    /// \param quasiStatic : if True, use our reachability criterion with the quasiStatic constraint
+    floatSeqSeq interpolate(in double timestep, in double path, in double robustnessTreshold, in unsigned short filterStates, in boolean testReachability, in boolean quasiStatic) raises (Error);
 
     /// Provided a path has already been computed, interpolates it and generates the statically stable
     /// constact configurations along it. setStartState and setEndState must have been called prior
@@ -352,7 +354,9 @@ module hpp
     /// \param path path computed.
     /// \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
     /// \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
-    floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold, in unsigned short filterStates) raises (Error);
+    /// \param testReachability : if true, check each contact transition with our reachability criterion
+    /// \param quasiStatic : if True, use our reachability criterion with the quasiStatic constraint
+    floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold, in unsigned short filterStates,in boolean testReachability, in boolean quasiStatic) raises (Error);
 
 
     /// returns the CWC of the robot at a given state

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -354,12 +354,14 @@ class FullBody (object):
      # \param pathId Id of the path to compute from
      # \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
      # \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
-     def interpolate(self, stepsize, pathId = 1, robustnessTreshold = 0, filterStates = False):
+     # \param testReachability : if true, check each contact transition with our reachability criterion
+     # \param quasiStatic : if True, use our reachability criterion with the quasiStatic constraint
+     def interpolate(self, stepsize, pathId = 1, robustnessTreshold = 0, filterStates = False,testReachability = True, quasiStatic = False):
           if(filterStates):
                 filt = 1
           else:
                 filt = 0
-          return self.client.rbprm.rbprm.interpolate(stepsize, pathId, robustnessTreshold, filt)
+          return self.client.rbprm.rbprm.interpolate(stepsize, pathId, robustnessTreshold, filt,testReachability, quasiStatic)
      
      ## Provided a discrete contact sequence has already been computed, computes
      # all the contact positions and normals for a given state, the next one, and the intermediate between them.
@@ -426,12 +428,14 @@ class FullBody (object):
      # \param pathId Id of the path to compute from
      # \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
      # \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
-     def interpolateConfigs(self, configs, robustnessTreshold = 0, filterStates = False):
+     # \param testReachability : if true, check each contact transition with our reachability criterion
+     # \param quasiStatic : if True, use our reachability criterion with the quasiStatic constraint
+     def interpolateConfigs(self, configs, robustnessTreshold = 0, filterStates = False, testReachability = True, quasiStatic = False):
           if(filterStates):
                 filt = 1
           else:
                 filt = 0
-          return self.client.rbprm.rbprm.interpolateConfigs(configs, robustnessTreshold, filt)
+          return self.client.rbprm.rbprm.interpolateConfigs(configs, robustnessTreshold, filt,testReachability, quasiStatic)
           
      ##
      # 

src/rbprmbuilder.impl.cc

@@ -1402,7 +1402,7 @@ namespace hpp {
         return res;
     }
 
-    floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error)
+    floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic) throw (hpp::Error)
     {
         try
         {
@@ -1421,7 +1421,7 @@ namespace hpp {
             {
                 throw hpp::Error ("No affordances found. Unable to interpolate.");
             }
-            hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = rbprm::interpolation::RbPrmInterpolation::create(fullBody(),startState_,endState_);
+            hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = rbprm::interpolation::RbPrmInterpolation::create(fullBody(),startState_,endState_,core::PathVectorConstPtr_t(),testReachability,quasiStatic);
             lastStatesComputedTime_ = interpolator->Interpolate(affMap, bindShooter_.affFilter_,configurations,robustnessTreshold, filterStates != 0);
             lastStatesComputed_ = TimeStatesToStates(lastStatesComputedTime_);
             hpp::floatSeqSeq *res;
@@ -2093,7 +2093,7 @@ namespace hpp {
     }
 
 
-    floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error)
+    floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic) throw (hpp::Error)
     {
         hppDout(notice,"### Begin interpolate");
         try
@@ -2120,7 +2120,7 @@ namespace hpp {
         }
 
         hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = 
-                    rbprm::interpolation::RbPrmInterpolation::create(fullBody(),startState_,endState_,problemSolver()->paths()[pathId]);
+                    rbprm::interpolation::RbPrmInterpolation::create(fullBody(),startState_,endState_,problemSolver()->paths()[pathId],testReachability,quasiStatic);
         lastStatesComputedTime_ = interpolator->Interpolate(affMap, bindShooter_.affFilter_,
                     timestep,robustnessTreshold, filterStates != 0);
 		lastStatesComputed_ = TimeStatesToStates(lastStatesComputedTime_);

src/rbprmbuilder.impl.hh

@@ -260,8 +260,8 @@ namespace hpp {
         virtual hpp::floatSeqSeq* computeContactPointsForLimb(unsigned short cId, const char* limbName) throw (hpp::Error);
         virtual hpp::floatSeqSeq* computeContactPointsAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char* limbName) throw (hpp::Error);
         virtual hpp::floatSeqSeq* computeCenterOfContactAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char *limbName) throw (hpp::Error);
-        virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error);
-        virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error);
+        virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic) throw (hpp::Error);
+        virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic) throw (hpp::Error);
         virtual hpp::floatSeqSeq* getContactCone(unsigned short stateId, double friction) throw (hpp::Error);
         virtual hpp::floatSeqSeq* getContactIntermediateCone(unsigned short stateId, double friction) throw (hpp::Error);
         virtual CORBA::Short generateComTraj(const hpp::floatSeqSeq& positions, const hpp::floatSeqSeq& velocities,