#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)
"""
