# 阶段一：PICO 遥操作双 RM75 平台配置。
#
# 当前控制方式是“相对 TCP + 单步 QP”：
# 按下 grip 时锁定当前手柄位姿和 TCP 位姿，之后将手柄相对位移和相对旋转
# 映射为目标 TCP 位姿，经过工作空间限幅、目标低通、姿态低通和单帧步长
# 限制后，通过 Placo 单步 QP 和 rm_movej_canfd 下发 7 个关节目标。
# cmd_vel 仅作为目标位姿变化率调试话题，不是机械臂执行命令。
# 末端外设由 peripherals_rm75.yaml 配置，真机连接阶段初始化后由遥操作节点复用。

left_arm_teleop:
  ros__parameters:
    arm_name: left_rm75
    controller_topic: /xr/left_controller
    control_rate_hz: 90.0
    command_timeout_sec: 0.12
    feedback_resync_timeout_sec: 0.5

    # 位姿目标生成与平滑参数。
    scale: 0.7
    deadband_m: 0.001
    target_filter_alpha: 0.65
    target_filter_alpha_fast: 0.9
    target_filter_fast_threshold_m: 0.03
    max_linear_speed: 0.15
    enable_position_axes: [true, true, true]
    enable_orientation_control: true
    enable_orientation_axes: [true, true, true]
    orientation_deadband_rad: 0.005
    orientation_filter_alpha: 0.65
    max_orientation_speed: 0.5
    workspace_min: [-0.70, -0.70, 0.10]
    workspace_max: [0.70, 0.10, 0.75]
    cyl_radius_limit: [0.10, 0.80]
    low_z_threshold: 0.1
    low_z_min_radius: 0.1

    # PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
    # 映射关系：机器人位移增量 = [-手柄y, 手柄z, -手柄x]。
    xr_to_robot_matrix: [0.0, -1.0, 0.0,
                         0.0, 0.0, 1.0,
                         -1.0, 0.0, 0.0]

    robot_ip: 192.168.192.18
    robot_port: 8080
    realtime_push_host_ip: 192.168.192.148
    realtime_push_port: 8089
    realtime_push_cycle_ms: 5
    avoid_singularity: 0
    follow: false
    canfd_trajectory_mode: 2
    canfd_radio: 0
    configure_safety_limits: true
    enable_tool_control: true
    enable_trigger_gripper_control: true
    trigger_close_threshold: 0.95
    configure_peripheral_on_connect: true
    max_line_speed: 0.25
    max_angular_speed: 0.6
    max_line_acc: 1.3
    max_angular_acc: 3.0
    joint_max_speed: 180.0
    joint_max_acc: 300.0
    move_to_initial_pose_on_connect: false
    initial_joint_pose: [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
    init_move_speed: 20
    debug_topic_prefix: /xr_rm

right_arm_teleop:
  ros__parameters:
    arm_name: right_rm75
    controller_topic: /xr/right_controller
    control_rate_hz: 90.0
    command_timeout_sec: 0.12
    feedback_resync_timeout_sec: 0.5

    scale: 0.7
    deadband_m: 0.001
    target_filter_alpha: 0.65
    target_filter_alpha_fast: 0.9
    target_filter_fast_threshold_m: 0.05
    max_linear_speed: 0.15
    enable_position_axes: [true, true, true]
    enable_orientation_control: true
    enable_orientation_axes: [true, true, true]
    orientation_deadband_rad: 0.005
    orientation_filter_alpha: 0.65
    max_orientation_speed: 0.5
    workspace_min: [-0.70, -0.70, 0.10]
    workspace_max: [0.70, 0.10, 0.75]
    cyl_radius_limit: [0.10, 0.80]
    low_z_threshold: 0.1
    low_z_min_radius: 0.1

    # PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
    # 映射关系：机器人位移增量 = [手柄y, 手柄z, 手柄x]。
    xr_to_robot_matrix: [0.0, 1.0, 0.0,
                         0.0, 0.0, 1.0,
                         1.0, 0.0, 0.0]

    robot_ip: 192.168.192.19
    robot_port: 8080
    realtime_push_host_ip: 192.168.192.148
    realtime_push_port: 8090
    realtime_push_cycle_ms: 5
    avoid_singularity: 0
    follow: false
    canfd_trajectory_mode: 2
    canfd_radio: 0
    configure_safety_limits: true
    enable_tool_control: true
    enable_trigger_gripper_control: true
    trigger_close_threshold: 0.95
    configure_peripheral_on_connect: true
    max_line_speed: 0.25
    max_angular_speed: 0.6
    max_line_acc: 1.3
    max_angular_acc: 3.0
    joint_max_speed: 180.0
    joint_max_acc: 300.0
    move_to_initial_pose_on_connect: false
    initial_joint_pose: [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
    init_move_speed: 20
    debug_topic_prefix: /xr_rm