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 vf.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen") ps.selectPathValidation ("Dichotomy", 0) import datetime as dt totalTime = dt.timedelta (0) totalNumberNodes = 0 N = 20 for i in range (N): ps.clearRoadmap () ps.resetGoalConfigs () ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) t1 = dt.datetime.now () ps.solve () t2 = dt.datetime.now () totalTime += t2 - t1 print (t2-t1) n = len (ps.client.problem.nodes ()) totalNumberNodes += n print ("Number nodes: " + str(n)) print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (N))) print ("Average number nodes: " + str (totalNumberNodes/float (N)))
pp = PathPlayer (robot.client, r) r.loadObstacleModel ("animals_description","scene_jump_harder","scene_jump_harder") q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001) robot.isConfigValid(q1); robot.isConfigValid(q2) r(q1) ps.setInitialConfig (q1); ps.addGoalConfig (q2) 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) ps.clearRoadmap(); solveTime = ps.solve () # 299 nodes pathId = ps.numberPaths()-offsetOrientedPath # path without orientation stuff pathSamples = plotSampleSubPath (cl, r, pathId, 70, "path0", [0,0,1,1]) plotCone (q1, cl, r, "cone_first", "friction_cone_SG2"); plotCone (q2, cl, r, "cone_second", "friction_cone_SG2") plotConeWaypoints (cl, pathId, r, "cone_wp_group", "friction_cone_WP2") gui.writeNodeFile('cone_wp_group','cones_path.dae') gui.writeNodeFile('cone_first','cone_start.dae') gui.writeNodeFile('cone_second','cone_goal.dae') # Second parabolas: vmax = 6.5m/s, mu = 0.5 # DO NOT SOLVE FIRST PATH BEFORE cl.problem.setFrictionCoef(0.5); cl.problem.setMaxVelocityLim(6.5) ps.clearRoadmap(); solveTime = ps.solve () # 4216 nodes .... 37848 edges
from hpp.corbaserver import ProblemSolver ps = ProblemSolver (robot) from hpp.gepetto import Viewer v = Viewer (ps) q_init = robot.getCurrentConfig () q_goal = q_init [::] q_init [0:3] = [1, 2, 1] q_goal [0:3] = [9.15, 7.5, 0.5] v.loadObstacleModel ("iai_maps", "env_simple", "simple") ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) ps.clearRoadmap() ps.selectPathPlanner("interactive") white=[1.0,1.0,1.0,1.0] brown=[0.85,0.75,0.15,0.5] ps.addPathOptimizer ("RandomShortcut") v.solveAndDisplay("rm1",50,white,0.05,1,brown) #ps.solve() from hpp.gepetto import PathPlayer pp = PathPlayer (robot.client, v) v(q_goal) pp (0) pp (1)