ps.createLockedJoint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', [0.5,]) 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)
Rule(grippers, [''], True), Rule(grippers, ['sphere0/handle'], True), Rule(grippers, ['sphere1/handle'], True) ] cg = ConstraintGraph.buildGenericGraph(robot=robot, name="manipulation", grippers=grippers, objects=objects, handlesPerObjects=handlesPerObject, shapesPerObjects=contactsPerObject, envNames=[], rules=rules) if lang == 'py': cg = ConstraintGraph(robot, "manipulation") 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()
exploreNodes.append(nodes[-2]) # release sphere1 grasps.remove(('r0/gripper', 'sphere1/handle')) nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) exploreNodes.append(nodes[-2]) # release cylinder0 : put assembly on the ground grasps.remove(('r1/gripper', 'cylinder0/handle')) nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) exploreNodes.append(nodes[-2]) cg = ConstraintGraph(robot, 'assembly') factory = ConstraintGraphFactory(cg) factory.setGrippers(grippers) #factory.environmentContacts (['table/pancake_table_table_top']) factory.setObjects(objects, handlesPerObjects, shapesPerObject) factory.setRules(rules) factory.generate() cg.initialize() edges, loops = getEdges(graph=cg, nodes=nodes, exploreNodes=exploreNodes) ps.selectPathProjector('Progressive', .05) ## Add a node to move robots in initial configurations nodes.append(nodes[-1]) edges.append( getTransitionConnectingStates(graph=cg, s1=nodes[-2], s2=nodes[-1])) loops.append(
boxes = ['box'] handlesPerObject = [ ['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]
# 3}}} 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)
ps.createPositionConstraint("look_at_gripper", "tiago/camera_color_optical_frame", tool_gripper, (0,0,0), (0,0,0), (True,True,True)) look_at_gripper = ps.hppcorba.problem.getConstraint("look_at_gripper") import hpp_idl look_at_gripper.setComparisonType([hpp_idl.hpp.EqualToZero,hpp_idl.hpp.EqualToZero,hpp_idl.hpp.Superior]) # Create "Look at part" constraint ps.createPositionConstraint("look_at_part", "tiago/camera_color_optical_frame", "part/to_tag_00001", (0,0,0), (0,0,0), (True,True,False)) look_at_part = ps.hppcorba.problem.getConstraint("look_at_part") # 3}}} # {{{3 Constraint graph instanciation from hpp.corbaserver.manipulation import ConstraintGraphFactory graph = ConstraintGraph(robot, 'graph') factory = ConstraintGraphFactory(graph) factory.setGrippers([ "tiago/gripper", "driller/drill_tip", ]) all_handles = ps.getAvailable('handle') part_handles = filter(lambda x: x.startswith("part/"), all_handles) test_handles = [ 'part/handle_10', 'part/handle_11', 'part/handle_12', 'part/handle_13', 'part/handle_14', 'part/handle_15', 'part/handle_16', 'part/handle_17', 'part/handle_18', 'part/handle_19', 'part/handle_20',
ps.createRelativeComConstraint("talos_com_constraint_to_la", "Talos_CoM", robot.leftAnkle, com_la.tolist(), (True, True, False)) ps.setMaxIterProjection(40) # Set the maximum number of iterations of the projection algorithm. Must be called before the graph creation. # constraint graph rules = [ Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "^$"], True), Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "desk/touchpoint_right_top"], True), Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "desk/touchpoint_right_front"], True), Rule(["talos/left_gripper", "talos/right_gripper"], ["desk/touchpoint_left_front", "^$"], True), Rule(["talos/left_gripper", "talos/right_gripper"], ["desk/touchpoint_left_drawer", "^$"], True), Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "desk/upper_drawer_spherical_handle"], True) ] graph = ConstraintGraph(robot, "graph") factory = ConstraintGraphFactory(graph) factory.setGrippers(["talos/left_gripper", "talos/right_gripper"]) factory.setObjects(["desk"], [["desk/upper_drawer_spherical_handle", "desk/touchpoint_right_top", "desk/touchpoint_right_front", "desk/touchpoint_left_front", "desk/touchpoint_left_drawer"]], [[]]) factory.setRules(rules) factory.generate() graph.addConstraints(graph = True, constraints = Constraints(numConstraints = ["talos_com_constraint_to_la", "talos_static_stability/pose-left-foot", "talos_static_stability/pose-right-foot"], lockedJoints = ["gripper_r_lock_idj", "gripper_r_lock_f1j", "gripper_r_lock_f2j", "gripper_r_lock_isj", "gripper_r_lock_f3j", "gripper_r_lock_j", "gripper_r_lock_msj", "gripper_l_lock_idj", "gripper_l_lock_f1j", "gripper_l_lock_f2j", "gripper_l_lock_isj", "gripper_l_lock_f3j", "gripper_l_lock_j", "gripper_l_lock_msj"])) graph.addConstraints(node = "talos/right_gripper grasps desk/touchpoint_right_top", constraints = Constraints(lockedJoints = ["drawer_lock"])) graph.addConstraints(node = "talos/right_gripper grasps desk/touchpoint_right_front", constraints = Constraints(lockedJoints = ["drawer_lock"])) graph.addConstraints(node = "talos/left_gripper grasps desk/touchpoint_left_front", constraints = Constraints(lockedJoints = ["drawer_lock"])) graph.addConstraints(node = "talos/left_gripper grasps desk/touchpoint_left_drawer", constraints = Constraints(lockedJoints = ["drawer_lock"])) graph.initialize()
grippers = ['romeo/r_hand', 'romeo/l_hand'] handlesPerObjects = [list(placard.handles.values ())] lang = 'py' if lang == 'cxx': cg = ConstraintGraph.buildGenericGraph (robot, "graph", grippers, [placard.name,], handlesPerObjects, [[],], [], rules) if lang == 'py': cg = ConstraintGraph (robot, "graph") factory = ConstraintGraphFactory (cg) factory.setGrippers (grippers) factory.setObjects ([placard.name,], handlesPerObjects, [[],]) factory.setRules (rules) factory.generate () cg.addConstraints (graph = True, constraints = commonConstraints) cg.initialize () # Define initial and final configurations q_goal = [-0.003429678026293006, 7.761615492429529e-05, 0.8333148411182841, -0.08000440760954532, 0.06905332841243099, -0.09070086400314036, 0.9902546570793265, 0.2097693637044623, 0.19739743868699455, -0.6079135018296973, 0.8508704420155889, -0.39897628829947995, -0.05274298289004072, 0.20772797293264825, 0.1846394290733244, -0.49824886682709824, 0.5042013065348324, -0.16158420369261683, -0.039828502509861335, -0.3827070014985058, -0.24118425356319423, 1.0157846623463191, 0.5637424355124602, -1.3378817283780955, -1.3151786907256797, -0.392409481224193, 0.11332560818107676, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.35936687035487364, -0.32595302056157444, -0.33115291290191723, 0.20387672048126043, 0.9007626913161502, -0.39038645767349395, 0.31725226129015516, 1.5475253831101246, -0.0104572058777634, 0.32681856374063933, 0.24476959944940427, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.412075621240969, 0.020809907186176854, 1.056724788359247, 0.0, 0.0, 0.0, 1.0] q_init = q_goal [::] q_init [r+3:r+7] = [0, 0, 1, 0] n = 'romeo/l_hand grasps placard/low' res, q_init, err = cg.applyNodeConstraints (n, q_init)
rules = [ Rule(grippers=grippers, handles=['box/handle1', '', ''], link=True), Rule(grippers=grippers, handles=['box/handle1', 'box/handle2', ''], link=True), 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 +\
ljs = list() ljs.append(lockJoint("tiago/root_joint", q0)) for n in robot.jointNames: if n.startswith('tiago/hand_'): ljs.append(lockJoint(n, q0)) ps.createPositionConstraint("gaze", "tiago/xtion_rgb_optical_frame", "driller/tag_joint", (0, 0, 0), (0, 0, 0), (True, True, False)) from hpp.corbaserver.manipulation import ConstraintGraphFactory graph = ConstraintGraph(robot, 'graph') factory = ConstraintGraphFactory(graph) factory.setGrippers([ "tiago/gripper", "driller/drill_tip", ]) factory.setObjects([ "driller", "skin", ], [ [ "driller/handle", ], [ "skin/hole", ], ], [
exploreNodes.append (nodes [-2]) # release sphere1 grasps.remove (('r0/gripper', 'sphere1/handle')) nodes.append (StateName (grasps)) rules.append (makeRule (grasps = grasps)) exploreNodes.append (nodes [-2]) # release cylinder0 : put assembly on the ground grasps.remove (('r1/gripper', 'cylinder0/handle')) nodes.append (StateName (grasps)) rules.append (makeRule (grasps = grasps)) exploreNodes.append (nodes [-2]) cg = ConstraintGraph (robot, 'assembly') factory = ConstraintGraphFactory (cg) factory.setGrippers (grippers) #factory.environmentContacts (['table/pancake_table_table_top']) factory.setObjects (objects, handlesPerObjects, shapesPerObject) factory.setRules (rules) factory.generate () cg.initialize () edges, loops = getEdges (graph = cg, nodes = nodes, exploreNodes = exploreNodes) ps.selectPathProjector ('Progressive', .05) ## Add a node to move robots in initial configurations nodes.append (nodes [-1]) edges.append (getTransitionConnectingStates (graph = cg, s1 = nodes [-2], s2 = nodes [-1])) loops.append (getTransitionConnectingStates (graph = cg, s1 = nodes [-1],