ps.createLockedJoint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', [0.5,]) grippers = ['pr2/l_gripper'] boxes = ['box'] handlesPerObject = [['box/handle']] # envSurfaces = ['kitchen_area/pancake_table_table_top', # 'kitchen_area/white_counter_top_sink'] # objContactSurfaces = [['box/box_surface']] 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")
['box/handle', 'box/handle2'], ] objContactSurfaces = [ ['box/box_surface'], ] envSurfaces = [ 'kitchen_area/pancake_table_table_top', ] # Build rules rules = [Rule([""], [""], True)] 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 = lockAll)) cg.initialize() res = ps.client.manipulation.problem.applyConstraints(cg.nodes['free'], q_init) if not res[0]: raise Exception('Init configuration could not be projected.') q_init = res[1] res = ps.client.manipulation.problem.applyConstraints(cg.nodes['free'], q_goal) if not res[0]:
handlesPerObject = list() handles = list() objContactSurfaces = list() for i in range(K): handlesPerObject.append([boxes[i] + "/handle2"]) handles.append(boxes[i] + "/handle2") objContactSurfaces.append([boxes[i] + "/box_surface"]) # Build rules rules = [Rule([".*"], [".*"], True)] # Get the built graph cg = ConstraintGraph(robot, 'graph') factory = ConstraintGraphFactory(cg) factory.setGrippers(grippers) factory.environmentContacts(['table/pancake_table_table_top']) factory.setObjects(boxes, handlesPerObject, objContactSurfaces) factory.setRules(rules) factory.generate() cg.addConstraints (graph = True, constraints =\ Constraints (numConstraints = lockAll)) cg.initialize() res = ps.client.manipulation.problem.applyConstraints( cg.nodes['free'], q_init) if not res[0]: raise Exception('Init configuration could not be projected.') q_init_proj = res[1] res = ps.client.manipulation.problem.applyConstraints( cg.nodes['free'], q_goal)
handlesPerObject = list () handles = list () objContactSurfaces = list () for i in xrange(K): handlesPerObject.append ([boxes[i] + "/handle2"]) handles.append (boxes[i] + "/handle2") objContactSurfaces .append ([boxes[i] + "/box_surface"]) # Build rules rules = [Rule ([".*"], [".*"], True)] # Get the built graph cg = ConstraintGraph (robot, 'graph') factory = ConstraintGraphFactory (cg) factory.setGrippers (grippers) factory.environmentContacts (['table/pancake_table_table_top']) factory.setObjects (boxes, handlesPerObject, objContactSurfaces) factory.setRules (rules) factory.generate () cg.addConstraints (graph = True, constraints =\ Constraints (lockedJoints = lockAll)) cg.initialize () res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init) if not res[0]: raise Exception ('Init configuration could not be projected.') q_init_proj = res [1] res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_goal) if not res[0]: raise Exception ('Goal configuration could not be projected.')
Rule(grippers=grippers, handles=['', 'box/handle2', ''], link=True), Rule(grippers=grippers, handles=['', 'box/handle2', 'box/handle1'], link=True), Rule(grippers=grippers, handles=['', '', 'box/handle1'], link=True), Rule(grippers=grippers, handles=['box/handle2', '', 'box/handle1'], link=True), Rule(grippers=grippers, handles=['box/handle2', '', ''], link=True) ] factory = ConstraintGraphFactory(cg) factory.setGrippers(grippers) factory.setObjects([obj.name for obj in objects], [obj.handles for obj in objects], [obj.contacts for obj in objects]) factory.environmentContacts([]) factory.setRules(rules) factory.generate() for edgename, edgeid in cg.edges.iteritems(): cg.addConstraints( edge=edgename, constraints=Constraints(numConstraints=["waist orientation"])) cg.setConstraints (graph=True, constraints = Constraints \ (numConstraints = quasiStaticConstraints + ['gaze'], lockedJoints = left_gripper_lock + right_gripper_lock +\ other_lock)) for e in cg.edges.keys(): if cg.getWeight(e) == -1: continue if e[:4] == "Loop":