[script] use hyq_contact6D in darpa (required for whole body motion)

script/scenarios/demos/darpa_hyq.py

 #Importing helper class for RBPRM
-from hpp.corbaserver.rbprm.hyq import Robot
+from hpp.corbaserver.rbprm.hyq_contact6D import Robot
 from hpp.corbaserver.problem_solver import ProblemSolver
 from hpp.gepetto import Viewer
 
@@ -23,10 +23,10 @@ fullBody.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4])
 nbSamples = 10000
 
 ps = tp.ProblemSolver(fullBody)
-r = tp.Viewer (ps, viewerClient=tp.r.client)
+v = tp.Viewer (ps, viewerClient=tp.v.client)
 
 rootName = 'base_joint_xyz'
-cType = "_3_DOF"
+cType = "_6_DOF"
 
 def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
 	fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
@@ -61,8 +61,8 @@ q_init = [-2.0,
  -0.9577096766517215,
  0.9384602821326071]
 """
-q_init=fullBody.referenceConfig[::]; q_init[0:7] = tp.q_init[0:7]; q_init[2]=fullBody.referenceConfig[2]+0.02
-q_goal = fullBody.referenceConfig[::]; q_goal[0:7] = tp.q_goal[0:7]; q_goal[2]=fullBody.referenceConfig[2]+0.02
+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.generateContacts(q_init, [0,0,1])
@@ -73,12 +73,12 @@ fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,f
 fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,fullBody.rArmId])
 
 
-r(q_init)
+v(q_init)
 configs = []
 
 
 from hpp.gepetto import PathPlayer
-pp = PathPlayer (fullBody.client, r)
+pp = PathPlayer (fullBody.client, v)
 
 
 import time
@@ -86,36 +86,36 @@ import time
 #DEMO METHODS
 
 def initConfig():
-	r.client.gui.setVisibility("hyq", "ON")
+	v.client.gui.setVisibility("hyq", "ON")
 	tp.cl.problem.selectProblem("default")
-	tp.r.client.gui.setVisibility("toto", "OFF")
-	tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
-	r(q_init)
+	tp.v.client.gui.setVisibility("toto", "OFF")
+	tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
+	v(q_init)
 	
 def endConfig():
-	r.client.gui.setVisibility("hyq", "ON")
+	v.client.gui.setVisibility("hyq", "ON")
 	tp.cl.problem.selectProblem("default")
-	tp.r.client.gui.setVisibility("toto", "OFF")
-	tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
-	r(q_goal)
+	tp.v.client.gui.setVisibility("toto", "OFF")
+	tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
+	v(q_goal)
 	
 
 def rootPath():
-	r.client.gui.setVisibility("hyq", "OFF")
+	v.client.gui.setVisibility("hyq", "OFF")
 	tp.cl.problem.selectProblem("rbprm_path")
-	tp.r.client.gui.setVisibility("toto", "OFF")
-	r.client.gui.setVisibility("hyq", "OFF")
-	tp.r.client.gui.setVisibility("hyq_trunk_large", "ON")
+	tp.v.client.gui.setVisibility("toto", "OFF")
+	v.client.gui.setVisibility("hyq", "OFF")
+	tp.v.client.gui.setVisibility("hyq_trunk_large", "ON")
 	tp.pp(0)
-	tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
-	r.client.gui.setVisibility("hyq", "ON")
+	tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
+	v.client.gui.setVisibility("hyq", "ON")
 	tp.cl.problem.selectProblem("default")
 	
 def genPlan(stepsize=0.06):
 	tp.cl.problem.selectProblem("default")
-	r.client.gui.setVisibility("hyq", "ON")
-	tp.r.client.gui.setVisibility("toto", "OFF")
-	tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
+	v.client.gui.setVisibility("hyq", "ON")
+	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)
@@ -123,12 +123,12 @@ def genPlan(stepsize=0.06):
 	print "Contact plan generated in " + str(end-start) + "seconds"
 	
 def contactPlan(step = 0.5):
-	r.client.gui.setVisibility("hyq", "ON")
+	v.client.gui.setVisibility("hyq", "ON")
 	tp.cl.problem.selectProblem("default")
-	tp.r.client.gui.setVisibility("toto", "OFF")
-	tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
+	tp.v.client.gui.setVisibility("toto", "OFF")
+	tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
 	for i in range(0,len(configs)):
-		r(configs[i]);
+		v(configs[i]);
 		time.sleep(step)
                 		
 		

script/scenarios/demos/darpa_hyq_path.py

@@ -21,13 +21,24 @@ rbprmBuilder.boundSO3([-0.4,0.4,-0.3,0.3,-0.3,0.3])
 # Creating an instance of HPP problem solver and the viewer
 from hpp.corbaserver.problem_solver import ProblemSolver
 ps = ProblemSolver( rbprmBuilder )
-r = Viewer (ps)
+from hpp.gepetto import ViewerFactory
+vf = ViewerFactory (ps)
+
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+#~ afftool.loadObstacleModel (packageName, "darpa", "planning", r, reduceSizes=[0.05,0.,0.])
+afftool.loadObstacleModel ("hpp-rbprm-corba", "darpa", "planning", vf,reduceSizes=[0.1,0,0])
+v = vf.createViewer()
+#afftool.visualiseAffordances('Support', v, [0.25, 0.5, 0.5])
+
+
 
 # Setting initial and goal configurations
 q_init = rbprmBuilder.getCurrentConfig ();
-q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); v (q_init)
 q_goal = q_init [::]
-q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
+q_goal [0:3] = [3, 0, 0.63]; v (q_goal)
 #~ q_goal [0:3] = [-1.5, 0, 0.75]; r (q_goal)
 
 # Choosing a path optimizer
@@ -35,12 +46,6 @@ ps.addPathOptimizer("RandomShortcut")
 ps.setInitialConfig (q_init)
 ps.addGoalConfig (q_goal)
 
-from hpp.corbaserver.affordance.affordance import AffordanceTool
-afftool = AffordanceTool ()
-#~ afftool.loadObstacleModel (packageName, "darpa", "planning", r, reduceSizes=[0.05,0.,0.])
-afftool.loadObstacleModel ("hpp-rbprm-corba", "darpa", "planning", r)
-afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
-
 # Choosing RBPRM shooter and path validation methods.
 # Note that the standard RRT algorithm is used.
 ps.client.problem.selectConfigurationShooter("RbprmShooter")
@@ -56,11 +61,11 @@ print "computation time for root path ", t
 
 # Playing the computed path
 from hpp.gepetto import PathPlayer
-pp = PathPlayer (r)
+pp = PathPlayer (v)
 
 q_far = q_init [::]
 q_far [0:3] = [-2, -3, 0.63]; 
-r(q_far)
+v(q_far)
 
 for i in range(1,10):
 	rbprmBuilder.client.problem.optimizePath(i)
@@ -76,6 +81,6 @@ rbprmBuilder2 = Robot ("toto")
 ps2 = ProblemSolver( rbprmBuilder2 )
 cl.problem.selectProblem("default")
 cl.problem.movePathToProblem(8,"rbprm_path",rbprmBuilder.getAllJointNames()[1:])
-r2 = Viewer (ps2, viewerClient=r.client)
+r2 = Viewer (ps2, viewerClient=v.client)
 r2(q_far)