demo iros for hyq

script/scenarios/demos/darpa_hyq.py

+#Importing helper class for RBPRM
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+from hpp.gepetto import Viewer
+
+#calling script darpa_hyq_path to compute root path
+import darpa_hyq_path as tp
+
+from os import environ
+ins_dir = environ['DEVEL_DIR']
+db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
+
+
+from hpp.corbaserver import Client
+
+
+packageName = "hyq_description"
+meshPackageName = "hyq_description"
+rootJointType = "freeflyer"
+
+#  Information to retrieve urdf and srdf files.
+urdfName = "hyq"
+urdfSuffix = ""
+srdfSuffix = ""
+
+#  This time we load the full body model of HyQ
+fullBody = FullBody () 
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
+
+#  Setting a number of sample configurations used
+nbSamples = 20000
+
+ps = tp.ProblemSolver(fullBody)
+r = tp.Viewer (ps)
+
+rootName = 'base_joint_xyz'
+
+cType = "_3_DOF"
+rLegId = 'rfleg'
+rLeg = 'rf_haa_joint'
+rfoot = 'rf_foot_joint'
+offset = [0.,-0.021,0.]
+normal = [0,1,0]
+legx = 0.02; legy = 0.02
+
+def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
+	fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
+
+fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.1, cType)
+
+lLegId = 'lhleg'
+lLeg = 'lh_haa_joint'
+lfoot = 'lh_foot_joint'
+fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
+#~ 
+rarmId = 'rhleg'
+rarm = 'rh_haa_joint'
+rHand = 'rh_foot_joint'
+fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
+
+larmId = 'lfleg'
+larm = 'lf_haa_joint'
+lHand = 'lf_foot_joint'
+fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
+
+fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
+fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
+fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
+fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
+
+q_0 = fullBody.getCurrentConfig(); 
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
+q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
+
+# Randomly generating a contact configuration at q_init
+fullBody.setCurrentConfig (q_init)
+q_init = fullBody.generateContacts(q_init, [0,0,1])
+
+# Randomly generating a contact configuration at q_end
+fullBody.setCurrentConfig (q_goal)
+q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+
+# specifying the full body configurations as start and goal state of the problem
+fullBody.setStartState(q_init,[])
+fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
+
+
+r(q_init)
+#~ configs = fullBody.interpolate(0.12, 10, 10, True)
+configs = []
+#~ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
+
+# calling draw with increasing i will display the sequence
+i = 0;
+#~ fullBody.draw(configs[i],r); i=i+1; i-1
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+
+from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
+
+	
+	
+limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector' : rfoot},  
+						lLegId : {'file': "hyq/"+lLegId+"_com.ineq", 'effector' : lfoot},  
+						rarmId : {'file': "hyq/"+rarmId+"_com.ineq", 'effector' : rHand},  
+						larmId : {'file': "hyq/"+larmId+"_com.ineq", 'effector' : lHand} }
+
+
+def act(i, numOptim = 0, use_window = 0, friction = 0.5, optim_effectors = True, verbose = False, draw = False):
+	return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.4, optim_effectors = optim_effectors, time_scale = 20., useCOMConstraints = True, use_window = use_window,
+	verbose = verbose, draw = draw)
+
+def play(frame_rate = 1./24.):
+	play_traj(fullBody,pp,frame_rate)
+	
+
+import time
+
+#DEMO METHODS
+
+def initConfig():
+	r.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)
+	
+def endConfig():
+	r.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)
+	
+
+def rootPath():
+	r.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.pp(0)
+	tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
+	r.client.gui.setVisibility("hyq", "ON")
+	tp.cl.problem.selectProblem("default")
+	
+def genPlan():
+	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")
+	global configs
+	start = time.clock() 
+	configs = fullBody.interpolate(0.12, 10, 10, True)
+	end = time.clock() 
+	print "Contact plan generated in " + str(end-start) + "seconds"
+	
+def contactPlan():
+	r.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")
+	for i in range(1,len(configs)):
+		r(configs[i]);
+		time.sleep(0.5)		
+		
+		
+def a():
+	print "initial configuration"
+	initConfig()
+		
+def b():
+	print "end configuration"
+	endConfig()
+		
+def c():
+	print "displaying root path"
+	rootPath()
+	
+def d():
+	print "computing contact plan"
+	genPlan()
+	
+def e():
+	print "displaying contact plan"
+	contactPlan()
+	
+print "Root path generated in " + str(tp.t) + " ms."

script/scenarios/demos/darpa_hyq_path.py

