q_goal[3:7] = [ 0, 0, 0, 1. ]
q_goal[0] = 1.1
q_goal[2] = Z_VALUE_WOOD
ps.resetGoalConfigs()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# Solve the planning problem :
t = ps.solve()
print("Guide planning done in "+str(t))
pidLast = ps.numberPaths()-1
# concatenate paths :
pid = pidBegin
ps.concatenatePath(pid,pidMiddle)
ps.concatenatePath(pid,pidLast)
"""
for i in range(5):
print "optimize pass : "******"""
try :
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
pp.dt=0.1
pp.displayPath(pid)#pp.displayVelocityPath(0) #
v.client.gui.setVisibility("path_"+str(pid)+"_root","ALWAYS_ON_TOP")
