[script] adapt hyq script to changes in robot-data

script/scenarios/demos/darpa_hyq.py

@@ -18,6 +18,8 @@ from hpp.corbaserver import Client
 #  This time we load the full body model of HyQ
 fullBody = Robot () 
 fullBody.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4])
+fullBody.client.robot.setDimensionExtraConfigSpace(6)
+fullBody.client.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0,0,0,0,0,0,0])
 
 #  Setting a number of sample configurations used
 nbSamples = 10000
@@ -31,38 +33,20 @@ cType = "_6_DOF"
 def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
 	fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
 
-fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.1, cType)
-fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
-fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
+fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.1, cType)
+fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.05, cType)
+fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.05, cType)
 fullBody.addLimb(fullBody.lArmId,fullBody.larm,fullBody.lhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
 
 #~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
 #~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
 #~ fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
 #~ fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
-"""
-q_init = [-2.0,
- 0.0,
- 0.6838277139631803,
- 0.0,
- 0.0,
- 0.0,
- 1.0,
- 0.14279812395541294,
- 0.934392553166556,
- -0.9968239786882757,
- -0.06521258938340457,
- -0.8831796268418511,
- 1.150049183494211,
- -0.06927610020154493,
- 0.9507443168724581,
- -0.8739975339028809,
- 0.03995660287873871,
- -0.9577096766517215,
- 0.9384602821326071]
-"""
-q_init=fullBody.referenceConfig[::]; q_init[0:7] = tp.q_init[0:7]; q_init[2]=fullBody.referenceConfig[2]
-q_goal = fullBody.referenceConfig[::]; q_goal[0:7] = tp.q_goal[0:7]; q_goal[2]=fullBody.referenceConfig[2]
+
+q_init=fullBody.referenceConfig[::] + [0]*6; q_init[0:7] = tp.q_init[0:7]; 
+#q_init[2]=fullBody.referenceConfig[2]
+q_goal = fullBody.referenceConfig[::]+ [0]*6; q_goal[0:7] = tp.q_goal[0:7]; 
+#q_goal[2]=fullBody.referenceConfig[2]
 
 
 q_init = fullBody.generateContacts(q_init, [0,0,1])
@@ -73,6 +57,7 @@ fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,f
 fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,fullBody.rArmId])
 
 
+
 v(q_init)
 configs = []
 
@@ -117,9 +102,9 @@ def genPlan(stepsize=0.06):
 	tp.v.client.gui.setVisibility("toto", "OFF")
 	tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
 	global configs
-	start = time.clock() 
-	configs = fullBody.interpolate(stepsize, 8, 0, filterStates = False, testReachability=False, quasiStatic=True)
-	end = time.clock() 
+	start = time.time() 
+	configs = fullBody.interpolate(stepsize, 5, 0., filterStates = True,testReachability = False)
+	end = time.time() 
 	print "Contact plan generated in " + str(end-start) + "seconds"
 	
 def contactPlan(step = 0.5):

script/scenarios/demos/darpa_hyq_path.py

@@ -36,9 +36,9 @@ v = vf.createViewer()
 
 # Setting initial and goal configurations
 q_init = rbprmBuilder.getCurrentConfig ();
-q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); v (q_init)
+q_init [0:3] = [-2, 0, 0.64]; rbprmBuilder.setCurrentConfig (q_init); v (q_init)
 q_goal = q_init [::]
-q_goal [0:3] = [3, 0, 0.63]; v (q_goal)
+q_goal [0:3] = [3, 0, 0.64]; v (q_goal)
 #~ q_goal [0:3] = [-1.5, 0, 0.75]; r (q_goal)
 
 # Choosing a path optimizer
@@ -64,7 +64,7 @@ from hpp.gepetto import PathPlayer
 pp = PathPlayer (v)
 
 q_far = q_init [::]
-q_far [0:3] = [-2, -3, 0.63]; 
+q_far [0:3] = [-2, -3, 0.6]; 
 v(q_far)
 
 for i in range(1,10):