From a327ca57338dc1708e3dc1e8c2dea5a0dbc69900 Mon Sep 17 00:00:00 2001
From: pFernbach <pierre.fernbach@gmail.com>
Date: Fri, 31 May 2019 20:04:18 +0200
Subject: [PATCH] [reachability] update isReachableFromState to new API in
rbprm
---
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl | 2 +-
src/hpp/corbaserver/rbprm/rbprmfullbody.py | 4 ++--
src/rbprmbuilder.impl.cc | 4 ++--
src/rbprmbuilder.impl.hh | 2 +-
4 files changed, 6 insertions(+), 6 deletions(-)
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 5945293c..3c0dfae5 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 f41c5f9e..fe5d8589 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 437675cb..0a714deb 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 76afa208..1d5a8370 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);
--
GitLab