控制模块

robot.system.set_work_mode(
    RobotModeParam(mode=RobotWorkMode.SDK)
)
robot.robot_control.set_manipulator_control_mode(
    ManipulatorControlModeParam(
        mode=ManipulatorControlMode.MANIPULATOR_END_POSE
    )
)

mode = robot.robot_control.get_manipulator_control_mode()
print(mode)

result = robot.robot_control.emergency_stop()
print(result.is_success)

# 恢复
result = robot.robot_control.recover_emergency_stop()
print(result.is_success)

# 回零
result = robot.robot_control.homing()
print(result.is_success)

# 设置为相对位置控制模式
robot.chassis.set_control_mode(
    ChassisControlModeParam(mode=ChassisControlMode.RELATIVE)
)
robot.chassis.move_to_relative_position(
    ChassisPosition(x=0.25, y=0.0, yaw=0.0)
)

# 设置绝对位置控制模式
robot.chassis.set_control_mode(
    ChassisControlModeParam(mode=ChassisControlMode.GLOBAL)
)
robot.chassis.move_to_global_position(
    ChassisPosition(
        x=current_position.x + 0.25,
        y=current_position.y,
        yaw=current_position.yaw,
    )
)

mode = robot.chassis.get_control_mode()

position = ChassisPosition(x=1.0, y=0.5, yaw=0.0)
result = robot.chassis.move_to_global_position(position)

current_position = robot.chassis.get_global_position()
print(
    f"全局位置: x={current_position.x}, "
    f"y={current_position.y}, yaw={current_position.yaw}"
)

robot.chassis.set_virtual_zero_point(current_position)
robot.chassis.set_control_mode(
    ChassisControlModeParam(mode=ChassisControlMode.RELATIVE)
)
robot.chassis.move_to_relative_position(
    ChassisPosition(x=0.25, y=0.0, yaw=0.0)
)

# 设置速度：x 方向 0.25 m/s，持续 2 秒
for i in range(40):
    cur_velocity = ChassisVelocity(
        vel_x=0.25,
        vel_y=0.0,
        vel_yaw=0.0,
    )
    robot.chassis.set_velocity(cur_velocity)
    time.sleep(0.05)

time.sleep(0.5)

current_position = robot.chassis.get_global_position()
print(
    f"全局位置: x={current_position.x}, "
    f"y={current_position.y}, yaw={current_position.yaw}"
)
robot.chassis.set_virtual_zero_point(current_position)

position = robot.chassis.get_virtual_zero_point()

current_position = robot.chassis.get_global_position()
print(
    f"全局位置: x={current_position.x}, "
    f"y={current_position.y}, yaw={current_position.yaw}"
)

current_position = robot.chassis.get_relative_position()
print(
    f"相对位置: x={current_position.x}, "
    f"y={current_position.y}, yaw={current_position.yaw}"
)

current_odometry = robot.chassis.get_odometry()
print(current_odometry)

stream = robot.chassis.get_odometry_stream()
for msg in stream:
    print(msg)

stream = robot.chassis.get_pose_stream()
for msg in stream:
    print(msg)

robot.head.set_pose(HeadPose(yaw=0.0, pitch=0.0))

head_state = robot.head.get_pose()
print(head_state)

robot.head.reset()

stream = robot.head.get_joint_states_stream()
for msg in stream:
    print(msg)

robot.system.set_work_mode(
    RobotModeParam(mode=RobotWorkMode.SDK)
)

robot.robot_control.set_manipulator_control_mode(
    ManipulatorControlModeParam(
        mode=ManipulatorControlMode.MANIPULATOR_JOINT_POSITIONS
    )
)

joint_state = robot.right_arm.get_joint_states()
target_positions = list(joint_state.position)
target_positions[5] += 0.01

result = robot.right_arm.set_joint_positions(
    JointPositions(positions=target_positions)
)
print(result)

robot.system.set_work_mode(
    RobotModeParam(mode=RobotWorkMode.SDK)
)

robot.robot_control.set_manipulator_control_mode(
    ManipulatorControlModeParam(
        mode=ManipulatorControlMode.MANIPULATOR_END_POSE
    )
)

