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

[transition test] add method to test kinematics constraints from a state in python

parent cee1b0e1
......@@ -693,7 +693,9 @@ module hpp
/// Throw an error if there is no trajectory computed for the given id/name
floatSeqSeqSeq getEffectorTrajectoryWaypoints(in unsigned short pathId,in string effectorName) raises (Error);
boolean isKinematicsConstraintsVerified(in floatSeq point)raises (Error);
boolean areKinematicsConstraintsVerified(in floatSeq point)raises (Error);
boolean areKinematicsConstraintsVerifiedForState(in unsigned short stateFrom,in floatSeq point)raises (Error);
boolean isReachableFromState(in unsigned short stateFrom, in unsigned short stateTo)raises (Error);
......
......@@ -1070,8 +1070,13 @@ class FullBody (object):
## 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 areKinematicsConstraintsVerified(self,point):
return self.client.rbprm.rbprm.areKinematicsConstraintsVerified(point)
def areKinematicsConstraintsVerifiedForState(self,stateFrom, point):
return self.client.rbprm.rbprm.areKinematicsConstraintsVerifiedForState(stateFrom,point)
def isReachableFromState(self,stateFrom,stateTo):
return self.client.rbprm.rbprm.isReachableFromState(stateFrom,stateTo)
......
......@@ -2994,7 +2994,7 @@ assert(s2 == s1 +1);
return limbsNames;
}
bool RbprmBuilder::isKinematicsConstraintsVerified(const hpp::floatSeq &point)throw (hpp::Error){
bool RbprmBuilder::areKinematicsConstraintsVerified(const hpp::floatSeq &point)throw (hpp::Error){
if(!fullBodyLoaded_){
throw std::runtime_error ("fullBody not loaded");
}
......@@ -3016,6 +3016,19 @@ assert(s2 == s1 +1);
return success;
}
bool RbprmBuilder::areKinematicsConstraintsVerifiedForState(unsigned short stateId,const hpp::floatSeq &point)throw (hpp::Error){
if(!fullBodyLoaded_){
throw std::runtime_error ("fullBody not loaded");
}
if(stateId >= lastStatesComputed_.size()){
throw std::runtime_error ("Unexisting state ID");
}
Configuration_t pt_config = dofArrayToConfig(3,point);
fcl::Vec3f pt(pt_config[0],pt_config[1],pt_config[2]);
return reachability::verifyKinematicConstraints(fullBody(),lastStatesComputed_[stateId],pt);
}
bool RbprmBuilder::isReachableFromState(unsigned short stateFrom,unsigned short stateTo)throw (hpp::Error){
if(!fullBodyLoaded_){
throw std::runtime_error ("fullBody not loaded");
......
......@@ -338,7 +338,8 @@ namespace hpp {
virtual Names_t* getEffectorsTrajectoriesNames(unsigned short pathId)throw (hpp::Error);
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 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);
......
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