#q1bis = q2; q2bis = [0.0, 0.0, -0.8, 1.0, 0.0, 0.0, 0.0] #ps.resetGoalConfigs (); ps.setInitialConfig (q1bis); ps.addGoalConfig (q2bis); ps.solve () # ps.resetGoalConfigs (); ps.setInitialConfig (q1); ps.addGoalConfig (q2bis); ps.solve () #i = ps.numberPaths()-1 ps.setInitialConfig (q1); ps.addGoalConfig (q2) ps.selectPathPlanner ("VisibilityPrmPlanner") #ps.selectPathValidation ("Dichotomy", 0.) #ps.saveRoadmap ('/local/mcampana/devel/hpp/data/puzzle_veryEasy_PRM.rdm') #ps.readRoadmap ('/local/mcampana/devel/hpp/data/puzzle_easy_RRT.rdm') #ps.readRoadmap ('/local/mcampana/devel/hpp/data/puzzle_easy_PRM1.rdm') # srand # problem ? #ps.readRoadmap ('/local/mcampana/devel/hpp/data/puzzle_easy_PRM1.rdm') # srand 1453110445(909sec) [COLL!] #ps.readRoadmap ('/local/mcampana/devel/hpp/data/puzzle_easy_PRM2.rdm') # srand # just after solve, GB OK. But after readroadmap+solve, segfault quaternions.... ps.readRoadmap ('/local/mcampana/devel/hpp/data/puzzle_easy_PRM_test1.rdm') #srand 1454520599 working0.05 # srand 1454521537 (no RM saved) works 7 -> 5, best 0.2 ps.solve () 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)
robot = Robot('lydia') robot.setJointBounds('base_joint_xyz', [-0.9, 0.9, -0.9, 0.9, -1.1, 1.1]) ps = ProblemSolver(robot) r = Viewer(ps) r.loadObstacleModel("hpp_benchmark", "obstacle", "obstacle") q_init = robot.getCurrentConfig() q_goal = q_init[::] q_init[2] = -0.6 q_goal[2] = 0.6 ps.selectPathPlanner("VisibilityPrmPlanner") ps.selectPathValidation("Dichotomy", 0.) ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) ps.readRoadmap("/local/mcampana/devel/hpp/src/hpp_benchmark/roadmap/lydia.rdm") #ps.solve () pp = PathPlayer(robot.client, r) """ ps.addPathOptimizer ("GradientBased") ps.optimizePath (0) ps.numberPaths() ps.pathLength(ps.numberPaths()-1) """