can assert blance of a configuration given contacts in pyhton

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -206,6 +206,13 @@ module hpp
     /// \return size 7 position + quaternion of the octree root
     floatSeq getOctreeTransform(in string limbname, in floatSeq dofArray) raises (Error);
 
+    /// returns octree transform for a given robot configuration
+    /// \param config configuration tested on the robot
+    /// \param contacts name of the limbs in contact
+    /// \param robustnessTreshold robustness treshold used
+    /// \return whether the configuration is quasi-statically balanced
+    short isConfigBalanced(in floatSeq config, in Names_t contacts,  in double robustnessTreshold) raises (Error);
+
   }; // interface Robot
   }; // module rbprm
   }; // module corbaserver

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -186,6 +186,17 @@ class FullBody (object):
     def interpolateConfigs(self, configs):
 		return self.client.rbprm.rbprm.interpolateConfigs(configs)
 		
+	## Given start and goal states
+	#  generate a contact sequence over a list of configurations
+	#
+    # \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):
+			return True
+		else:
+			return False
+		
 	## Create octree nodes representation for a given limb
 	#
     # \param stepSize discretization step

src/rbprmbuilder.impl.cc

@@ -624,6 +624,43 @@ namespace hpp {
         }
     }
 
+    CORBA::Short RbprmBuilder::isConfigBalanced(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs, double robustnessTreshold) throw (hpp::Error)
+    {
+        try{
+        rbprm::State testedState;
+        model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration);
+        model::Configuration_t save = fullBody_->device_->currentConfiguration();
+        std::vector<std::string> names = stringConversion(contactLimbs);
+        for(std::vector<std::string>::const_iterator cit = names.begin(); cit != names.end();++cit)
+        {
+            std::cout << "name " << * cit << std::endl;
+            const hpp::rbprm::RbPrmLimbPtr_t limb =fullBody_->GetLimbs().at(std::string(*cit));
+            testedState.contacts_[*cit] = true;
+            testedState.contactPositions_[*cit] = limb->effector_->currentTransformation().getTranslation();
+            testedState.contactRotation_[*cit] = limb->effector_->currentTransformation().getRotation();
+            // normal given by effector normal
+            const fcl::Vec3f normal = limb->effector_->currentTransformation().getRotation() * limb->normal_;
+            testedState.contactNormals_[*cit] = normal;
+            testedState.configuration_ = config;
+            ++testedState.nbContacts;
+        }
+        fullBody_->device_->currentConfiguration(save);
+        fullBody_->device_->computeForwardKinematics();
+        if (stability::IsStable(fullBody_, testedState) >= robustnessTreshold)
+        {
+            return (CORBA::Short)(1);
+        }
+        else
+        {
+            return (CORBA::Short)(0);
+        }
+        }
+        catch(std::runtime_error& e)
+        {
+            throw Error(e.what());
+        }
+    }
+
     void RbprmBuilder::SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver)
     {
         problemSolver_ = problemSolver;

src/rbprmbuilder.impl.hh

@@ -120,6 +120,7 @@ namespace hpp {
         virtual void saveComputedStates(const char* filepath) 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);
         public:
         void SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver);