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)) left_gripper_lock = [] 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("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), Rule(["talos/left_gripper", "talos/right_gripper"], ["desk/touchpoint_left_front", "^$"], True), Rule(["talos/left_gripper", "talos/right_gripper"], ["desk/touchpoint_left_drawer", "^$"], True),