Master Arm Control - QUANTA X1 Pro 1.0

The master arm control examples use robot.master_left_arm / robot.master_right_arm. The following examples cover master arm state reading, master arm control mode switching, master arm joint small-range control, and master arm state stream reading. For more complete command-line examples, refer to examples/quanta_x1/master_arm_control.py, which includes the --write safety switch, target joint positions, end pose, and TOPPRA trajectory examples.

In the Quanta X1 environment, the master arm write control capability depends on whether the robot side is connected to the master arm hardware, whether the corresponding service is started, and whether the write control topic is enabled in the configuration; if the current environment provides only read-only data, the write interface will return a failure or an RPC error.

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("Please ensure the area around the master arm is safe before performing write control")
        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)