plotSphere (q2, cl, r, "sphere_q2", [0,1,0,1], 0.02) # same as robot nPointsPlot = 50 offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2 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)
ps.setInitialConfig (q1); ps.addGoalConfig (q2) plotSphere (q2, cl, r, "sphere1", [0,1,0,1], 0.02) nPointsPlot = 60 offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2 plotFrame (r, "_", [0,0,3.1], 0.5) # First parabola: vmax = 7m/s, mu = 1.2 cl.problem.setFrictionCoef(1.2); cl.problem.setMaxVelocityLim(7) plotCone (q1, cl, r, "cone2", "friction_cone2"); plotCone (q2, cl, r, "cone22", "friction_cone2") solveTime = ps.solve () 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))) # Second parabola: vmax = 7m/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(7) ps.clearRoadmap(); solveTime = ps.solve () pahtId = ps.numberPaths()-offsetOrientedPath samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path1", [0.1,0.8,0.8,1]) plotConeWaypoints (cl, pahtId, r, "wp1", "friction_cone") plotSpheresWaypoints (cl, pahtId, r, "sphere_wp1", [0.1,0.8,0.8,1], 0.02) print "solve duration: " + str(solveTime) print "path length: " + str(ps.pathLength(pahtId))
ps.setInitialConfig (q1); ps.addGoalConfig (q2) plotSphere (q2, cl, r, "sphere_q2", [0,1,0,1], 0.02) # same as robot nPointsPlot = 50 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")
plotSphere (q2, cl, r, "sphere_q2", [0,1,0,1], 0.02) # same as robot nPointsPlot = 50 offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2 plotFrame (r, 'frame_group', [0,0,0], 0.6) # First parabolas: vmax = 7m/s, mu = 1.2 cl.problem.setFrictionCoef(1.2); cl.problem.setMaxVelocityLim(7) plotCone (q1, cl, r, "cone_11", "friction_cone2"); plotCone (q2, cl, r, "cone_21", "friction_cone2") ps.clearRoadmap(); solveTime = ps.solve () pahtId = ps.numberPaths()-offsetOrientedPath # path without orientation stuff samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path0", [0,0,1,1]) plotConeWaypoints (cl, pahtId, r, "cone_wp_group", "friction_cone2") plotSpheresWaypoints (cl, pahtId, r, "sphere_wp_group", [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, "cone12", "friction_cone"); plotCone (q2, cl, r, "cone22", "friction_cone") cl.problem.setFrictionCoef(0.5); cl.problem.setMaxVelocityLim(6.5) ps.clearRoadmap(); solveTime = ps.solve () pahtId = ps.numberPaths()-offsetOrientedPath samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path2", [0.9,0.1,0.1,1]) # Red plotConeWaypoints (cl, pahtId, r, "cone_wp_group2", "friction_cone") plotSpheresWaypoints (cl, pahtId, r, "sphere_wp_group2", [0.9,0.1,0.1,1], 0.02)