+# Importing helper class for setting up a reachability planning problem
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+
+# Importing Gepetto viewer helper class
+from hpp.gepetto import Viewer
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+# URDF file describing the trunk of the robot HyQ
+urdfName = 'hyq_trunk_large'
+# URDF files describing the reachable workspace of each limb of HyQ
+urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+
+# Creating an instance of the helper class, and loading the robot
+rbprmBuilder = Builder ()
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
+# The following lines set constraint on the valid configurations:
+# a configuration is valid only if all limbs can create a contact ...
+rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
+rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support'])
+rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support'])
+rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
+# We also bound the rotations of the torso.
+rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
+
+# Creating an instance of HPP problem solver and the viewer
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+ps = ProblemSolver( rbprmBuilder )
+r = Viewer (ps)
+
+# 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_goal = q_init [::]
+q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
+
+# Choosing a path optimizer
+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)
+#~ 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")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+
+# Solve the problem
+t = ps.solve ()
+if isinstance(t, list):
+	t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]	
+
+# Playing the computed path
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+
+q_far = q_init [::]
+q_far [0:3] = [-2, -3, 0.63]; 
+r(q_far)
+
+for i in range(1,10):
+	rbprmBuilder.client.basic.problem.optimizePath(i)
+	
+
+from hpp.corbaserver import Client
+from hpp.corbaserver.robot import Robot as Parent
+
+class Robot (Parent):
+	rootJointType = 'freeflyer'
+	packageName = 'hpp-rbprm-corba'
+	meshPackageName = 'hpp-rbprm-corba'
+	# URDF file describing the trunk of the robot HyQ
+	urdfName = 'hyq_trunk_large'
+	urdfSuffix = ""
+	srdfSuffix = ""
+	def __init__ (self, robotName, load = True):
+		Parent.__init__ (self, robotName, self.rootJointType, load)
+		self.tf_root = "base_footprint"
+		self.client.basic = Client ()
+		self.load = load
+		
+ #DEMO code to play root path and final contact plan
+cl = Client()
+cl.problem.selectProblem("rbprm_path")
+rbprmBuilder2 = Robot ("toto")
+ps2 = ProblemSolver( rbprmBuilder2 )
+cl.problem.selectProblem("default")
+cl.problem.movePathToProblem(3,"rbprm_path",rbprmBuilder.getAllJointNames())
+r2 = Viewer (ps2)
+r2(q_far)
+

script/scenarios/demos/ground_crouch_hyq_interp.py

+import matplotlib
+#~ matplotlib.use('Agg')
+import matplotlib.pyplot as plt
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+from hpp.gepetto import Viewer
+
+from os import environ
+ins_dir = environ['DEVEL_DIR']
+db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
+
+import ground_crouch_hyq_path as tp
+#~ import ground_crouch_hyq_path_bridge as tp
+
+packageName = "hyq_description"
+meshPackageName = "hyq_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hyq"
+urdfSuffix = ""
+srdfSuffix = ""
+
+fullBody = FullBody ()
+ 
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [-6,5, -4, 4, 0.6, 2])
+
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+nbSamples = 20000
+
+ps = tp.ProblemSolver( fullBody )
+r = tp.Viewer (ps)
+
+rootName = 'base_joint_xyz'
+
+#  Creating limbs
+# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
+cType = "_3_DOF"
+# string identifying the limb
+rLegId = 'rfleg'
+# First joint of the limb, as in urdf file
+rLeg = 'rf_haa_joint'
+# Last joint of the limb, as in urdf file
+rfoot = 'rf_foot_joint'
+# Specifying the distance between last joint and contact surface
+offset = [0.,-0.021,0.]
+# Specifying the contact surface direction when the limb is in rest pose
+normal = [0,1,0]
+# Specifying the rectangular contact surface length
+legx = 0.02; legy = 0.02
+# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
+
+
+
+def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
+	fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
+
+fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "forward", 0.1, cType)
+
+lLegId = 'lhleg'
+lLeg = 'lh_haa_joint'
+lfoot = 'lh_foot_joint'
+fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
+
+rarmId = 'rhleg'
+rarm = 'rh_haa_joint'
+rHand = 'rh_foot_joint'
+fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
+#~ 
+larmId = 'lfleg'
+larm = 'lf_haa_joint'
+lHand = 'lf_foot_joint'
+fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
+
+fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
+fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
+fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
+fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
+
+q_0 = fullBody.getCurrentConfig(); 
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
+q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
+
+fullBody.setCurrentConfig (q_init)
+q_init = fullBody.generateContacts(q_init, [0,0,1])
+q_0 = fullBody.getCurrentConfig(); 
+
+fullBody.setCurrentConfig (q_goal)
+q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+
+fullBody.setStartState(q_init,[])
+fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
+
+r(q_init)
+
+configs = []
+#~ configs = fullBody.interpolate(0.08,1,5) # bridge
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+
+from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
+
+	
+limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector' : rfoot},  
+						lLegId : {'file': "hyq/"+lLegId+"_com.ineq", 'effector' : lfoot},  
+						rarmId : {'file': "hyq/"+rarmId+"_com.ineq", 'effector' : rHand},  
+						larmId : {'file': "hyq/"+larmId+"_com.ineq", 'effector' : lHand} }
+
+
+
+def act(i, numOptim = 0, use_window = 0, friction = 0.5, optim_effectors = True, time_scale = 20,  verbose = False, draw = False, trackedEffectors = []):
+	return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, friction, optim_effectors = optim_effectors, time_scale = time_scale, useCOMConstraints = False, use_window = use_window,
+	verbose = verbose, draw = draw, trackedEffectors = trackedEffectors)
+
+def play(frame_rate = 1./24.):
+	play_traj(fullBody,pp,frame_rate)
+
+#~ for i in range(6,25):
+	#~ act(i, 60, optim_effectors = True)
+
+
+import time
+
+#DEMO METHODS
+
+
+def initConfig():
+	r.client.gui.setVisibility("hyq", "ON")
+	tp.cl.problem.selectProblem("default")
+	tp.r.client.gui.setVisibility("toto", "OFF")
+	tp.r.client.gui.setVisibility("hyq_trunk", "OFF")
+	r(q_init)
+	
+def endConfig():
+	r.client.gui.setVisibility("hyq", "ON")
+	tp.cl.problem.selectProblem("default")
+	tp.r.client.gui.setVisibility("toto", "OFF")
+	tp.r.client.gui.setVisibility("hyq_trunk", "OFF")
+	r(q_goal)
+	
+
+def rootPath():
+	r.client.gui.setVisibility("hyq", "OFF")
+	tp.cl.problem.selectProblem("rbprm_path")
+	tp.r.client.gui.setVisibility("toto", "OFF")
+	r.client.gui.setVisibility("hyq", "OFF")
+	r.client.gui.setVisibility("hyq_trunk", "ON")
+	tp.pp(0)
+	r.client.gui.setVisibility("hyq_trunk", "OFF")
+	r.client.gui.setVisibility("hyq", "ON")
+	tp.cl.problem.selectProblem("default")
+	
+def genPlan():
+	r.client.gui.setVisibility("hyq", "ON")
+	tp.cl.problem.selectProblem("default")
+	tp.r.client.gui.setVisibility("toto", "OFF")
+	tp.r.client.gui.setVisibility("hyq_trunk", "OFF")
+	global configs
+	start = time.clock() 
+	configs = fullBody.interpolate(0.12, 10, 10, True)
+	end = time.clock() 
+	print "Contact plan generated in " + str(end-start) + "seconds"
+	
+def contactPlan():
+	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", "OFF")
+	for i in range(1,len(configs)):
+		r(configs[i]);
+		time.sleep(0.5)		
+		
+		
+def a():
+	print "initial configuration"
+	initConfig()
+		
+def b():
+	print "end configuration"
+	endConfig()
+		
+def c():
+	print "displaying root path"
+	rootPath()
+	
+def d():
+	print "computing contact plan"
+	genPlan()
+	
+def e():
+	print "displaying contact plan"
+	contactPlan()
+
+print "Root path generated in " + str(tp.t) + " ms."
+	

