#q_goal[7:10] = [vMax,0,-2]
#q_goal [0:3] = [0, 1, 0.8]; r(q_goal)
r(q_goal)
#~ q_goal [0:3] = [-1.5, 0, 0.63]; r (q_goal)
# Choosing a path optimizer
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
# Choosing RBPRM shooter and path validation methods.
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation", 0.05)
# Choosing kinodynamic methods :
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)
q1 = q_init[::]
q1[0:3] = [-0.3, 0, 0.77]
q2 = q_goal[::]
q2[0:3] = [0.8, 0, 0.77]
ps.setInitialConfig(q1)
ps.addGoalConfig(q2)
# Choosing RBPRM shooter and path validation methods.
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation", 0.01)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("KinodynamicDistance")
#ps.addPathOptimizer("RandomShortcutDynamic")
#ps.addPathOptimizer("OrientedPathOptimizer")
ps.selectPathPlanner("BiRRTPlanner")
#ps.selectPathPlanner("DynamicPlanner")
#solve the problem :
r(q_init)
ps.client.problem.prepareSolveStepByStep()
"""
q1=q_init[::]
q1[0:3] = [-0.45, 0, 0.75]
q2=q_goal[::]
q2[0:3] = [0.9, 0, 0.75]
pbCl = rbprmBuilder.client.basic.problem
pbCl.addConfigToRoadmap (q1)