fixed anymal slalom scripts + start state in rbprmFullBody

script/scenarios/sandbox/ANYmal/anymal_plinth_path.py

@@ -28,7 +28,7 @@ for rom in rbprmBuilder.urdfNameRom :
 rbprmBuilder.boundSO3([-0.1,0.1,-0.5,0.5,-0.1,0.1])
 # Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root
 rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof)
-c# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
+# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
 rbprmBuilder.client.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,-vMax,vMax,-aMax,aMax,-aMax,aMax,-aMaxZ,aMaxZ])
 indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.robot.getDimensionExtraConfigSpace()
 
@@ -39,7 +39,7 @@ ps.setParameter("Kinodynamic/velocityBound",vMax)
 ps.setParameter("Kinodynamic/accelerationBound",aMax)
 ps.setParameter("Kinodynamic/verticalAccelerationBound",aMaxZ)
 # force the orientation of the trunk to match the direction of the motion
-ps.setParameter("Kinodynamic/forceOrientation",True)
+ps.setParameter("Kinodynamic/forceAllOrientation",True)
 ps.setParameter("DynamicPlanner/sizeFootX",0.1)
 ps.setParameter("DynamicPlanner/sizeFootY",0.1)
 ps.setParameter("DynamicPlanner/friction",mu)

script/scenarios/sandbox/ANYmal/anymal_slalom_debris.py

@@ -77,8 +77,8 @@ v.addLandmark('anymal/base',0.3)
 v(q_init)
 
 # specify the full body configurations as start and goal state of the problem
-fullBody.setStartState(q_init,fullBody.limbs_names)
-fullBody.setEndState(q_goal,fullBody.limbs_names)
+fullBody.setStartState(q_init,fullBody.limbs_names, [[0.,0.,1.] for _ in range(4)])
+fullBody.setEndState(q_goal,fullBody.limbs_names, [[0.,0.,1.] for _ in range(4)])
 
 
 print "Generate contact plan ..."

script/scenarios/sandbox/ANYmal/anymal_slalom_debris_path.py

@@ -37,7 +37,7 @@ ps = ProblemSolver( rbprmBuilder )
 ps.setParameter("Kinodynamic/velocityBound",vMax)
 ps.setParameter("Kinodynamic/accelerationBound",aMax)
 # force the orientation of the trunk to match the direction of the motion
-ps.setParameter("Kinodynamic/forceOrientation",True)
+ps.setParameter("Kinodynamic/forceAllOrientation",True)
 ps.setParameter("DynamicPlanner/sizeFootX",0.01)
 ps.setParameter("DynamicPlanner/sizeFootY",0.01)
 ps.setParameter("DynamicPlanner/friction",mu)

script/scenarios/sandbox/ANYmal/run.sh

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

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -285,7 +285,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)
+               cl.addNewContact(sId, limbName, p, normal, num_max_sample, True)
           return cl.setStartStateId(sId)
           
           
@@ -303,7 +303,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)
+               cl.addNewContact(sId, limbName, p, normal, num_max_sample, True)
           return cl.setEndStateId(sId)
 
      ## Initialize the first state of the path interpolation