r.client.gui.writeNodeFile(0, 'scene.osg') # osgconvd -O NoExtras scene.osg scene.dae from hpp.gepetto.blender.exportmotion import exportStates, exportPath exportPath(r, cl.robot, cl.problem, 0, 1, 'path2.txt') exportStates(r, cl.robot, q11, 'configs.txt') from gepettoimport import loadmotion loadmotion('/local/mcampana/devel/hpp/videos/path2.txt') # and rename first node manually ... # --------------------------------------------------------------------# ## DEBUG commands cl.obstacle.getObstaclePosition('decor_base') robot.isConfigValid(q1) robot.setCurrentConfig(q1) res=robot.distancesToCollision() r( ps.configAtParam(0,5) ) ps.optimizePath (0) ps.clearRoadmap () ps.resetGoalConfigs () from numpy import * argmin(robot.distancesToCollision()[0]) robot.getJointNames () robot.getConfigSize () r.client.gui.getNodeList() ['0_scene_hpp_', 'inclined_plane_3d', 'inclined_plane_3d/l_one', 'inclined_plane_3d/l_one_0', 'inclined_plane_3d/obstacle_base', 'inclined_plane_3d/obstacle_base_0', 'robot', 'robot/base_link', 'robot/base_link_0'] # get all theta: for i in range(0,len(wp)): print wp[i][10]
q2hard = [7.60, -2.41, 0.545, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.8, 0.0, -0.4, -0.55, 0.0, -0.6, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -2.8, 0.0, 0.1, -0.2, -0.1, 0.4, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.1, 1.2, -0.4, 0.2, -0.3, 0.0, -0.4, 0.2, 0.7, 0.0] robot.isConfigValid(q1) 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)
import datetime as dt totalSolveTime = dt.timedelta (0) totalOptimTime = dt.timedelta (0) totalNumberNodes = 0 N = 20 for i in range (N): ps.clearPathOptimizers() ps.clearRoadmap () ps.resetGoalConfigs () ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) t1 = dt.datetime.now () ps.solve () t2 = dt.datetime.now () ps.addPathOptimizer ("SplineGradientBased_bezier3") ps.optimizePath (ps.numberPaths() - 1) t3 = dt.datetime.now () totalSolveTime += t2 - t1 totalOptimTime += t3 - t2 print "Solve:", t2-t1 print "Optim:", t3-t2 n = len (ps.client.problem.nodes ()) totalNumberNodes += n print ("Number nodes: " + str(n)) print ("Average solve time: " + str ((totalSolveTime.seconds+1e-6*totalSolveTime.microseconds)/float (N))) print ("Average optim time: " + str ((totalOptimTime.seconds+1e-6*totalOptimTime.microseconds)/float (N))) print ("Average number nodes: " + str (totalNumberNodes/float (N)))
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.clearPathOptimizers() ps.addPathOptimizer('PartialShortcut')
q_init = robot.getCurrentConfig() q_goal = q_init[::] q_init[0:2] = [-3.2, -4] rank = robot.rankInConfiguration["torso_lift_joint"] q_init[rank] = 0.2 # r (q_init) q_goal[0:2] = [-3.2, -4] rank = robot.rankInConfiguration["l_shoulder_lift_joint"] q_goal[rank] = 0.5 rank = robot.rankInConfiguration["l_elbow_flex_joint"] q_goal[rank] = -0.5 rank = robot.rankInConfiguration["r_shoulder_lift_joint"] q_goal[rank] = 0.5 rank = robot.rankInConfiguration["r_elbow_flex_joint"] q_goal[rank] = -0.5 # r (q_goal) # r.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen") ps.loadObstacleFromUrdf("iai_maps", "kitchen_area", "kitchen/") ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) print(ps.solve()) ps.addPathOptimizer("RandomShortcut") print(ps.optimizePath(0))
ps.setInitialConfig(q1) ps.addGoalConfig(q2) #cl.obstacle.loadObstacleModel('room_description','room','') #cl.obstacle.loadObstacleModel('room_description','walls','') #ps.selectPathPlanner ("VisibilityPrmPlanner") begin = time.time() ps.solve() end = time.time() print "Solving time: " + str(end - begin) ps.addPathOptimizer("GradientBased") begin = time.time() ps.optimizePath(0) end = time.time() print "Optim time: " + str(end - begin) cl.problem.getIterationNumber() ps.pathLength(0) ps.pathLength(1) ps.optimizePath(1) # first use of active comp 1.601s 19iter ps.pathLength(2) len(ps.getWaypoints(0)) pp(1) ## Video recording
ps.setInitialConfig (q3); ps.addGoalConfig (q4); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q4); ps.addGoalConfig (q5); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q5); ps.addGoalConfig (q6); ps.solve (); ps.resetGoalConfigs () 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") """
q_init = robot.getCurrentConfig () q_goal = q_init [::] q_init [0:2] = [-3.2, -4] rank = robot.rankInConfiguration ['torso_lift_joint'] q_init [rank] = 0.2 # r (q_init) q_goal [0:2] = [-3.2, -4] rank = robot.rankInConfiguration ['l_shoulder_lift_joint'] q_goal [rank] = 0.5 rank = robot.rankInConfiguration ['l_elbow_flex_joint'] q_goal [rank] = -0.5 rank = robot.rankInConfiguration ['r_shoulder_lift_joint'] q_goal [rank] = 0.5 rank = robot.rankInConfiguration ['r_elbow_flex_joint'] q_goal [rank] = -0.5 # r (q_goal) # r.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen") ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "kitchen/") ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) print ps.solve () ps.addPathOptimizer ("RandomShortcut") print ps.optimizePath (0)
""" ps.setInitialConfig (q1); ps.addGoalConfig (q2) #cl.obstacle.loadObstacleModel('room_description','room','') #cl.obstacle.loadObstacleModel('room_description','walls','') #ps.selectPathPlanner ("VisibilityPrmPlanner") begin=time.time() ps.solve () end=time.time() print "Solving time: "+str(end-begin) ps.addPathOptimizer("GradientBased") begin=time.time() ps.optimizePath (0) end=time.time() print "Optim time: "+str(end-begin) cl.problem.getIterationNumber () ps.pathLength(0) ps.pathLength(1) ps.optimizePath (1) # first use of active comp 1.601s 19iter ps.pathLength(2) len(ps.getWaypoints (0)) pp (1)
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-sphere-PRM.rdm') #ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-sphere-RRT.rdm') ps.selectPathPlanner ("VisibilityPrmPlanner") #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
q8 = [0, 0, 0, 0.707106781, 0, 0, -0.707106781] ps.setInitialConfig (q7); ps.addGoalConfig (q8) ps.solve (); ps.resetGoalConfigs () q9 = [0, 0, 0, 1, 0, 0, 0] ps.setInitialConfig (q8); ps.addGoalConfig (q9) ps.solve (); ps.resetGoalConfigs () """ # Only path to do a useless turn ! (Not the case if solved separately) # r( ps.configAtParam(7,2.17) ) = [0.0, 0.0, 0.0, -0.9999559023922103, 0.0, 0.0, -0.00939112724262876] ps.setInitialConfig (q1); ps.addGoalConfig (q5) # do a turn as expected ps.solve () ps.optimizePath (4) # reduced to a point as expected len(ps.getWaypoints (0)) ps.pathLength(5) # = 0 ##############################" # TEST quaternions x0 HRP2 + optimisation = meme resultat (-1) (un ou 2 tours)? # Oui mais il faut imposer alpha = alpha_init et ne pas MàJ H1_ # les valeurs propres de la Hessienne sont bien >0 q1 = [0, 0, 0, 0.9999028455952614, -0.00643910090823595, -0.012362740316661774, 1.3620998722148461e-06] q2 = [0, 0, 0, 0.8258496711518952, -0.2042733013060623, -0.19724860126354768, -0.4871732015735299] ps.setInitialConfig (q1); ps.addGoalConfig (q2) ps.solve (); ps.resetGoalConfigs () q3 = [0, 0, 0, 0.6659085913630002, -0.04107471639409278, 0.06948325706470262, -0.7416540248726288]
ps = ProblemSolver (robot) cl = robot.client cl.obstacle.loadObstacleModel('robot_2d_description','cylinder_obstacle','') # 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 ()
#ps.createPositionConstraint ("posConstraint", "torso_lift_joint", "", [0,0,0], [-3.25, -4.0, 0.790675], [1,1,0]) #ps.setNumericalConstraints ("constraints", ["posConstraint"]) ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "") # environment ps.setInitialConfig (q_init); ps.addGoalConfig (q_goal) #ps.selectPathPlanner ("VisibilityPrmPlanner") begin=time.time() ps.solve () end=time.time() print "Solving time: "+str(end-begin) ps.addPathOptimizer("GradientBased") begin=time.time() ps.optimizePath(0) end=time.time() print "Optim time: "+str(end-begin) cl = robot.client cl.problem.getIterationNumber () 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.addGoalConfig(q_goal) # Choosing RBPRM shooter and path validation methods. ps.selectConfigurationShooter("RbprmShooter") ps.addPathOptimizer("RandomShortcutDynamic") ps.selectPathValidation("RbprmPathValidation", 0.05) # Choosing kinodynamic methods : ps.selectSteeringMethod("RBPRMKinodynamic") ps.selectDistance("Kinodynamic") ps.selectPathPlanner("DynamicPlanner") # Solve the planning problem : t = ps.solve() print "Guide planning time : ", t ps.optimizePath(1) pid = ps.numberPaths() - 1 # display solution : from hpp.gepetto import PathPlayer pp = PathPlayer(v) pp.dt = 0.1 #pp.displayVelocityPath(pid) #v.client.gui.setVisibility("path_2_root","ALWAYS_ON_TOP") pp.dt = 0.01 #pp(0) # move the robot out of the view before computing the contacts q_far = q_init[::] q_far[2] = -2 v(q_far)
q_init = robot.getCurrentConfig() q_goal = q_init[::] q_init[0:2] = [-3.2, -4] rank = robot.rankInConfiguration['torso_lift_joint'] q_init[rank] = 0.2 # r (q_init) q_goal[0:2] = [-3.2, -4] rank = robot.rankInConfiguration['l_shoulder_lift_joint'] q_goal[rank] = 0.5 rank = robot.rankInConfiguration['l_elbow_flex_joint'] q_goal[rank] = -0.5 rank = robot.rankInConfiguration['r_shoulder_lift_joint'] q_goal[rank] = 0.5 rank = robot.rankInConfiguration['r_elbow_flex_joint'] q_goal[rank] = -0.5 # r (q_goal) # r.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen") ps.loadObstacleFromUrdf("iai_maps", "kitchen_area", "kitchen/") ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) print ps.solve() ps.addPathOptimizer("RandomShortcut") print ps.optimizePath(0)
q_goal[2] = Z_VALUE_WOOD ps.resetGoalConfigs() ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) # Solve the planning problem : t = ps.solve() print("Guide planning done in " + str(t)) pidLast = ps.numberPaths() - 1 # concatenate paths : pid = pidBegin ps.concatenatePath(pid, pidMiddle) ps.concatenatePath(pid, pidLast) """ ps.optimizePath(pid) pid = ps.numberPaths()-1 """ try: # display solution : from hpp.gepetto import PathPlayer pp = PathPlayer(v) pp.dt = 0.1 pp.displayPath(pid) #pp.displayVelocityPath(0) # v.client.gui.setVisibility("path_" + str(pid) + "_root", "ALWAYS_ON_TOP") pp.dt = 0.01 pp(pid) except Exception: pass
# Choosing kinodynamic methods : ps.selectSteeringMethod("RBPRMKinodynamic") ps.selectDistance("Kinodynamic") ps.selectPathPlanner("DynamicPlanner") ps.addPathOptimizer ("RandomShortcutDynamic") #ps.addPathOptimizer ("RandomShortcut") # Solve the planning problem : t = ps.solve () print("Guide planning time : ",t) #v.solveAndDisplay('rm',2,0.001) #v.client.gui.removeFromGroup("rm",v.sceneName) pid = ps.numberPaths()-1 for i in range(5): print("Optimization pass ",i) ps.optimizePath(pid) pid = ps.numberPaths()-1 # display solution : from hpp.gepetto import PathPlayer pp = PathPlayer (v) pp.dt=0.1 pp.displayVelocityPath(pid) #v.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP") pp.dt = 0.01 pp(pid) # move the robot out of the view before computing the contacts q_far = q_init[::] q_far[2] = -2
ps.setInitialConfig (Q[i]); ps.addGoalConfig (Q[i+1]); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (Q[0]); ps.addGoalConfig (Q[len(Q)-1]); ps.solve (); nInitialPath = ps.numberPaths()-1 #8 ps.pathLength(nInitialPath) #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
ps.resetGoalConfigs() ps.setInitialConfig(q8) ps.addGoalConfig(q9) ps.solve() ps.resetGoalConfigs() ps.setInitialConfig(q9) ps.addGoalConfig(q10) ps.solve() ps.resetGoalConfigs() ps.setInitialConfig(q1) ps.addGoalConfig(q10) ps.solve() print ps.pathLength(9) ps.addPathOptimizer("GradientBased") ps.optimizePath(9) ps.numberPaths() print ps.pathLength(ps.numberPaths() - 1) """ # pp(9) = p0 final ps.optimizePath(9) # pp(10) = p1 final ps.pathLength(9) ps.pathLength(10) ps.addPathOptimizer('RandomShortcut') ps.optimizePath (9) ps.pathLength(10) ps.clearPathOptimizers()