rbprmBuilder.boundSO3([-0.1,0.1,-1,1,-1,1])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ();
q_init = [-6,-3,0.8,1,0,0,0]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = [4, 4, 0.8, 1, 0, 0, 0]; r (q_goal)
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
r.loadObstacleModel (packageName, name_of_scene, "planning")
r.addLandmark(r.sceneName,1)
#~ ps.solve ()
t = ps.solve ()
if isinstance(t, list):
t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
# f = open('log.txt', 'a')
# f.write("path computation " + str(t) + "\n")
# f.close()
#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.1, "obstacle_hyq_robust_10_path.txt")
rbprmBuilder.setCurrentConfig(q_init)
q_goal = q_init[::]
#q_goal[3:7] = [0.7071,0,0,0.7071]
#q_goal [0:3] = [1, 5, 0.65]; r(q_goal)
q_goal[3:7] = [1, 0, 0, 0]
q_goal[0:3] = [2, 1, 0.60]
r(q_goal)
#q_goal[3:7] = [0.9659,0,0.2588,0]
#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_goal = q_init[::]
q_goal[0:3] = [1.2, 0, 0.53]
r(q_goal)
#q_goal [0:3] = [1, 0, 0.75]; r(q_goal)
r(q_goal)
# Choosing a path optimizer
#ps.setInitialConfig (q_init)
#ps.addGoalConfig (q_goal)
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)