half_sitting = [ -0.74,0,1.0192720229567027,0,0,0,1, # root_joint 0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, # leg_left 0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, # leg_right 0, 0.006761, # torso 0.25847, 0.173046, -0.0002, -0.525366, 0, 0, 0.1, # arm_left 0, 0, 0, 0, 0, 0, 0, # gripper_left -0.25847, -0.173046, 0.0002, -0.525366, 0, 0, 0.1, # arm_right 0, 0, 0, 0, 0, 0, 0, # gripper_right 0, 0, # head 0,0,0,0,0,0,1, # box ] q_init = robot.getCurrentConfig() ps.addPartialCom ("talos", ["talos/root_joint"]) ps.addPartialCom ("talos_box", ["talos/root_joint", "box/root_joint"]) ps.createStaticStabilityConstraints ("balance", half_sitting, "talos", ProblemSolver.FIXED_ON_THE_GROUND) foot_placement = [ "balance/pose-left-foot", "balance/pose-right-foot" ] foot_placement_complement = [ ] robot.setCurrentConfig(half_sitting) com_wf = np.array(ps.getPartialCom("talos")) tf_la = Transform (robot.getJointPosition(robot.leftAnkle)) com_la = tf_la.inverse().transform(com_wf) ps.createRelativeComConstraint ("com_talos_box", "talos_box", robot.leftAnkle, com_la.tolist(), (True, True, True)) ps.createRelativeComConstraint ("com_talos" , "talos" , robot.leftAnkle, com_la.tolist(), (True, True, True)) ps.createPositionConstraint ("gaze", "talos/rgbd_optical_joint", "box/root_joint", (0,0,0), (0,0,0), (True, True, False))
lockrhand = list() for j, v in robot.rightHandOpen.iteritems(): lockrhand.append('romeo/' + j) if type(v) is float or type(v) is int: val = [ v, ] else: val = v ps.createLockedJoint('romeo/' + j, robot.displayName + '/' + j, val) lockHands = lockrhand + locklhand ## Create static stability constraint robot.leftAnkle = robot.displayName + '/' + robot.leftAnkle robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle ps.addPartialCom('romeo', ['romeo/root_joint']) q = robot.getInitialConfig() r = placard.rank q[r:r + 3] = [.4, 0, 1.2] ps.createStaticStabilityConstraints('balance-romeo', q, 'romeo', type=ProblemSolver.FIXED_ON_THE_GROUND) commonConstraints = Constraints(numConstraints=ps.balanceConstraints(), lockedJoints=lockHands) # build graph rules = [ Rule([ "romeo/l_hand", "romeo/r_hand",
0, 0, 0, # gripper_right 0, 0, # head 0, 0, 0, 0, 0, 0, 1, # mire ] q_init = robot.getCurrentConfig() ps.addPartialCom("talos", ["talos/root_joint"]) ps.addPartialCom("talos_mire", ["talos/root_joint", "mire/root_joint"]) ps.createStaticStabilityConstraints("balance", half_sitting, "talos", ProblemSolver.FIXED_ON_THE_GROUND) foot_placement = ["balance/pose-left-foot", "balance/pose-right-foot"] foot_placement_complement = [] robot.setCurrentConfig(half_sitting) com_wf = np.array(ps.getPartialCom("talos")) tf_la = Transform(robot.getJointPosition(robot.leftAnkle)) com_la = tf_la.inverse().transform(com_wf) ps.createRelativeComConstraint("com_talos_mire", "talos_mire", robot.leftAnkle, com_la.tolist(), (True, True, True)) ps.createRelativeComConstraint("com_talos", "talos", robot.leftAnkle,
lockrhand = list() for j, v in robot.rightHandOpen.items(): lockrhand.append('romeo/' + j) if type(v) is float or type(v) is int: val = [ v, ] else: val = v ps.createLockedJoint('romeo/' + j, robot.robotNames[0] + '/' + j, val) lockHands = lockrhand + locklhand ## Create static stability constraint robot.leftAnkle = robot.robotNames[0] + '/' + robot.leftAnkle robot.rightAnkle = robot.robotNames[0] + '/' + robot.rightAnkle ps.addPartialCom('romeo', ['romeo/root_joint']) q = robot.getInitialConfig() r = placard.rank q[r:r + 3] = [.4, 0, 1.2] ps.addPartialCom("romeo", ["romeo/root_joint"]) robot.createStaticStabilityConstraint('balance/', 'romeo', robot.leftAnkle, robot.rightAnkle, q) balanceConstraints = [ 'balance/pose-left-foot', 'balance/pose-right-foot', 'balance/relative-com', ] commonConstraints = Constraints (numConstraints = balanceConstraints + \ lockHands) # build graph
lockrhand = list() for j,v in robot.rightHandOpen.iteritems(): lockrhand.append ('romeo/' + j) if type(v) is float or type(v) is int: val = [v,] else: val = v; ps.createLockedJoint ('romeo/' + j, robot.displayName + '/' + j, val) lockHands = lockrhand + locklhand ## Create static stability constraint robot.leftAnkle = robot.displayName + '/' + robot.leftAnkle robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle ps.addPartialCom ('romeo', ['romeo/root_joint']) q = robot.getInitialConfig () r = placard.rank q [r:r+3] = [.4, 0, 1.2] ps.createStaticStabilityConstraints ('balance-romeo', q, 'romeo', type = ProblemSolver.FIXED_ON_THE_GROUND) 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']
ps.createLockedJoint("gripper_r_lock_f3j", "talos/gripper_right_fingertip_3_joint", [0]) ps.createLockedJoint("gripper_r_lock_j", "talos/gripper_right_joint", [0]) ps.createLockedJoint("gripper_r_lock_msj", "talos/gripper_right_motor_single_joint", [0]) # lock gripper left open ps.createLockedJoint("gripper_l_lock_idj", "talos/gripper_left_inner_double_joint", [0]) ps.createLockedJoint("gripper_l_lock_f1j", "talos/gripper_left_fingertip_1_joint", [0]) ps.createLockedJoint("gripper_l_lock_f2j", "talos/gripper_left_fingertip_2_joint", [0]) ps.createLockedJoint("gripper_l_lock_isj", "talos/gripper_left_inner_single_joint", [0]) ps.createLockedJoint("gripper_l_lock_f3j", "talos/gripper_left_fingertip_3_joint", [0]) ps.createLockedJoint("gripper_l_lock_j", "talos/gripper_left_joint", [0]) ps.createLockedJoint("gripper_l_lock_msj", "talos/gripper_left_motor_single_joint", [0]) # lock drawer closed ps.createLockedJoint("drawer_lock", "desk/upper_case_bottom_upper_drawer_bottom_joint", [0]) # static stability constraint ps.addPartialCom("Talos_CoM", ["talos/root_joint"]) ps.createStaticStabilityConstraints("talos_static_stability", q, "Talos_CoM", ProblemSolver.FIXED_ON_THE_GROUND) tpc = ps.getPartialCom("Talos_CoM") rla = robot.getJointPosition(robot.leftAnkle) com_wf = np.array(tpc) tf_la = Transform(rla) 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),
# ps.createLockedJoint ('laser', 'pr2/laser_tilt_mount_joint', # [q_init[robot.rankInConfiguration['pr2/laser_tilt_mount_joint']]]) # lockAll = lockhands + lockHeadAndTorso + lockKitchen lockAll = lockhands + lockKitchen # 4}}} # Create constraints corresponding to each object {{{4 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)
lockAll = lockhands + lockKitchen # 4}}} # Create constraints corresponding to each object {{{4 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', ])
lockscrewgun = ps.lockFreeFlyerJoint ('screw_gun/base_joint', 'screwgun_lock') locklhand = ['larm_6','lhand_0','lhand_1','lhand_2','lhand_3','lhand_4'] ps.createLockedJoint ('larm_6' , 'hrp2/LARM_JOINT6' , [q_init[ilh],]) ps.createLockedJoint \ ('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)
0, 0, 0.1, # arm_right 0, 0, 0, 0, 0, 0, 0, # gripper_right 0, 0, # head ] q_init = robot.getCurrentConfig() ps.addPartialCom("talos", ["talos/root_joint"]) ps.createStaticStabilityConstraints("balance", half_sitting, "talos", ProblemSolver.FIXED_ON_THE_GROUND) foot_placement = ["balance/pose-left-foot", "balance/pose-right-foot"] foot_placement_complement = [] robot.setCurrentConfig(half_sitting) com_wf = np.array(robot.getPartialCom("talos")) tf_la = Transform(robot.getJointPosition(robot.leftAnkle)) com_la = tf_la.inverse().transform(com_wf) ps.createRelativeComConstraint("com_talos", "talos", robot.leftAnkle, com_la.tolist(), (True, True, True)) left_gripper_lock = []
locklhand = ['larm_6', 'lhand_0', 'lhand_1', 'lhand_2', 'lhand_3', 'lhand_4'] ps.createLockedJoint('larm_6', 'hrp2/LARM_JOINT6', [ q_init[ilh], ]) ps.createLockedJoint \ ('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)