script/scenarios/demos/ground_crouch_hyq_path.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+from hpp.corbaserver import Client
+from hpp.corbaserver.robot import Robot as Parent
+
+class Robot (Parent):
+	rootJointType = 'freeflyer'
+	packageName = 'hpp-rbprm-corba'
+	meshPackageName = 'hpp-rbprm-corba'
+	# URDF file describing the trunk of the robot HyQ
+	urdfName = 'hyq_trunk_large'
+	urdfSuffix = ""
+	srdfSuffix = ""
+	def __init__ (self, robotName, load = True):
+		Parent.__init__ (self, robotName, self.rootJointType, load)
+		self.tf_root = "base_footprint"
+		self.client.basic = Client ()
+		self.load = load
+		
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'hyq_trunk'
+urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+
+rbprmBuilder = Builder ()
+
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,5, -4, 4, 0.6, 2])
+rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
+rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support'])
+rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support'])
+rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
+rbprmBuilder.boundSO3([-0.1,0.1,-1,1,-1,1])
+
+#~ from hpp.corbaserver.rbprm. import ProblemSolver
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+ps = ProblemSolver( rbprmBuilder )
+
+r = Viewer (ps)
+
+
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init [0:3] = [-5, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_goal = q_init [::]
+q_goal [0:3] = [-2.5, 0, 0.63]; #pass the hole
+#~ q_goal [0:3] = [5, 0, 0.63]; r (q_goal) #pass the bridge
+
+ps.addPathOptimizer("RandomShortcut")
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.loadObstacleModel (packageName, "groundcrouch", "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")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+t = ps.solve ()
+
+
+if isinstance(t, list):
+	t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+
+#~ pp (0)
+#~ pp (1)
+r (q_init)
+
+q_far = q_init [::]
+q_far [0:3] = [-2, -3, 0.63]; 
+r(q_far)
+
+for i in range(1,10):
+	rbprmBuilder.client.basic.problem.optimizePath(i)
+	
+        
+cl = Client()
+cl.problem.selectProblem("rbprm_path")
+rbprmBuilder2 = Robot ("toto")
+ps2 = ProblemSolver( rbprmBuilder2 )
+cl.problem.selectProblem("default")
+cl.problem.movePathToProblem(3,"rbprm_path",rbprmBuilder.getAllJointNames())