diff --git a/script/scenarios/demos/darpa_hyq.py b/script/scenarios/demos/darpa_hyq.py
index eab116f7868f6e5fe15afc81bda3978c6aeee3f9..e0fe2666240ae56aa5384030286cf4d907e4a6d2 100644
--- a/script/scenarios/demos/darpa_hyq.py
+++ b/script/scenarios/demos/darpa_hyq.py
@@ -1,5 +1,5 @@
#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)
diff --git a/script/scenarios/demos/darpa_hyq_path.py b/script/scenarios/demos/darpa_hyq_path.py
index 984461592a7ff6f6b078e56e85e6b1e2c94b9fab..25541b07e053e29ee158fdc156f8eaf1e7673087 100644
--- a/script/scenarios/demos/darpa_hyq_path.py
+++ b/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)