[Feat] add clone state method, projectroot can now take offset, added helper...

[Feat] add clone state method, projectroot can now take offset, added helper method to move limbs into non contacting

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -154,11 +154,18 @@ module hpp
     /// \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
+    short cloneState(in unsigned short stateId) 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);
+    /// \param offset specific point to be projected in root frame. If different than 0 root orientation is ignored
+    double projectStateToRoot(in unsigned short stateId,  in floatSeq root, in floatSeq offset) raises (Error);
 
 
     /// Project a state into a COM
@@ -799,6 +806,8 @@ module hpp
     floatSeqSeq getPathAsBezier(in unsigned short pathId)raises (Error);
 
 
+    boolean toggleNonContactingLimb(in string limbname)raises (Error);
+
     boolean areKinematicsConstraintsVerified(in floatSeq point)raises (Error);
 
     boolean areKinematicsConstraintsVerifiedForState(in unsigned short stateFrom,in floatSeq point)raises (Error);

script/scenarios/sandbox/anymal_one_step/config.py

 SCENE="multicontact/ground"
 INIT_CONFIG_ROOT =   [0,0,0.465,0,0,0,1]
 INIT_CONFIG_WB   =   None
-ROOT_BOUNDS      =  [-20,20, -20, 20, 0.4, 0.5]
+ROOT_BOUNDS      =  [-20,20, -20, 20, 0.1, 0.6]

script/scenarios/sandbox/anymal_one_step/demo.py

@@ -12,13 +12,14 @@ n_goal = q_init[:7][:]
 n_goal[0] += 2
 n_goal[1] += 1
 n_goal[3:7] = [0.,0.,0.7071,0.7071]
+#~ sos.fullBody.toggleNonContactingLimb(sos.fullBody.prongFrontId)
 states, configs = fsp.goToQuasiStatic(initState,n_goal, displayGuidePath = True)
 #equivalent to
  # fsp.goToQuasiStatic(self, initState, n_goal, stepsize = 0.002, goalLimbsInContact = None, goalNormals = None, displayGuidePath = False)
 
 
 #display computed States:
-sos.dispContactPlan(states,0.051) #2nd argument is frame rateue
+#~ sos.dispContactPlan(states,0.051) #2nd argument is frame rateue
 
 
 s = states[-1] #last state computed
@@ -29,3 +30,14 @@ viewer(s.q())
 #some helpers: 
 s.q() # configuration associated to state
 #~ initState. # configuration associated to state
+
+from hpp.corbaserver.rbprm import rbprmstate
+target = sos.fullBody.getJointPosition(sos.fullBody.prongFrontId)[:3]
+target[2] = 0.
+
+
+s = rbprmstate.StateHelper.cloneState(states[-1])[0]
+fb = sos.fullBody
+
+#~ sos.fullBody.toggleNonContactingLimb(sos.fullBody.prongFrontId)
+#~ rbprmstate.StateHelper.addNewContact(s,sos.fullBody.prongFrontId,target,[0.,0.,1.])

script/scenarios/sandbox/anymal_one_step/rund.sh

+#!/bin/bash         
+
+gepetto-gui & 
+ipython -i --no-confirm-exit ./$1
+
+pkill -f  'gepetto-gui'

script/scenarios/sandbox/anymal_one_step/setup_one_step.py

@@ -27,6 +27,7 @@ rbprmBuilder.setJointBounds ("root_joint", root_bounds)
 rbprmBuilder.setFilter(rbprmBuilder.urdfNameRom)
 for rom in rbprmBuilder.urdfNameRom :
     rbprmBuilder.setAffordanceFilter(rom, ['Support'])
+#~ rbprmBuilder.setAffordanceFilter("front_prong", ['Support'])
 
 # We also bound the rotations of the torso. (z, y, x)
 #~ rbprmBuilder.boundSO3([-1.7,1.7,-0.1,0.1,-0.1,0.1])
@@ -103,7 +104,8 @@ pp = PathPlayer (v)
 
 ###### WHOLE BODY INIT
 
-from hpp.corbaserver.rbprm.anymal import Robot
+#~ from hpp.corbaserver.rbprm.anymal import Robot
+from hpp.corbaserver.rbprm.anymal_prong import Robot
 #~ from hpp.gepetto import Viewer
 from tools.display_tools import *
 import time
