ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("KinodynamicDistance")
ps.selectPathPlanner("DynamicPlanner")
#solve the problem :
r(q_init)
#ps.client.problem.prepareSolveStepByStep()
q_far = q_init[::]
q_far[2] = -3
r(q_far)
#r.solveAndDisplay("rm",1,0.01)
ps.solve()
# seed = 1486546394 (hyq_trunk, 2m)
"""
camera = [0.6293167471885681,
-9.560577392578125,
10.504343032836914,
0.9323806762695312,
0.36073973774909973,
0.008668755181133747,
0.02139890193939209]
r.client.gui.setCameraTransform(0,camera)
"""
"""
r.client.gui.removeFromGroup("rm",r.sceneName)
r.client.gui.removeFromGroup("rmstart",r.sceneName)
r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ();
q_init = [-6,-3,0.8,1,0,0,0]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = [4, 4, 0.8, 1, 0, 0, 0]; r (q_goal)
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
r.loadObstacleModel (packageName, name_of_scene, "planning")
r.addLandmark(r.sceneName,1)
#~ ps.solve ()
t = ps.solve ()
if isinstance(t, list):
t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
# f = open('log.txt', 'a')
# f.write("path computation " + str(t) + "\n")
# f.close()
#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.1, "obstacle_hyq_robust_10_path.txt")
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (0)
#~ pp (1)
r (q_init)
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.setAffordanceConfig('Lean', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel(packageName, "scale", "planning", r)
#~ afftool.analyseAll()
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
afftool.visualiseAffordances('Lean', r, [0, 0, 0.9])
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation", 0.05)
#~ ps.solve ()
t = ps.solve()
print t
if isinstance(t, list):
t = t[0] * 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
f = open('log.txt', 'a')
f.write("path computation " + str(t) + "\n")
f.close()
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~
#~ pp (2)
#~ pp (0)
q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_init[0:7] = [0.0, 0.0, 0.648702, 1.0, 0.0, 0.0, 0.0]; r (q_init)
q_goal = q_init [::]
q_goal [0:2] = [1.05,0]
ps.setInitialConfig (q_init)
r.loadObstacleModel (packageName, "ground", "planning")
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
### Walk forward
if walk_forward:
ps.addGoalConfig (q_goal)
ps.solve ()
print "Solved forward"
### Walk sideway
q_goal = q_init[:]
q_goal [0:2] = [0, 0.95]
if walk_sideway:
ps.resetGoalConfigs ()
ps.addGoalConfig (q_goal)
ps.solve ()
print "Solved sideway"
### Walk in oblique direction
q_goal = q_init[:]
q_goal [0:2] = [1.05, -0.95]
if walk_oblique: