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.")