#gui.removeFromGroup("path0",r.sceneName)
#gui.getNodeList()
#gui.createGroup ('sphere_wp0_group')
#gui.getGroupNodeList ('sphere_wp0_group')
## IMPORT SCENE AND CONFIGS TO BLENDER ##
# I skip the "collada generation part" since I already have them from blender ...
blender/urdf_to_blender.py -p /local/mcampana/devel/hpp/videos/ -i /local/mcampana/devel/hpp/src/animals_description/urdf/sphere_mesh.urdf -o robot_blend.py # generate robot loadable by Blender
from viewer_display_library import pathToYamlFile, writeEdgeSamples, writePathSamples, writeSkipList
len(np.arange(0, ps.pathLength(pathId), 0.02))
pathToYamlFile (cl, r, "frames.yaml ", "robot/base_link", pathId, q2, 0.02)
ps.numberNodes()
# Plot cones and edges in viewer
plotConesRoadmap (cl, r, 'cone_rm_group', "friction_cone2")
plotEdgesRoadmap (cl, r, 'edgeGroup', 70, [0,1,0.2,1])
gui.writeNodeFile('cone_rm_group','cones_RM.dae')
## Write EDGES in a file, which will be parsed by a Blender-Python script
writeEdgeSamples (cl, 'edges.txt', 70)
## Write PATH samples in a file, which will be parsed by a Blender-Python script
pathSamples = plotSampleSubPath (cl, r, pathId, 70, "path0", [0,0,1,1])
writePathSamples (pathSamples, 'path.txt')
plotFrame (r, "_", [0,0,1.5], 0.5) # First parabolas: vmax = 8m/s, mu = 1.2 cl.problem.setFrictionCoef(1.2); cl.problem.setMaxVelocityLim(4.5) plotCone (q1, cl, r, "cone2", "friction_cone2"); plotCone (q2, cl, r, "cone22", "friction_cone2") ps.clearRoadmap(); solveTime = ps.solve () # 0.312 s pahtId = ps.numberPaths()-offsetOrientedPath # path without orientation stuff samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path0", [0,0,1,1]) plotConeWaypoints (cl, pahtId, r, "wp0", "friction_cone2") plotSpheresWaypoints (cl, pahtId, r, "sphere_wp0", [0,0,1,1], 0.02) print "solve duration: " + str(solveTime) print "path length: " + str(ps.pathLength(pahtId)) print "number of waypoints: " + str(len(ps.getWaypoints (pahtId))) print "number of nodes: " + str(ps.numberNodes ()) cl.problem.getResultValues() # Second parabolas: vmax = 6.5m/s, mu = 0.5 plotCone (q1, cl, r, "cone1", "friction_cone"); plotCone (q2, cl, r, "cone12", "friction_cone") cl.problem.setFrictionCoef(0.5); cl.problem.setMaxVelocityLim(4.5) ps.clearRoadmap(); solveTime = ps.solve () # 0.9689 s pahtId = ps.numberPaths()-offsetOrientedPath samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path2", [1,0,0,1]) plotConeWaypoints (cl, pahtId, r, "wp2", "friction_cone") plotSpheresWaypoints (cl, pahtId, r, "sphere_wp2", [1,0,0,1], 0.02) print "solve duration: " + str(solveTime) print "path length: " + str(ps.pathLength(pahtId)) print "number of waypoints: " + str(len(ps.getWaypoints (pahtId))) print "number of nodes: " + str(ps.numberNodes ())
#gui.removeFromGroup("path0",r.sceneName)
#gui.getNodeList()
#gui.createGroup ('sphere_wp0_group')
#gui.getGroupNodeList ('sphere_wp0_group')
#ffmpeg -i untitled.mp4 -vcodec libx264 -crf 24 video.mp4 # to compress a video
## IMPORT SCENE AND CONFIGS TO BLENDER ##
blender/urdf_to_blender.py -p /local/mcampana/devel/hpp/videos/ -i /local/mcampana/devel/hpp/src/animals_description/urdf/sphere_mesh.urdf -o robot_blend.py # generate robot loadable by Blender
from viewer_display_library import pathToYamlFile, writeEdgeSamples, writePathSamples, writeSkipList
len(np.arange(0, ps.pathLength(pathId), 0.02))
pathToYamlFile (cl, r, "frames.yaml ", "robot/base_link", pathId, q2, 0.02)
ps.numberNodes()
# Plot cones and edges in viewer
plotConesRoadmap (cl, r, 'cone_rm_group', "friction_cone2")
plotEdgesRoadmap (cl, r, 'edgeGroup', 70, [0,1,0.2,1])
gui.writeNodeFile('cone_rm_group','cones_RM.dae')
## Write EDGES in a file, which will be parsed by a Blender-Python script
writeEdgeSamples (cl, 'edges.txt', 70)
## Write PATH samples in a file, which will be parsed by a Blender-Python script
pathSamples = plotSampleSubPath (cl, r, pathId, 70, "path0", [0,0,1,1])
writePathSamples (pathSamples, 'path.txt')
offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2 plotFrame (r, "_", [0,0,2.8], 0.5) # First parabola(s): vmax = 6.8m/s, mu = 1.2 cl.problem.setFrictionCoef(1.2); cl.problem.setMaxVelocityLim(6.8) plotCone (q1, cl, r, "cone2", "friction_cone2"); plotCone (q2, cl, r, "cone22", "friction_cone2") ps.clearRoadmap(); solveTime = ps.solve () # 0.085 s pahtId = ps.numberPaths()-offsetOrientedPath # path without orientation stuff samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path0", [0,0,1,1]) plotConeWaypoints (cl, pahtId, r, "wp0", "friction_cone2") plotSpheresWaypoints (cl, pahtId, r, "sphere_wp0", [0,0,1,1], 0.02) print "solve duration: " + str(solveTime) print "path length: " + str(ps.pathLength(pahtId)) print "number of waypoints: " + str(len(ps.getWaypoints (pahtId))) print "number of nodes: " + str(ps.numberNodes ()) cl.problem.getResultValues () # Second parabola(s): vmax = 5.3m/s, mu = 0.5 plotCone (q1, cl, r, "cone1", "friction_cone"); plotCone (q2, cl, r, "cone12", "friction_cone") cl.problem.setFrictionCoef(0.5); cl.problem.setMaxVelocityLim(5.3) ps.clearRoadmap(); solveTime = ps.solve () # 0.738 s pahtId = ps.numberPaths()-offsetOrientedPath samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path2", [0.2,0.8,0.2,1]) plotConeWaypoints (cl, pahtId, r, "wp2", "friction_cone") plotSpheresWaypoints (cl, pahtId, r, "sphere_wp2", [0.2,0.8,0.2,1], 0.02) print "solve duration: " + str(solveTime) print "path length: " + str(ps.pathLength(pahtId)) print "number of waypoints: " + str(len(ps.getWaypoints (pahtId)))
toSeconds = np.array ([60*60,60,1,1e-3])
offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2
imax=3;
f = open('results.txt','a')
# Assuming that seed in modified directly in HPP (e.g. in core::PathPlanner::solve or ProblemSolver constr)
for i in range(0, imax):
print i
ps.clearRoadmap ()
solveTimeVector = ps.solve ()
solveTime = np.array (solveTimeVector).dot (toSeconds)
pathId = ps.numberPaths()-offsetOrientedPath
pathLength = ps.pathLength (pathId)
pathNumberWaypoints = len(ps.getWaypoints (pathId))
roadmapNumberNodes = ps.numberNodes ()
#TODO: number collisions (checked ???)
#TODO: number parabola that has failed (because of which constraint ??)
#ps.addPathOptimizer("Prune")
#ps.optimizePath (pathId)
#prunePathId = ps.numberPaths()-1
# Write important results #
f.write('Try number: '+str(i)+'\n')
f.write('with parameters: vmax='+str(vmax)+' and mu='+str(mu)+'\n')
f.write('solve duration: '+str(solveTime)+'\n')
f.write('path length: '+str(pathLength)+'\n')
f.write('number of waypoints: '+str(pathNumberWaypoints)+'\n')
f.write('number of nodes: '+str(roadmapNumberNodes)+'\n')
toSeconds = np.array([60 * 60, 60, 1, 1e-3])
offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2
imax = 3
f = open('results.txt', 'a')
# Assuming that seed in modified directly in HPP (e.g. in core::PathPlanner::solve or ProblemSolver constr)
for i in range(0, imax):
print i
ps.clearRoadmap()
solveTimeVector = ps.solve()
solveTime = np.array(solveTimeVector).dot(toSeconds)
pathId = ps.numberPaths() - offsetOrientedPath
pathLength = ps.pathLength(pathId)
pathNumberWaypoints = len(ps.getWaypoints(pathId))
roadmapNumberNodes = ps.numberNodes()
#TODO: number collisions (checked ???)
#TODO: number parabola that has failed (because of which constraint ??)
#ps.addPathOptimizer("Prune")
#ps.optimizePath (pathId)
#prunePathId = ps.numberPaths()-1
# Write important results #
f.write('Try number: ' + str(i) + '\n')
f.write('with parameters: vmax=' + str(vmax) + ' and mu=' + str(mu) + '\n')
f.write('solve duration: ' + str(solveTime) + '\n')
f.write('path length: ' + str(pathLength) + '\n')
f.write('number of waypoints: ' + str(pathNumberWaypoints) + '\n')
f.write('number of nodes: ' + str(roadmapNumberNodes) + '\n')
