ps.selectSteeringMethod("RBPRMKinodynamic") ps.selectDistance("KinodynamicDistance") ps.selectPathPlanner("DynamicPlanner") #solve the problem : r(q_init) #ps.client.problem.prepareSolveStepByStep() q_far = q_init[::] q_far[2] = -3 r(q_far) #r.solveAndDisplay("rm",1,0.01) ps.solve() # seed = 1486546394 (hyq_trunk, 2m) """ camera = [0.6293167471885681, -9.560577392578125, 10.504343032836914, 0.9323806762695312, 0.36073973774909973, 0.008668755181133747, 0.02139890193939209] r.client.gui.setCameraTransform(0,camera) """ """ r.client.gui.removeFromGroup("rm",r.sceneName) r.client.gui.removeFromGroup("rmstart",r.sceneName)
r = Viewer (ps) q_init = rbprmBuilder.getCurrentConfig (); q_init = [-6,-3,0.8,1,0,0,0]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) q_goal = [4, 4, 0.8, 1, 0, 0, 0]; r (q_goal) ps.addPathOptimizer("RandomShortcut") ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectPathValidation("RbprmPathValidation",0.01) r.loadObstacleModel (packageName, name_of_scene, "planning") r.addLandmark(r.sceneName,1) #~ ps.solve () t = ps.solve () if isinstance(t, list): t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3] # f = open('log.txt', 'a') # f.write("path computation " + str(t) + "\n") # f.close() #~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.1, "obstacle_hyq_robust_10_path.txt") from hpp.gepetto import PathPlayer pp = PathPlayer (rbprmBuilder.client.basic, r) #~ pp (0) #~ pp (1) r (q_init)
ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.setAffordanceConfig('Lean', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel(packageName, "scale", "planning", r) #~ afftool.analyseAll() afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) afftool.visualiseAffordances('Lean', r, [0, 0, 0.9]) ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectPathValidation("RbprmPathValidation", 0.05) #~ ps.solve () t = ps.solve() print t if isinstance(t, list): t = t[0] * 3600000 + t[1] * 60000 + t[2] * 1000 + t[3] f = open('log.txt', 'a') f.write("path computation " + str(t) + "\n") f.close() from hpp.gepetto import PathPlayer pp = PathPlayer(rbprmBuilder.client.basic, r) #~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path") #~ #~ pp (2) #~ pp (0)
q_init = rbprmBuilder.getCurrentConfig (); r (q_init) q_init[0:7] = [0.0, 0.0, 0.648702, 1.0, 0.0, 0.0, 0.0]; r (q_init) q_goal = q_init [::] q_goal [0:2] = [1.05,0] ps.setInitialConfig (q_init) r.loadObstacleModel (packageName, "ground", "planning") ps.addPathOptimizer("RandomShortcut") ps.setInitialConfig (q_init) ### Walk forward if walk_forward: ps.addGoalConfig (q_goal) ps.solve () print "Solved forward" ### Walk sideway q_goal = q_init[:] q_goal [0:2] = [0, 0.95] if walk_sideway: ps.resetGoalConfigs () ps.addGoalConfig (q_goal) ps.solve () print "Solved sideway" ### Walk in oblique direction q_goal = q_init[:] q_goal [0:2] = [1.05, -0.95] if walk_oblique: