anymal box scenario functional

script/scenarios/sandbox/anymal_one_step/anymal_box.py

+import setup_one_step as sos
+
+
+fsp = sos.fewStepPlanner #planner instance
+q_init =  sos.q_init  #initial configuration
+initState = sos.initState #initial state. 
+viewer = sos.v
+
+### Go somewhere
+
+n_goal = q_init[:7][:]
+n_goal[0] += .5
+n_goal[1] += 0
+#~ n_goal[2] = [0.,0.,0.7071,0.7071]
+n_goal[3:7] = [0.,0.,0.7071,0.7071]
+#~ sos.fullBody.toggleNonContactingLimb(sos.fullBody.prongFrontId)
+states, configs = fsp.goToQuasiStatic(initState,n_goal, stepsize = 0.001,  displayGuidePath = False, erasePreviousStates = True)
+#~ pId = fsp.guidePath(initState.q()[:7], n_goal, displayPath = True)
+#~ fsp.setPlanningContext()
+#~ fsp.pathPlayer(pId)
+#equivalent to
+ # fsp.goToQuasiStatic(self, initState, n_goal, stepsize = 0.002, goalLimbsInContact = None, goalNormals = None, displayGuidePath = False)
+
+
+#display computed States:
+
+#~ n_goal[3:7] = [0.,0.,0.7071,0.7071]
+
+#~ s = states[-1] #last state computed
+#~ states, configs = fsp.goToQuasiStatic(s,n_goal, displayGuidePath = False, erasePreviousStates = True, stepsize = 0.001)
+
+#display configuration
+#~ viewer(s.q())
+
+#~ states+=states2
+#~ configs+=configs2
+
+configs = [q + [0. for _ in range(6)] for q in configs]
+
+fullBody = sos.fullBody
+v = viewer
+
+#~ sos.dispContactPlan(states,0.051) #2nd argument is frame rateue
+
+#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.])
+fullBody.resetJointsBounds()
+

script/scenarios/sandbox/anymal_one_step/anymal_box.sh

+#! /bin/bash
+
+
+gepetto-gui &
+hpp-rbprm-server &
+cp *.py /media/data/dev/linux/hpp/src/hpp-rbprm-corba/script/scenarios/memmo
+#~ ipython -i --no-confirm-exit $DEVEL_HPP_DIR/src/multicontact-locomotion-planning/scripts/run_mlp.py talos_circle
+ipython -i --no-confirm-exit $DEVEL_HPP_DIR/src/multicontact-locomotion-planning/scripts/run_mlp.py anymal_box.py
+
+pkill -f  'gepetto-gui'
+pkill -f  'hpp-rbprm-server'
+

script/scenarios/sandbox/anymal_one_step/config.py

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

script/scenarios/sandbox/anymal_one_step/demo.py

@@ -9,24 +9,36 @@ viewer = sos.v
 ### Go somewhere
 
 n_goal = q_init[:7][:]
-n_goal[0] += 2
-n_goal[1] += 1
-n_goal[3:7] = [0.,0.,0.7071,0.7071]
+n_goal[0] += 3
+n_goal[1] += 0
+#~ n_goal[2] = [0.,0.,0.7071,0.7071]
+#~ 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)
+states, configs = fsp.goToQuasiStatic(initState,n_goal, displayGuidePath = False)
+#~ pId = fsp.guidePath(initState.q()[:7], n_goal, displayPath = True)
+#~ fsp.setPlanningContext()
+#~ fsp.pathPlayer(pId)
 #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
 
+n_goal[3:7] = [0.,0.,0.7071,0.7071]
 
 s = states[-1] #last state computed
+states2, configs2 = fsp.goToQuasiStatic(s,n_goal, displayGuidePath = False)
 
 #display configuration
 viewer(s.q())
 
+states+=states2
+configs+=configs2
+
+configs = [q + [0. for _ in range(6)] for q in configs]
+
+sos.dispContactPlan(states,0.051) #2nd argument is frame rateue
+
 #some helpers: 
 s.q() # configuration associated to state
 #~ initState. # configuration associated to state

src/CMakeLists.txt

@@ -85,6 +85,7 @@ INSTALL(
         ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmfullbody.py
         ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmstate.py
         ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/state_alg.py
+        ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/fewstepsplanner.py
   DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm
   )
 INSTALL(

src/hpp/corbaserver/rbprm/fewstepsplanner.py

@@ -100,7 +100,7 @@ class FewStepPlanner(object):
   def interpolateStates(self, stepsize, pathId = 1, robustnessTreshold = 0, filterStates = False, testReachability = True, quasiStatic = False, erasePreviousStates = True):
     return _interpolateState(self.problemSolver, self.fullBody, stepsize, pathId, robustnessTreshold, filterStates, testReachability, quasiStatic, erasePreviousStates)
     
-  def goToQuasiStatic(self, currentState, toPos, stepsize = 0.002, goalLimbsInContact = None, goalNormals = None, displayGuidePath = False):
+  def goToQuasiStatic(self, currentState, toPos, stepsize = 0.002, goalLimbsInContact = None, goalNormals = None, displayGuidePath = False, erasePreviousStates = False):
     pId = self.guidePath(currentState.q()[:7], toPos, displayPath = displayGuidePath)    
     self.fullBody.setStartStateId(currentState.sId)
     targetState = currentState.q()[:]; targetState[:7] = toPos
@@ -109,7 +109,7 @@ class FewStepPlanner(object):
     if goalNormals is None:
       goalNormals = [[0.,0.,1.] for _ in range(len(goalLimbsInContact))]
     self.fullBody.setEndState(targetState, goalLimbsInContact,goalNormals)
-    return self.interpolateStates(stepsize,pathId=pId,robustnessTreshold = 1, filterStates = True,quasiStatic=True, erasePreviousStates = False)
+    return self.interpolateStates(stepsize,pathId=pId,robustnessTreshold = 1, filterStates = True,quasiStatic=True, erasePreviousStates = erasePreviousStates)
     
        
               

src/hpp/corbaserver/rbprm/state_alg.py

@@ -23,7 +23,7 @@ try:
     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"
+    #~ print "WARNING: in state_alg, some optinal dependencies were not found"
     pass
 
 ## algorithmic methods on state
@@ -81,8 +81,8 @@ def computeIntermediateState(sfrom, sto):
 # \param p 3d position of the point
 # \param n 3d normal of the contact location center
 # \return (State, success) whether the creation was successful, as well as the new state
-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