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 ...@@ -681,8 +681,9 @@ module hpp
/// \param config configuration tested on the robot /// \param config configuration tested on the robot
/// \param contacts name of the limbs in contact /// \param contacts name of the limbs in contact
/// \param robustnessTreshold robustness treshold used /// \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 /// \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 /// run and store an analysis on all limb databases
/// \param analysis name of the analysis existing if analysis ="all", /// \param analysis name of the analysis existing if analysis ="all",
......
...@@ -757,8 +757,8 @@ class FullBody (object): ...@@ -757,8 +757,8 @@ class FullBody (object):
# #
# \param stepSize discretization step # \param stepSize discretization step
# \param pathId Id of the path to compute from # \param pathId Id of the path to compute from
def isConfigBalanced(self, config, names, robustness = 0): def isConfigBalanced(self, config, names, robustness = 0,CoM = [0,0,0]):
if (self.client.rbprm.rbprm.isConfigBalanced(config, names, robustness) == 1): if (self.client.rbprm.rbprm.isConfigBalanced(config, names, robustness,CoM) == 1):
return True return True
else: else:
return False return False
......
...@@ -3007,12 +3007,13 @@ assert(s2 == s1 +1); ...@@ -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{ try{
rbprm::State testedState; rbprm::State testedState;
model::Configuration_t config = dofArrayToConfig (fullBody()->device_, configuration); model::Configuration_t config = dofArrayToConfig (fullBody()->device_, configuration);
model::Configuration_t save = fullBody()->device_->currentConfiguration(); 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); std::vector<std::string> names = stringConversion(contactLimbs);
fullBody()->device_->currentConfiguration(config); fullBody()->device_->currentConfiguration(config);
fullBody()->device_->computeForwardKinematics(); fullBody()->device_->computeForwardKinematics();
...@@ -3030,7 +3031,7 @@ assert(s2 == s1 +1); ...@@ -3030,7 +3031,7 @@ assert(s2 == s1 +1);
} }
fullBody()->device_->currentConfiguration(save); fullBody()->device_->currentConfiguration(save);
fullBody()->device_->computeForwardKinematics(); 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); return (CORBA::Short)(1);
} }
......
...@@ -346,7 +346,7 @@ namespace hpp { ...@@ -346,7 +346,7 @@ namespace hpp {
virtual CORBA::Short computeIntermediary(unsigned short state1, unsigned short state2) throw (hpp::Error); 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::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 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 double isStateBalanced(unsigned short stateId) throw (hpp::Error);
virtual void runSampleAnalysis(const char* analysis, double isstatic) 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); 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