if not res[0]: raise Exception('Goal configuration could not be projected.') q_goal = res[1] 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)) print("Average time: " + str((totalTime.seconds + 1e-6 * totalTime.microseconds) / float(args.N))) print("Average number nodes: " + str(totalNumberNodes / float(args.N))) if args.display: v = vf.createViewer() pp = PathPlayer(v, robot.client.basic) if args.run: pp(0)
factory = ConstraintGraphFactory (cg) factory.setGrippers (grippers) factory.environmentContacts (envContactSurfaces) factory.setObjects (objects, handlesPerObject, contactSurfacesPerObject) factory.setRules (rules) factory.generate () cg.addConstraints (graph = True, constraints = Constraints \ (lockedJoints = locklhand)) cg.initialize () # 2}}} res, q_init_proj, err = cg.applyNodeConstraints("free", q_init) res, q_goal_proj, err = cg.applyNodeConstraints("free", q_goal) ps.setInitialConfig (q_init_proj) ps.addGoalConfig (q_goal_proj) print ps.solve() ps.setTargetState (cg.nodes["pr2/l_gripper grasps box/handle2"]) print ps.solve() # 1}}} v = vf.createViewer () v (q_init_proj) pp = PathPlayer (v, robot.client.basic) # vim: foldmethod=marker foldlevel=1
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 (robot.client.basic, v) # vim: foldmethod=marker foldlevel=1
import datetime as dt totalTime = dt.timedelta (0) totalNumberNodes = 0 N = 20 for i in range (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)) print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (N))) print ("Average number nodes: " + str (totalNumberNodes/float (N))) # 1}}} from hpp.gepetto import PathPlayer r = vf.createViewer (); r (q_init) pp = PathPlayer (robot.client.basic, r) # vim: foldmethod=marker foldlevel=1
graph.setConstraints (edge='grasp_box_l1_e0', lockDof = lockbox) graph.setConstraints (edge='grasp_box_l2_e0', lockDof = lockbox) graph.setConstraints (edge='grasp_box_r1_e0', lockDof = lockbox) graph.setConstraints (edge='grasp_box_r2_e0', lockDof = lockbox) graph.setConstraints (node='hold_box_l1', grasps = ['l1_grasp',]) graph.setConstraints (node='hold_box_l2', grasps = ['l2_grasp',]) graph.setConstraints (node='hold_box_r1', grasps = ['r1_grasp',]) graph.setConstraints (node='hold_box_r2', grasps = ['r2_grasp',]) # graph.setConstraints (edge='keep_grasp_l1', lockDof = lockbox) # graph.setConstraints (edge='keep_grasp_l2', lockDof = lockbox) # graph.setConstraints (edge='keep_grasp_r1', lockDof = lockbox) # graph.setConstraints (edge='keep_grasp_r2', lockDof = lockbox) r = fk.createViewer () q_init = robot.halfSitting + [.75,0,.5,1,0,0,0] res = ps.client.manipulation.problem.applyConstraints (graph.nodes ['free'], q_init) q_init = res [1] q_goal = robot.halfSitting + [.75,0,.3,1,0,0,0] res = ps.client.manipulation.problem.applyConstraints (graph.nodes ['free'], q_goal) q_goal = res [1] ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) #ps.solve () #Create configurations and paths by hand. res, q1, err = ps.client.manipulation.problem.applyConstraintsWithOffset \ (graph.edges ['grasp_box_r1_e0'], q_init, q_init) res, i1, i2 = ps.client.manipulation.problem.buildAndProjectPath \