diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl index 5945293c06e80bf89980648c63c284b7c61146dd..3c0dfae555065c4a6340cb7ac5017091135fd744 100644 --- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl +++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl @@ -792,7 +792,7 @@ module hpp boolean areKinematicsConstraintsVerifiedForState(in unsigned short stateFrom,in floatSeq point)raises (Error); - floatSeq isReachableFromState(in unsigned short stateFrom, in unsigned short stateTo)raises (Error); + floatSeq isReachableFromState(in unsigned short stateFrom, in unsigned short stateTo, in boolean useIntermediateState)raises (Error); floatSeq isDynamicallyReachableFromState(in unsigned short stateFrom, in unsigned short stateTo,in boolean addPathPerPhase, in floatSeq timings,in short numPointsPerPhase)raises (Error); diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py index f41c5f9e80553877068eec764ba1eec98a479f3d..fe5d85891bcbdf08e3e09998df7981e60e3cb569 100755 --- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py +++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py @@ -970,8 +970,8 @@ class FullBody (Robot): def areKinematicsConstraintsVerifiedForState(self,stateFrom, point): return self.clientRbprm.rbprm.areKinematicsConstraintsVerifiedForState(stateFrom,point) - def isReachableFromState(self,stateFrom,stateTo,computePoint=False): - raw = self.clientRbprm.rbprm.isReachableFromState(stateFrom,stateTo) + def isReachableFromState(self,stateFrom,stateTo,computePoint=False,useIntermediateState=True): + raw = self.clientRbprm.rbprm.isReachableFromState(stateFrom,stateTo,useIntermediateState) if computePoint : res = [] res += [raw[0]>0.] diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc index 437675cb1818035d24c4c3a30287855c1daea835..0a714deb9b092720e011bd68d3ed63a1e0cd8acc 100644 --- a/src/rbprmbuilder.impl.cc +++ b/src/rbprmbuilder.impl.cc @@ -3403,14 +3403,14 @@ namespace hpp { } - hpp::floatSeq* RbprmBuilder::isReachableFromState(unsigned short stateFrom,unsigned short stateTo)throw (hpp::Error){ + hpp::floatSeq* RbprmBuilder::isReachableFromState(unsigned short stateFrom, unsigned short stateTo , const bool useIntermediateState)throw (hpp::Error){ if(!fullBodyLoaded_){ throw std::runtime_error ("fullBody not loaded"); } if(stateTo >= lastStatesComputed_.size() || stateFrom >= lastStatesComputed_.size()){ throw std::runtime_error ("Unexisting state ID"); } - reachability::Result res = reachability::isReachable(fullBody(),lastStatesComputed_[stateFrom],lastStatesComputed_[stateTo]); + reachability::Result res = reachability::isReachable(fullBody(),lastStatesComputed_[stateFrom],lastStatesComputed_[stateTo], fcl::Vec3f::Zero(),useIntermediateState); // convert vector of int to floatSeq : _CORBA_ULong size; diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh index 76afa208ce6fd65a2a5765c6329f158895dd1329..1d5a837017e5fbfffdfc8b87cf38b211a35c5978 100644 --- a/src/rbprmbuilder.impl.hh +++ b/src/rbprmbuilder.impl.hh @@ -359,7 +359,7 @@ namespace hpp { virtual bool areKinematicsConstraintsVerified(const hpp::floatSeq &point)throw (hpp::Error); virtual bool areKinematicsConstraintsVerifiedForState(unsigned short stateId,const hpp::floatSeq &point)throw (hpp::Error); - virtual hpp::floatSeq *isReachableFromState(unsigned short stateFrom,unsigned short stateTo)throw (hpp::Error); + virtual hpp::floatSeq *isReachableFromState(unsigned short stateFrom,unsigned short stateTo,const bool useIntermediateState)throw (hpp::Error); virtual hpp::floatSeq* isDynamicallyReachableFromState(unsigned short stateFrom, unsigned short stateTo, bool addPathPerPhase, const hpp::floatSeq &timings, short numPointPerPhase )throw (hpp::Error);