[python api] add state.projectToRoot method

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -148,25 +148,32 @@ module hpp
     /// [z_inf, z_sup, y_inf, y_sup, x_inf, x_sup]
     void boundSO3(in floatSeq limitszyx) raises (Error);
 
-		/// Project a state into a COM
-		///
-		/// \param stateId target state
-		/// \param com target com
-                double projectStateToCOM(in unsigned short stateId,  in floatSeq com, in  unsigned short max_num_sample) raises (Error);
+    /// Project a state into a COM
+    ///
+    /// \param stateId target state
+    /// \param com target com
+    double projectStateToCOM(in unsigned short stateId,  in floatSeq com, in  unsigned short max_num_sample) raises (Error);
 
-                /// Project a state into a COM
-                ///
-                /// \param stateId target state
-                /// \param com target com
-                double setConfigAtState(in unsigned short stateId,  in floatSeq config) raises (Error);
+    /// Project a state into a given root position and orientation
+    ///
+    /// \param stateId target state
+    /// \param root the root configuration (size 7)
+    double projectStateToRoot(in unsigned short stateId,  in floatSeq root) raises (Error);
 
 
-		/// Create a state and push it to the state array
-		///
-		/// \param q configuration
-		/// \param names list of effectors in contact
-		/// \return stateId
-		short createState(in floatSeq configuration, in Names_t contactLimbs) raises (Error);
+    /// Project a state into a COM
+    ///
+    /// \param stateId target state
+    /// \param com target com
+    double setConfigAtState(in unsigned short stateId,  in floatSeq config) raises (Error);
+
+
+    /// Create a state and push it to the state array
+    ///
+    /// \param q configuration
+    /// \param names list of effectors in contact
+    /// \return stateId
+    short createState(in floatSeq configuration, in Names_t contactLimbs) raises (Error);
 
     /// Get Sample configuration by its id
     /// \param sampleName name of the limb from which to retrieve a sample

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -734,6 +734,14 @@ class FullBody (Robot):
      # \return whether the projection was successful
      def projectStateToCOM(self, state, targetCom, maxNumSample = 0):
           return self.clientRbprm.rbprm.projectStateToCOM(state, targetCom, maxNumSample)     > 0
+
+     ## Project a given state into a given root position
+     # and update the state configuration.
+     # \param state index of first state.
+     # \param root : root configuration (size 7)
+     # \return whether the projection was successful
+     def projectStateToRoot(self, state, root):
+          return self.clientRbprm.rbprm.projectStateToRoot(state, root)     > 0
           
      ## Project a given state into a given COM position
      # and update the state configuration.

src/hpp/corbaserver/rbprm/rbprmstate.py

@@ -140,6 +140,9 @@ class State (object):
             return self.fullBody.projectToCom(self.sId, targetCom)
         else:
             return self.fullBody.projectStateToCOM(self.sId, targetCom,maxNumSample)     > 0
+
+    def projectToRoot(self,targetRoot):
+        return self.fullBody.projectStateToRoot(self.sId,targetRoot) > 0
         
     def getComConstraint(self, limbsCOMConstraints, exceptList = []):
         return get_com_constraint(self.fullBody, self.sId, self.cl.getConfigAtState(self.sId), limbsCOMConstraints, interm = self.isIntermediate, exceptList = exceptList)
@@ -168,10 +171,13 @@ class StateHelper(object):
     # \param limbName name of the considered limb to create contact with
     # \param p 3d position of the point
     # \param n 3d normal of the contact location center
+    # \param max_num_sample number of times it will try to randomly sample a configuration before failing
+    # \param lockOtherJoints : if True, the values of all the joints exepct the ones of 'limbName' are constrained to be constant.
+    #                This only affect the first projection, if max_num_sample > 0 and the first projection was unsuccessfull, the parameter is ignored
     # \return (State, success) whether the creation was successful, as well as the new state
     @staticmethod
-    def addNewContact(state, limbName, p, n, num_max_sample = 0):
-        sId = state.cl.addNewContact(state.sId, limbName, p, n, num_max_sample)
+    def addNewContact(state, limbName, p, n, num_max_sample = 0, lockOtherJoints= False):
+        sId = state.cl.addNewContact(state.sId, limbName, p, n, num_max_sample,lockOtherJoints)
         if(sId != -1):
             return State(state.fullBody, sId = sId), True
         return state, False

src/rbprmbuilder.impl.cc

@@ -851,6 +851,28 @@ namespace hpp {
         return projectStateToCOMEigen(stateId, com_target, max_num_sample);
     }
 
+    double RbprmBuilder::projectStateToRoot(unsigned short stateId, const hpp::floatSeq& root) throw (hpp::Error)
+    {
+        pinocchio::Configuration_t root_target = dofArrayToConfig (7, root);
+        if(lastStatesComputed_.size() <= stateId)
+        {
+            throw std::runtime_error ("Unexisting state " + std::string(""+(stateId)));
+        }
+        State s = lastStatesComputed_[stateId];
+        projection::ProjectionReport rep = projection::projectToRootConfiguration(fullBody(),root_target,s);
+        double success = 0.;
+        if(rep.success_){
+          ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
+          CollisionValidationPtr_t val = fullBody()->GetCollisionValidation();
+          if(val->validate(rep.result_.configuration_,rport)){
+            lastStatesComputed_[stateId] = rep.result_;
+            lastStatesComputedTime_[stateId].second = rep.result_;
+            success = 1.;
+          }
+        }
+        return success;
+    }
+
 
     rbprm::State RbprmBuilder::generateContacts_internal(const hpp::floatSeq& configuration,
       const hpp::floatSeq& direction,const hpp::floatSeq& acceleration, const double robustnessThreshold ) throw (hpp::Error)

src/rbprmbuilder.impl.hh

@@ -330,6 +330,7 @@ namespace hpp {
         double projectStateToCOMEigen(unsigned short stateId, const pinocchio::Configuration_t& com_target, unsigned short maxNumeSamples)throw (hpp::Error);
 
         virtual double projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com, unsigned short max_num_sample) throw (hpp::Error);
+        virtual double projectStateToRoot(unsigned short stateId, const hpp::floatSeq& root) throw (hpp::Error);
         virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
         virtual void saveLimbDatabase(const char* limbname,const char* filepath) throw (hpp::Error);
         virtual hpp::floatSeq* getOctreeBox(const char* limbName, double sampleId) throw (hpp::Error);