try:
t1 = dt.datetime.now()
ps.solve()
success += 1
t2 = dt.datetime.now()
totalTime += t2 - t1
print(t2 - t1)
n = ps.numberNodes()
totalNumberNodes += n
print("Number nodes: " + str(n))
except:
print("Failed to plan path.")
print("Number of successes: " + str(success))
print("Average time: " +
str((totalTime.seconds + 1e-6 * totalTime.microseconds) /
float(success)))
print("Average number nodes: " + str(totalNumberNodes / float(success)))
#check if there are collisions, discontinuities or wrong configurations in each path
p = PathChecker(ps, q_init, q_goal)
dtime = 0.001
for i in range(ps.numberPaths()):
print("\n---Path {}---".format(i))
p.check_path(i, dtime)
# 1}}}
if args.display:
from hpp.gepetto import PathPlayer
v = vf.createViewer()
v(q_init)
pp = PathPlayer(v, robot.client.basic)
if args.run:
pp(0)
ps.resetGoalConfigs()
ps.setInitialConfig(q_init_proj)
ps.addGoalConfig(q_goal_proj)
t1 = dt.datetime.now()
ps.solve()
t2 = dt.datetime.now()
totalTime += t2 - t1
print(t2 - t1)
n = ps.numberNodes()
totalNumberNodes += n
print("Number nodes: " + str(n))
if args.N != 0:
print("Average time: " +
str((totalTime.seconds + 1e-6 * totalTime.microseconds) /
float(args.N)))
print("Average number nodes: " + str(totalNumberNodes / float(args.N)))
#check if there are collisions, discontinuities or wrong configurations in each path
p = PathChecker(ps, q_init_proj, q_goal_proj)
dtime = 0.001
for i in range(ps.numberPaths()):
print("\n---Path {}---".format(i))
p.check_path(i, dtime)
if args.display:
v = vf.createViewer()
pp = PathPlayer(v)
if args.run:
pp(0)
A = '\n'
sys.stdout.write(A)
q_init = robot.getCurrentConfig()
q_init[0:4] = [-3.2, -4, 1, 0] # FIX ME ! (see up )
rank = robot.rankInConfiguration['pr2/r_gripper_l_finger_joint']
q_init[rank] = 0.5
rank = robot.rankInConfiguration['pr2/r_gripper_r_finger_joint']
q_init[rank] = 0.5
rank = robot.rankInConfiguration['pr2/l_gripper_l_finger_joint']
q_init[rank] = 0.5
rank = robot.rankInConfiguration['pr2/l_gripper_r_finger_joint']
q_init[rank] = 0.5
rank = robot.rankInConfiguration['pr2/torso_lift_joint']
q_init[rank] = 0.2
q_goal = q_init[::]
#check if there are collisions in each path
p = PathChecker(ps, q_init, q_goal)
dtime = 0.001
for i in range(ps.numberPaths()):
print("\n---Path {}---".format(i))
p.check_path(i, dtime)
rank = robot.rankInConfiguration['box/root_joint']
c = sqrt(2) / 2
#q_init [rank:rank+4] = [c, 0, c, 0]
q_init[rank:rank + 7] = [-2.5, -3.6, 0.76, 0, c, 0, c]
#rank = robot.rankInConfiguration ['box/base_joint_SO3']
rank = robot.rankInConfiguration['box/root_joint']
q_goal[rank:rank + 7] = [-2.5, -4.4, 0.76, 0, -c, 0, c]
#rank = robot.rankInConfiguration ['box/base_joint_SO3']
#q_goal [rank:rank+4] = [c, 0, -c, 0]
ps.selectPathValidation ("Progressive", 0.025)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
for i in range (args.N):
ps.client.problem.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q1proj)
ps.addGoalConfig (q2proj)
t1 = dt.datetime.now ()
ps.solve ()
t2 = dt.datetime.now ()
totalTime += t2 - t1
print((t2-t1))
n = len (ps.client.problem.nodes ())
totalNumberNodes += n
print(("Number nodes: " + str(n)))
print(("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (args.N))))
print(("Average number nodes: " + str (totalNumberNodes/float (args.N))))
#check if there are collisions, discontinuities or wrong configurations in each path
p = PathChecker(ps, q1proj, q2proj)
dtime = 0.001
for i in range (ps.numberPaths()):
print("\n---Path {}---".format(i))
p.check_path(i, dtime)
vf = ViewerFactory (ps)