Commit 60a2399a authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[transition check] add python method to test if a point is inside the...

[transition check] add python method to test if a point is inside the kinematics constraints for the current configuration
parent d1a12d1c
......@@ -340,11 +340,11 @@ install(FILES
)
install(FILES
data/com_inequalities/LLEG_JOINT0_com_constraints.obj
#data/com_inequalities/LLEG_JOINT0_com_constraints.obj
data/com_inequalities/RLEG_JOINT0_com_constraints.obj
data/com_inequalities/LLEG_JOINT0_com_constraints.dae
#data/com_inequalities/LLEG_JOINT0_com_constraints.dae
data/com_inequalities/RLEG_JOINT0_com_constraints.dae
data/com_inequalities/LLEG_LINK5_Kinematics.dae
#data/com_inequalities/LLEG_LINK5_Kinematics.dae
data/com_inequalities/RLEG_LINK5_Kinematics.dae
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/com_inequalities
)
......
......@@ -687,6 +687,8 @@ 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);
}; // interface Robot
}; // module rbprm
}; // module corbaserver
......
......@@ -1051,3 +1051,8 @@ class FullBody (object):
# \param stateId : index of the state
def getAllLimbsInContact(self,stateId):
return self.getLimbsInContact(self.getAllLimbsNames(),stateId)
## check the kinematics constraints for the given point
# \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)
......@@ -50,6 +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>
#ifdef PROFILE
#include "hpp/rbprm/rbprm-profiler.hh"
#endif
......@@ -2957,6 +2958,8 @@ assert(s2 == s1 +1);
}
Names_t* RbprmBuilder::getAllLimbsNames()throw (hpp::Error)
{
if(!fullBodyLoaded_){
......@@ -2973,6 +2976,29 @@ assert(s2 == s1 +1);
return limbsNames;
}
bool RbprmBuilder::isKinematicsConstraintsVerified(const hpp::floatSeq &point)throw (hpp::Error){
if(!fullBodyLoaded_){
throw std::runtime_error ("fullBody not loaded");
}
Configuration_t pt_config = dofArrayToConfig(3,point);
fcl::Vec3f pt(pt_config[0],pt_config[1],pt_config[2]);
bool success(true);
bool successLimb;
hppDout(notice,"Test kinematic constraint for point : "<<pt);
for(CIT_Limb lit = fullBody()->GetLimbs().begin() ; lit != fullBody()->GetLimbs().end() ; ++lit){
hppDout(notice,"for limb : "<<lit->first);
if(lit->second->kinematicConstraints_.first.size()==0){
hppDout(notice,"Kinematics constraints not initialized");
}else{
successLimb = verifyKinematicConstraints(lit->second->kinematicConstraints_,lit->second->effector_->currentTransformation(),pt);
hppDout(notice,"kinematic constraints verified : "<<successLimb);
success = success && successLimb;
}
}
return success;
}
} // namespace impl
} // namespace rbprm
} // namespace hpp
......@@ -337,6 +337,7 @@ 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);
void selectFullBody (const char* name) throw (hpp::Error)
{
......
Markdown is supported
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