'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))) print("Average number nodes: " + str(totalNumberNodes / float(args.N)))
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.") 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))) # 1}}} from hpp.gepetto import PathPlayer #v = vf.createViewer (); v (q_init) #pp = PathPlayer (v, robot.client.basic)