Commit 7dd6ee86 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[transition test] adapt isDynamicallyReachable to the new API

parent 62948e3b
......@@ -706,7 +706,7 @@ module hpp
boolean isReachableFromState(in unsigned short stateFrom, in unsigned short stateTo)raises (Error);
short isDynamicallyReachableFromState(in unsigned short stateFrom, in unsigned short stateTo,in floatSeq timings,in double T)raises (Error);
short isDynamicallyReachableFromState(in unsigned short stateFrom, in unsigned short stateTo,in floatSeq timings,in unsigned short numPointsPerPhase)raises (Error);
}; // interface Robot
......
......@@ -1095,12 +1095,6 @@ class FullBody (object):
def isReachableFromState(self,stateFrom,stateTo):
return self.client.rbprm.rbprm.isReachableFromState(stateFrom,stateTo)
def isDynamicallyReachableFromState(self,stateFrom,stateTo,timings=[],T=0):
if T>0 and len(timings) != 3: # timings vector must be of size 3
if len(timings) > 3:
timings = timings[:3]
else:
while len(timings) < 3:
timings +=[0]
return self.client.rbprm.rbprm.isDynamicallyReachableFromState(stateFrom,stateTo,timings,T)
def isDynamicallyReachableFromState(self,stateFrom,stateTo,timings=[],numPointsPerPhases=3):
return self.client.rbprm.rbprm.isDynamicallyReachableFromState(stateFrom,stateTo,timings,numPointsPerPhases)
......@@ -3086,7 +3086,7 @@ assert(s2 == s1 +1);
CORBA::Short RbprmBuilder::isDynamicallyReachableFromState(unsigned short stateFrom,unsigned short stateTo,const hpp::floatSeq &timings, double T )throw (hpp::Error){
CORBA::Short RbprmBuilder::isDynamicallyReachableFromState(unsigned short stateFrom, unsigned short stateTo, const hpp::floatSeq &timings, unsigned short numPointPerPhase )throw (hpp::Error){
if(!fullBodyLoaded_){
throw std::runtime_error ("fullBody not loaded");
}
......@@ -3094,14 +3094,15 @@ assert(s2 == s1 +1);
throw std::runtime_error ("Unexisting state ID");
}
reachability::Result res;
if(T>0){
if(timings.length()>0){
std::vector<double> Ts;
Configuration_t t_config = dofArrayToConfig(3,timings);
for(size_t i = 0 ; i < 3 ; ++i)
Configuration_t t_config = dofArrayToConfig(timings.length(),timings);
for(size_t i = 0 ; i < timings.length() ; ++i)
Ts.push_back(t_config[i]);
res = reachability::isReachableDynamic(fullBody(),lastStatesComputed_[stateFrom],lastStatesComputed_[stateTo],Ts,T);
res = reachability::isReachableDynamic(fullBody(),lastStatesComputed_[stateFrom],lastStatesComputed_[stateTo],false,Ts,numPointPerPhase);
}else{
res = reachability::isReachableDynamic(fullBody(),lastStatesComputed_[stateFrom],lastStatesComputed_[stateTo]);
res = reachability::isReachableDynamic(fullBody(),lastStatesComputed_[stateFrom],lastStatesComputed_[stateTo],false);
}
if (res.success()){
core::PathVectorPtr_t pathVector = core::PathVector::create(res.path_->outputSize(),res.path_->outputDerivativeSize());
......
......@@ -346,7 +346,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 bool isReachableFromState(unsigned short stateFrom,unsigned short stateTo)throw (hpp::Error);
virtual CORBA::Short isDynamicallyReachableFromState(unsigned short stateFrom,unsigned short stateTo,const hpp::floatSeq &timings, double T )throw (hpp::Error);
virtual CORBA::Short isDynamicallyReachableFromState(unsigned short stateFrom, unsigned short stateTo, const hpp::floatSeq &timings, unsigned short numPointPerPhase )throw (hpp::Error);
void selectFullBody (const char* name) throw (hpp::Error)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment