#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')