envSurfaces = [] objContactSurfaces = [[]] # Get the built graph cg = ConstraintGraph (robot, 'graph') factory = ConstraintGraphFactory (cg) factory.setGrippers (grippers) factory.environmentContacts (envSurfaces) factory.setObjects (boxes, handlesPerObject, objContactSurfaces) factory.setRules (rules) factory.generate () cg.addConstraints (graph = True, constraints =\ Constraints (lockedJoints = locklhand)) cg.setWeight ('Loop | f', 1) cg.setWeight ('Loop | 0-0', 1) cg.initialize () res, q_init, err = cg.applyNodeConstraints ('free', q_init) 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
factory = ConstraintGraphFactory(cg) factory.setGrippers(grippers) factory.setObjects(objects, handlesPerObject, contactsPerObject) factory.generate() for e in [ 'ur3/gripper > sphere0/handle | f_ls', 'ur3/gripper > sphere1/handle | f_ls' ]: 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
## Set constraints of nodes and edges graph.addConstraints (node='placement', constraints = \ Constraints (numConstraints = ['placement'],)) graph.addConstraints (node='grasp', constraints = Constraints (numConstraints = ['grasp'])) graph.addConstraints (edge='transit', constraints = \ Constraints (numConstraints = ['placement/complement'])) graph.addConstraints (edge='grasp-ball', constraints = \ Constraints (numConstraints = ['placement/complement'])) # These edges are in node 'grasp' graph.addConstraints (edge='transfer', constraints = Constraints ()) graph.addConstraints (edge='release-ball', constraints = Constraints ()) ps.selectPathValidation ("Dichotomy", 0) ps.selectPathProjector ("Progressive", 0.1) graph.initialize () ## Project initial configuration on state 'placement' res, q_init, error = graph.applyNodeConstraints ('placement', q1) q2 = q1 [::] q2 [7] = .2 ## Project goal configuration on state 'placement' res, q_goal, error = graph.applyNodeConstraints ('placement', q2) ## Define manipulation planning problem ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) # v = vf.createViewer () # pp = PathPlayer (v)