def refresh(self): self.cg = ConstraintGraph(self.plugin.r, "", False) try: self.fillGripper() self.fillHandles() self.applyFilters() except Exception as e: pass
def makeGraph(robot): graph = ConstraintGraph.buildGenericGraph( robot, 'graph', # [ "talos/left_gripper", "talos/right_gripper", "table/pose", ], [ "talos/left_gripper", "table/pose", ], [ "box", ], [ Object.handles + Object.poses, ], # [ Object.contacts, ], [ [], ], Table.contacts, [ Rule([ "table/pose", ], [ "box/handle[12]", ], False), Rule([ "talos/left_gripper", ], [ "box/pose[12]", ], False), Rule([ "table/pose", ], [ "box/pose1", ], False), Rule([ "talos/left_gripper", ], [ Object.handles[1], ], False), Rule([ "talos/left_gripper", ], [ Object.handles[0], ], True), # Rule([ "talos/right_gripper", ], [ Object.handles[1], ], True), Rule([ "table/pose", ], [ "box/pose2", ], True), ]) return graph
def makeGraph(robot, table, objects): graph = ConstraintGraph(robot, "graph") factory = ConstraintGraphFactory(graph) grippers = ["talos/left_gripper", "talos/right_gripper"] 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(table.contacts) factory.constraints.strict = True factory.setRules (makeRules (robot, grippers)) factory.generate() return graph
def createInitializationGraph(name): graph = ConstraintGraph.buildGenericGraph( robot, name, ["talos/left_gripper", "talos/right_gripper"], ["box"], [Object.handles], [[]], [], [ Rule(["talos/left_gripper"], [Object.handles[0]], True), Rule(["talos/right_gripper"], [Object.handles[1]], True), ], ) return graph
def makeGraph(robot): from hpp.corbaserver.manipulation.constraint_graph_factory import ( ConstraintGraphFactory, ) graph = ConstraintGraph(robot, "graph") factory = ConstraintGraphFactory(graph) factory.setGrippers(["talos/left_gripper"]) factory.setObjects(["box"], [Object.handles], [Object.contacts]) factory.environmentContacts(Table.contacts) factory.setRules( [ Rule(["talos/left_gripper"], [Object.handles[1]], False), # Rule([ "talos/left_gripper", ], [ Object.handles[0], ], True), Rule(["talos/left_gripper"], [".*"], True), # Rule([ "talos/right_gripper", ], [ Object.handles[1], ], True), ] ) factory.generate() return graph
def makeGraph(ps, table, objects): robot = ps.robot graph = ConstraintGraph(robot, "graph") factory = ConstraintGraphFactory(graph) grippers = ["talos/left_gripper", "talos/right_gripper"] 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(table.contacts) factory.constraints.strict = True factory.setRules (makeRules (robot, grippers)) factory.setPreplacementDistance("box", 0.1) factory.generate() sm = SecurityMargins(ps, factory, ["talos", "box", "table"]) sm.setSecurityMarginBetween ("talos", "box", 0.02) sm.setSecurityMarginBetween ("box", "table", 0.03) sm.apply() return graph, factory
vf.moveObstacle ('box/base_link_2', [0.3, 0.04, 0.04, 0, 0, 0, 1]) vf.moveObstacle ('box/base_link_3', [0.3, -0.04, 0.04, 0, 0, 0, 1]) # Loading our pokeball which plays our ball's role vf.loadObjectModel (Pokeball, 'pokeball') robot.setJointBounds ('pokeball/root_joint', [-.4,.4,-.4,.4,-.1,1., -1.0001, 1.0001,-1.0001, 1.0001, -1.0001, 1.0001,-1.0001, 1.0001,]) r = vf.createViewer () q1 = [0, -1.57, 1.57, 0, 0, 0, .3, 0, 0.025, 0, 0, 0, 1] r (q1) ## Create graph graph = ConstraintGraph (robot, 'graph') # Contraint of ball relative position while it is located in the gripper ballInGripper = [0, .137, 0, 0.5, 0.5, -0.5, 0.5] ps.createTransformationConstraint ('grasp', gripperName, ballName, ballInGripper, 6*[True,]) ## Create nodes and edges # Warning the order of the nodes is important. When checking in which node # a configuration lies, node constraints will be checked in the order of node # creation. # We are creating our graph as described in the instructions graph.createNode (['grasp-placement', 'ball-above-ground','gripper-above-ball','grasp','placement']) graph.createEdge ('grasp-placement','ball-above-ground','take-ball-up',1,'grasp') graph.createEdge ('ball-above-ground','grasp-placement','put-ball-down',1,'grasp') graph.createEdge ('grasp-placement','gripper-above-ball','move-gripper-up',1,'placement')
s = robot.getJointConfigSize(n) r = robot.rankInConfiguration[n] if n.startswith ("talos/gripper_right"): ps.createLockedJoint(n, n, half_sitting[r:r+s]) right_gripper_lock.append(n) elif n.startswith ("talos/gripper_left"): ps.createLockedJoint(n, n, half_sitting[r:r+s]) left_gripper_lock.append(n) elif n in other_lock: ps.createLockedJoint(n, n, half_sitting[r:r+s]) graph = ConstraintGraph.buildGenericGraph(robot, 'graph', [ "talos/left_gripper", "talos/right_gripper", ], [ "box", ], [ Object.handles, ], [ [ ], ], [ ], [ Rule([ "talos/left_gripper", ], [ Object.handles[0], ], True), Rule([ "talos/right_gripper", ], [ Object.handles[1], ], True), ] ) graph.setConstraints (graph=True, lockDof = left_gripper_lock + right_gripper_lock + other_lock, numConstraints = [ "com_talos_box", "gaze"] + foot_placement) graph.initialize() res, q_init, err = graph.applyNodeConstraints("talos/left_gripper grasps box/top", half_sitting) res, q_goal, err = graph.applyNodeConstraints("talos/right_gripper grasps box/bottom", half_sitting) print ps.directPath(q_init, q_init, True) ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal)
lockAll = lockhands + lockHeadAndTorso + lockPlanar # 3}}} # Create the graph. {{{3 grippers = ['pr2/l_gripper', 'pr2/r_gripper'] 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]
]) grippers = ['pr2/l_gripper'] boxes = ['box'] handlesPerObject = [['box/handle']] # envSurfaces = ['kitchen_area/pancake_table_table_top', # 'kitchen_area/white_counter_top_sink'] # contactPerObject = [['box/box_surface']] envSurfaces = [] contactPerObject = [[]] ps.client.manipulation.graph.autoBuild('graph', grippers, boxes, handlesPerObject, contactPerObject, envSurfaces, rules) cg = ConstraintGraph(robot, 'graph', makeGraph=False) cg.setConstraints (graph = True, constraints =\ Constraints (lockedJoints = locklhand)) cg.setWeight('pr2/l_gripper < box/handle | 0-0_ls', 0) cg.setWeight('Loop | f', 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)
rank = robot.rankInConfiguration ['box/root_joint'] q_goal [rank:rank+7] = [-2.5, -4.4, 0.76,0,-c,0,c] #rank = robot.rankInConfiguration ['box/base_joint_SO3'] #q_goal [rank:rank+4] = [c, 0, -c, 0] del c # 3}}} robot.client.basic.problem.resetRoadmap () robot.client.basic.problem.setErrorThreshold (1e-3) robot.client.basic.problem.setMaxIterProjection (40) ps.selectPathProjector ('Progressive', 0.2) # Create constraints. {{{3 from hpp.corbaserver.manipulation import ConstraintGraph cg = ConstraintGraph (robot, 'graph') robot.client.manipulation.problem.createPlacementConstraint \ ('box_placement', ['box/box_surface',], ['kitchen_area/pancake_table_table_top',]) jointNames = dict () jointNames['all'] = robot.getJointNames () jointNames['pr2'] = list () jointNames['allButPR2LeftArm'] = list () for n in jointNames['all']: if n.startswith ("pr2"): jointNames['pr2'].append (n) if not n.startswith ("pr2/l_gripper"): jointNames['allButPR2LeftArm'].append (n)
left_arm_lock = createLeftArmLockedJoints(ps) right_arm_lock = createRightArmLockedJoints(ps) if args.arm == 'left': arm_locked = right_arm_lock elif args.arm == 'right': arm_locked = left_arm_lock else: arm_locked = list() left_gripper_lock, right_gripper_lock = createGripperLockedJoints(ps, initConf) com_constraint, foot_placement, foot_placement_complement = \ createQuasiStaticEquilibriumConstraint (ps, initConf) gaze_constraint = createGazeConstraint(ps, args.arm) waist_constraint = createWaistYawConstraint(ps) graph = ConstraintGraph(robot, "graph") graph.createNode(["free", "starting_state"]) graph.createEdge("starting_state", "free", "starting_motion", isInNode="starting_state") graph.createEdge("free", "starting_state", "go_to_starting_state", isInNode="starting_state") graph.createEdge("free", "free", "Loop | f", isInNode="starting_state") # Set constraints graph.addConstraints( node="starting_state",
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'] # contactPerObject = [['box/box_surface']] envSurfaces = [] contactPerObject = [[]] ps.client.manipulation.graph.autoBuild ('graph', grippers, boxes, handlesPerObject, contactPerObject, envSurfaces, rules) cg = ConstraintGraph (robot, 'graph', makeGraph = False) cg.setConstraints (graph = True, constraints =\ Constraints (lockedJoints = locklhand)) cg.setWeight ('pr2/l_gripper < box/handle | 0-0_ls', 0) cg.setWeight ('Loop | f', 1) 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)
rank = robot.rankInConfiguration ['box2/base_joint_xyz'] q1[rank:rank+7] = [ -0.14391940018238783, 1.092383723903555, 0.7460100959532432, 0.44542854851747, 0.5489390643072047, 0.4452361192941091, -0.5495672023684386] q2[rank:rank+3] = [ -0.445, 0.268, 0.7460100959532432] # 3}}} robot.client.basic.problem.resetRoadmap () robot.client.basic.problem.setErrorThreshold (1e-3) robot.client.basic.problem.setMaxIterations (20) #ps.selectPathValidation ('Graph-Discretized', 0.05) ps.selectPathProjector ('Progressive', 0.1) # ps.selectPathProjector ('Global', 0.2) # Create constraints. {{{3 from hpp.corbaserver.manipulation import ConstraintGraph cg = ConstraintGraph (robot, 'graph') # Create passive DOF lists {{{4 jointNames = dict () jointNames['all'] = robot.getJointNames () jointNames['baxter'] = list () jointNames['baxterRightSide'] = list () jointNames['baxterLeftSide'] = list () jointNames['box1'] = list () jointNames['box2'] = list () for n in jointNames['all']: if n.startswith ("baxter"): jointNames['baxter'].append (n) if n.startswith ("baxter/left_"): jointNames['baxterLeftSide'].append (n) if n.startswith ("baxter/right_"):
for n in jointNames["baxterLeftSide"]: ps.createLockedJoint (n, n, [0,]) lockAll = lockFingers + lockHead # 4}}} # 3}}} handles = list () shapes = list () for i in xrange(K): handles.append ([boxes[i] + "/handle2"]) shapes .append ([boxes[i] + "/box_surface"]) cg = ConstraintGraph.buildGenericGraph (robot, "graph", grippers, boxes, handles, shapes, ['table/pancake_table_table_top'], []) # Get the built graph cg.setConstraints (graph = True, lockDof = lockAll) # cg.graph.setTargetNodeList (cg.subGraphId, # [ # cg.nodes['free'], # cg.nodes['baxter/r_gripper grasps box0/handle2'], # cg.nodes['free'], # cg.nodes['baxter/r_gripper grasps box1/handle2'], # cg.nodes['free'], # cg.nodes['baxter/r_gripper grasps box2/handle2'], # cg.nodes['free'], # cg.nodes['baxter/r_gripper grasps box3/handle2'],
rank = robot.rankInConfiguration ['cup/base_joint_xyz'] q_init [rank:rank+7] = [-4.8, -4.6, 0.91,0,sqrt(2)/2,sqrt(2)/2,0] q_goal = q_init [::] q_goal [rank:rank+7] = [-4.8, -3.35, 0.9, 0,sqrt(2)/2,sqrt(2)/2,0] # 3}}} robot.client.basic.problem.resetRoadmap () robot.client.basic.problem.setErrorThreshold (1e-3) robot.client.basic.problem.setMaxIterations (40) # ps.selectPathProjector ('Progressive', 0.2) ps.selectPathProjector ('Global', 0.2) # Create constraints. {{{3 from hpp.corbaserver.manipulation import ConstraintGraph cg = ConstraintGraph (robot, 'graph') # Create passive DOF lists {{{4 jointNames = dict () jointNames['all'] = robot.getJointNames () jointNames['romeo'] = list () jointNames['romeoWithoutLeftArm'] = list () jointNames['romeoWithoutRightArm'] = list () jointNames['kitchen_area'] = list () jointNames['cup'] = list () for n in jointNames['all']: if n.startswith ("romeo"): jointNames['romeo'].append (n) if not n.startswith ("romeo/l_"): jointNames['romeoWithoutLeftArm'].append (n) if not n.startswith ("romeo/r_"):
# Create the constraint graph. {{{2 # Define the set of grippers used for manipulation grippers = [ "pr2/l_gripper", ] # Define the set of objects that can be manipulated objects = [ "box", ] # Define the set of handles for each object handlesPerObject = [ ["box/handle2", ], ] # Define the set of contact surfaces used for each object contactSurfacesPerObject = [ ["box/box_surface", ], ] # Define the set of contact surfaces of the environment used to put objects envContactSurfaces = [ "kitchen_area/pancake_table_table_top", ] # Define rules for associating grippers and handles (here all associations are # allowed) rules = [ Rule([".*"], [".*"], True), ] cg = ConstraintGraph (robot, 'graph') 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)
del c q_inter = [-0.8017105239677402, -0.5977125025958312, -0.796440524800078, 0.6047168680102916, -0.4879605145323316, 0.8728657034488995, -0.988715265911429, 0.1498069522875767, -0.012487804646172175, -0.9999220243274567, -0.9479271854727237, -0.31848712853388694, 0.06700549180802896, -0.9977526066453368, 0.15793164217459785, 0.9874500475467277, 0.9804271799015071, -0.19688205837601827, 0.06981400674906149, 0.9975600254930236, 0.8026666074307995, -0.5964279649006498, -0.8558688410761539, -0.5171929300318802, 0.07633365848037467, 2.5514381844999448, 1.1951774265118278, -0.5864281075389233, 0.24807300661555917, 1.0730239901832896, -0.9931474461781542, 0.5380253563010143, -0.8429286541440898, 0.0, -0.9291311234626017, 0.36975039609596555, 0.5, 0.07192830903452277, 0.0516497980242827, 0.5, 0.2978673015357309, 0.011873305063635719, 0.2207828272342602, 0.9968680838221816, -1.1330407965502385, 0.1474961939381539, -0.9059397450606351, -0.9591666722669869, 0.8241613711518598, -0.5663550426199861, -2.094, 0.7924979452316735, 0.6098745828476339, 0.5, 0.35889873246983567, 0.10845342945887403, 0.5, 0.02916333341652683, 0.025597731231524482, 0.16145134862579935, -2.785939904956431, -4.563075760833335, 0.8958690128585236, -0.19634763254425533, -0.7205092487114027, 0.6650461296003519, 0.005260724207565836] # 3}}} robot.client.basic.problem.resetRoadmap () robot.client.basic.problem.selectPathOptimizer ('None') robot.client.basic.problem.setErrorThreshold (1e-3) robot.client.basic.problem.setMaxIterations (40) from hpp.corbaserver.manipulation import ProblemSolver p = ProblemSolver (robot) # Create constraints. {{{3 from hpp.corbaserver.manipulation import ConstraintGraph cg = ConstraintGraph (robot, 'graph') robot.client.manipulation.problem.createPlacementConstraint ( 'box_placement', 'box', 'box/base_joint_SO3', 'box_surface', 'pancake_table_table_top') jointNames = dict () jointNames['all'] = robot.getJointNames () jointNames['pr2'] = list () jointNames['allButPR2LeftArm'] = list () for n in jointNames['all']: if n.startswith ("pr2"): jointNames['pr2'].append (n) if not n.startswith ("pr2/l_gripper"): jointNames['allButPR2LeftArm'].append (n) cg.createGrasp ('l_grasp', 'pr2/l_gripper', 'box/handle', jointNames['pr2'])
q_init = half_sitting [::] # Open hands ilh = robot.rankInConfiguration ['hrp2/LARM_JOINT6'] q_init [ilh:ilh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75] irh = robot.rankInConfiguration ['hrp2/RARM_JOINT6'] q_init [irh:irh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75] ibjxyz = robot.rankInConfiguration ['hrp2/base_joint_xyz'] q_goal = q_init [::] q_goal [ibjxyz:ibjxyz+2] = [0.5, 0] #q_goal [ibjxyz:ibjxyz+2] = [0.2, 0] # 3}}} # Generate constraints {{{3 graph = ConstraintGraph (robot, 'graph') robot.setCurrentConfig (q_init) leftpos = robot.getJointPosition (robot.leftAnkle) rightpos = robot.getJointPosition (robot.rightAnkle) leftfoot = [ [ # "leftfoot/comz" , "leftfoot/com", "leftfoot/z" , "leftfoot/rxry"], ["leftfoot/xy", ]] # "leftfoot/rxryrz"]] ps.createRelativeComConstraint (leftfoot[0][0], "", robot.leftAnkle, (0,0,0.7028490491159864), (True,True,True)) ps.createPositionConstraint (leftfoot[0][1], robot.leftAnkle, "", (0,0,0), leftpos[0:3], (False, False, True)) ps.createPositionConstraint (leftfoot[1][0], robot.leftAnkle, "", (0,0,0), leftpos[0:3], (True, True, False)) ps.client.basic.problem.setConstantRightHandSide (leftfoot[1][0], False) ps.createOrientationConstraint (leftfoot[0][2], robot.leftAnkle, "", (1,0,0,0), (True, True, True))
q_init = half_sitting[::] # Open hands ilh = robot.rankInConfiguration['hrp2/LARM_JOINT6'] q_init[ilh:ilh + 6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75] irh = robot.rankInConfiguration['hrp2/RARM_JOINT6'] q_init[irh:irh + 6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75] ibjxyz = robot.rankInConfiguration['hrp2/base_joint_xyz'] q_goal = q_init[::] q_goal[ibjxyz:ibjxyz + 2] = [0.5, 0] #q_goal [ibjxyz:ibjxyz+2] = [0.2, 0] # 3}}} # Generate constraints {{{3 graph = ConstraintGraph(robot, 'graph') robot.setCurrentConfig(q_init) leftpos = robot.getJointPosition(robot.leftAnkle) rightpos = robot.getJointPosition(robot.rightAnkle) leftfoot = [ [ # "leftfoot/comz" , "leftfoot/com", "leftfoot/z", "leftfoot/rxry" ], [ "leftfoot/xy", ] ]
commonConstraints = Constraints (numConstraints = ps.balanceConstraints (), lockedJoints = lockHands) # build graph rules = [Rule (["romeo/l_hand","romeo/r_hand",], ["placard/low", ""], True), Rule (["romeo/l_hand","romeo/r_hand",], ["", "placard/high"], True), Rule (["romeo/l_hand","romeo/r_hand",], ["placard/low", "placard/high"], True), ] grippers = ['romeo/r_hand', 'romeo/l_hand'] handlesPerObjects = [placard.handles.values ()] cg = ConstraintGraph.buildGenericGraph (robot, "graph", grippers, [placard.name,], handlesPerObjects, [[],], [], rules) # 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
-0.19634763254425533, -0.7205092487114027, 0.6650461296003519, 0.005260724207565836 ] # 3}}} robot.client.basic.problem.resetRoadmap() robot.client.basic.problem.selectPathOptimizer('None') robot.client.basic.problem.setErrorThreshold(1e-3) robot.client.basic.problem.setMaxIterations(40) from hpp.corbaserver.manipulation import ProblemSolver p = ProblemSolver(robot) # Create constraints. {{{3 from hpp.corbaserver.manipulation import ConstraintGraph cg = ConstraintGraph(robot, 'graph') robot.client.manipulation.problem.createPlacementConstraint( 'box_placement', 'box', 'box/base_joint_SO3', 'box_surface', 'pancake_table_table_top') jointNames = dict() jointNames['all'] = robot.getJointNames() jointNames['pr2'] = list() jointNames['allButPR2LeftArm'] = list() for n in jointNames['all']: if n.startswith("pr2"): jointNames['pr2'].append(n) if not n.startswith("pr2/l_gripper"): jointNames['allButPR2LeftArm'].append(n)
# build graph rules = [Rule (["romeo/l_hand","romeo/r_hand",], ["placard/low", ""], True), Rule (["romeo/l_hand","romeo/r_hand",], ["", "placard/high"], True), Rule (["romeo/l_hand","romeo/r_hand",], ["placard/low", "placard/high"], True), ] 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
lockrhand = ['r_l_finger','r_r_finger']; 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")
com_la = tf_la.inverse().transform(com_wf) 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"]))
]) 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 (numConstraints = 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)
lockjoints = list() lockjoints.extend(lockbox) lockjoints.extend(lockur5) robot.setCurrentConfig(q_init) tf = robot.getJointPosition('ur5/wrist_1_joint') #robot.client.basic.problem.createPositionConstraint \ #('pos', '', 'ur5/wrist_1_joint', (0, tf[1], tf[2]), (0,0,0), (False, True, True)) robot.client.basic.problem.createPositionConstraint \ ('pos', '', 'ur5/wrist_1_joint', (0, 0, tf[2]), (0,0,0), (False, False, True)) # 3}}} # Create the graph. {{{3 from hpp.corbaserver.manipulation import ConstraintGraph cg = ConstraintGraph(robot, 'graph') cg.createNode(['free']) cg.setConstraints(node='free', numConstraints=['pos']) cg.setConstraints(graph=True, lockDof=lockjoints) cg.createEdge('free', 'free', 'move_free', 1) # 3}}} 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)
q_init = half_sitting [::] # Open hands ilh = robot.rankInConfiguration ['hrp2/LARM_JOINT6'] q_init [ilh:ilh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75] irh = robot.rankInConfiguration ['hrp2/RARM_JOINT6'] q_init [irh:irh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75] ibjxyz = robot.rankInConfiguration ['hrp2/base_joint_xyz'] q_goal = q_init [::] q_goal [ibjxyz:ibjxyz+2] = [0.5, 0] # q_goal [ibjxyz:ibjxyz+2] = [0.2, 0] # 3}}} # Generate constraints {{{3 graph = ConstraintGraph (robot, 'graph') robot.setCurrentConfig (q_init) leftpos = robot.getJointPosition (robot.leftAnkle) rightpos = robot.getJointPosition (robot.rightAnkle) leftfoot = [ [ # "leftfoot/comz" , "leftfoot/com", "leftfoot/z" , "leftfoot/rxry"], ["leftfoot/xy", ]] # "leftfoot/rxryrz"]] ps.createRelativeComConstraint (leftfoot[0][0], "", robot.leftAnkle, (0,0,0.7028490491159864), (True,True,True)) ps.createPositionConstraint (leftfoot[0][1], robot.leftAnkle, "", (0,0,0), leftpos[0:3], (False, False, True)) ps.createPositionConstraint (leftfoot[1][0], robot.leftAnkle, "", (0,0,0), leftpos[0:3], (True, True, False)) ps.client.basic.problem.setConstantRightHandSide (leftfoot[1][0], False) ps.createOrientationConstraint (leftfoot[0][2], robot.leftAnkle, "", (1,0,0,0), (True, True, True))
ps = ProblemSolver(robot) ## ViewerFactory is a class that generates Viewer on the go. It means you can ## restart the server and regenerate a new windows. ## To generate a window: ## fk.createRealClient () fk = ViewerFactory(ps) fk.loadObjectModel(Box, 'box') fk.loadEnvironmentModel(Environment, "kitchen_area") robot.setJointBounds("pr2/base_joint_xy", [-5, -2, -5.2, -2.7]) robot.setJointBounds("box/base_joint_xyz", [-5.1, -2, -5.2, -2.7, 0, 1.5]) # 2}}} # Load the Python class ConstraintGraph. {{{2 graph = ConstraintGraph(robot, 'graph') # 2}}} # 1}}} # Initialization. {{{1 # Set parameters. {{{2 robot.client.basic.problem.resetRoadmap() robot.client.basic.problem.setErrorThreshold(1e-3) robot.client.basic.problem.setMaxIterations(40) # 2}}} # Create lists of joint names - useful to define passive joints. {{{2 jointNames = dict() jointNames['all'] = robot.getJointNames()
from hpp.corbaserver.manipulation import ConstraintGraph, Constraints from hpp.corbaserver import Client Client ().problem.resetProblem () from manipulation import robot, vf, ps, Ground, Box, Pokeball, PathPlayer, gripperName, ballName vf.loadEnvironmentModel (Ground, 'ground') vf.loadObjectModel (Pokeball, 'pokeball') robot.setJointBounds ('pokeball/root_joint', [-.4,.4,-.4,.4,-.1,2., -1.0001, 1.0001,-1.0001, 1.0001, -1.0001, 1.0001,-1.0001, 1.0001,]) q1 = [0, -1.57, 1.57, 0, 0, 0, .3, 0, 0.025, 0, 0, 0, 1] ## Create constraint graph graph = ConstraintGraph (robot, 'graph') ## Create constraint of relative position of the ball in the gripper when ball ## is grasped ballInGripper = [0, .137, 0, 0.5, 0.5, -0.5, 0.5] ps.createTransformationConstraint ('grasp', gripperName, ballName, ballInGripper, 6*[True,]) ## Create nodes and edges # Warning the order of the nodes is important. When checking in which node # a configuration lies, node constraints will be checked in the order of node # creation. graph.createNode (['grasp', 'placement']) graph.createEdge ('placement', 'placement', 'transit', 1, 'placement') graph.createEdge ('grasp', 'grasp', 'transfer', 1, 'grasp') graph.createEdge ('placement', 'grasp', 'grasp-ball', 1, 'placement')
elif n.startswith("talos/gripper_left"): ps.createLockedJoint(n, n, half_sitting[r:r + s]) left_gripper_lock.append(n) elif n.startswith("talos/head"): ps.createLockedJoint(n, n, half_sitting[r:r + s]) head_lock.append(n) elif n in other_lock: ps.createLockedJoint(n, n, half_sitting[r:r + s]) graph = ConstraintGraph.buildGenericGraph( robot, "graph", ["talos/left_gripper", "talos/right_gripper"], ["mire"], [Mire.handles], [[]], [], [ Rule(["talos/left_gripper"], [Mire.handles[0]], True), Rule(["talos/right_gripper"], [Mire.handles[1]], True), ], ) graph.setConstraints( graph=True, lockDof=left_gripper_lock + right_gripper_lock + other_lock, numConstraints=["com_talos_mire"] + foot_placement, ) graph.initialize()
]) grippers = ['pr2/l_gripper'] boxes = ['box'] handlesPerObject = [['box/handle']] # envSurfaces = ['kitchen_area/pancake_table_table_top', # 'kitchen_area/white_counter_top_sink'] # contactPerObject = [['box/box_surface']] envSurfaces = [] contactPerObject = [[]] ps.client.manipulation.graph.autoBuild('graph', grippers, boxes, handlesPerObject, contactPerObject, envSurfaces, rules) cg = ConstraintGraph(robot, 'graph', makeGraph=False) cg.setConstraints(graph=True, lockDof=locklhand) cg.setWeight('pr2/l_gripper < box/handle | 0-0_ls', 0) 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)
lockAll = lockFingers + lockHead # 4}}} # Create gaze constraints {{{4 ps.createPositionConstraint ("gaze1", "baxter/display_joint", "box1/base_joint_xyz", [0,0,0], [0,0,0], [1,0,0]) ps.createPositionConstraint ("gaze2", "baxter/display_joint", "box2/base_joint_xyz", [0,0,0], [0,0,0], [1,0,0]) # 4}}} # 3}}} # Build the constraint graph cg = ConstraintGraph.buildGenericGraph (robot, "graph", [ "baxter/r_gripper" , "baxter/l_gripper"], [ "box1" , "box2"], [["box1/handle2",] , ["box2/handle2"]], [["box1/box_surface"], ["box2/box_surface"]], ['table/pancake_table_table_top'], []) cg.setConstraints (graph = True, lockDof = lockAll) 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.') q_goal_proj = res [1]
q_goal [rank:rank+3] = [-2.5, -4.4, 0.76] rank = robot.rankInConfiguration ['box/base_joint_SO3'] q_goal [rank:rank+4] = [c, 0, -c, 0] del c q_inter = [-0.8017105239677402, -0.5977125025958312, -0.796440524800078, 0.6047168680102916, -0.4879605145323316, 0.8728657034488995, -0.988715265911429, 0.1498069522875767, -0.012487804646172175, -0.9999220243274567, -0.9479271854727237, -0.31848712853388694, 0.06700549180802896, -0.9977526066453368, 0.15793164217459785, 0.9874500475467277, 0.9804271799015071, -0.19688205837601827, 0.06981400674906149, 0.9975600254930236, 0.8026666074307995, -0.5964279649006498, -0.8558688410761539, -0.5171929300318802, 0.07633365848037467, 2.5514381844999448, 1.1951774265118278, -0.5864281075389233, 0.24807300661555917, 1.0730239901832896, -0.9931474461781542, 0.5380253563010143, -0.8429286541440898, 0.0, -0.9291311234626017, 0.36975039609596555, 0.5, 0.07192830903452277, 0.0516497980242827, 0.5, 0.2978673015357309, 0.011873305063635719, 0.2207828272342602, 0.9968680838221816, -1.1330407965502385, 0.1474961939381539, -0.9059397450606351, -0.9591666722669869, 0.8241613711518598, -0.5663550426199861, -2.094, 0.7924979452316735, 0.6098745828476339, 0.5, 0.35889873246983567, 0.10845342945887403, 0.5, 0.02916333341652683, 0.025597731231524482, 0.16145134862579935, -2.785939904956431, -4.563075760833335, 0.8958690128585236, -0.19634763254425533, -0.7205092487114027, 0.6650461296003519, 0.005260724207565836] # 3}}} robot.client.basic.problem.resetRoadmap () robot.client.basic.problem.setErrorThreshold (1e-3) robot.client.basic.problem.setMaxIterations (40) ps.selectPathProjector ('Progressive', 0.2) # Create constraints. {{{3 from hpp.corbaserver.manipulation import ConstraintGraph cg = ConstraintGraph (robot, 'graph') robot.client.manipulation.problem.createPlacementConstraint ( 'box_placement', 'box/box_surface', 'kitchen_area/pancake_table_table_top') jointNames = dict () jointNames['all'] = robot.getJointNames () jointNames['pr2'] = list () jointNames['allButPR2LeftArm'] = list () for n in jointNames['all']: if n.startswith ("pr2"): jointNames['pr2'].append (n) if not n.startswith ("pr2/l_gripper"): jointNames['allButPR2LeftArm'].append (n) ps.addPassiveDofs ('pr2', jointNames ['pr2'])
lockjoints = list () lockjoints.extend (lockbox) lockjoints.extend (lockur5) robot.setCurrentConfig (q_init) tf = robot.getJointPosition ('ur5/wrist_1_joint') #robot.client.basic.problem.createPositionConstraint \ #('pos', '', 'ur5/wrist_1_joint', (0, tf[1], tf[2]), (0,0,0), (False, True, True)) robot.client.basic.problem.createPositionConstraint \ ('pos', '', 'ur5/wrist_1_joint', (0, 0, tf[2]), (0,0,0), (False, False, True)) # 3}}} # Create the graph. {{{3 from hpp.corbaserver.manipulation import ConstraintGraph cg = ConstraintGraph (robot, 'graph') cg.createNode (['free']) cg.setConstraints (node = 'free', numConstraints = ['pos']) cg.setConstraints (graph = True, lockDof = lockjoints) cg.createEdge ('free', 'free', 'move_free', 1) # 3}}} 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)
shapes = list() for i in xrange(K): handles.append([boxes[i] + "/handle2"]) shapes.append([boxes[i] + "/box_surface"]) if K is 2: rules = [ Rule(grippers[0], handles[1][0], False), Rule(grippers[1], handles[0][0], False), ] else: rules = [] # Build the constraint graph cg = ConstraintGraph.buildGenericGraph(robot, "graph", grippers, boxes, handles, shapes, ['table/pancake_table_table_top'], rules) cg.setConstraints(graph=True, lockDof=lockAll) 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.') q_goal_proj = res[1] ps.setInitialConfig(q_init_proj) ps.addGoalConfig(q_goal_proj)
Rule([ "romeo/l_hand", "romeo/r_hand", ], ["", "placard/high"], True), Rule([ "romeo/l_hand", "romeo/r_hand", ], ["placard/low", "placard/high"], True), ] grippers = ['romeo/r_hand', 'romeo/l_hand'] handlesPerObjects = [placard.handles.values()] cg = ConstraintGraph.buildGenericGraph(robot, "graph", grippers, [ placard.name, ], handlesPerObjects, [ [], ], [], rules) # 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.6098745828476339, 0.5, 0.35889873246983567, 0.10845342945887403, 0.5, 0.02916333341652683, 0.025597731231524482, 0.16145134862579935, -2.785939904956431, -4.563075760833335, 0.8958690128585236, -0.19634763254425533, -0.7205092487114027, 0.6650461296003519, 0.005260724207565836 ] # 3}}} robot.client.basic.problem.resetRoadmap() robot.client.basic.problem.setErrorThreshold(1e-3) robot.client.basic.problem.setMaxIterations(40) ps.selectPathProjector('Progressive', 0.2) # Create constraints. {{{3 from hpp.corbaserver.manipulation import ConstraintGraph cg = ConstraintGraph(robot, 'graph') robot.client.manipulation.problem.createPlacementConstraint( 'box_placement', 'box/box_surface', 'kitchen_area/pancake_table_table_top') jointNames = dict() jointNames['all'] = robot.getJointNames() jointNames['pr2'] = list() jointNames['allButPR2LeftArm'] = list() for n in jointNames['all']: if n.startswith("pr2"): jointNames['pr2'].append(n) if not n.startswith("pr2/l_gripper"): jointNames['allButPR2LeftArm'].append(n) ps.addPassiveDofs('pr2', jointNames['pr2'])
for j, v in robot.openHand (None, .24, "left").iteritems () : lockedJointName = "close/"+j closeLeftHand.append (lockedJointName) ps.createLockedJoint (lockedJointName, j, [v,]) closeRightHand = [] for j, v in robot.openHand (None, .24, "right").iteritems () : lockedJointName = "close/"+j closeRightHand.append (lockedJointName) ps.createLockedJoint (lockedJointName, j, [v,]) lockbox = ps.lockFreeFlyerJoint ('box/base_joint', 'box_lock') lockBase = ps.lockPlanarJoint ('tom/base_joint', 'base_lock', [0,0,1,0]) # Build constraint graph graph = ConstraintGraph (robot, 'graph') graph.createNode (['hold_box_l1', 'hold_box_l2', 'hold_box_r1', 'hold_box_r2', 'free']) jointNames = dict () jointNames ['tom'] = list () for j in robot.jointNames: if j.startswith ('tom'): jointNames ['tom'].append (j) ps.addPassiveDofs ('tom', jointNames ['tom']) graph.createGrasp ('l2_grasp', 'tom/l_finger_tip', 'box/handle2', passiveJoints = 'tom') graph.createGrasp ('l1_grasp', 'tom/l_finger_tip', 'box/handle', passiveJoints = 'tom')
b1 = q_goal_inverted[rankB1:rankB1+7] q_goal_inverted[rankB1:rankB1+7] = q_goal_inverted[rankB2:rankB2+7] q_goal_inverted[rankB2:rankB2+7] = b1 # 3}}} robot.client.basic.problem.resetRoadmap () robot.client.basic.problem.setErrorThreshold (1e-3) robot.client.basic.problem.setMaxIterations (20) ps.selectPathValidation ('Discretized', 0.05) ps.selectPathProjector ('Progressive', 0.1) # ps.selectPathProjector ('Global', 0.2) # Create constraints. {{{3 from hpp.corbaserver.manipulation import ConstraintGraph cg = ConstraintGraph (robot, 'graph') # Create passive DOF lists {{{4 jointNames = dict () jointNames['all'] = robot.getJointNames () jointNames['baxter'] = list () jointNames['baxterRightSide'] = list () jointNames['baxterLeftSide'] = list () jointNames['box1'] = list () jointNames['box2'] = list () for n in jointNames['all']: if n.startswith ("baxter"): jointNames['baxter'].append (n) if n.startswith ("baxter/left_"): jointNames['baxterLeftSide'].append (n) if n.startswith ("baxter/right_"):
tool_gripper = "driller/drill_tip" 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',
grippers = ['pr2/l_gripper', 'pr2/r_gripper'] 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]
shapes = list () for i in xrange(K): handlesPerObject.append ([boxes[i] + "/handle2"]) handles.append (boxes[i] + "/handle2") shapes .append ([boxes[i] + "/box_surface"]) # Build rules rules = [Rule ([".*"], [".*"], True)] ps.client.manipulation.graph.autoBuild ("graph", grippers, boxes, handlesPerObject, shapes, ['table/pancake_table_table_top'], rules) # Get the built graph cg = ConstraintGraph (robot, 'graph', makeGraph = False) cg.setConstraints (graph = True, lockDof = lockAll) 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.') q_goal_proj = res [1] ps.setInitialConfig (q_init_proj) ps.addGoalConfig (q_goal_proj)
srdfSuffix = "" robot = Robot ('pr2-box', 'pr2') ps = ProblemSolver (robot) fk = FakeViewer (ps) fk.loadObjectModel (Box, 'box') fk.buildCompositeRobot (['pr2', 'box']) fk.loadEnvironmentModel (Environment, "kitchen_area") robot.setJointBounds ("pr2/base_joint_xy" , [-5,-2,-5.2,-2.7] ) robot.setJointBounds ("box/base_joint_xyz", [-5.1,-2,-5.2,-2.7,0,1.5]) # 2}}} # Load the Python class ConstraintGraph. {{{2 graph = ConstraintGraph (robot, 'graph') # 2}}} # 1}}} # Initialization. {{{1 # Set parameters. {{{2 robot.client.basic.problem.resetRoadmap () robot.client.basic.problem.selectPathOptimizer ('None') robot.client.basic.problem.setErrorThreshold (1e-3) robot.client.basic.problem.setMaxIterations (40) # 2}}} # Create lists of joint names - useful to define passive joints. {{{2 jointNames = dict ()
half_sitting = q = robot.getInitialConfig () q_init = half_sitting [::] # Set initial position of screw-driver # q_init [-7:] = [2, 1, 0.65, 0.7071067811865476, 0, -0.7071067811865475, 0] q_init [-7:] = [2, 1, 0.65, 0.7071067811865476, 0.5, -0.5, 0] # Open left hand ilh = robot.rankInConfiguration ['hrp2/LARM_JOINT6'] q_init [ilh:ilh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75] q_goal = q_init [::] q_goal [-7:] = [2, -1, 0.65, 0.7071067811865476, 0, -0.7071067811865475, 0] # 3}}} # Generate constraints {{{3 graph = ConstraintGraph (robot, 'graph') jointNames = dict () jointNames['all'] = robot.getJointNames () jointNames['hrp2'] = list () jointNames['allButHRP2LeftArm'] = list () for n in jointNames['all']: if n.startswith ("hrp2"): jointNames['hrp2'].append (n) if not n.startswith ("hrp2/LARM"): jointNames['allButHRP2LeftArm'].append (n) ps.addPassiveDofs ('hrp2', jointNames ['hrp2']) graph.createGrasp ('l_grasp', 'hrp2/leftHand', 'screw_gun/handle2', 'hrp2') graph.createPreGrasp ('l_pregrasp', 'hrp2/leftHand', 'screw_gun/handle2')
0, 1, ] lang = 'py' if lang == 'cxx': rules = [ 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' ]:
1.0001, -1.0001, 1.0001, -1.0001, 1.0001, -1.0001, 1.0001, ]) r = vf.createViewer() q1 = [0, -1.57, 1.57, 0, 0, 0, .3, 0, 0.025, 0, 0, 0, 1] r(q1) ## Create graph graph = ConstraintGraph(robot, 'graph') ps.selectPathValidation("Discretized", 0.01) ps.selectPathProjector("Progressive", 0.1) graph.initialize() res, q_init, error = graph.applyNodeConstraints('placement', q1) q2 = q1[::] q2[7] = .2 res, q_goal, error = graph.applyNodeConstraints('placement', q2) ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) pp = PathPlayer(ps.client.basic, r)