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}}}
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) graph.setConstraints (edge='grasp_e0', lockDof = lockscrewgun) graph.setConstraints (node='grasp_n0', pregrasp = 'l_pregrasp') graph.setConstraints (edge='grasp_e1', lockDof = lockscrewgun) graph.client.graph.setLevelSetConstraints (graph.edges['keep_grasp_ls'], [], lockscrewgun) graph.setConstraints (graph=True, lockDof = locklhand, numConstraints=ps.balanceConstraints ()) # 3}}}
#('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.') q_goal_proj = res [1] ps.setInitialConfig (q_init_proj) ps.addGoalConfig (q_goal_proj)
cg.createWaypointEdge('both', 'left', 'b_r_ungrasp', 1, 1) cg.createWaypointEdge('left', 'both', 'b_r_grasp', 1, 10) cg.setConstraints(edge='b_r_ungrasp_e1', lockDof=lockbox) cg.setConstraints(node='b_r_ungrasp_n0', grasp='l_grasp', pregrasp='r_pregrasp') cg.setConstraints(edge='b_r_ungrasp_e0', lockDof=lockbox) cg.setConstraints(edge='b_r_grasp_e1', lockDof=lockbox) cg.setConstraints(node='b_r_grasp_n0', grasp='l_grasp', pregrasp='r_pregrasp') cg.setConstraints(edge='b_r_grasp_e0', lockDof=lockbox) # 4}}} # Loops {{{4 cg.createEdge('free', 'free', 'move_free', 1) cg.createEdge('left', 'left', 'l_keep_grasp', 5) cg.createLevelSetEdge('left', 'left', 'l_keep_grasp_ls', 10) cg.createEdge('right', 'right', 'r_keep_grasp', 5) cg.createLevelSetEdge('right', 'right', 'r_keep_grasp_ls', 10) cg.setConstraints(edge='move_free', lockDof=lockbox) cg.client.graph.setLevelSetConstraints(cg.edges['l_keep_grasp_ls'], [], lockbox) cg.client.graph.setLevelSetConstraints(cg.edges['r_keep_grasp_ls'], [], lockbox) # 4}}} cg.setConstraints(graph=True, lockDof=lockhands)
cg.setContainingNode (ungraspEdge + "_e1", grasp) cg.setShort (ungraspEdge + "_e1", True) cg.setConstraints (edge=ungraspEdge + '_ls_e1', lockDof = otherLockBox) cg.setContainingNode (ungraspEdge + "_ls_e1", grasp) cg.setShort (ungraspEdge + "_ls_e1", True) cg.setLevelSetFoliation (edge=ungraspEdge + '_ls_e1', condNC=[placement,], paramLJ = lockBox) cg.setConstraints (edge=ungraspEdge + '_e0', lockDof = otherLockBox) cg.setContainingNode (ungraspEdge + "_e0", grasp) genGrasp (1, lockBox1, lockBox2) genGrasp (2, lockBox2, lockBox1) cg.createEdge ('free', 'free', 'transit', 0, 'free') cg.setConstraints (edge='transit', lockDof=lockBox1+lockBox2) cg.createEdge ('grasp1', 'grasp1', 'transfer1', 0, 'grasp1') cg.setConstraints (edge='transfer1', lockDof=lockBox2) cg.createEdge ('grasp2', 'grasp2', 'transfer2', 0, 'grasp2') cg.setConstraints (edge='transfer2', lockDof=lockBox1) cg.setConstraints (graph = True, lockDof = lockHeadFingersAndLeftSide) # 3}}} res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q1) if not res[0]: raise Exception ('Goal configuration could not be projected.') q1_proj = res [1] res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q0)
## 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,]) ps.setConstantRightHandSide ('placement', True)
pregrasps = ['l_pregrasp']) cg.createWaypointEdge ('both', 'left', 'b_r_ungrasp', 1, 1) cg.createWaypointEdge ('left', 'both', 'b_r_grasp', 1, 10) cg.setConstraints (node='b_r_ungrasp_n0', grasps = ['l_grasp'], pregrasps = ['r_pregrasp']) cg.setConstraints (edge='b_r_ungrasp_e0', lockDof = lockbox) cg.setConstraints (edge='b_r_grasp_e1', lockDof = lockbox) cg.setConstraints (node='b_r_grasp_n0', grasps = ['l_grasp'], pregrasps = ['r_pregrasp']) # 4}}} # Loops {{{4 cg.createEdge ('free', 'free', 'move_free', 1) cg.createEdge ('left', 'left', 'l_keep_grasp', 5) cg.createEdge ('right', 'right', 'r_keep_grasp', 5) cg.setConstraints (edge='move_free', lockDof = lockbox) # 4}}} cg.setConstraints (graph = True, lockDof = lockAll) # 3}}} res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init) if not res[0]: raise Exception ('Init configuration could not be projected.')
# Fridge {{{4 cg.setConstraints(node='fridge', grasps=[ 'r_fridge_grasp', ]) cg.createWaypointEdge('free', 'fridge', 'r_fridge_grasp', 1, 10) cg.setConstraints(edge='r_fridge_grasp_e1', lockDof=lockCup + lockFridge) cg.setConstraints(node='r_fridge_grasp_n0', pregrasps=[ 'r_fridge_pregrasp', ]) cg.setConstraints(edge='r_fridge_grasp_e0', lockDof=lockCup + lockFridge) # 4}}} # Loops {{{4 cg.createEdge('free', 'free', 'move_free', 1) cg.createEdge('cup', 'cup', 'cup_keep_grasp', 10) cg.createEdge('fridge', 'fridge', 'fridge_keep_grasp', 10) cg.createEdge('both', 'both', 'both_keep_grasp', 1) cg.setConstraints(edge='move_free', lockDof=lockFridge + lockCup) cg.setConstraints(edge='cup_keep_grasp', lockDof=lockFridge) cg.setConstraints(edge='fridge_keep_grasp', lockDof=lockCup) # 4}}} cg.setConstraints(graph=True, lockDof=lockAll, numConstraints=ps.balanceConstraints())
_["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) graph.setConstraints (node='both', numConstraints = bothfeet[0]) graph.setConstraints (node='d2l_n0', numConstraints = [ "leftfoot/com", "leftfoot/z", "leftfoot/rxry", "rightfoot/z", "rightfoot/rxry"])
_["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","both_right","both","left","right"]) graph.createEdge ('both', 'both', 'double', weight = 0) graph.createEdge ('both', 'both_left', 'double_to_leftside', isInNodeFrom = True) graph.createEdge ('both_left', 'left', 'leftside_to_left', isInNodeFrom = False) graph.createEdge ('both_left', 'both', 'leftside_to_double', isInNodeFrom = False) graph.createEdge ('left', 'both_left', 'left_to_leftside', isInNodeFrom = True, weight = 0) graph.createLevelSetEdge ('left', 'both_left', 'left_to_leftside_ls', isInNodeFrom = True) graph.createEdge ('left', 'left', 'left_support', weight = 0) graph.createEdge ('both', 'both_right', 'double_to_rightside', isInNodeFrom = True) graph.createEdge ('both_right', 'right', 'rightside_to_right', isInNodeFrom = False) graph.createEdge ('both_right', 'both', 'rightside_to_double', isInNodeFrom = False) graph.createEdge ('right', 'both_right', 'right_to_rightside', isInNodeFrom = True, weight = 0) graph.createLevelSetEdge ('right', 'both_right', 'right_to_rightside_ls', isInNodeFrom = True) graph.createEdge ('right', 'right', 'right_support', weight = 0)
('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) graph.setConstraints (edge='grasp_e0', lockDof = lockscrewgun) graph.setConstraints (node='grasp_n0', pregrasp = 'l_pregrasp') graph.setConstraints (edge='grasp_e1', lockDof = lockscrewgun) graph.setLevelSetConstraints ('keep_grasp_ls', lockDof = lockscrewgun) graph.setConstraints (graph=True, lockDof = locklhand, numConstraints=ps.balanceConstraints ()) # 3}}}
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) ps.setParameter('SimpleTimeParameterization/order', 2) ps.setParameter('SimpleTimeParameterization/maxAcceleration', .5) ps.setParameter('SimpleTimeParameterization/safety', 0.5)
cg.setContainingNode(ungraspEdge + "_ls_e1", grasp) cg.setShort(ungraspEdge + "_ls_e1", True) cg.setLevelSetFoliation(edge=ungraspEdge + '_ls_e1', condNC=[ placement, ], paramLJ=lockBox) cg.setConstraints(edge=ungraspEdge + '_e0', lockDof=otherLockBox) cg.setContainingNode(ungraspEdge + "_e0", grasp) genGrasp(1, lockBox1, lockBox2) genGrasp(2, lockBox2, lockBox1) cg.createEdge('free', 'free', 'transit', 0, 'free') cg.setConstraints(edge='transit', lockDof=lockBox1 + lockBox2) cg.createEdge('grasp1', 'grasp1', 'transfer1', 0, 'grasp1') cg.setConstraints(edge='transfer1', lockDof=lockBox2) cg.createEdge('grasp2', 'grasp2', 'transfer2', 0, 'grasp2') cg.setConstraints(edge='transfer2', lockDof=lockBox1) cg.setConstraints(graph=True, lockDof=lockHeadFingersAndLeftSide) # 3}}} res = ps.client.manipulation.problem.applyConstraints(cg.nodes['free'], q1) if not res[0]: raise Exception('Goal configuration could not be projected.') q1_proj = res[1] res = ps.client.manipulation.problem.applyConstraints(cg.nodes['free'], q0)
graph.createGrasp ('r1_grasp', 'tom/r_finger_tip', 'box/handle', passiveJoints = 'tom') graph.createPreGrasp ('l2_pregrasp', 'tom/l_finger_tip', 'box/handle2') graph.createPreGrasp ('l1_pregrasp', 'tom/l_finger_tip', 'box/handle') graph.createPreGrasp ('r2_pregrasp', 'tom/r_finger_tip', 'box/handle2') graph.createPreGrasp ('r1_pregrasp', 'tom/r_finger_tip', 'box/handle') graph.createWaypointEdge ('free', 'hold_box_l1', 'grasp_box_l1', nb=1, weight=10) graph.createWaypointEdge ('free', 'hold_box_l2', 'grasp_box_l2', nb=1, weight=10) graph.createWaypointEdge ('free', 'hold_box_r1', 'grasp_box_r1', nb=1, weight=10) graph.createWaypointEdge ('free', 'hold_box_r2', 'grasp_box_r2', nb=1, weight=10) graph.createEdge ('free', 'free', 'move_free', 1) graph.createEdge ('hold_box_l1', 'hold_box_l1', 'keep_grasp_l1', 5) graph.createEdge ('hold_box_l2', 'hold_box_l2', 'keep_grasp_l2', 5) graph.createEdge ('hold_box_r1', 'hold_box_r1', 'keep_grasp_r1', 5) graph.createEdge ('hold_box_r2', 'hold_box_r2', 'keep_grasp_r2', 5) graph.setConstraints (graph=True, lockDof = openLeftHand + openRightHand + lockBase) graph.setConstraints (edge='move_free', lockDof = lockbox) graph.setConstraints (edge='grasp_box_l1_e1', lockDof = lockbox) graph.setConstraints (edge='grasp_box_l2_e1', lockDof = lockbox) graph.setConstraints (edge='grasp_box_r1_e1', lockDof = lockbox) graph.setConstraints (edge='grasp_box_r2_e1', lockDof = lockbox)
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)) graph.addConstraints( node="free", constraints=Constraints(numConstraints=com_constraint + foot_placement +
_["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", "both_right", "both", "left", "right"]) graph.createEdge('both', 'both', 'double', weight=0) graph.createEdge('both', 'both_left', 'double_to_leftside', isInNodeFrom=True) graph.createEdge('both_left', 'left', 'leftside_to_left', isInNodeFrom=False) graph.createEdge('both_left', 'both', 'leftside_to_double', isInNodeFrom=False) graph.createEdge('left', 'both_left', 'left_to_leftside', isInNodeFrom=True, weight=0) graph.createLevelSetEdge('left', 'both_left', 'left_to_leftside_ls', isInNodeFrom=True) graph.createEdge('left', 'left', 'left_support', weight=0)
cg.setConstraints (edge='b_l_grasp_e1', lockDof = lockbox) cg.setConstraints (node='b_l_grasp_n0', pregrasps = ['l_pregrasp',]) cg.createWaypointEdge ('both', 'left', 'b_r_ungrasp', 1, 1, False, True) cg.createWaypointEdge ('left', 'both', 'b_r_grasp', 1, 10, True, True) cg.setConstraints (node='b_r_ungrasp_n0', pregrasps = ['r_pregrasp',]) cg.setConstraints (edge='b_r_ungrasp_e0', lockDof = lockbox) cg.setConstraints (edge='b_r_grasp_e1', lockDof = lockbox) cg.setConstraints (node='b_r_grasp_n0', pregrasps = ['r_pregrasp',]) # 4}}} # Loops {{{4 cg.createEdge ('free', 'free', 'move_free', 1, True) cg.createEdge ('left', 'left', 'l_keep_grasp', 5, True) cg.createEdge ('right', 'right', 'r_keep_grasp', 5, True) cg.createEdge ('both', 'both', 'move_both', 1, True) cg.setConstraints (edge='move_free', lockDof = lockbox) # 4}}} cg.setConstraints (graph = True, lockDof = lockAll) # 3}}} res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init) if not res[0]:
cg.setConstraints (node='l_cup_grasp_n0', pregrasps = ['l_cup_pregrasp',]) cg.setConstraints (edge='l_cup_grasp_e0', lockDof = lockCup + lockFridge) # 4}}} # Fridge {{{4 cg.setConstraints (node = 'fridge', grasps = ['r_fridge_grasp',]) cg.createWaypointEdge ('free', 'fridge', 'r_fridge_grasp', 1, 10) cg.setConstraints (edge='r_fridge_grasp_e1', lockDof = lockCup + lockFridge) cg.setConstraints (node='r_fridge_grasp_n0', pregrasps = ['r_fridge_pregrasp',]) cg.setConstraints (edge='r_fridge_grasp_e0', lockDof = lockCup + lockFridge) # 4}}} # Loops {{{4 cg.createEdge ('free', 'free', 'move_free', 1) cg.createEdge ('cup', 'cup', 'cup_keep_grasp', 10) cg.createEdge ('fridge', 'fridge', 'fridge_keep_grasp', 10) cg.createEdge ('both', 'both', 'both_keep_grasp', 1) cg.setConstraints (edge='move_free', lockDof = lockFridge + lockCup) cg.setConstraints (edge='cup_keep_grasp', lockDof = lockFridge) cg.setConstraints (edge='fridge_keep_grasp', lockDof = lockCup) # 4}}} cg.setConstraints (graph = True, lockDof = lockAll, numConstraints=ps.balanceConstraints ()) # 3}}}
## 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], [False, False, True, True, True, False,])
"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,
_["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',
#('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.') q_goal_proj = res[1] ps.setInitialConfig(q_init_proj) ps.addGoalConfig(q_goal_proj)
cg.graph.setContainingNode (cg.edges[edgeUBN + "_e1"], cg.nodes[rN]) cg.graph.setShort (cg.edges[edgeUBN + "_e1"], True) cg.setConstraints (edge=edgeUBN + '_ls_e1', lockDof = lFixedBox) cg.graph.setContainingNode (cg.edges[edgeUBN + "_ls_e1"], cg.nodes[rN]) cg.graph.setShort (cg.edges[edgeUBN + "_ls_e1"], True) cg.setLevelSetFoliation (edge=edgeUBN + '_ls_e1', condNC=[pcN,], paramLJ = lMovingBox) # , paramNC=[pcN + '/complement']) cg.setConstraints (edge=edgeUBN + '_e0', lockDof = lFixedBox) cg.graph.setContainingNode (cg.edges[edgeUBN + "_e0"], cg.nodes[rN]) # cg.graph.setShort (cg.edges["r_ungrasp_e0"], True) genGrasp (1, lockBox1, lockBox2) genGrasp (2, lockBox2, lockBox1) cg.createEdge ('free', 'free', 'move_free', weight = 0) cg.setConstraints (edge='move_free', lockDof=lockBox1+lockBox2) cg.createEdge ('r1', 'r1', 'kg1', weight = 0) cg.setConstraints (edge='kg1', lockDof=lockBox2) cg.createEdge ('r2', 'r2', 'kg2', weight = 0) cg.setConstraints (edge='kg2', lockDof=lockBox1) cg.setConstraints (graph = True, lockDof = lockAll) # cg.setConstraints (graph = True, numConstraints = ["gaze",], passiveJoints = ["box",], lockDof = lockAll) # cg.graph.setNumericalConstraints (cg.graphId , ["gaze",], ["box",]) # cg.graph.setLockedDofConstraints (cg.graphId , lockAll) # 3}}} res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init) if not res[0]: raise Exception ('Init configuration could not be projected.')
]) 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) # graph.setConstraints (edge="ungrasp_e1", lockDof = lockbox) # graph.setConstraints (node="ungrasp_n0", pregrasps = ['l_pregrasp',]) #graph.setConstraints (edge="ungrasp_e0", lockDof = lockboth) # graph.setConstraints (edge="ungrasp_e0", lockDof = lockbox) #graph.setConstraints (edge="grasp_e1", lockDof = lockboth) graph.setConstraints(edge="grasp_e1", lockDof=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') graph.setConstraints (edge="ungrasp_e0", lockDof = lockbox) graph.setConstraints (edge="grasp_e1", lockDof = lockbox) graph.setConstraints (node="grasp_n0", pregrasp = 'l_pregrasp') graph.setConstraints (edge="grasp_e0", lockDof = lockbox) graph.setLevelSetConstraints ("keep_grasp_ls", lockDof = lockbox) graph.setConstraints (graph = True, lockDof = locklhand)
vf = ViewerFactory(ps) left_arm_lock = createLeftArmLockedJoints(ps) right_arm_lock = createRightArmLockedJoints(ps) arm_locked = right_arm_lock if args.arm == 'left' else left_arm_lock 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", "apply_constraints", isInNode="starting_state") graph.createEdge("free", "starting_state", "release_constraints", 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)) graph.addConstraints( node="free", constraints=Constraints(numConstraints=com_constraint + foot_placement +