cg.setWeight(e, 100)
for e in [
'ur3/gripper < sphere0/handle | 0-0_ls',
'ur3/gripper < sphere1/handle | 0-1_ls'
]:
cg.setWeight(e, 100)
cg.initialize()
# Run benchmark
#
import datetime as dt
totalTime = dt.timedelta(0)
totalNumberNodes = 0
for i in range(args.N):
ps.clearRoadmap()
ps.resetGoalConfigs()
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
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)))
if not res:
raise RuntimeError ("Failed to project initial configuration")
res, q_goal, err = cg.applyNodeConstraints ('free', q_goal)
if not res:
raise RuntimeError ("Failed to project initial configuration")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.setMaxIterPathPlanning (5000)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20; success = 0
for i in range (N):
ps.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
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.")
