r.client.gui.applyConfiguration (lightName, [xStone,yEmu-0.5,zEmu,1,0,0,0])
r.client.gui.refresh ()
lightName = "li2"
r.client.gui.addLight (lightName, r.windowId, 0.001, [0.4,0.4,0.4,0.5])
r.client.gui.addToGroup (lightName, r.sceneName)
r.client.gui.applyConfiguration (lightName, [xStone-2,yEmu+0.5,zEmu,1,0,0,0])
r.client.gui.refresh ()
lightName = "l3"
r.client.gui.addLight (lightName, r.windowId, 0.001, [0.4,0.4,0.4,0.5])
r.client.gui.addToGroup (lightName, r.sceneName)
r.client.gui.applyConfiguration (lightName, [xStone+2,yEmu+0.5,zEmu,1,0,0,0])
r.client.gui.refresh ()
## Video recording
pp.dt = 0.01
pp.speed = 1.5
r(q1)
r.startCapture ("capture","png")
pp(nInitialPath)
#pp(ps.numberPaths()-1)
r(q11)
r.stopCapture ()
## ffmpeg commands
ffmpeg -r 50 -i capture_0_%d.png -r 25 -vcodec libx264 video.mp4
x=0; for i in *png; do counter=$(printf %03d $x); ln "$i" new"$counter".png; x=$(($x+1)); done
ffmpeg -r 50 -i new%03d.png -r 25 -vcodec libx264 video.mp4
# Load obstacles in HPP
r(ps.configAtParam(0,2))
ps.getWaypoints (0)
# Add light to scene
lightName = "li4"
r.client.gui.addLight (lightName, r.windowId, 0.01, [0.4,0.4,0.4,0.5])
r.client.gui.addToGroup (lightName, r.sceneName)
r.client.gui.applyConfiguration (lightName, [1,0,0,1,0,0,0])
r.client.gui.refresh ()
## Video recording
pp.dt = 0.02
r.startCapture ("capture","png")
#pp(1)
pp(ps.numberPaths()-1)
r.stopCapture ()
## ffmpeg commands
ffmpeg -r 50 -i capture_0_%d.png -r 25 -vcodec libx264 video.mp4
x=0; for i in *png; do counter=$(printf %03d $x); ln "$i" new"$counter".png; x=$(($x+1)); done
ffmpeg -r 50 -i new%03d.png -r 25 -vcodec libx264 video.mp4
# Load obstacles in HPP
cl.obstacle.loadObstacleModel('gravity_description','gravity_decor','')
cl.obstacle.loadObstacleModel('gravity_description','emu','')
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.client.gui.removeFromGroup("rmgoal",r.sceneName)
for i in range(0,ps.numberNodes()):
r.client.gui.removeFromGroup("vecRM"+str(i),r.sceneName)
"""
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")
"""
# for seed 1486657707
ps.client.problem.extractPath(0,0,2.15)
# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
pp.dt=0.03
pp.displayVelocityPath(1)
r.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
#display path
pp.speed=0.3
#pp (0)
# Choosing RBPRM shooter and path validation methods.
ps.selectConfigurationShooter("RbprmShooter")
ps.addPathOptimizer("RandomShortcutDynamic")
ps.selectPathValidation("RbprmPathValidation", 0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")
# Solve the planning problem :
ps.prepareSolveStepByStep()
ps.finishSolveStepByStep()
#t = ps.solve ()
#print "Guide planning time : ",t
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer(v)
pp.dt = 0.1
pp.displayVelocityPath(0)
v.client.gui.setVisibility("path_0_root", "ALWAYS_ON_TOP")
pp.dt = 0.01
#pp(0)
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)
# copy extraconfig for start and init configurations
q_init[configSize:configSize + 3] = dir_init[::]
q_init[configSize + 3:configSize + 6] = acc_init[::]
q_goal[configSize:configSize + 3] = dir_goal[::]
q_goal[configSize + 3:configSize + 6] = acc_goal[::]
# specifying the full body configurations as start and goal state of the problem
fullBody.setStartStateId(s_init.sId)
fullBody.setEndStateId(s_goal.sId)
q_far = q_init[::]
q_far[2] = -5
from hpp.gepetto import PathPlayer
pp = PathPlayer(fullBody.client.basic, r)
pp.dt = 0.0001
r(q_init)
# computing the contact sequence
#~ configs = fullBody.interpolate(0.08,pathId=1,robustnessTreshold = 2, filterStates = True)
configs = fullBody.interpolate(0.001,
pathId=0,
robustnessTreshold=1,
filterStates=True)
r(configs[-1])
print("number of configs =", len(configs))
r(configs[-1])
from hpp.gepetto import PathPlayer
print(ps.solve())
# display the computed roadmap. Note that the edges are all represented as straight line
# and may not show the real motion of the robot between the nodes :
v.displayRoadmap("rm")
# Alternatively, use the following line instead of ps.solve() to display the roadmap as
# it's computed (note that it slow down a lot the computation)
# v.solveAndDisplay('rm',1)
# Highlight the solution path used in the roadmap
v.displayPathMap("pm", 0)
# remove the roadmap for the scene :
# v.client.gui.removeFromGroup("rm",v.sceneName)
# v.client.gui.removeFromGroup("pm",v.sceneName)
# Connect to a viewer server and play solution paths
pp = PathPlayer(v)
# play path before optimization
pp(0)
# Display the optimized path, with a color variation depending on the velocity along the
# path (green for null velocity, red for maximal velocity)
pp.dt = 0.1
pp.displayVelocityPath(1)
# play path after random shortcut
pp.dt = 0.001
pp(1)
parabPlotDoubleProjCones (cl, 0, theta, NconeOne, pointsConeOne, NconeTwo, pointsConeTwo, plt)
plt.show()
angles = parseAlphaAngles (num_log, '285: alpha_0_min: ', '286: alpha_0_max: ', '303: alpha_lim_minus: ', '302: alpha_lim_plus: ', '317: alpha_imp_inf: ', '318: alpha_imp_sup: ', '290: alpha_inf4: ')
i = 0
parabPlotOriginCones (cl, 0, theta, NconeOne, pointsConeOne, angles, i, 0.6, plt)
plt.show()
# --------------------------------------------------------------------#
## Video capture ##
import time
pp.dt = 0.01; pp.speed=0.5
r(q1)
r.startCapture ("capture","png")
r(q1); time.sleep(0.2); r(q1)
pp(0)
#pp(ps.numberPaths()-1)
r(q2); time.sleep(1);
r.stopCapture ()
## ffmpeg commands
ffmpeg -r 50 -i capture_0_%d.png -r 25 -vcodec libx264 video.mp4
x=0; for i in *png; do counter=$(printf %04d $x); ln "$i" new"$counter".png; x=$(($x+1)); done
ffmpeg -r 50 -i new%04d.png -r 25 -vcodec libx264 video.mp4
mencoder video.mp4 -channels 6 -ovc xvid -xvidencopts fixed_quant=4 -vf harddup -oac pcm -o video.avi
ffmpeg -i untitled.mp4 -vcodec libx264 -crf 24 video.mp4
# --------------------------------------------------------------------#
## Add light to scene ##
lightName = "li3"
r.client.gui.addLight (lightName, r.windowId, 0.001, [0.5,0.5,0.5,1])
r.client.gui.addToGroup (lightName, r.sceneName)
#r.client.gui.applyConfiguration (lightName, [6,0,0.5,1,0,0,0])
#r.client.gui.applyConfiguration (lightName, [4.5,-3,1,1,0,0,0])
r.client.gui.applyConfiguration (lightName, [-4,-1,3,1,0,0,0])
r.client.gui.refresh ()
#r.client.gui.removeFromGroup (lightName, r.sceneName)
# --------------------------------------------------------------------#
## Video capture ##
import time
pp.dt = 0.02; r(q1)
r.startCapture ("capture","png")
r(q1); time.sleep(0.2)
#pp(1)
r(q2); time.sleep(1)
r.stopCapture ()
## ffmpeg commands
ffmpeg -r 50 -i capture_0_%d.png -r 25 -vcodec libx264 video.mp4
x=0; for i in *png; do counter=$(printf %04d $x); ln "$i" new"$counter".png; x=$(($x+1)); done
ffmpeg -r 50 -i new%04d.png -r 25 -vcodec libx264 video.mp4
mencoder video.mp4 -channels 6 -ovc xvid -xvidencopts fixed_quant=4 -vf harddup -oac pcm -o video.avi
ffmpeg -i untitled.mp4 -vcodec libx264 -crf 24 video.mp4
# --------------------------------------------------------------------#
timeValuesGB4 = cl.problem.getTimeValues (); gainValuesGB4 = cl.problem.getGainValues ()
newGainValuesGB4 = ((1-np.array(gainValuesGB4))*100).tolist() #percentage of initial length-value
## -------------------------------------
# Add light to scene
lightName = "li2"
r.client.gui.addLight (lightName, r.windowId, 0.001, [0.8,0.8,0.8,0.7])
r.client.gui.addToGroup (lightName, r.sceneName)
#r.client.gui.applyConfiguration (lightName, [-25,0,0,1,0,0,0])
r.client.gui.applyConfiguration (lightName, [1,0,2,1,0,0,0])
r.client.gui.refresh ()
## Video recording
import time
pp.dt = 0.02
pp.speed=0.5
r(q1)
r.startCapture ("capture","png")
r(q1); time.sleep(0.2)
r(q1)
pp(16)
r(q2); time.sleep(1);
r.stopCapture ()
## ffmpeg commands
ffmpeg -r 50 -i capture_0_%d.png -r 25 -vcodec libx264 video.mp4
x=0; for i in *png; do counter=$(printf %04d $x); ln "$i" new"$counter".png; x=$(($x+1)); done
ffmpeg -r 50 -i new%04d.png -r 25 -vcodec libx264 video.mp4
mencoder video.mp4 -channels 6 -ovc xvid -xvidencopts fixed_quant=4 -vf harddup -oac pcm -o video.avi
ffmpeg -i untitled.mp4 -vcodec libx264 -crf 24 video.mp4
r(q_init)
ps.client.problem.prepareSolveStepByStep()
ps.client.problem.finishSolveStepByStep()
#i = 0
#r.displayRoadmap("rm"+str(i),0.02)
#ps.client.problem.executeOneStep() ;i = i+1; r.displayRoadmap("rm"+str(i),0.02) ; r.client.gui.removeFromGroup("rm"+str(i-1),r.sceneName) ;
#t = ps.solve ()
# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
pp.dt = 1. / 30.
#r.client.gui.removeFromGroup("rm0",r.sceneName)
pp.displayVelocityPath(0)
pp.speed = 0.2
pp(0)
"""
#r.client.gui.removeFromGroup("rm",r.sceneName)
r.client.gui.removeFromGroup("rmPath",r.sceneName)
r.client.gui.removeFromGroup("path_1_root",r.sceneName)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
math.sqrt((np.linalg.norm(u)*np.linalg.norm(u)) * (np.linalg.norm(v)*np.linalg.norm(v))
from hpp import quaternion as Quaternion
ps.addPathOptimizer("RandomShortcutDynamic")
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")
t = ps.solve()
print("Guide planning time : ", t)
#v.solveAndDisplay("rm",10,0.01)
for i in range(20):
print("Optimize path, " + str(i + 1) + "/20 ... ")
ps.optimizePath(ps.numberPaths() - 1)
pathId = ps.numberPaths() - 1
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer(v)
pp.dt = 0.01
pp.displayPath(pathId)
#v.client.gui.setVisibility("path_"+str(pathId)+"_root","ALWAYS_ON_TOP")
pp.dt = 0.01
#pp(pathId)
#v.client.gui.writeNodeFile("path_"+str(pathId)+"_root","guide_path_maze_hard.obj")
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)
# display the computed roadmap. Note that the edges are all represented as straight line and may not show the real motion of the robot between the nodes :
v.displayRoadmap("rm")
#Alternatively, use the following line instead of ps.solve() to display the roadmap as it's computed (note that it slow down a lot the computation)
#v.solveAndDisplay('rm',1)
# Highlight the solution path used in the roadmap
v.displayPathMap('pm',0)
# remove the roadmap for the scene :
#v.client.gui.removeFromGroup("rm",v.sceneName)
#v.client.gui.removeFromGroup("pm",v.sceneName)
# Connect to a viewer server and play solution paths
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
#play path before optimization
pp (0)
# Display the optimized path, with a color variation depending on the velocity along the path (green for null velocity, red for maximal velocity)
pp.dt=0.1
pp.displayVelocityPath(1)
# play path after random shortcut
pp.dt=0.001
pp (1)