pose = Pose()
pose.position = Point(x=0.0, y=0.0, z=0.0)
pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)

pose.position.z += 0.03
robot.right_arm.set_end_pose(pose)
time.sleep(0.2)

joint_state = robot.left_arm.get_joint_states()
print(f"关节名称: {joint_state.name}")
print(f"关节位置: {joint_state.position}")
print(f"关节速度: {joint_state.velocity}")
print(f"关节力矩: {joint_state.effort}")

cur_pose = robot.left_arm.get_end_pose()
print(f"current_position: {cur_pose}")

stream = robot.left_arm.get_joint_states_stream()
for msg in stream:
    print(msg)

stream = robot.left_arm.get_end_pose_stream()
for msg in stream:
    print(msg)

from x2robot.sdk import (
    ManipulatorControlMode,
    ManipulatorControlModeParam,
)

robot.master_left_arm.set_control_mode(
    ManipulatorControlModeParam(
        mode=ManipulatorControlMode.MANIPULATOR_END_POSE
    )
)

mode = robot.master_left_arm.get_control_mode()
print(mode)

from x2robot.sdk import (
    JointPositions,
    ManipulatorControlMode,
    ManipulatorControlModeParam,
)

master_arm = robot.master_left_arm
master_arm.set_control_mode(
    ManipulatorControlModeParam(
        mode=ManipulatorControlMode.MANIPULATOR_JOINT_POSITIONS
    )
)

joint_state = master_arm.get_joint_states()
target = list(joint_state.position)
target[0] += 0.01

result = master_arm.set_joint_positions(
    JointPositions(positions=target)
)
print(result.is_success, result.error_message)

from x2robot.geometry_msgs import Pose, Point, Quaternion
from x2robot.sdk import (
    ManipulatorControlMode,
    ManipulatorControlModeParam,
)

master_arm = robot.master_left_arm
master_arm.set_control_mode(
    ManipulatorControlModeParam(
        mode=ManipulatorControlMode.MANIPULATOR_END_POSE
    )
)

current = master_arm.get_end_pose()
pose = Pose()
pose.position = Point(
    x=current.pose.position.x,
    y=current.pose.position.y,
    z=current.pose.position.z + 0.002,
)
pose.orientation = Quaternion(
    x=current.pose.orientation.x,
    y=current.pose.orientation.y,
    z=current.pose.orientation.z,
    w=current.pose.orientation.w,
)

result = master_arm.set_end_pose(pose)
print(result.is_success, result.error_message)

joint_state = robot.master_left_arm.get_joint_states()
print(f"关节名称: {joint_state.name}")
print(f"关节位置: {joint_state.position}")
print(f"关节速度: {joint_state.velocity}")
print(f"关节力矩: {joint_state.effort}")

cur_pose = robot.master_left_arm.get_end_pose()
print(f"current_position: {cur_pose}")

trigger = robot.master_left_arm.get_gripper_position()
print(trigger.position)

stream = robot.master_left_arm.get_joint_states_stream()
for msg in stream:
    print(msg)

stream = robot.master_left_arm.get_end_pose_stream()
for msg in stream:
    print(msg)

stream = robot.master_left_arm.get_gripper_state_stream()
for msg in stream:
    print(msg.position)

robot.system.set_work_mode(
    RobotModeParam(mode=RobotWorkMode.SDK)
)

robot.right_gripper.set_position(
    GripperPosition(position=0.5)
)
sleep(0.05)

position = robot.right_gripper.get_position()
print(position)

position = robot.left_gripper.get_position()
print(position)

stream = robot.left_gripper.get_joint_states_stream()
for msg in stream:
    print(msg)

cur_position = robot.lift.get_lift_position()
print(f"current_position: {cur_position}")

lift_position = LiftPosition(
    position=cur_position.position + 0.2
)
robot.lift.set_lift_position(lift_position)

position = robot.lift.get_lift_position()

stream = robot.lift.get_joint_states_stream()
for msg in stream:
    print(msg)