@@ -195,7 +197,7 @@ fullBody.setCurrentConfig (q_init)
 v(q_init)
 
 
-v.addLandmark('anymal/base_0',0.3)
+#~ v.addLandmark('anymal/base_0',0.3)
 v(q_init)
 #fullBody.setReferenceConfig(fullBody.referenceConfig_legsApart[::]+[0]*6)
 

src/CMakeLists.txt

@@ -93,6 +93,7 @@ INSTALL(
 				${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/generateROMs.py
 				${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/com_constraints.py
         ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/plot_analytics.py
+        ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/affordance_centroids.py
         ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
         ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
         ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/path_to_trajectory.py

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -290,7 +290,7 @@ class FullBody (Robot):
           num_max_sample = 1
           for (limbName, normal) in  zip(contacts, normals):
                p = cl.getEffectorPosition(limbName,configuration)[0]
-               cl.addNewContact(sId, limbName, p, normal, num_max_sample, True)
+               cl.addNewContact(sId, limbName, p, normal, num_max_sample, False)
           return cl.setStartStateId(sId)
           
           
@@ -308,7 +308,7 @@ class FullBody (Robot):
           num_max_sample = 1
           for (limbName, normal) in  zip(contacts, normals):
                p = cl.getEffectorPosition(limbName,configuration)[0]
-               cl.addNewContact(sId, limbName, p, normal, num_max_sample, True)
+               cl.addNewContact(sId, limbName, p, normal, num_max_sample, False)
           return cl.setEndStateId(sId)
 
      ## Initialize the first state of the path interpolation
@@ -745,9 +745,10 @@ class FullBody (Robot):
      # and update the state configuration.
      # \param state index of first state.
      # \param root : root configuration (size 7)
+     # \param offset specific point to be projected in root frame. If different than 0 root orientation is ignored
      # \return whether the projection was successful
-     def projectStateToRoot(self, state, root):
-          return self.clientRbprm.rbprm.projectStateToRoot(state, root)     > 0
+     def projectStateToRoot(self, state, root, offset = [0,0,0.]):
+          return self.clientRbprm.rbprm.projectStateToRoot(state, root, offset)     > 0
           
      ## Project a given state into a given COM position
      # and update the state configuration.
@@ -990,4 +991,7 @@ class FullBody (Robot):
 
      def isDynamicallyReachableFromState(self,stateFrom,stateTo,addPathPerPhase = False,timings=[],numPointsPerPhases=5):
           return self.clientRbprm.rbprm.isDynamicallyReachableFromState(stateFrom,stateTo,addPathPerPhase,timings,numPointsPerPhases)
+          
+     def toggleNonContactingLimb(self,limbName):
+          return self.clientRbprm.rbprm.toggleNonContactingLimb(limbName)
 

src/hpp/corbaserver/rbprm/rbprmstate.py

@@ -141,8 +141,14 @@ class State (object):
         else:
             return self.fullBody.projectStateToCOM(self.sId, targetCom,maxNumSample)     > 0
 
-    def projectToRoot(self,targetRoot):
-        return self.fullBody.projectStateToRoot(self.sId,targetRoot) > 0
+    ## Project a state into a given root position and orientation
+    #
+    # \param stateId target state
+    # \param root the root configuration (size 7)
+    # \param offset specific point to be projected in root frame. If different than 0 root orientation is ignored
+    # \return Whether the projection has been successful or not
+    def projectToRoot(self,targetRoot, offset = [0., 0., 0.]):
+        return self.fullBody.projectStateToRoot(self.sId,targetRoot, offset) > 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)
@@ -181,6 +187,13 @@ class StateHelper(object):
         if(sId != -1):
             return State(state.fullBody, sId = sId), True
         return state, False
+        
+    @staticmethod
+    def cloneState(state):
+        sId = state.cl.cloneState(state.sId)
+        if(sId != -1):
+            return State(state.fullBody, sId = sId), True
+        return state, False
 
     ## tries to remove a new contact from a state
     ## if the limb is already in contact, replace the

src/hpp/corbaserver/rbprm/state_alg.py

@@ -18,9 +18,13 @@
 
 from __future__ import print_function
 from hpp.corbaserver.rbprm.rbprmstate import State
-from lp_find_point import find_valid_c_cwc, find_valid_c_cwc_qp, lp_ineq_4D
-from hpp.corbaserver.rbprm.tools.com_constraints import *
-from CWC_methods import compute_CWC, is_stable
+try:
+    from hpp.corbaserver.rbprm.tools.com_constraints import *
+    from CWC_methods import compute_CWC, is_stable
+    from lp_find_point import find_valid_c_cwc, find_valid_c_cwc_qp, lp_ineq_4D
+except:
+    print "WARNING: in state_alg, some optinal dependencies were not found"
+    pass
 
 ## algorithmic methods on state
    

src/rbprmbuilder.impl.cc

@@ -841,21 +841,40 @@ namespace hpp {
         return (CORBA::Short)(lastStatesComputed_.size()-1);
     }
 
+    CORBA::Short RbprmBuilder::cloneState(unsigned short stateId) throw (hpp::Error)
+    {
+        try
+        {
+            if(lastStatesComputed_.size() <= stateId){
+              throw std::runtime_error ("Can't clone state: invalid state id : "+std::string(""+stateId)+" number of state = "+std::string(""+lastStatesComputed_.size()));
+            }
+            State newState = lastStatesComputed_[stateId];
+            lastStatesComputed_.push_back(newState);
+            lastStatesComputedTime_.push_back(std::make_pair(-1., newState));
+            return (CORBA::Short)(lastStatesComputed_.size()-1);
+        }
+        catch(std::runtime_error& e)
+        {
+            throw Error(e.what());
+        }
+    }
+
     double RbprmBuilder::projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com, unsigned short max_num_sample) throw (hpp::Error)
     {
         pinocchio::Configuration_t com_target = dofArrayToConfig (3, com);
         return projectStateToCOMEigen(stateId, com_target, max_num_sample);
     }
 
-    double RbprmBuilder::projectStateToRoot(unsigned short stateId, const hpp::floatSeq& root) throw (hpp::Error)
+    double RbprmBuilder::projectStateToRoot(unsigned short stateId, const hpp::floatSeq& root, const hpp::floatSeq& offset) throw (hpp::Error)
     {
         pinocchio::Configuration_t root_target = dofArrayToConfig (7, root);
+        pinocchio::Configuration_t offset_target = dofArrayToConfig (3, offset);
         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);
+        projection::ProjectionReport rep = projection::projectToRootConfiguration(fullBody(),root_target,s,offset_target);
         double success = 0.;
         if(rep.success_){
           ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
@@ -3461,6 +3480,15 @@ namespace hpp {
       return limbsNames;
     }
 
+    bool RbprmBuilder::toggleNonContactingLimb(const char* limbName)throw (hpp::Error)
+    {
+        if(!fullBodyLoaded_){
+          throw std::runtime_error ("fullBody not loaded");
+        }
+        const std::string limb (limbName);
+        return fullBody()->toggleNonContactingLimb(limb);
+    }
+
     bool RbprmBuilder::areKinematicsConstraintsVerified(const hpp::floatSeq &point)throw (hpp::Error){
         if(!fullBodyLoaded_){
           throw std::runtime_error ("fullBody not loaded");

src/rbprmbuilder.impl.hh

@@ -339,7 +339,8 @@ 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 CORBA::Short cloneState(unsigned short stateId) throw (hpp::Error);
+        virtual double projectStateToRoot(unsigned short stateId, const hpp::floatSeq& root, const hpp::floatSeq& offset) 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);
@@ -366,7 +367,7 @@ namespace hpp {
         virtual hpp::floatSeqSeqSeq* getEffectorTrajectoryWaypoints(unsigned short pathId,const char* effectorName)throw (hpp::Error);
         virtual hpp::floatSeqSeq* getPathAsBezier(unsigned short pathId)throw (hpp::Error);
 
-
+        virtual bool toggleNonContactingLimb(const char* limbName)throw (hpp::Error);
         virtual bool areKinematicsConstraintsVerified(const hpp::floatSeq &point)throw (hpp::Error);
         virtual bool areKinematicsConstraintsVerifiedForState(unsigned short stateId,const hpp::floatSeq &point)throw (hpp::Error);
         virtual hpp::floatSeq *isReachableFromState(unsigned short stateFrom,unsigned short stateTo,const bool useIntermediateState)throw (hpp::Error);