[stability] isConfigBalanced take an optional CoM position as argument

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -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",

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -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

src/rbprmbuilder.impl.cc

@@ -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);
         }

src/rbprmbuilder.impl.hh

@@ -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);