[script] anymal_slalom_debris working

script/scenarios/sandbox/ANYmal/anymal_slalom_debris.py

-from hpp.corbaserver.rbprm.anymal_contact6D import Robot
+from hpp.corbaserver.rbprm.anymal import Robot
 from hpp.gepetto import Viewer
 from tools.display_tools import *
 import time
@@ -13,13 +13,14 @@ pId = tp.ps.numberPaths() -1
 fullBody = Robot ()
 # Set the bounds for the root, take slightly larger bounding box than during root planning
 root_bounds = tp.root_bounds[::]
-root_bounds[0] -= 0.2
-root_bounds[1] += 0.2
-root_bounds[2] -= 0.2
-root_bounds[3] += 0.2
+root_bounds[0] -= 0.5
+root_bounds[1] += 0.5
+root_bounds[2] -= 0.5
+root_bounds[3] += 0.5
 root_bounds[-1] = 0.8
-root_bounds[-2] = 0.4
+root_bounds[-2] = 0.3
 fullBody.setJointBounds ("root_joint",  root_bounds)
+fullBody.setVeryConstrainedJointsBounds()
 # add the 6 extraDof for velocity and acceleration (see *_path.py script)
 fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
 fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,-0,0])
@@ -34,11 +35,12 @@ q_ref = fullBody.referenceConfig[::]+[0,0,0,0,0,0]
 q_init = q_ref[::]
 fullBody.setReferenceConfig(q_ref)
 fullBody.setCurrentConfig (q_init)
-
+fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
+#fullBody.usePosturalTaskContactCreation(True)
 
 print "Generate limb DB ..."
 tStart = time.time()
-fullBody.loadAllLimbs("static","ReferenceConfigurationCustom",nbSamples=100000)
+fullBody.loadAllLimbs("fixedStep04","ReferenceConfiguration")
 tGenerate =  time.time() - tStart
 print "Done."
 print "Databases generated in : "+str(tGenerate)+" s"
@@ -47,14 +49,14 @@ fullBody.setReferenceConfig(q_ref)
 #define initial and final configurations : 
 configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace()
 
-q_init[0:7] = tp.ps.configAtParam(pId,0.01)[0:7] # use this to get the correct orientation
+q_init[0:7] = tp.ps.configAtParam(pId,0)[0:7] # use this to get the correct orientation
 q_goal = q_init[::]; q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
-dir_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS:tp.indexECS+3]
-acc_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS+3:tp.indexECS+6]
-dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId)-0.01)[tp.indexECS:tp.indexECS+3]
+dir_init = [0,0,0]
+acc_init = [0,0,0]
+dir_goal = [0,0,0]
 acc_goal = [0,0,0]
 
-robTreshold = 5
+robTreshold = 3
 # copy extraconfig for start and init configurations
 q_init[configSize:configSize+3] = dir_init[::]
 q_init[configSize+3:configSize+6] = acc_init[::]
@@ -71,7 +73,7 @@ v(q_init)
 fullBody.setCurrentConfig (q_goal)
 v(q_goal)
 
-v.addLandmark('anymal_reachability/base',0.3)
+v.addLandmark('anymal/base',0.3)
 v(q_init)
 
 # specify the full body configurations as start and goal state of the problem
@@ -81,16 +83,13 @@ fullBody.setEndState(q_goal,fullBody.limbs_names)
 
 print "Generate contact plan ..."
 tStart = time.time()
-configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=True)
+configs = fullBody.interpolate(0.002,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=True)
 tInterpolateConfigs = time.time() - tStart
 print "Done. "
 print "Contact sequence computed in "+str(tInterpolateConfigs)+" s."
 print "number of configs :", len(configs)
-#raw_input("Press Enter to display the contact sequence ...")
-#v.startCapture("capture_cs/capture","png")
-#displayContactSequence(v,configs)
-#v.stopCapture()
 
 
+fullBody.resetJointsBounds()
 
 

script/scenarios/sandbox/ANYmal/anymal_slalom_debris_path.py

@@ -4,6 +4,7 @@ from hpp.corbaserver import Client
 from hpp.corbaserver import ProblemSolver
 import time
 
+Robot.urdfName+="_large"
 
 
 vMax = 0.2# linear velocity bound for the root
@@ -13,7 +14,7 @@ mu=0.5# coefficient of friction
 # Creating an instance of the helper class, and loading the robot
 rbprmBuilder = Robot()
 # Define bounds for the root : bounding box of the scenario
-root_bounds = [-1.7,2.5, -0.2, 2, 0.444, 0.444]
+root_bounds = [-1.7,2.5, -0.2, 2, 0.465, 0.465]
 rbprmBuilder.setJointBounds ("root_joint", root_bounds)
 
 # The following lines set constraint on the valid configurations:
@@ -52,21 +53,21 @@ vf = ViewerFactory (ps)
 from hpp.corbaserver.affordance.affordance import AffordanceTool
 afftool = AffordanceTool ()
 afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
-afftool.loadObstacleModel ("hpp_environments", "multicontact/slalom_debris", "planning", vf,reduceSizes=[0.05,0,0])
+afftool.loadObstacleModel ("hpp_environments", "multicontact/slalom_debris", "planning", vf,reduceSizes=[0.1,0,0])
 v = vf.createViewer(displayArrows = True)
 #afftool.visualiseAffordances('Support', v, v.color.lightBrown)
 #v.addLandmark(v.sceneName,1)
 
 # Setting initial configuration
 q_init = rbprmBuilder.getCurrentConfig ();
-q_init [0:3] = [-1.5,0,0.444]
+q_init [0:3] = [-1.5,0,0.465]
 q_init[-6:-3] = [0.05,0,0]
 v (q_init)
 ps.setInitialConfig (q_init)
 # set goal config
 rbprmBuilder.setCurrentConfig (q_init)
 q_goal = q_init [::]
-q_goal[0:3] = [2.2,0,0.444]
+q_goal[0:3] = [2.2,0,0.465]
 q_goal[-6:-3] = [0.05,0,0]
 v(q_goal)
 
@@ -92,7 +93,7 @@ print "Guide planning time : ",t
 from hpp.gepetto import PathPlayer
 pp = PathPlayer (v)
 pp.dt=0.1
-#pp.displayVelocityPath(1)
+pp.displayVelocityPath(1)
 #v.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
 pp.dt = 0.01
 #v.startCapture("capture_root/capture","png")