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') graph.createEdge ('gripper-above-ball','grasp-placement','grasp-ball',1,'placement') graph.createEdge ('gripper-above-ball','placement','move-gripper-away',1,'placement') graph.createEdge ('placement','gripper-above-ball','approach-ball',1,'placement') graph.createEdge ('placement', 'placement', 'transit', 1, 'placement') graph.createEdge ('ball-above-ground','grasp','take-ball-away',1,'grasp') graph.createEdge ('grasp','ball-above-ground','approach-ground',1,'grasp') graph.createEdge ('grasp', 'grasp', 'transfer', 1, 'grasp') # placement constraint - ball is in horizontal plane with free rotation around z allowed ps.createTransformationConstraint ('placement', '', ballName, [0,0,0.025,0, 0, 0, 1],
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", constraints=Constraints(numConstraints=com_constraint + foot_placement + left_gripper_lock + right_gripper_lock))
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') graph.createEdge ('grasp', 'placement', 'release-ball', 1, 'grasp') ## Create transformation constraint : ball is in horizontal plane with free ## rotation around z ps.createTransformationConstraint ('placement', '', ballName, [0,0,0.025,0, 0, 0, 1], [False, False, True, True, True, False,]) # Create complement constraint ps.createTransformationConstraint ('placement/complement', '', ballName, [0,0,0.025,0, 0, 0, 1], [True, True, False, False, False, True,])
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') graph.createGrasp ('r2_grasp', 'tom/r_finger_tip', 'box/handle2', passiveJoints = 'tom')
Rule([ "tiago/gripper", "driller/drill_tip", ], [ "driller/handle", ".*", ], True), ]) factory.generate() graph.addConstraints(graph=True, constraints=Constraints(numConstraints=ljs)) free = "tiago/gripper grasps driller/handle" loop_free = 'Loop | 0-0' for n in graph.nodes.keys(): if n == free: continue graph.addConstraints(node=n, constraints=Constraints(numConstraints=["look_at_gripper"])) for e in graph.edges.keys(): graph.addConstraints(edge=e, constraints=Constraints(numConstraints=["tiago_base"])) #Add alignment constraints for handle in part_handles: addAlignmentConstrainttoEdge(ps, handle, graph) graph.createNode('home', priority=1000) graph.createEdge('home', 'home', 'move_base') graph.createEdge('home', free , 'start_arm', isInNode="home") graph.createEdge( free , 'home', 'end_arm', isInNode=free) graph.addConstraints(node="home", constraints=Constraints(numConstraints=lock_arm+lock_head)) graph.addConstraints(edge="end_arm", constraints=Constraints(numConstraints=["tiago_base", "lock_part"])) graph.addConstraints(edge="move_base", constraints=Constraints(numConstraints=['tiago/gripper grasps driller/handle', "lock_part"])) graph.addConstraints(edge="start_arm", constraints=Constraints(numConstraints=['tiago/gripper grasps driller/handle', "lock_part"])) cproblem = ps.hppcorba.problem.getProblem() cgraph = cproblem.getConstraintGraph() graph.initialize() # 3}}}
p.createLockedDofConstraint('l_r_finger', 'pr2/l_gripper_r_finger_joint', 0.5, 0, 0) lockrhand = ['r_l_finger', 'r_r_finger'] p.createLockedDofConstraint('r_l_finger', 'pr2/r_gripper_l_finger_joint', 0.5, 0, 0) p.createLockedDofConstraint('r_r_finger', 'pr2/r_gripper_r_finger_joint', 0.5, 0, 0) lockhands = ['l_l_finger', 'l_r_finger', 'r_l_finger', 'r_r_finger'] # 3}}} # Create the graph. {{{3 cg.createNode(['both', 'right', 'left', 'free']) cg.setConstraints(node='free', numConstraints=['box_placement']) # This is not need as the object is locked in node free. #graph.setNumericalConstraintsForPath (id["free"], ['box_placement']) # Right hand {{{4 cg.setConstraints(node='right', grasp='r_grasp') cg.createWaypointEdge('right', 'free', 'r_ungrasp', nb=1, weight=1) cg.createWaypointEdge('free', 'right', 'r_grasp', nb=1, weight=10) #graph.setLockedDofConstraints (id["r_ungrasp_w"], lockbox) cg.setConstraints(edge='r_ungrasp_e1', lockDof=lockbox) cg.setConstraints(node='r_ungrasp_n0',
# Create constraints corresponding to each object {{{4 lockBox1 = ps.lockFreeFlyerJoint ("box1/" + Box.joint, 'box1_lock') lockBox2 = ps.lockFreeFlyerJoint ("box2/" + Box.joint, 'box2_lock') # 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}}} # Create the graph. {{{3 cg.createNode (['grasp2', 'grasp1', 'free']) cg.setConstraints (node='free', numConstraints=['box1_on_table','box2_on_table']) cg.setConstraints (node='grasp1', grasps = ['grasp1',]) cg.setConstraints (node='grasp2', grasps = ['grasp2',]) def genGrasp (iBox, lockBox, otherLockBox): iStr = str(iBox) grasp = 'grasp' + iStr edgeBaseName = grasp+'_edge' placement = 'box' + iStr + "_on_table" box_above_table = 'box' + iStr + "_above_table" # Free to right cg.createWaypointEdge ('free', grasp, edgeBaseName, nb=3, weight=1, isInNode = 'free', automaticBuilder=True)
lockFridge = ["fridge_lock", ] ps.createLockedJoint ("fridge_lock", Kitchen.joint, [0,]) lockCup = ps.lockFreeFlyerJoint (Cup.joint, 'cup_lock') robot.leftAnkle = robot.displayName + '/' + robot.leftAnkle robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle ps.addPartialCom ('romeo', ['romeo/base_joint_xyz']) ps.createStaticStabilityConstraints ("balance-romeo", q_init, 'romeo') # 4}}} # 3}}} # Create the graph. {{{3 cg.createNode (['both', 'cup', 'fridge', 'free']) # Cup and fridge {{{4 cg.setConstraints (node = 'both', grasps = [ 'l_cup_grasp', 'r_fridge_grasp', ]) cg.createWaypointEdge ('fridge', 'both', 'l_cup_grasp_both', 1, 10) cg.setConstraints (edge='l_cup_grasp_both_e1', lockDof = lockCup) cg.setConstraints (node='l_cup_grasp_both_n0', grasps = ['r_fridge_grasp'], pregrasps = ['l_cup_pregrasp',]) cg.setConstraints (edge='l_cup_grasp_both_e0', lockDof = lockCup) cg.createWaypointEdge ('cup', 'both', 'r_fridge_grasp_both', 1, 10) cg.setConstraints (edge='r_fridge_grasp_both_e1', lockDof = lockFridge) cg.setConstraints (node='r_fridge_grasp_both_n0', grasps = ['l_cup_grasp'], pregrasps = ['r_fridge_pregrasp',]) cg.setConstraints (edge='r_fridge_grasp_both_e0', lockDof = lockFridge)
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") graph.createEdge("grasp", "placement", "release-ball", 1, "grasp") # Create transformation constraint : ball is in horizontal plane with free # rotation around z ps.createTransformationConstraint( "placement", "", ballName, [0, 0, 0.025, 0, 0, 0, 1], [ False, False,
0, ]) lockCup = ps.lockFreeFlyerJoint(Cup.joint, 'cup_lock') robot.leftAnkle = robot.displayName + '/' + robot.leftAnkle robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle ps.addPartialCom('romeo', ['romeo/base_joint_xyz']) ps.createStaticStabilityConstraints("balance-romeo", q_init, 'romeo') # 4}}} # 3}}} # Create the graph. {{{3 cg.createNode(['both', 'cup', 'fridge', 'free']) # Cup and fridge {{{4 cg.setConstraints(node='both', grasps=[ 'l_cup_grasp', 'r_fridge_grasp', ]) cg.createWaypointEdge('fridge', 'both', 'l_cup_grasp_both', 1, 10) cg.setConstraints(edge='l_cup_grasp_both_e1', lockDof=lockCup) cg.setConstraints(node='l_cup_grasp_both_n0', grasps=['r_fridge_grasp'], pregrasps=[ 'l_cup_pregrasp', ])
_["double_to_leftside"]="" _["leftside_to_left"]="" _["left_to_leftside"]="" _["left_to_leftside_ls"]="" _["leftside_to_double"]="" _["left_support"]="" _["double_to_rightside"]="" _["rightside_to_right"]="" _["right_to_rightside"]="" _["right_to_rightside_ls"]="" _["rightside_to_double"]="" _["right_support"]="" # 4}}} # graph.setTextToTeXTranslation (_) graph.createNode (["both","left","right"]) graph.createEdge ('both', 'both', 'd2d', weight = 0) graph.createWaypointEdge ('both', 'left', 'd2l', nb = 1, isInNodeFrom = True, weight = 1) graph.graph.isInNodeFrom (graph.edges["d2l_e1"], False) graph.createEdge ('left', 'both', 'l2d', isInNodeFrom = True, weight = 1) graph.createLevelSetEdge ('left', 'both', 'l2d_ls', isInNodeFrom = True, weight = 5) graph.createEdge ('left', 'left', 'l2l', weight = 0) graph.createWaypointEdge ('both', 'right', 'd2r', nb = 1, isInNodeFrom = True, weight = 1) graph.graph.isInNodeFrom (graph.edges["d2r_e1"], False) graph.createEdge ('right', 'both', 'r2d', isInNodeFrom = True, weight = 1) graph.createLevelSetEdge ('right', 'both', 'r2d_ls', isInNodeFrom = True, weight = 5) graph.createEdge ('right', 'right', 'r2r', weight = 0)
('lhand_0', 'hrp2/LHAND_JOINT0', [q_init[ilh + 1],]) ps.createLockedJoint \ ('lhand_1', 'hrp2/LHAND_JOINT1', [q_init[ilh + 2],]) ps.createLockedJoint \ ('lhand_2', 'hrp2/LHAND_JOINT2', [q_init[ilh + 3],]) ps.createLockedJoint \ ('lhand_3', 'hrp2/LHAND_JOINT3', [q_init[ilh + 4],]) ps.createLockedJoint \ ('lhand_4', 'hrp2/LHAND_JOINT4', [q_init[ilh + 5],]) ps.addPartialCom ('hrp2', ['hrp2/base_joint_xyz']) ps.createStaticStabilityConstraints ("balance-hrp2", q_init, 'hrp2') # 3}}} # Create the graph of constraints {{{3 graph.createNode (["screwgun", "free"]) graph.createWaypointEdge ('screwgun', 'free', 'ungrasp', nb=1, weight=1) graph.createWaypointEdge ('free', 'screwgun', 'grasp', nb=1, weight=5) graph.createEdge ('free', 'free', 'move_free', 5) graph.createEdge ('screwgun', 'screwgun', 'keep_grasp', 10) graph.createLevelSetEdge ('screwgun', 'screwgun', 'keep_grasp_ls', 5) graph.setConstraints (node='screwgun', grasp='l_grasp') graph.setConstraints (edge='move_free', lockDof = lockscrewgun) graph.setConstraints (edge='ungrasp_e0', lockDof = lockscrewgun) graph.setConstraints (node='ungrasp_n0', pregrasp = 'l_pregrasp') graph.setConstraints (edge='ungrasp_e1', lockDof = lockscrewgun)
right_gripper_lock = [] other_lock = ["talos/torso_1_joint"] for n in robot.jointNames: 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]) cg = ConstraintGraph(robot, 'graph') cg.createNode(['static stability']) cg.createEdge(nodeFrom='static stability', nodeTo='static stability', name='move', weight=1, isInNode='static stability') cg.setConstraints(node='static stability', constraints=Constraints( numConstraints=quasiStaticConstraints, lockedJoints=left_gripper_lock + right_gripper_lock)) cg.initialize() q_init = half_sitting[::] q_goal = generateRandomConfig(robot, cg) ps.setInitialConfig(q_init) ps.resetGoalConfigs() ps.addGoalConfig(q_goal)
# 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}}} # Create the graph. {{{3 cg.createNode(['grasp2', 'grasp1', 'free']) cg.setConstraints(node='free', numConstraints=['box1_on_table', 'box2_on_table']) cg.setConstraints(node='grasp1', grasps=[ 'grasp1', ]) cg.setConstraints(node='grasp2', grasps=[ 'grasp2', ]) def genGrasp(iBox, lockBox, otherLockBox): iStr = str(iBox) grasp = 'grasp' + iStr edgeBaseName = grasp + '_edge'
_["right"]="Right grasp" _["free"]="No grasp" # _["r_grasp"]="" # _["l_grasp"]="" # _["b_r_grasp"]="" # _["b_l_grasp"]="" # _["b_r_ungrasp"]="" # _["b_l_ungrasp"]="" _["move_free"]="move_free" _["r_keep_grasp"]="r_keep_grasp" _["l_keep_grasp"]="l_keep_grasp" # 4}}} cg.setTextToTeXTranslation (_) cg.createNode (['both', 'right', 'left', 'free']) cg.setConstraints (node='free', numConstraints=['box_placement']) # Right hand {{{4 cg.setConstraints (node='right', grasps=['r_grasp']) cg.createWaypointEdge ('free', 'right', 'r_grasp', nb=1, weight=10) cg.setConstraints (edge='r_grasp_e1', lockDof = lockbox) cg.setConstraints (node='r_grasp_n0', pregrasps = ['r_pregrasp']) cg.setConstraints (edge='r_grasp_e0', lockDof = lockbox) # 4}}} # Left hand {{{4 cg.setConstraints (node = 'left', grasps = ['l_grasp'])
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) if not res[0]: raise Exception('Goal configuration could not be projected.')
_["double_to_leftside"] = "" _["leftside_to_left"] = "" _["left_to_leftside"] = "" _["left_to_leftside_ls"] = "" _["leftside_to_double"] = "" _["left_support"] = "" _["double_to_rightside"] = "" _["rightside_to_right"] = "" _["right_to_rightside"] = "" _["right_to_rightside_ls"] = "" _["rightside_to_double"] = "" _["right_support"] = "" # 4}}} # graph.setTextToTeXTranslation (_) graph.createNode(["both", "left", "right"]) graph.createEdge('both', 'both', 'd2d', weight=0) graph.createWaypointEdge('both', 'left', 'd2l', nb=1, isInNodeFrom=True, weight=1) graph.graph.isInNodeFrom(graph.edges["d2l_e1"], False) graph.createEdge('left', 'both', 'l2d', isInNodeFrom=True, weight=1) graph.createLevelSetEdge('left', 'both', 'l2d_ls', isInNodeFrom=True, weight=5) graph.createEdge('left', 'left', 'l2l', weight=0) graph.createWaypointEdge('both',
lockboth = lockpr2[:] lockboth.extend(lockbox) locklhand = ['l_l_finger', 'l_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, ]) # 2}}} # Create the constraint graph. {{{2 # Create nodes and edges. {{{3 graph.createNode(['box', 'free']) we = dict() # we["ungrasp"] = graph.createWaypointEdge ('box', 'free', "ungrasp", nb=1, weight=1) we["grasp"] = graph.createWaypointEdge('free', 'box', "grasp", nb=1, weight=10) graph.createEdge('free', 'free', 'move_free', 1) graph.createEdge('box', 'box', 'keep_grasp', 5) # graph.createLevelSetEdge ('box', 'box', 'keep_grasp_ls', 10) # 3}}} # Set the constraints of the component of the graph. {{{3 graph.setConstraints(node='box', grasps=[ 'l_grasp', ]) graph.setConstraints(edge='move_free', lockDof=lockbox)
# Create constraints corresponding to each object {{{4 lockBox1 = ps.lockFreeFlyerJoint ("box1/" + Box.joint, 'box1_lock') lockBox2 = ps.lockFreeFlyerJoint ("box2/" + Box.joint, 'box2_lock') # 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}}} # Create the graph. {{{3 # cg.createNode (['r2', 'r1', '2on1', 'free']) cg.createNode (['r2', 'r1', 'free']) cg.setConstraints (node='free', numConstraints=['box1_on_table','box2_on_table']) # cg.setConstraints (node='2on1', numConstraints=['box1_on_table','box2_on_box1']) cg.setConstraints (node='r1', grasps = ['r1_grasp',]) cg.setConstraints (node='r2', grasps = ['r2_grasp',]) def genGrasp (iBox, lMovingBox, lFixedBox, reuseBoxPositionFactor = 10, newBoxPositionFactor = 1): iStr = str(iBox) rN = 'r' + iStr edgeBN = rN+'_grasp' pcN = 'box' + iStr + "_on_table" ppcN = 'pre_box' + iStr + "_on_table" # Free to right cg.createWaypointEdge ('free', rN, edgeBN , nb=3, weight=1, automaticBuilder=True)
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) if not res[0]: raise Exception ('Goal configuration could not be projected.')
graph.createGrasp ('l_grasp', 'pr2/l_gripper', 'box/handle', passiveJoints = 'pr2') graph.createPreGrasp ('l_pregrasp', 'pr2/l_gripper', 'box/handle') lockbox = ps.lockFreeFlyerJoint ('box/base_joint', 'box_lock', compType = 'Equality') lockpr2 = ps.lockPlanarJoint ('pr2/base_joint', 'pr2_lock', compType = 'Equality') lockboth = lockpr2[:]; lockboth.extend (lockbox) locklhand = ['l_l_finger','l_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,]) # 2}}} # Create the constraint graph. {{{2 # Create nodes and edges. {{{3 graph.createNode (['box', 'free']) we = dict () we["ungrasp"] = graph.createWaypointEdge ('box', 'free', "ungrasp", nb=1, weight=1) we[ "grasp"] = graph.createWaypointEdge ('free', 'box', "grasp", nb=1, weight=10) graph.createEdge ('free', 'free', 'move_free', 1) graph.createEdge ('box', 'box', 'keep_grasp', 5) graph.createLevelSetEdge ('box', 'box', 'keep_grasp_ls', 10) # 3}}} # Set the constraints of the component of the graph. {{{3 graph.setConstraints (node='box', grasp='l_grasp') graph.setConstraints (edge='move_free', lockDof = lockbox) graph.setConstraints (edge="ungrasp_e1", lockDof = lockbox) graph.setConstraints (node="ungrasp_n0", pregrasp = 'l_pregrasp')
ps.createLockedJointConstraint \ ('lhand_0', 'hrp2/LHAND_JOINT0', [q_init[ilh + 1],]) ps.createLockedJointConstraint \ ('lhand_1', 'hrp2/LHAND_JOINT1', [q_init[ilh + 2],]) ps.createLockedJointConstraint \ ('lhand_2', 'hrp2/LHAND_JOINT2', [q_init[ilh + 3],]) ps.createLockedJointConstraint \ ('lhand_3', 'hrp2/LHAND_JOINT3', [q_init[ilh + 4],]) ps.createLockedJointConstraint \ ('lhand_4', 'hrp2/LHAND_JOINT4', [q_init[ilh + 5],]) ps.createStaticStabilityConstraints ("balance", q_init) # 3}}} # Create the graph of constraints {{{3 graph.createNode (["screwgun", "free"]) graph.createWaypointEdge ('screwgun', 'free', 'ungrasp', nb=1, weight=1) graph.createWaypointEdge ('free', 'screwgun', 'grasp', nb=1, weight=5) graph.createEdge ('free', 'free', 'move_free', 5) graph.createEdge ('screwgun', 'screwgun', 'keep_grasp', 10) graph.createLevelSetEdge ('screwgun', 'screwgun', 'keep_grasp_ls', 5) graph.setConstraints (node='screwgun', grasp='l_grasp') graph.setConstraints (edge='move_free', lockDof = lockscrewgun) graph.setConstraints (edge='ungrasp_e0', lockDof = lockscrewgun) graph.setConstraints (node='ungrasp_n0', pregrasp = 'l_pregrasp') graph.setConstraints (edge='ungrasp_e1', lockDof = lockscrewgun)