主臂控制 - 量子一号Pro 1.0

主臂控制示例使用 robot.master_left_arm / robot.master_right_arm。下面示例覆盖主臂状态读取、主臂控制模式切换、主臂关节小幅控制和主臂状态流读取。更完整的命令行示例可参考 examples/quanta_x1/master_arm_control.py，其中包含 --write 安全开关、目标关节位置、末端位姿和 TOPPRA 轨迹示例。

量子 1 号环境中，主臂写控制能力依赖机器人端是否接入主臂硬件、是否启动对应服务以及配置中是否启用写控制 topic；如果当前环境只提供只读数据，写接口会返回失败或 RPC 错误。

import time
from typing import Annotated
import typer

from x2robot import Robot, connect
from x2robot.sdk import JointPositions
from x2robot.sdk import ManipulatorControlMode, ManipulatorControlModeParam


def select_master_arm(robot: Robot, arm: str):
    if arm == "left":
        return robot.master_left_arm
    if arm == "right":
        return robot.master_right_arm
    raise ValueError("arm must be left or right")


def set_master_mode(master_arm, mode: str):
    if mode == "joint_pos":
        target_mode = ManipulatorControlMode.MANIPULATOR_JOINT_POSITIONS
    elif mode == "end_pose":
        target_mode = ManipulatorControlMode.MANIPULATOR_END_POSE
    else:
        raise ValueError("mode must be joint_pos or end_pose")

    result = master_arm.set_control_mode(ManipulatorControlModeParam(mode=target_mode))
    print(f"set_control_mode result: {result.is_success}, {result.error_message}")
    return result


def read_master_arm(master_arm):
    print(f"control_mode: {master_arm.get_control_mode()}")

    joint_state = master_arm.get_joint_states()
    print(f"joint names: {joint_state.name}")
    print(f"joint positions: {joint_state.position}")

    end_pose = master_arm.get_end_pose()
    print(f"end pose: {end_pose}")

    trigger = master_arm.get_gripper_position()
    print(f"trigger position: {trigger.position}")


def move_master_joint(master_arm, joint_index: int = 0, delta: float = 0.01):
    set_master_mode(master_arm, "joint_pos")

    joint_state = master_arm.get_joint_states()
    original = list(joint_state.position)
    target = list(original)
    target[joint_index] += delta

    result = master_arm.set_joint_positions(JointPositions(positions=target))
    print(f"set_joint_positions result: {result.is_success}, {result.error_message}")
    time.sleep(0.5)

    result = master_arm.set_joint_positions(JointPositions(positions=original))
    print(f"return result: {result.is_success}, {result.error_message}")


def stream_master_arm(master_arm, samples: int = 10):
    count = 0
    for joint_state in master_arm.get_joint_states_stream():
        print(joint_state)
        count += 1
        if count >= samples:
            break
        time.sleep(0.1)


def main(
    server: Annotated[str, typer.Option(help="server address")] = "192.168.10.1:50051",
    model: Annotated[str, typer.Option(help="robot model")] = "auto",
    arm: Annotated[str, typer.Option(help="left or right")] = "left",
    action: Annotated[str, typer.Option(help="read, move_joint, stream")] = "read",
):
    robot = connect(f"x2://{server}", model=model)
    master_arm = select_master_arm(robot, arm)

    if action == "read":
        read_master_arm(master_arm)
    elif action == "move_joint":
        print("请确认主臂周围安全，再执行写控制")
        if input("continue? (y/n): ").lower() == "y":
            move_master_joint(master_arm)
    elif action == "stream":
        stream_master_arm(master_arm)
    else:
        raise ValueError("action must be read, move_joint or stream")


if __name__ == "__main__":
    typer.run(main)