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)
"""
# Playing the computed path
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")
#display path
pp.speed = 0.3
#pp (0)
#display path with post-optimisation
ps.client.problem.extractPath(0, 0, 1.95)
r.client.gui.removeFromGroup("path_0_root", r.sceneName)
pp.displayVelocityPath(1)
#pp (1)
"""
q_far = q_init[::]
q_far[2] = -3
r(q_far)
"""
"""
for i in range(1,10):
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
cl.obstacle.loadObstacleModel('gravity_description','gravity_decor','')
"""
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)))
###########################
#display path with post-optimisation
"""
print("Press Enter to display the optimization ...")
raw_input()
i=0
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 = "li1"
r.client.gui.addLight (lightName, r.windowId, 0.005, [0.5,0.5,0.5,0.5])
r.client.gui.addToGroup (lightName, r.sceneName)
r.client.gui.applyConfiguration (lightName, [-2,0,0,1,0,0,0])
#r.client.gui.applyConfiguration (lightName, [2,0,0,1,0,0,0])
r.client.gui.refresh ()
## Video recording
import time
pp.dt = 0.02
pp.speed=0.7
r(q1)
r.startCapture ("capture","png")
r(q1); time.sleep(0.2)
r(q1)
pp(0)
#pp(ps.numberPaths()-1) # 2363
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
urdfName = "spiderman"
urdfSuffix = ""
srdfSuffix = ""
#~ V0list = tp.V0list
#~ Vimplist = tp.Vimplist
base_joint_xyz_limits = tp.base_joint_xyz_limits
fullBody = FullBody ()
robot = fullBody.client.basic.robot
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", base_joint_xyz_limits)
ps = path_planner.ProblemSolver( fullBody )
r = path_planner.Viewer (ps, viewerClient=path_planner.r.client)
rr = r
pp = PathPlayer (fullBody.client.basic, rr); pp.speed = 0.6
q_0 = fullBody.getCurrentConfig(); rr(q_0)
rLegId = 'RFoot'
lLegId = 'LFoot'
rarmId = 'RHand'
larmId = 'LHand'
rfoot = 'SpidermanRFootSphere'
lfoot = 'SpidermanLFootSphere'
lHand = 'SpidermanLHandSphere'
rHand = 'SpidermanRHandSphere'
nbSamples = 50000; x = 0.03; y = 0.08
fullBody.addLimb(rLegId,'RThigh_rx','SpidermanRFootSphere',[0,0,0],[0,0,1], x, y, nbSamples, "EFORT_Normal", 0.01,"_6_DOF")
fullBody.addLimb(lLegId,'LThigh_rx','SpidermanLFootSphere',[0,0,0],[0,0,1], x, y, nbSamples, "EFORT_Normal", 0.01,"_6_DOF")
fullBody.addLimb(rarmId,'RHumerus_rx','SpidermanRHandSphere',[0,0,0],[0,-1,0], x, y, nbSamples, "EFORT_Normal", 0.01,"_6_DOF", grasp = True)
