[reachability] update isReachableFromState to new API in rbprm

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);
 

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.]

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;

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);