diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl index 7b3d7a1a02b4dbb6fb01178dcc79d464fa7f414f..b35d0f0446890058774b8c8fe3854157311d24de 100755 --- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl +++ b/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 diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py index 0294aeba56113993efc7ca2fae0026dddfa0b769..a2717947bd93414082f22c7500a0149d11f09322 100755 --- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py +++ b/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) ## # diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc index 09cafae4a5cd00aba96ccee9c5087381a8ea582b..2c545ff81431278bc6548d87cbdb2245b5a5027d 100755 --- a/src/rbprmbuilder.impl.cc +++ b/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_); diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh index d7575a68243bfcb295545d9f1dba48c351238658..47da979582f5ac587d3e7bf2ae9d43b6c72d1101 100755 --- a/src/rbprmbuilder.impl.hh +++ b/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,