ps.solve () # PROBLEM !! not finding solution for environment_3d_window with mu=0.5 V0max=6.5 Projectionshooter .... V0 or Vimp too much limiting ?? 'cause V0max=7 gives a too "easy" solution ... samples = plotSampleSubPath (cl, r, 0, 20, "curvy", [0,0.8,0.2,1]) plotConeWaypoints (cl, 0, r, "wp", "friction_cone") #ps.saveRoadmap ('/local/mcampana/devel/hpp/data/PARAB_envir3d_with_window.rdm') r.client.gui.setVisibility('robot/l_bounding_sphere',"OFF") samples = plotSampleSubPath (cl, r, 0, 20, "curvy", [0,0.8,0.2,1]) r(ps.configAtParam(0,0.001)) ps.pathLength(0) ps.getWaypoints (0) ## 3D Plot tools ## q0 = [0, 0, 5, 0, 0, 0, 1, 0, 0, 1, 0]; r(q0) plotFrame (r, "_", [0,0,4], 0.5) plotPath (cl, 0, r, "pathy", 0.1) plotThetaPlane (q1, q2, r, "ThetaPlane2") plotCone (q1, cl, r, 0.5, 0.4, "c1") plotCone (q2, cl, r, 0.5, 0.4, "c2")
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)) from hpp.gepetto import Viewer, PathPlayer r = Viewer(ps) pp = PathPlayer(cl, r) r.loadObstacleModel("potential_description", "obstacles_concaves", "obstacles_concaves") import numpy as np dt = 0.1 nPath = 0 points = [] for t in np.arange(0., cl.problem.pathLength(nPath), dt): points.append([ cl.problem.configAtParam(nPath, t)[0], cl.problem.configAtParam(nPath, t)[1], 0
ps.setInitialConfig (q1proj); ps.addGoalConfig (q2proj) ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "") # environment #RRT-connect: path of 36s with 68 waypoints (2600 var to optimize) #ps.selectPathPlanner ("VisibilityPrmPlanner") ps.solve () ps.optimizePath(0) pp = PathPlayer (robot.client, r) pp (0) pp (1) len(ps.getWaypoints (0)) robot.client.problem.getIterationNumber () ps.pathLength(0) ps.pathLength(1) ## Video recording r.startCapture ("capture","png") pp(0) r.stopCapture () ## DEBUG commands cl.obstacle.getObstaclePosition('obstacle_base') cl.robot.getJointOuterObjects('CHEST_JOINT1') cl.robot.getCurrentConfig() robot.isConfigValid(q1)
ps.solve() ps.resetGoalConfigs() ps.setInitialConfig(q10) ps.addGoalConfig(q11) ps.solve() ps.resetGoalConfigs() ps.setInitialConfig(q11) ps.addGoalConfig(q12) ps.solve() ps.resetGoalConfigs() ps.setInitialConfig(q1) ps.addGoalConfig(q12) ps.solve() # 11 cl.problem.pathLength(11) len(ps.getWaypoints(11)) ps.addPathOptimizer("Prune") ps.optimizePath(11) ps.numberPaths() ps.pathLength(ps.numberPaths() - 1) len(ps.getWaypoints(ps.numberPaths() - 1)) ps.clearPathOptimizers() ps.addPathOptimizer("GradientBased") ps.optimizePath(11) ps.numberPaths() ps.pathLength(ps.numberPaths() - 1) tGB = cl.problem.getTimeGB() timeValuesGB = cl.problem.getTimeValues() gainValuesGB = cl.problem.getGainValues()
# q = [x, y, theta] # (z not considered since planar) q1 = [-4, 4, 1, 0]; q2 = [4, -4, 1, 0] # obstS 1 #q1 = [-4, 4, 0]; q2 = [4, -4, 0] # obstS 1 ps.setInitialConfig (q1); ps.addGoalConfig (q2) cl.obstacle.loadObstacleModel('potential_description','obstacles_concaves','obstacles_concaves') #ps.createOrientationConstraint ("orConstraint", "base_joint_rz", "", [1,0,0,0], [0,0,1]) #ps.setNumericalConstraints ("constraints", ["orConstraint"]) ps.selectPathPlanner ("VisibilityPrmPlanner") ps.selectPathValidation ("Dichotomy", 0.) ps.solve () ps.pathLength(0) len(ps.getWaypoints (0)) 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 (1) ps.numberPaths() ps.pathLength(ps.numberPaths()-1)
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) print "solve duration: " + str(solveTime) print "path length: " + str(ps.pathLength(pahtId)) print "number of waypoints: " + str(len(ps.getWaypoints (pahtId)))
cl.problem.getComputationTime () ps.pathLength(0) ps.pathLength(1) begin=time.time() ps.optimizePath(1) end=time.time() print "Optim2 time: "+str(end-begin) cl.problem.getIterationNumber () ps.pathLength(0) ps.pathLength(1) ps.pathLength(2) len(ps.getWaypoints (0)) ps.optimizePath(3) ps.pathLength(4) cl.problem.getIterationNumber () pp (0) pp (1) ## Video recording r.startCapture ("capture","png") pp(1) r.stopCapture () r.startCapture ("capture","png") pp(2) r.stopCapture ()
robot.setJointBounds('base_joint_xyz', [-6, 6.7, -2.5, 3.2, 0, 3]) # start to bottom q11 = [5.7, 0.5, 0.5, 0, 0, 0, 1, 0, 0, 1, 0] q22 = [-3.5, 1.7, 0.4, 0, 0, 0, 1, 0, 0, 1, 0] cl.problem.generateValidConfig(2) r.client.gui.setVisibility('robot/l_bounding_sphere',"OFF") -5.36273,1.76984,3.28844,0.707032,0.0102817,-0.707032,0.0102817,-0.999577,-0.0290779,-0,-4.57484e+238 r(ps.configAtParam(0,0.001)) ps.pathLength(0) wp = ps.getWaypoints (0) # Get projected random configs CONES and display them num_log = 7308 qrands = parseConfig(num_log,'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/parabola/parabola-planner.cc:157: q_proj: ') sphereNamePrefix="qrand_sphere_" for i in range(0,len(qrands)): qrand = qrands[i] #sphereName = sphereNamePrefix+str(i) #r.client.gui.addSphere (sphereName,0.1,[0.8,0.1,0.1,1]) # red #r.client.gui.applyConfiguration (sphereName, [qrand[0],qrand[1],qrand[2],1,0,0,0]) #r.client.gui.addToGroup (sphereName, r.sceneName) coneName = sphereNamePrefix+str(i) qCone = cl.robot.setOrientation (qrand.tolist()) r.loadObstacleModel ("animals_description","friction_cone2",coneName) r.client.gui.applyConfiguration (coneName, qCone[0:7])
#cl.problem.setFrictionCoef(mu); cl.problem.setMaxVelocityLim(vmax) 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')
from hpp.gepetto import ViewerFactory vf = ViewerFactory(ps) q_init = robot.getCurrentConfig() q_goal = q_init[::] q_init[0:2] = [-60, -4] vf.loadObstacleModel("iai_maps", "ville1", "ville1_link") v = vf.createViewer() ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) ps.addPathOptimizer("RandomShortcut") print(ps.solve()) ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) ps.addPathOptimizer("RandomShortcut") from hpp.gepetto import PathPlayer pp = PathPlayer(v) pp.displayPath(1) #pp (0) import json a = ps.getWaypoints(1) with open('roadmap.txt', 'w') as f: for i in a[0]: json.dump(i, f) f.write("\n")
ps.setInitialConfig (q6); ps.addGoalConfig (q7); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q7); ps.addGoalConfig (q8); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q1); ps.addGoalConfig (q8); ps.solve (); # 7 nInitialPath = ps.numberPaths()-1 #8 ps.pathLength(nInitialPath) ps.addPathOptimizer("GradientBased") #ps.addPathOptimizer("Prune") #ps.addPathOptimizer("PartialRandomShortcut") ps.optimizePath(nInitialPath) ps.pathLength(ps.numberPaths()-1) ps.getWaypoints (nInitialPath) ps.getWaypoints (ps.numberPaths()-1) """ from hpp.gepetto import Viewer, PathPlayer Viewer.withFloor = True r = Viewer (ps) pp = PathPlayer (cl, r) r.loadObstacleModel ("robot_2d_description","cylinder_obstacle","cylinder_obstacle") """ git commit -m "Remove inverse Hessian computation from Cost" ## Debug Optimization Tools ##############
#cl.problem.setFrictionCoef(mu); cl.problem.setMaxVelocityLim(vmax) 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')
# q = [x, y] # limits in URDF file q1 = [-2, 0]; q2 = [-0.2, 2]; q3 = [0.2, 2]; q4 = [2, 0] ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q2); ps.addGoalConfig (q3); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q3); ps.addGoalConfig (q4); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q1); ps.addGoalConfig (q4); ps.solve (); ps.resetGoalConfigs () # pp(3) = p0 final #ps.addPathOptimizer("GradientBased") #ps.addPathOptimizer("Prune") ps.addPathOptimizer("PartialRandomShortcut") ps.optimizePath(3) # pp(4) = p1 final ps.pathLength(3) ps.pathLength(4) ps.getWaypoints (3) ps.getWaypoints (4) # should be [-0.07 0] [0.07 0] if alpha_init=1 """ q1 = [-2, 0]; q2 = [-1, 1] ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve () ps.resetGoalConfigs () q1 = [-1, 1]; q2 = [-1.2, 1.8] ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve () ps.resetGoalConfigs () q1 = [-1.2, 1.8]; q2 = [-0.2, 1.2] ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve () ps.resetGoalConfigs () q1 = [-0.2, 1.2]; q2 = [0.5, 1.9]
# q = [x, y, theta] # (z not considered since planar) q1 = [-2, 0, 1, 0]; q2 = [2, 0, 1, 0] ps.setInitialConfig (q1); ps.addGoalConfig (q2) cl.obstacle.loadObstacleModel('potential_description','cylinder_obstacle','') ps.createOrientationConstraint ("orConstraint", "base_joint_rz", "", [1,0,0,0], [0,0,1]) # OK ps.setNumericalConstraints ("constraints", ["orConstraint"]) ps.solve () ps.configAtParam(0,2) ps.configAtParam(0,6) ps.configAtParam(0,13) ps.getWaypoints (0)[1] ps.optimizePath(0) ps.pathLength(0) ps.pathLength(1) ps.configAtParam(1,2) ps.configAtParam(1,6) ps.configAtParam(1,13) ps.getWaypoints (1)[1] len(ps.getWaypoints (0)) cl.problem.getIterationNumber() """
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") 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))
# Configs : [x, y, z, qw, qx, qy, qz, nx, ny, nz, theta] q1 = [0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0] q2 = [1, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0] from hpp.gepetto import Viewer, PathPlayer r = Viewer (ps) pp = PathPlayer (robot.client, r) #r.loadObstacleModel ("animals_description","cave","cave") #cl.obstacle.loadObstacleModel('animals_description','cave','cave') r(q2) ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve () # verif orientation: r(ps.getWaypoints (0)[0]) # ref r(ps.getWaypoints (1)[0]) # should be good... index = cl.robot.getConfigSize () - 4 q = q1[::] plotStraightLine ([q[index], q[index+1], q[index+2]], q, r, "normale") plotCone (q1, cl, r, 0.5, 0.2, "cone1") plotCone (q2, cl, r, 0.5, 0.2, "cone2") r( ps.configAtParam(0,2) ) ps.pathLength(0) ps.getWaypoints (0) ps.getWaypoints (1) plotPath (cl, 0, r, "pathy", 0.1) # time-step should depend on sub-path length ?
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]) r.client.gui.addToGroup (lightName, r.sceneName) r.client.gui.applyConfiguration (lightName, [1,0,0,1,0,0,0]) r.client.gui.refresh () ## Video recording pp.dt = 0.02 r.startCapture ("capture","png") #pp(1) pp(ps.numberPaths()-1)
#ps.saveRoadmap ('/local/mcampana/devel/hpp/data/PARAB_envir3d_with_window.rdm') # start to bottom ps.resetGoalConfigs () robot.setJointBounds('base_joint_xyz', [-6, 6.7, -2.5, 3.2, 0, 3]) # start to bottom q11 = [5.7, 0.5, 0.5, 0, 0, 0, 1, 0, 0, 1, 0] q22 = [-3.5, 1.7, 0.4, 0, 0, 0, 1, 0, 0, 1, 0] cl.problem.generateValidConfig(2) r.client.gui.setVisibility('robot/l_bounding_sphere',"OFF") r(ps.configAtParam(0,0.001)) ps.pathLength(0) wp = ps.getWaypoints (0) # Get projected random configs CONES and display them num_log = 7308 qrands = parseConfig(num_log,'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/parabola/parabola-planner.cc:157: q_proj: ') sphereNamePrefix="qrand_sphere_" for i in range(0,len(qrands)): qrand = qrands[i] #sphereName = sphereNamePrefix+str(i) #r.client.gui.addSphere (sphereName,0.1,[0.8,0.1,0.1,1]) # red #r.client.gui.applyConfiguration (sphereName, [qrand[0],qrand[1],qrand[2],1,0,0,0]) #r.client.gui.addToGroup (sphereName, r.sceneName) coneName = sphereNamePrefix+str(i) qCone = cl.robot.setOrientation (qrand.tolist()) r.loadObstacleModel ("animals_description","friction_cone2",coneName) r.client.gui.applyConfiguration (coneName, qCone[0:7])
#ps.addPathOptimizer('RandomShortcut') #9 #ps.optimizePath (nInitialPath) #ps.pathLength(ps.numberPaths()-1) #ps.clearPathOptimizers() ps.addPathOptimizer("GradientBased") ps.optimizePath (nInitialPath) ps.numberPaths() ps.pathLength(ps.numberPaths()-1) pp(ps.numberPaths()-1) ps.configAtParam(2,0.5) r(ps.configAtParam(0,2)) ps.getWaypoints (0) ps.getWaypoints (ps.numberPaths()-1) # plot paths import numpy as np dt = 0.1 nPath = ps.numberPaths()-1 lineNamePrefix = "optimized_path" for t in np.arange(0., cl.problem.pathLength(nPath), dt): lineName = lineNamePrefix+str(t) r.client.gui.addLine(lineName,[cl.problem.configAtParam(nPath, t)[0],cl.problem.configAtParam(nPath, t)[1],0],[cl.problem.configAtParam(nPath, t+dt)[0],cl.problem.configAtParam(nPath, t+dt)[1],0],[0,1,0.3,1]) r.client.gui.addToGroup (lineName, r.sceneName) nPath = nInitialPath lineNamePrefix = "initial_path" for t in np.arange(0., cl.problem.pathLength(nPath), dt):
#cl.problem.generateValidConfig(2) q1 = cl.robot.projectOnObstacle (q11, 0.01) robot.isConfigValid(q1) q2 = cl.robot.projectOnObstacle (q22, 0.01) robot.isConfigValid(q2) r(q2) ps.setInitialConfig (q1); ps.addGoalConfig (q2) ps.solve () samples = plotSampleSubPath (cl, r, 0, 20, "curvy", [0,0.8,0.2,1]) #ps.resetGoalConfigs () #ps.saveRoadmap ('/local/mcampana/devel/hpp/data/PARAB_ant_in_room1.rdm') wp = ps.getWaypoints (1) r.client.gui.setVisibility('robot/l_bounding_sphere',"OFF") r(ps.configAtParam(0,0.001)) ps.pathLength(0) ps.getWaypoints (0) ## 3D Plot tools ## q0 = [0, 0, 5, 0, 0, 0, 1, 0, 0, 1, 0]; r(q0) plotFrame (r, "_", [0,0,4], 0.5) plotPath (cl, 0, r, "pathy", 0.1)