r(q_init)
#r.solveAndDisplay("rm",1,0.01)
#t = ps.solve ()
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
pp.dt = 0.03
pp.displayVelocityPath(0)
r.client.gui.setVisibility("path_0_root", "ALWAYS_ON_TOP")
import parse_bench
parse_bench.parseBenchmark(t)
print "path lenght = ", str(ps.client.problem.pathLength(ps.numberPaths() - 1))
"""
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()
"""
"""
for i in range(0,9):
t = ps.solve()
if isinstance(t, list):
ts = t[0]* 3600. + t[1] * 60. + t[2] + t[3]/1000.
f= open("/local/dev_hpp/logs/benchHrp2_slope_LP.txt","a")
f.write("t = "+str(ts) + "\n")
f.write("path_length = "+str(ps.client.problem.pathLength(i)) + "\n")
"""
t = ps.solve()
#r.displayRoadmap('rm',radiusSphere=0.01)
#r.displayPathMap("pm",0)
#tf=r.solveAndDisplay("rm",1,0.01)
#t = [0,0,tf,0]
#r.client.gui.removeFromGroup("rm_group",r.sceneName)
# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
pp.dt = 0.03
pp.displayVelocityPath(ps.numberPaths() - 1)
#r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
#display path
pp.speed = 0.5
#pp (0)
import parse_bench
parse_bench.parseBenchmark(t)
print "path lenght = ", str(ps.client.problem.pathLength(ps.numberPaths() - 1))
###########################
#display path with post-optimisation
"""
print("Press Enter to display the optimization ...")
raw_input()
-9.560577392578125,
10.504343032836914,
0.9323806762695312,
0.36073973774909973,
0.008668755181133747,
0.02139890193939209]
r.client.gui.setCameraTransform(0,camera)
"""
q = [
1.0220000000000002, -0.01, 0.6955999999999999, 1.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.3, 0.0, -0.17000000000000023, 0.0, 0.0, 0.0
] # BUG
# Playing the computed path
pi = ps.numberPaths() - 1
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
pp.dt = 0.03
pp.displayVelocityPath(pi)
r.client.gui.setVisibility("path_" + str(pi) + "_root", "ALWAYS_ON_TOP")
#display path
pp.speed = 1
#pp (pi)
"""
import parse_bench
parse_bench.parseBenchmark(t)
"""
print("path lenght = ",
str(ps.client.problem.pathLength(ps.numberPaths() - 1)))
0.008668755181133747,
0.02139890193939209]
r.client.gui.setCameraTransform(0,camera)
"""
t = ps.solve()
#r.displayRoadmap('rm',radiusSphere=0.01)
#r.displayPathMap("pm",0)
#tf=r.solveAndDisplay("rm",1,0.01)
#t = [0,0,tf,0]
#r.client.gui.removeFromGroup("rm_group",r.sceneName)
# Playing the computed path
pi = ps.numberPaths() - 1
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
pp.dt = 0.03
pp.displayVelocityPath(pi)
r.client.gui.setVisibility("path_" + str(pi) + "_root", "ALWAYS_ON_TOP")
#display path
pp.speed = 1
#pp (0)
import parse_bench
parse_bench.parseBenchmark(t)
###########################
#display path with post-optimisation
