Commit cee1b0e1 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[transition test] add method 'isReachableFromState' in python

parent 0dd01228
......@@ -695,6 +695,8 @@ module hpp
boolean isKinematicsConstraintsVerified(in floatSeq point)raises (Error);
boolean isReachableFromState(in unsigned short stateFrom, in unsigned short stateTo)raises (Error);
}; // interface Robot
}; // module rbprm
}; // module corbaserver
......
......@@ -1068,7 +1068,11 @@ class FullBody (object):
def getAllLimbsInContact(self,stateId):
return self.getLimbsInContact(self.getAllLimbsNames(),stateId)
## check the kinematics constraints for the given point
## check the kinematics constraints for the given point, assuming all contact are established
# \param point : test if the point is inside the kinematics constraints for the current configuration
def isKinematicsConstraintsVerified(self,point):
return self.client.rbprm.rbprm.isKinematicsConstraintsVerified(point)
def isReachableFromState(self,stateFrom,stateTo):
return self.client.rbprm.rbprm.isReachableFromState(stateFrom,stateTo)
......@@ -50,7 +50,7 @@
#include <hpp/rbprm/planner/random-shortcut-dynamic.hh>
#include <hpp/rbprm/planner/oriented-path-optimizer.hh>
#include <hpp/rbprm/sampling/heuristic-tools.hh>
#include <hpp/rbprm/contact_generation/kinematics_constraints.hh>
#include <hpp/rbprm/contact_generation/reachability.hh>
#ifdef PROFILE
#include "hpp/rbprm/rbprm-profiler.hh"
#endif
......@@ -3016,6 +3016,18 @@ assert(s2 == s1 +1);
return success;
}
bool RbprmBuilder::isReachableFromState(unsigned short stateFrom,unsigned short stateTo)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]);
return (res.status==reachability::REACHABLE);
}
} // namespace impl
} // namespace rbprm
......
......@@ -339,6 +339,8 @@ namespace hpp {
virtual hpp::floatSeqSeqSeq* getEffectorTrajectoryWaypoints(unsigned short pathId,const char* effectorName)throw (hpp::Error);
virtual bool isKinematicsConstraintsVerified(const hpp::floatSeq &point)throw (hpp::Error);
virtual bool isReachableFromState(unsigned short stateFrom,unsigned short stateTo)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