pp = PathPlayer (robot.client, r)
r.loadObstacleModel ("ur_description","obstacles","obstacles")
r.loadObstacleModel ("ur_description","table","table")
r.loadObstacleModel ("ur_description","wall","wall")
r(q1)
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-RRT.rdm')
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-RRT1.rdm')
#ps.selectPathPlanner ("VisibilityPrmPlanner")
#ps.selectPathValidation ("Dichotomy", 0.)
ps.solve ()
ps.pathLength(0)
ps.saveRoadmap ('/local/mcampana/devel/hpp/data/ur5-RRT-testConv1.rdm')
from viewer_display_library_OPTIM import plotPointBodyCurvPath
dt = 0.06
jointName = 'wrist_3_joint'
plotPointBodyCurvPath (r, cl, robot, dt, 0, jointName, [0.28,0,0], 'pathPoint_'+jointName, [0.9,0.1,0.1,1])
ps.addPathOptimizer('RandomShortcut')
ps.optimizePath (0)
ps.pathLength(1)
plotPointBodyCurvPath (r, cl, robot, dt, 1, jointName, [0.28,0,0], 'pathPointRS_'+jointName, [0.1,0.1,0.9,1])
#ps.clearPathOptimizers()
#ps.addPathOptimizer('PartialShortcut')
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
r.loadObstacleModel ("ur_description","table","table")
r.loadObstacleModel ("ur_description","obstacle_sphere","obstacle_sphere")
r(q1)
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-RRT.rdm')
ps.selectPathPlanner ("VisibilityPrmPlanner")
#ps.selectPathValidation ("Dichotomy", 0.)
ps.solve ()
ps.pathLength(0)
ps.saveRoadmap ('/local/mcampana/devel/hpp/data/ur5-PRM1.rdm')
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(q2)