robot.isConfigValid(q2) # qf should be invalid qf = [1, -3, 3, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.2, 1.0, -0.4, -1.0, 0.0, -0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.5, -0.2, 0.1, -0.3, 0.1, 0.1, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0] robot.isConfigValid(qf) ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve () ps.solve () ps.pathLength(0) ps.addPathOptimizer('RandomShortcut') ps.optimizePath (0) ps.pathLength(1) ps.clearPathOptimizers() ps.addPathOptimizer("GradientBased") ps.optimizePath (0) ps.numberPaths() ps.pathLength(ps.numberPaths()-1) pp(ps.numberPaths()-1) 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])
ps.pathLength(0) len(ps.getWaypoints (0)) r(q1) import numpy as np """ ps.addPathOptimizer("Prune") ps.optimizePath (0) ps.numberPaths() ps.pathLength(ps.numberPaths()-1) len(ps.getWaypoints (ps.numberPaths()-1)) """ ps.clearPathOptimizers() cl.problem.setAlphaInit (0.05) ps.addPathOptimizer("GradientBased") ps.optimizePath (0) ps.numberPaths() ps.pathLength(ps.numberPaths()-1) tGB = cl.problem.getTimeGB () timeValuesGB = cl.problem.getTimeValues () gainValuesGB = cl.problem.getGainValues () newGainValuesGB = ((1-np.array(gainValuesGB))*100).tolist() #percentage of initial length-value ps.clearPathOptimizers() ps.addPathOptimizer('RandomShortcut') ps.optimizePath (0) ps.pathLength(ps.numberPaths()-1)
ps.pathLength(ps.numberPaths() - 1) import matplotlib.pyplot as plt from mutable_trajectory_plot import planarPlot, addNodePlot from parseLog import parseCollConstrPoints num_log = 31891 contactPoints = parseCollConstrPoints(num_log, '77: contact point = (') plt = planarPlot(cl, 0, ps.numberPaths() - 1, plt, 1.5, 5) plt = addNodePlot(contactPoints, 'ko', '', 5.5, plt) plt.show() ps.addPathOptimizer('RandomShortcut') ps.optimizePath(0) ps.pathLength(1) ps.clearPathOptimizers() ps.addPathOptimizer('PartialShortcut') ps.optimizePath(0) ps.pathLength(ps.numberPaths() - 1) ps.clearPathOptimizers() ps.addPathOptimizer("GradientBased") ps.optimizePath(0) ps.numberPaths() ps.pathLength(ps.numberPaths() - 1) pp(ps.numberPaths() - 1) ps.clearPathOptimizers() len(ps.getWaypoints(0))
#ps.selectPathValidation ("Dichotomy", 0.) ps.solve () ps.pathLength(0) len(ps.getWaypoints (0)) #ps.saveRoadmap ('/local/mcampana/devel/hpp/data/ur5-sphere-PRM.rdm') ps.addPathOptimizer("Prune") # NO CHANGE WITH PRM+DISCR ps.optimizePath (0) ps.numberPaths() ps.pathLength(ps.numberPaths()-1) len(ps.getWaypoints (ps.numberPaths()-1)) ps.clearPathOptimizers() cl.problem.setAlphaInit (0.3) ps.addPathOptimizer("GradientBased") ps.optimizePath (0) ps.numberPaths() ps.pathLength(ps.numberPaths()-1) tGB = cl.problem.getTimeGB () timeValuesGB = cl.problem.getTimeValues () gainValuesGB = cl.problem.getGainValues () newGainValuesGB = ((1-np.array(gainValuesGB))*100).tolist() #percentage of initial length-value ps.clearPathOptimizers() ps.addPathOptimizer('RandomShortcut') ps.optimizePath (0) ps.pathLength(ps.numberPaths()-1)