ps.addGoalConfig(q_goal)
# Choosing RBPRM shooter and path validation methods.
ps.selectConfigurationShooter("RbprmShooter")
ps.addPathOptimizer("RandomShortcutDynamic")
ps.selectPathValidation("RbprmPathValidation", 0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")
# Solve the planning problem :
ps.prepareSolveStepByStep()
ps.finishSolveStepByStep()
#t = ps.solve ()
#print "Guide planning time : ",t
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer(v)
pp.dt = 0.1
pp.displayVelocityPath(0)
v.client.gui.setVisibility("path_0_root", "ALWAYS_ON_TOP")
pp.dt = 0.01
#pp(0)
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
