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

[stability] isConfigBalanced take an optional CoM position as argument

parent 5d5af2e9
......@@ -681,8 +681,9 @@ module hpp
/// \param config configuration tested on the robot
/// \param contacts name of the limbs in contact
/// \param robustnessTreshold robustness treshold used
/// \param CoM optional, if specified use this CoM position instead of the one computed from the configuration
/// \return whether the configuration is quasi-statically balanced
short isConfigBalanced(in floatSeq config, in Names_t contacts, in double robustnessTreshold) raises (Error);
short isConfigBalanced(in floatSeq config, in Names_t contacts, in double robustnessTreshold,in floatSeq CoM) raises (Error);
/// run and store an analysis on all limb databases
/// \param analysis name of the analysis existing if analysis ="all",
......
......@@ -757,8 +757,8 @@ class FullBody (object):
#
# \param stepSize discretization step
# \param pathId Id of the path to compute from
def isConfigBalanced(self, config, names, robustness = 0):
if (self.client.rbprm.rbprm.isConfigBalanced(config, names, robustness) == 1):
def isConfigBalanced(self, config, names, robustness = 0,CoM = [0,0,0]):
if (self.client.rbprm.rbprm.isConfigBalanced(config, names, robustness,CoM) == 1):
return True
else:
return False
......
......@@ -3007,12 +3007,13 @@ assert(s2 == s1 +1);
}
}
CORBA::Short RbprmBuilder::isConfigBalanced(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs, double robustnessTreshold) throw (hpp::Error)
CORBA::Short RbprmBuilder::isConfigBalanced(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs, double robustnessTreshold, const hpp::floatSeq &CoM) throw (hpp::Error)
{
try{
rbprm::State testedState;
model::Configuration_t config = dofArrayToConfig (fullBody()->device_, configuration);
model::Configuration_t save = fullBody()->device_->currentConfiguration();
fcl::Vec3f comFCL((double)CoM[(_CORBA_ULong)0],(double)CoM[(_CORBA_ULong)1],(double)CoM[(_CORBA_ULong)2]);
std::vector<std::string> names = stringConversion(contactLimbs);
fullBody()->device_->currentConfiguration(config);
fullBody()->device_->computeForwardKinematics();
......@@ -3030,7 +3031,7 @@ assert(s2 == s1 +1);
}
fullBody()->device_->currentConfiguration(save);
fullBody()->device_->computeForwardKinematics();
if (stability::IsStable(fullBody(), testedState) >= robustnessTreshold)
if (stability::IsStable(fullBody(), testedState,fcl::Vec3f(0,0,0),comFCL) >= robustnessTreshold)
{
return (CORBA::Short)(1);
}
......
......@@ -346,7 +346,7 @@ namespace hpp {
virtual CORBA::Short computeIntermediary(unsigned short state1, unsigned short state2) throw (hpp::Error);
virtual hpp::floatSeqSeq* getOctreeBoxes(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
virtual hpp::floatSeq* getOctreeTransform(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
virtual CORBA::Short isConfigBalanced(const hpp::floatSeq& config, const hpp::Names_t& contactLimbs, double robustnessTreshold) throw (hpp::Error);
virtual CORBA::Short isConfigBalanced(const hpp::floatSeq& config, const hpp::Names_t& contactLimbs, double robustnessTreshold,const hpp::floatSeq& CoM) throw (hpp::Error);
virtual double isStateBalanced(unsigned short stateId) throw (hpp::Error);
virtual void runSampleAnalysis(const char* analysis, double isstatic) throw (hpp::Error);
virtual hpp::floatSeq* runLimbSampleAnalysis(const char* limbname, const char* analysis, double isstatic) 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