Enhance orientation control for RM75 arms in XR teleoperation

CODEX.md

@@ -15,7 +15,7 @@ PICO/XR UDP JSON
   -> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped
 ```
 
-核心控制方式是相对位姿透传遥操作：`grip=true` 的第一帧锁定手柄起点和机器人 TCP 起点，之后用手柄位移增量生成目标 TCP，并通过 `rm_movep_canfd` 下发位姿目标；`grip=false`、`pose_valid=false`、UDP 超时、异常或节点退出必须停止。
+核心控制方式是相对位姿透传遥操作：`grip=true` 的第一帧锁定手柄起点和机器人 TCP 起点，之后用手柄相对位移和相对旋转生成目标 TCP，并通过 `rm_movep_canfd` 下发位姿目标；`grip=false`、`pose_valid=false`、UDP 超时、异常或节点退出必须停止。
 
 ## 固定工作流
 
@@ -97,7 +97,7 @@ ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000
   --pattern axis_sweep --seconds 60 --both-mode staggered
 ```
 
-当前 `sample_udp_sender` 默认用 `axis_sweep` 扫轴轨迹；`--both-mode staggered` 会先左后右，适合双臂方向检查。
+当前 `sample_udp_sender` 默认用 `axis_sweep` 扫轴轨迹；`--both-mode staggered` 会先左后右，适合双臂方向检查。需要检查姿态链路时可额外加 `--rotation-pattern rpy_steps`。
 
 常用话题：
 
@@ -180,8 +180,8 @@ git diff --check
 ## 包职责
 
 - `xr_rm_interfaces`：定义 `XrController`。
-- `xr_rm_input`：接收 UDP controller JSON，发布左右手柄话题；`pose_valid=false` 会强制 `grip=false`；提供 `sample_udp_sender` 扫轴/正弦调试数据。
+- `xr_rm_input`：接收 UDP controller JSON，发布左右手柄话题；`pose_valid=false` 会强制 `grip=false`；提供 `sample_udp_sender` 扫轴/正弦和姿态调试数据。
-- `xr_rm_teleop`：把手柄相对位移映射成 RM75 笛卡尔位姿透传目标。
+- `xr_rm_teleop`：把手柄相对位移和相对旋转映射成 RM75 笛卡尔位姿透传目标。
 - `xr_rm_bringup`：维护 launch、YAML、现场 UI 和运行入口。
 - `unity/XR_RM_PICO_UDP_Sender`：PICO 4 Ultra UDP Sender Unity 工程，发送 `seq`、`source_time`、`pose_valid`、`pose_source`、tracking/controller status 等诊断字段。
 

README.md

@@ -11,7 +11,7 @@ PICO/XR 双手柄 UDP JSON
   -> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题
 ```
 
-当前控制方式是“手柄相对位姿透传”遥操作：按住 `grip` 时锁定当前手柄位置和机械臂 TCP 位置，之后根据手柄相对位移生成目标 TCP，经过工作空间限幅、目标低通和单帧步长限制后，通过 `rm_movep_canfd` 下发目标位姿。松开 `grip`、UDP 超时或节点退出时会请求机械臂慢停。
+当前控制方式是“手柄相对位姿透传”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP，经过工作空间限幅、目标低通、姿态低通和单帧步长限制后，通过 `rm_movep_canfd` 下发目标位姿。松开 `grip`、UDP 超时或节点退出时会请求机械臂慢停。
 
 ## 当前范围
 
@@ -160,7 +160,7 @@ ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000
   --pattern axis_sweep --seconds 60 --both-mode staggered
 ```
 
-`sample_udp_sender` 默认使用 `axis_sweep` 成对扫轴轨迹，并在终端打印 `XR +X/-X/+Y/-Y/+Z/-Z` 标签。`--hand both --both-mode staggered --seconds 60` 会先左后右，适合肉眼确认左右臂方向；如果只想左右同时动，可用 `--both-mode synchronized`。
+`sample_udp_sender` 默认使用 `axis_sweep` 成对扫轴轨迹，并在终端打印 `XR +X/-X/+Y/-Y/+Z/-Z` 标签。`--hand both --both-mode staggered --seconds 60` 会先左后右，适合肉眼确认左右臂方向；如果只想左右同时动，可用 `--both-mode synchronized`。需要检查末端姿态时可增加 `--rotation-pattern rpy_steps --rotation-amplitude-deg 25`。
 
 观察：
 
@@ -241,6 +241,9 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 - `target_filter_alpha` / `target_filter_alpha_fast`：目标 TCP 低通滤波系数，快速移动时自动使用更大的系数。
 - `target_filter_fast_threshold_m`：进入快速滤波区间的目标变化阈值。
 - `max_linear_speed`：目标位姿单帧步长限制对应的最大线速度。
+- `enable_orientation_control`：是否把手柄相对旋转映射到 TCP 姿态。
+- `orientation_filter_alpha` / `orientation_deadband_rad`：目标 TCP 姿态低通和死区。
+- `max_orientation_speed`：目标姿态单帧步长限制对应的最大角速度。
 - `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
 - `cyl_radius_limit`：基座圆柱半径限制。
 - `xr_to_robot_matrix`：`/xr/*_controller` Project 位移到 RM75 base 坐标的映射矩阵。
@@ -332,10 +335,10 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa
 - `t` / `source_time`：Unity 端 `Time.realtimeSinceStartupAsDouble`，用于后续延迟分析。
 - `seq`：Unity 端递增包序号，用于后续丢包分析。
 - `frame_id`：默认 `xr_world`，会写入 `XrController.header.frame_id`。
-- `grip`：运动使能。`true` 时进入相对位移控制，`false` 时停止。
+- `grip`：运动使能。`true` 时进入相对位姿控制，`false` 时停止。
 - `trigger`：扳机值，范围 `0.0-1.0`。真机模式下跨过 `0.95` 的上升沿会切换对应夹爪开/关状态。
 - `pos`：手柄位置，长度 3。
-- `quat`：手柄姿态四元数，默认按 `xyzw` 解析。
+- `quat`：手柄姿态四元数，默认按 `xyzw` 解析；遥操作节点会用 grip 锁定后的相对旋转控制 TCP 姿态。
 - `pose_valid`：姿态是否可信。ROS 接收端看到 `false` 会强制 `grip=false`。
 - `pose_source`：`pxr_predict`、`unity_xr` 或 `none`，用于判断姿态来自 PICO 预测接口还是 Unity XR fallback。
 - `tracking_state` / `controller_status`：Unity/PICO 侧追踪诊断值，只用于日志和排查。
@@ -354,9 +357,10 @@ PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并
 3. 单臂启动，`move_to_initial_pose_on_connect:=false`。
 4. 手握急停，按住 `grip` 后只做小幅单轴移动。
 5. 逐个确认上/下、前/后、左/右方向。
-6. 点击对应 `trigger`，确认每次点击都会切换对应夹爪状态，松开 trigger 后状态保持且左右不串臂。
+6. 小角度转动手柄，确认 `/xr_rm/<arm>/target_pose` 姿态和 `/xr_rm/<arm>/cmd_vel.twist.angular` 变化符合预期。
-7. 确认松开 `grip` 后机械臂慢停，`/xr_rm/<arm>/cmd_vel` 回到零；trigger 仍只影响夹爪，不影响机械臂运动门控。
+7. 点击对应 `trigger`，确认每次点击都会切换对应夹爪状态，松开 trigger 后状态保持且左右不串臂。
-8. 左右臂都确认后，再进入双臂模式。
+8. 确认松开 `grip` 后机械臂慢停，`/xr_rm/<arm>/cmd_vel` 回到零；trigger 仍只影响夹爪，不影响机械臂运动门控。
+9. 左右臂都确认后，再进入双臂模式。
 
 当前项目没有双臂碰撞检测。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。
 

docs/pico_udp_sender_ubuntu22_setup.md

@@ -270,7 +270,7 @@ sudo ufw status
 - `UDP Sending ON` 后，ROS2 的 `/xr/left_controller` 与 `/xr/right_controller` 应持续刷新。
 - HUD 显示包计数、发送端点、左右姿态状态和 KeepAwake 状态。
 - `pose_valid=false` 时，ROS 接收端会强制该手柄 `grip=false`，即使 PICO 端按下了 Grip。
-- 按住 `grip` 并移动手柄时，mock 模式下 `/xr_rm/*/target_pose` 应连续变化，`/xr_rm/*/cmd_vel` 会显示目标位姿变化率。
+- 按住 `grip` 并移动或小角度转动手柄时，mock 模式下 `/xr_rm/*/target_pose` 应连续变化，`/xr_rm/*/cmd_vel` 会显示目标位姿变化率。
 - 真机模式下，点击对应手柄 `trigger` 会切换并保持对应夹爪开/关状态。
 - 松开 `grip` 后，机械臂慢停，`cmd_vel` 应回到零。
 
@@ -352,9 +352,10 @@ ros2 topic echo /xr_rm/right_rm75/target_pose
 1. 启动 PICO Unity 应用。
 2. 确认 `/xr/left_controller` 和 `/xr/right_controller` 正常刷新。
 3. 左手按住 `grip`，只移动左手一小段，观察 `/xr_rm/left_rm75/target_pose` 和 `/xr_rm/left_rm75/cmd_vel`。
-4. 松开左手 `grip`，确认 `cmd_vel` 回到 0。
+4. 左手保持小角度转动，确认 `target_pose.pose.orientation` 和 `cmd_vel.twist.angular` 会变化。
-5. 右手重复同样流程。
+5. 松开左手 `grip`，确认 `cmd_vel` 回到 0。
-6. 用 `ros2 topic hz` 确认频率稳定，建议接近 PICO 端配置的发送频率。
+6. 右手重复同样流程。
+7. 用 `ros2 topic hz` 确认频率稳定，建议接近 PICO 端配置的发送频率。
 
 如果要排除 PICO 端问题，可用本机 sample sender 验证 ROS 端：
 
@@ -364,6 +365,7 @@ ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000
 ```
 
 `axis_sweep` 会按 `XR +X/-X/+Y/-Y/+Z/-Z` 打印方向标签。双手 `staggered` 模式先左后右，便于现场逐只确认映射。
+如需同时验证姿态链路，可增加 `--rotation-pattern rpy_steps --rotation-amplitude-deg 25`。
 
 ## 11. UDP JSON 协议
 
@@ -431,7 +433,7 @@ Unity APK 每个周期发送一个双手柄 JSON 包：
 | `grip` | bool | ROS 遥操作启停 |
 | `trigger` | float | `0.0-1.0`；真机模式下跨过 `0.95` 的上升沿切换夹爪开/关状态 |
 | `pos` | float[3] | `[x, y, z]` |
-| `quat` | float[4] | `[qx, qy, qz, qw]` |
+| `quat` | float[4] | `[qx, qy, qz, qw]`；ROS 遥操作节点会用相对旋转控制 TCP 姿态 |
 | `pose_valid` | bool | 姿态有效性；`false` 时 ROS receiver 强制 `grip=false` |
 | `pose_source` | string | `pxr_predict`、`unity_xr` 或 `none` |
 | `tracking_state` | int | Unity XR tracking state 原始值 |
@@ -479,8 +481,9 @@ PXR `pxr_predict` 原始坐标按现场实测通常表现为右移 `native.z+`
 1. 只启动 `use_mock:=true`。
 2. APK 中选择 `Project (+Z back)`，按住左手 `grip`，每次只沿右/左、上/下、前/后移动一个方向。
 3. 记录 `/xr/left_controller.pose.position` 的变化方向。
-4. 记录 `/xr_rm/left_rm75/target_pose` 的方向。
+4. 记录 `/xr_rm/left_rm75/target_pose` 的位置方向。
-5. 右手重复。
+5. 小角度转动左手手柄，记录 `/xr/left_controller.pose.orientation` 与 `/xr_rm/left_rm75/target_pose.pose.orientation` 是否连续变化。
+6. 右手重复。
 
 如果两个手柄在 ROS topic 里的某个轴都反了，优先检查 Unity/PICO 发送到 `/xr/*_controller.pose.position` 的实际方向。如果 ROS topic 正确，但某一只机械臂运动方向不符合现场坐标，只改对应 YAML 的 `xr_to_robot_matrix`。
 
@@ -494,6 +497,7 @@ PXR `pxr_predict` 原始坐标按现场实测通常表现为右移 `native.z+`
 - 机械臂工作区清空。
 - PICO topic 在 mock 下已经稳定。
 - `grip=false` 时机械臂慢停，`/xr_rm/<arm>/cmd_vel` 为 0。
+- mock 下小角度转动手柄时，`/xr_rm/<arm>/cmd_vel.twist.angular` 有连续小幅变化。
 - 夹爪验证时点击对应手柄 `trigger`，确认每次点击都会切换并保持开/关状态。
 - `move_to_initial_pose_on_connect` 保持 `false`。
 

xr_rm_bringup/config/dual_arm_rm75.yaml

@@ -1,9 +1,10 @@
 # 阶段一：PICO 遥操作双 RM75 平台配置。
 #
 # 当前控制方式是“相对位姿透传”：
-# 按下 grip 时锁定当前手柄位姿和 TCP 位姿，之后将手柄相对位移映射为目标
+# 按下 grip 时锁定当前手柄位姿和 TCP 位姿，之后将手柄相对位移和相对旋转
-# TCP 位姿，经过工作空间限幅、目标低通和单帧步长限制后，通过 rm_movep_canfd
+# 映射为目标 TCP 位姿，经过工作空间限幅、目标低通、姿态低通和单帧步长
-# 下发。cmd_vel 仅作为目标位姿变化率调试话题，不是机械臂执行命令。
+# 限制后，通过 rm_movep_canfd 下发。cmd_vel 仅作为目标位姿变化率调试话题，
+# 不是机械臂执行命令。
 # 末端外设由 peripherals_rm75.yaml 配置，真机连接阶段初始化后由遥操作节点复用。
 
 left_arm_teleop:
@@ -21,6 +22,11 @@ left_arm_teleop:
     target_filter_fast_threshold_m: 0.03
     max_linear_speed: 0.2
     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.6
     current_pose_poll_hz: 10.0
 
     workspace_min: [-0.70, -0.60, 0.10]
@@ -71,6 +77,11 @@ right_arm_teleop:
     target_filter_fast_threshold_m: 0.03
     max_linear_speed: 0.2
     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.6
     current_pose_poll_hz: 10.0
 
     workspace_min: [-0.70, -0.60, 0.10]

xr_rm_bringup/config/left_arm_rm75.yaml

@@ -8,7 +8,7 @@ single_arm_velocity_teleop:
     control_rate_hz: 90.0
     command_timeout_sec: 0.12
 
-    # 手柄相对位移 -> 目标 TCP 位姿；随后做目标低通和单帧步长限制。
+    # 手柄相对位姿 -> 目标 TCP 位姿；随后做目标低通、姿态低通和单帧步长限制。
     scale: 1.0
     deadband_m: 0.001
     target_filter_alpha: 0.65
@@ -16,6 +16,11 @@ single_arm_velocity_teleop:
     target_filter_fast_threshold_m: 0.03
     max_linear_speed: 0.3
     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.6
     current_pose_poll_hz: 10.0
 
     workspace_min: [-0.70, -0.60, 0.10]

xr_rm_bringup/config/right_arm_rm75.yaml

@@ -8,17 +8,22 @@ single_arm_velocity_teleop:
     control_rate_hz: 90.0
     command_timeout_sec: 0.12
 
-    scale: 0.75
+    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
+    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
     current_pose_poll_hz: 10.0
 
-    workspace_min: [-0.70, -0.60, 0.10]
+    workspace_min: [-0.60, -0.60, 0.10]
-    workspace_max: [0.70, 0.40, 0.70]
+    workspace_max: [0.60, 0.70, 0.55]
     cyl_radius_limit: [0.10, 0.70]
     low_z_threshold: 0.1
     low_z_min_radius: 0.1

xr_rm_input/xr_rm_input/sample_udp_sender.py

@@ -32,6 +32,14 @@ AXIS_SWEEP_SEGMENTS = (
     ("XR -Z  front", (0.0, 0.0, -1.0), "hold"),
     ("CENTER / Z done", (0.0, 0.0, 0.0), "center"),
 )
+RPY_DIRECTIONS = (
+    ("XR roll +", (1.0, 0.0, 0.0)),
+    ("XR roll -", (-1.0, 0.0, 0.0)),
+    ("XR pitch +", (0.0, 1.0, 0.0)),
+    ("XR pitch -", (0.0, -1.0, 0.0)),
+    ("XR yaw +", (0.0, 0.0, 1.0)),
+    ("XR yaw -", (0.0, 0.0, -1.0)),
+)
 
 
 def _validate_positive(name: str, value: float) -> float:
@@ -66,6 +74,12 @@ def _pattern_period(pattern: str, hold_seconds: float, center_seconds: float) ->
     return 5.0
 
 
+def _rotation_pattern_period(pattern: str, hold_seconds: float, center_seconds: float) -> float:
+    if pattern == "rpy_steps":
+        return (hold_seconds + center_seconds) * len(RPY_DIRECTIONS)
+    return 0.0
+
+
 def _axis_sweep_position(
     t: float,
     amplitude: float,
@@ -121,6 +135,66 @@ def _sine_position(t: float, amplitude: float, phase: float) -> tuple[str, list[
     ]
 
 
+def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> list[float]:
+    cy = math.cos(yaw * 0.5)
+    sy = math.sin(yaw * 0.5)
+    cp = math.cos(pitch * 0.5)
+    sp = math.sin(pitch * 0.5)
+    cr = math.cos(roll * 0.5)
+    sr = math.sin(roll * 0.5)
+    return [
+        sr * cp * cy - cr * sp * sy,
+        cr * sp * cy + sr * cp * sy,
+        cr * cp * sy - sr * sp * cy,
+        cr * cp * cy + sr * sp * sy,
+    ]
+
+
+def _rpy_steps_quaternion(
+    t: float,
+    amplitude_rad: float,
+    hold_seconds: float,
+    center_seconds: float,
+    initial_center_seconds: float,
+) -> tuple[str, list[float]]:
+    if t < initial_center_seconds:
+        return "ROT center / lock origin", [0.0, 0.0, 0.0, 1.0]
+
+    segment_seconds = hold_seconds + center_seconds
+    period = segment_seconds * len(RPY_DIRECTIONS)
+    cycle_t = (t - initial_center_seconds) % period
+    direction_index = int(cycle_t // segment_seconds)
+    segment_t = cycle_t - direction_index * segment_seconds
+    label, direction = RPY_DIRECTIONS[direction_index]
+    if segment_t >= hold_seconds:
+        return f"ROT center after {label}", [0.0, 0.0, 0.0, 1.0]
+
+    return label, _euler_to_quaternion(
+        amplitude_rad * direction[0],
+        amplitude_rad * direction[1],
+        amplitude_rad * direction[2],
+    )
+
+
+def _rotation_quaternion(
+    pattern: str,
+    t: float,
+    amplitude_rad: float,
+    hold_seconds: float,
+    center_seconds: float,
+    initial_center_seconds: float,
+) -> tuple[str, list[float]]:
+    if pattern == "rpy_steps":
+        return _rpy_steps_quaternion(
+            t,
+            amplitude_rad,
+            hold_seconds,
+            center_seconds,
+            initial_center_seconds,
+        )
+    return "ROT none", [0.0, 0.0, 0.0, 1.0]
+
+
 def _pattern_position(
     pattern: str,
     t: float,
@@ -186,6 +260,18 @@ def main() -> None:
         default=0.0,
         help="发送的 trigger 值。方向调试默认不驱动夹爪。",
     )
+    parser.add_argument(
+        "--rotation-pattern",
+        choices=("none", "rpy_steps"),
+        default="none",
+        help="可选姿态测试轨迹；none 保持 identity 四元数，rpy_steps 逐轴发送小角度姿态。",
+    )
+    parser.add_argument(
+        "--rotation-amplitude-deg",
+        type=_positive_float("rotation-amplitude-deg"),
+        default=25.0,
+        help="rpy_steps 模式下的姿态幅度，单位 degree。",
+    )
     parser.add_argument("--hand", choices=("left", "right", "both"), default="right")
     parser.add_argument(
         "--both-mode",
@@ -212,17 +298,22 @@ def main() -> None:
         }
         for hand in hands
     }
-    pattern_period = _pattern_period(args.pattern, args.hold_seconds, args.center_seconds)
+    pattern_period = max(
+        _pattern_period(args.pattern, args.hold_seconds, args.center_seconds),
+        _rotation_pattern_period(args.rotation_pattern, args.hold_seconds, args.center_seconds),
+    )
     one_hand_seconds = args.initial_center_seconds + pattern_period
     full_sweep_seconds = one_hand_seconds * len(hands) if args.both_mode == "staggered" else one_hand_seconds
+    rotation_amplitude_rad = math.radians(args.rotation_amplitude_deg)
     print(
         "Sample UDP sender: "
         f"hand={args.hand}, both_mode={args.both_mode}, pattern={args.pattern}, "
+        f"rotation_pattern={args.rotation_pattern}, "
         f"amplitude={args.amplitude:.3f}m, hold={args.hold_seconds:.2f}s, "
         f"center={args.center_seconds:.2f}s, seconds={args.seconds:.1f}s",
         flush=True,
     )
-    if args.pattern != "sine" and args.seconds < full_sweep_seconds:
+    if (args.pattern != "sine" or args.rotation_pattern != "none") and args.seconds < full_sweep_seconds:
         print(
             "提示：当前 seconds 不足以跑完一轮完整方向序列；"
             f"建议至少 {full_sweep_seconds:.1f}s。",
@@ -241,8 +332,10 @@ def main() -> None:
                 hand_end_t = hand_start_t + one_hand_seconds
                 if t < hand_start_t:
                     label, pos = "WAIT / center", BASE_POS.copy()
+                    rot_label, quat = "ROT wait / center", [0.0, 0.0, 0.0, 1.0]
                 elif t >= hand_end_t:
                     label, pos = "DONE / center", BASE_POS.copy()
+                    rot_label, quat = "ROT done / center", [0.0, 0.0, 0.0, 1.0]
                 else:
                     pattern_t = t - hand_start_t
                     label, pos = _pattern_position(
@@ -254,6 +347,14 @@ def main() -> None:
                         args.initial_center_seconds,
                         index * math.pi,
                     )
+                    rot_label, quat = _rotation_quaternion(
+                        args.rotation_pattern,
+                        pattern_t,
+                        rotation_amplitude_rad,
+                        args.hold_seconds,
+                        args.center_seconds,
+                        args.initial_center_seconds,
+                    )
             else:
                 label, pos = _pattern_position(
                     args.pattern,
@@ -264,15 +365,30 @@ def main() -> None:
                     args.initial_center_seconds,
                     index * math.pi,
                 )
+                rot_label, quat = _rotation_quaternion(
+                    args.rotation_pattern,
+                    pattern_t,
+                    rotation_amplitude_rad,
+                    args.hold_seconds,
+                    args.center_seconds,
+                    args.initial_center_seconds,
+                )
 
-            if label != last_labels[hand]:
+            combined_label = label if args.rotation_pattern == "none" else f"{label} | {rot_label}"
-                print(f"[{t:5.2f}s] {hand:<5} {label:<22} pos={pos}", flush=True)
+            if combined_label != last_labels[hand]:
-                last_labels[hand] = label
+                quat_text = [round(value, 4) for value in quat]
+                print(
+                    f"[{t:5.2f}s] {hand:<5} {combined_label:<46} "
+                    f"pos={pos} quat={quat_text}",
+                    flush=True,
+                )
+                last_labels[hand] = combined_label
 
             packet["t"] = t
             packet["grip"] = True
             packet["trigger"] = min(max(args.trigger, 0.0), 1.0)
             packet["pos"] = pos
+            packet["quat"] = quat
             sock.sendto(json.dumps(packet).encode("utf-8"), (args.host, args.port))
         time.sleep(dt)
 
@@ -280,6 +396,7 @@ def main() -> None:
         packet["grip"] = False
         packet["trigger"] = 0.0
         packet["pos"] = BASE_POS.copy()
+        packet["quat"] = [0.0, 0.0, 0.0, 1.0]
         sock.sendto(json.dumps(packet).encode("utf-8"), (args.host, args.port))
 
 

xr_rm_teleop/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
   <name>xr_rm_teleop</name>
   <version>0.1.0</version>
-  <description>XR relative-motion teleoperation controllers for RealMan RM75 arms.</description>
+  <description>XR relative-pose teleoperation controllers for RealMan RM75 arms.</description>
   <maintainer email="user@example.com">Yikai Fu</maintainer>
   <license>Apache-2.0</license>
 

xr_rm_teleop/setup.py

@@ -1,6 +1,6 @@
 """xr_rm_teleop 包安装配置。
 
-该包提供基于 XR 相对位移的 RM75 笛卡尔位姿透传遥操作节点。
+该包提供基于 XR 相对位姿的 RM75 笛卡尔位姿透传遥操作节点。
 """
 
 from setuptools import setup

xr_rm_teleop/test/test_orientation_control.py

@@ -0,0 +1,124 @@
+import math
+from types import SimpleNamespace
+
+import pytest
+
+from xr_rm_teleop.realman_adapter import ArmPose, MockRealManAdapter
+from xr_rm_teleop.single_arm_velocity_teleop import (
+    SingleArmVelocityTeleop,
+    _euler_to_quaternion,
+    _normalize_quaternion,
+    _quaternion_to_euler,
+)
+
+
+def _make_teleop_for_orientation() -> SingleArmVelocityTeleop:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._enable_orientation_control = True
+    teleop._enable_orientation_axes = [True, True, True]
+    teleop._controller_orientation_start = (0.0, 0.0, 0.0, 1.0)
+    teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.1, -0.2, 0.3)
+    teleop._xr_to_robot_matrix = [
+        0.0, 1.0, 0.0,
+        0.0, 0.0, 1.0,
+        1.0, 0.0, 0.0,
+    ]
+    return teleop
+
+
+def assert_angles_close(actual: list[float] | tuple[float, ...], expected: list[float]) -> None:
+    assert len(actual) == len(expected)
+    for actual_value, expected_value in zip(actual, expected):
+        assert math.atan2(math.sin(actual_value - expected_value), math.cos(actual_value - expected_value)) == pytest.approx(0.0)
+
+
+def test_identity_controller_orientation_keeps_tcp_orientation() -> None:
+    teleop = _make_teleop_for_orientation()
+
+    target = teleop._raw_orientation_from_controller((0.0, 0.0, 0.0, 1.0))
+
+    assert_angles_close(target, teleop._robot_start_pose.rpy())
+
+
+def test_xr_relative_rotation_maps_through_xr_to_robot_matrix() -> None:
+    teleop = _make_teleop_for_orientation()
+    teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.0, 0.0, 0.0)
+    xr_roll = _euler_to_quaternion(0.2, 0.0, 0.0)
+
+    target = teleop._raw_orientation_from_controller(xr_roll)
+
+    assert_angles_close(target, [0.0, 0.0, 0.2])
+
+
+def test_orientation_deadband_filter_and_speed_limit() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._orientation_deadband_rad = 0.01
+    teleop._orientation_filter_alpha = 0.5
+    teleop._max_orientation_speed = 0.5
+    teleop._dt = 0.1
+    teleop._last_sent_orientation = [0.0, 0.0, 0.0]
+    teleop._filtered_orientation_target = [0.0, 0.0, 0.0]
+
+    assert teleop._apply_orientation_deadband([0.001, 0.0, 0.0]) == [0.0, 0.0, 0.0]
+
+    filtered = teleop._filter_orientation_target([0.2, 0.0, 0.0])
+    assert_angles_close(filtered, [0.1, 0.0, 0.0])
+
+    limited, was_limited = teleop._limit_orientation_step([0.2, 0.0, 0.0])
+    assert was_limited
+    assert_angles_close(limited, [0.05, 0.0, 0.0])
+
+
+def test_invalid_controller_quaternion_stops_current_tick() -> None:
+    class FakeTime:
+        def __sub__(self, other):
+            del other
+            return SimpleNamespace(nanoseconds=0)
+
+    class FakeClock:
+        def now(self):
+            return FakeTime()
+
+    class FakeLogger:
+        def warn(self, *args, **kwargs):
+            del args, kwargs
+
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._last_msg = SimpleNamespace(
+        grip=True,
+        pose=SimpleNamespace(
+            position=SimpleNamespace(x=0.0, y=0.0, z=0.0),
+            orientation=SimpleNamespace(x=0.0, y=0.0, z=0.0, w=0.0),
+        ),
+    )
+    teleop._last_msg_time = FakeTime()
+    teleop._arm_name = "test_rm75"
+    teleop._command_timeout_sec = 0.12
+    teleop._enable_orientation_control = True
+    stopped = []
+    teleop.get_clock = lambda: FakeClock()
+    teleop.get_logger = lambda: FakeLogger()
+    teleop._safe_stop = lambda reset_active: stopped.append(reset_active)
+
+    teleop._control_tick()
+
+    assert stopped == [True]
+
+
+def test_quaternion_roundtrip_for_small_rpy() -> None:
+    quat = _normalize_quaternion(_euler_to_quaternion(0.2, -0.1, 0.3))
+    assert_angles_close(_quaternion_to_euler(quat), [0.2, -0.1, 0.3])
+
+
+def test_zero_quaternion_is_invalid() -> None:
+    with pytest.raises(ValueError):
+        _normalize_quaternion([0.0, 0.0, 0.0, 0.0])
+
+
+def test_mock_adapter_uses_shortest_angular_velocity() -> None:
+    adapter = MockRealManAdapter([0.0, 0.0, 0.0, 3.13, 0.0, -3.13], 0.1)
+
+    adapter.send_cartesian_target(ArmPose(0.0, 0.0, 0.0, -3.13, 0.0, 3.13), False)
+
+    assert abs(adapter.last_velocity[3]) < 1.0
+    assert abs(adapter.last_velocity[5]) < 1.0

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -6,11 +6,16 @@
 
 from __future__ import annotations
 
+import math
 from dataclasses import dataclass
 from numbers import Number
 from typing import Any
 
 
+def _angle_delta(target: float, current: float) -> float:
+    return math.atan2(math.sin(target - current), math.cos(target - current))
+
+
 @dataclass
 class ArmPose:
     x: float
@@ -23,6 +28,9 @@ class ArmPose:
     def xyz(self) -> list[float]:
         return [self.x, self.y, self.z]
 
+    def rpy(self) -> list[float]:
+        return [self.rx, self.ry, self.rz]
+
 
 class MockRealManAdapter:
     """无机械臂时使用的运动学模拟器，用于验证 ROS2 遥操链路。"""
@@ -45,9 +53,9 @@ class MockRealManAdapter:
             (pose.x - self._pose.x) / self._dt,
             (pose.y - self._pose.y) / self._dt,
             (pose.z - self._pose.z) / self._dt,
-            0.0,
+            _angle_delta(pose.rx, self._pose.rx) / self._dt,
-            0.0,
+            _angle_delta(pose.ry, self._pose.ry) / self._dt,
-            0.0,
+            _angle_delta(pose.rz, self._pose.rz) / self._dt,
         ]
         self._pose = pose
 

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -1,6 +1,6 @@
 """RM75 单臂 XR 相对位姿透传遥操作节点。
 
-节点订阅左/右手柄位姿，在 grip 按下时锁定手柄和 TCP 起点，把手柄相对位移
+节点订阅左/右手柄位姿，在 grip 按下时锁定手柄和 TCP 起点，把手柄相对位姿
 映射成机器人坐标系中的目标 TCP，并通过 rm_movep_canfd 持续下发目标位姿。
 """
 
@@ -30,8 +30,123 @@ def _clamp(value: float, low: float, high: float) -> float:
     return min(max(value, low), high)
 
 
+def _wrap_angle(angle: float) -> float:
+    return math.atan2(math.sin(angle), math.cos(angle))
+
+
+def _angle_delta(target: float, current: float) -> float:
+    return _wrap_angle(target - current)
+
+
+def _normalize_quaternion(values: Iterable[float]) -> tuple[float, float, float, float]:
+    x, y, z, w = [float(value) for value in values]
+    norm = math.sqrt(x * x + y * y + z * z + w * w)
+    if norm <= 1e-9 or not math.isfinite(norm):
+        raise ValueError("quaternion norm is zero or non-finite")
+    return x / norm, y / norm, z / norm, w / norm
+
+
+def _quaternion_conjugate(
+    quat: tuple[float, float, float, float],
+) -> tuple[float, float, float, float]:
+    x, y, z, w = quat
+    return -x, -y, -z, w
+
+
+def _quaternion_multiply(
+    left: tuple[float, float, float, float],
+    right: tuple[float, float, float, float],
+) -> tuple[float, float, float, float]:
+    lx, ly, lz, lw = left
+    rx, ry, rz, rw = right
+    return _normalize_quaternion(
+        (
+            lw * rx + lx * rw + ly * rz - lz * ry,
+            lw * ry - lx * rz + ly * rw + lz * rx,
+            lw * rz + lx * ry - ly * rx + lz * rw,
+            lw * rw - lx * rx - ly * ry - lz * rz,
+        )
+    )
+
+
+def _quaternion_to_matrix(quat: tuple[float, float, float, float]) -> list[float]:
+    x, y, z, w = quat
+    xx = x * x
+    yy = y * y
+    zz = z * z
+    xy = x * y
+    xz = x * z
+    yz = y * z
+    wx = w * x
+    wy = w * y
+    wz = w * z
+    return [
+        1.0 - 2.0 * (yy + zz),
+        2.0 * (xy - wz),
+        2.0 * (xz + wy),
+        2.0 * (xy + wz),
+        1.0 - 2.0 * (xx + zz),
+        2.0 * (yz - wx),
+        2.0 * (xz - wy),
+        2.0 * (yz + wx),
+        1.0 - 2.0 * (xx + yy),
+    ]
+
+
+def _matrix_multiply(left: list[float], right: list[float]) -> list[float]:
+    return [
+        sum(left[row * 3 + k] * right[k * 3 + col] for k in range(3))
+        for row in range(3)
+        for col in range(3)
+    ]
+
+
+def _matrix_transpose(matrix: list[float]) -> list[float]:
+    return [
+        matrix[0], matrix[3], matrix[6],
+        matrix[1], matrix[4], matrix[7],
+        matrix[2], matrix[5], matrix[8],
+    ]
+
+
+def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> tuple[float, float, float, float]:
+    cy = math.cos(yaw * 0.5)
+    sy = math.sin(yaw * 0.5)
+    cp = math.cos(pitch * 0.5)
+    sp = math.sin(pitch * 0.5)
+    cr = math.cos(roll * 0.5)
+    sr = math.sin(roll * 0.5)
+
+    qw = cr * cp * cy + sr * sp * sy
+    qx = sr * cp * cy - cr * sp * sy
+    qy = cr * sp * cy + sr * cp * sy
+    qz = cr * cp * sy - sr * sp * cy
+    return _normalize_quaternion((qx, qy, qz, qw))
+
+
+def _euler_to_matrix(roll: float, pitch: float, yaw: float) -> list[float]:
+    return _quaternion_to_matrix(_euler_to_quaternion(roll, pitch, yaw))
+
+
+def _matrix_to_euler(matrix: list[float]) -> tuple[float, float, float]:
+    sy = -_clamp(matrix[6], -1.0, 1.0)
+    pitch = math.asin(sy)
+    cp = math.cos(pitch)
+    if abs(cp) > 1e-9:
+        roll = math.atan2(matrix[7], matrix[8])
+        yaw = math.atan2(matrix[3], matrix[0])
+    else:
+        roll = math.atan2(-matrix[5], matrix[4])
+        yaw = 0.0
+    return _wrap_angle(roll), _wrap_angle(pitch), _wrap_angle(yaw)
+
+
+def _quaternion_to_euler(quat: tuple[float, float, float, float]) -> tuple[float, float, float]:
+    return _matrix_to_euler(_quaternion_to_matrix(quat))
+
+
 class SingleArmVelocityTeleop(Node):
-    """基于 XR 手柄相对位移的 RM75 单臂位姿透传遥操节点。
+    """基于 XR 手柄相对位姿的 RM75 单臂位姿透传遥操节点。
 
     类名和可执行入口沿用旧名称，避免已有 launch、UI 和命令失效。
     """
@@ -50,6 +165,11 @@ class SingleArmVelocityTeleop(Node):
         self.declare_parameter("target_filter_fast_threshold_m", 0.03)
         self.declare_parameter("max_linear_speed", 0.25)
         self.declare_parameter("enable_position_axes", [True, True, True])
+        self.declare_parameter("enable_orientation_control", False)
+        self.declare_parameter("enable_orientation_axes", [True, True, True])
+        self.declare_parameter("orientation_deadband_rad", 0.005)
+        self.declare_parameter("orientation_filter_alpha", 0.65)
+        self.declare_parameter("max_orientation_speed", 0.6)
         self.declare_parameter("workspace_min", [0.20, -0.35, 0.10])
         self.declare_parameter("workspace_max", [0.65, 0.35, 0.60])
         self.declare_parameter("cyl_radius_limit", [0.20, 0.60])
@@ -102,6 +222,11 @@ class SingleArmVelocityTeleop(Node):
         )
         self._max_linear_speed = float(self.get_parameter("max_linear_speed").value)
         self._enable_position_axes = self._bool_list_parameter("enable_position_axes", 3)
+        self._enable_orientation_control = self._bool_parameter("enable_orientation_control")
+        self._enable_orientation_axes = self._bool_list_parameter("enable_orientation_axes", 3)
+        self._orientation_deadband_rad = float(self.get_parameter("orientation_deadband_rad").value)
+        self._orientation_filter_alpha = float(self.get_parameter("orientation_filter_alpha").value)
+        self._max_orientation_speed = float(self.get_parameter("max_orientation_speed").value)
         self._workspace_min = self._float_list_parameter("workspace_min", 3)
         self._workspace_max = self._float_list_parameter("workspace_max", 3)
         self._cyl_radius_limit = self._float_list_parameter("cyl_radius_limit", 2)
@@ -122,9 +247,12 @@ class SingleArmVelocityTeleop(Node):
         self._last_msg_time: Time | None = None
         self._active = False
         self._controller_start: list[float] | None = None
+        self._controller_orientation_start: tuple[float, float, float, float] | None = None
         self._robot_start_pose: ArmPose | None = None
         self._filtered_target: list[float] | None = None
+        self._filtered_orientation_target: list[float] | None = None
         self._last_sent_target: list[float] | None = None
+        self._last_sent_orientation: list[float] | None = None
         self._last_command_time: Time | None = None
         self._last_current_pose: ArmPose | None = None
         self._last_current_pose_time: Time | None = None
@@ -150,7 +278,8 @@ class SingleArmVelocityTeleop(Node):
         self.create_timer(self._dt, self._control_tick)
         self.get_logger().info(
             f"{self._arm_name} 位姿透传遥操节点已启动，监听话题：{topic}, "
-            f"dt={self._dt:.4f}s, follow={self._follow}"
+            f"dt={self._dt:.4f}s, follow={self._follow}, "
+            f"orientation_control={self._enable_orientation_control}"
         )
 
     def _make_adapter(self):
@@ -332,39 +461,66 @@ class SingleArmVelocityTeleop(Node):
             return
 
         controller_now = self._controller_xyz(self._last_msg)
+        try:
+            controller_quat = self._controller_quaternion(self._last_msg)
+        except ValueError as exc:
+            self.get_logger().warn(
+                f"{self._arm_name} XR 手柄姿态无效，停止输出：{exc}",
+                throttle_duration_sec=1.0,
+            )
+            self._safe_stop(reset_active=True)
+            return
         if not self._active:
-            self._enter_active_control(controller_now, now)
+            self._enter_active_control(controller_now, controller_quat, now)
             return
 
         assert self._controller_start is not None
         assert self._robot_start_pose is not None
 
         self._maybe_refresh_current_pose(now)
-        raw_target = self._raw_target_from_controller(controller_now)
+        raw_target_xyz = self._raw_target_from_controller(controller_now)
-        workspace_target, workspace_clamped = self._clamp_workspace_with_flag(raw_target)
+        raw_target_rpy = self._raw_orientation_from_controller(controller_quat)
+        workspace_target, workspace_clamped = self._clamp_workspace_with_flag(raw_target_xyz)
         desired_target = self._apply_deadband(workspace_target)
         filtered_target = self._filter_target(desired_target)
         sent_target, step_limited = self._limit_target_step(filtered_target)
         sent_target, final_clamped = self._clamp_workspace_with_flag(sent_target)
+        desired_orientation = self._apply_orientation_deadband(raw_target_rpy)
+        filtered_orientation = self._filter_orientation_target(desired_orientation)
+        sent_orientation, orientation_step_limited = self._limit_orientation_step(filtered_orientation)
 
-        velocity = self._estimate_command_velocity(sent_target, now)
+        velocity = self._estimate_command_velocity(sent_target, sent_orientation, now)
-        target_clamped = workspace_clamped or step_limited or final_clamped
+        target_clamped = workspace_clamped or step_limited or final_clamped or orientation_step_limited
+        raw_target_pose = ArmPose(
+            x=raw_target_xyz[0],
+            y=raw_target_xyz[1],
+            z=raw_target_xyz[2],
+            rx=raw_target_rpy[0],
+            ry=raw_target_rpy[1],
+            rz=raw_target_rpy[2],
+        )
         target_pose = ArmPose(
             x=sent_target[0],
             y=sent_target[1],
             z=sent_target[2],
-            rx=self._robot_start_pose.rx,
+            rx=sent_orientation[0],
-            ry=self._robot_start_pose.ry,
+            ry=sent_orientation[1],
-            rz=self._robot_start_pose.rz,
+            rz=sent_orientation[2],
         )
 
-        self._publish_debug(raw_target, sent_target, velocity, target_clamped)
+        self._publish_debug(raw_target_pose, target_pose, velocity, target_clamped)
         if self._send_cartesian_target(target_pose):
             self._last_sent_target = sent_target
+            self._last_sent_orientation = sent_orientation
             self._last_command_time = now
             self._stop_sent = False
 
-    def _enter_active_control(self, controller_now: list[float], now: Time) -> None:
+    def _enter_active_control(
+        self,
+        controller_now: list[float],
+        controller_quat: tuple[float, float, float, float],
+        now: Time,
+    ) -> None:
         try:
             robot_pose = self._read_current_pose_for_control(now)
         except Exception as exc:
@@ -378,18 +534,33 @@ class SingleArmVelocityTeleop(Node):
         robot_xyz = robot_pose.xyz()
         self._active = True
         self._controller_start = controller_now
+        self._controller_orientation_start = controller_quat
         self._robot_start_pose = robot_pose
         self._filtered_target = robot_xyz
+        self._filtered_orientation_target = robot_pose.rpy()
         self._last_sent_target = robot_xyz
+        self._last_sent_orientation = robot_pose.rpy()
         self._last_command_time = now
         self._stop_sent = True
         self.get_logger().info(f"{self._arm_name} Grip 按下，已锁定手柄和机械臂初始位姿。")
-        self._publish_debug(robot_xyz, robot_xyz, [0.0] * 6, False)
+        self._publish_debug(robot_pose, robot_pose, [0.0] * 6, False)
 
     @staticmethod
     def _controller_xyz(msg: XrController) -> list[float]:
         return [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]
 
+    def _controller_quaternion(self, msg: XrController) -> tuple[float, float, float, float]:
+        if not self._enable_orientation_control:
+            return 0.0, 0.0, 0.0, 1.0
+        return _normalize_quaternion(
+            (
+                msg.pose.orientation.x,
+                msg.pose.orientation.y,
+                msg.pose.orientation.z,
+                msg.pose.orientation.w,
+            )
+        )
+
     def _raw_target_from_controller(self, controller_now: list[float]) -> list[float]:
         assert self._controller_start is not None
         assert self._robot_start_pose is not None
@@ -410,6 +581,38 @@ class SingleArmVelocityTeleop(Node):
             matrix[6] * delta[0] + matrix[7] * delta[1] + matrix[8] * delta[2],
         ]
 
+    def _raw_orientation_from_controller(
+        self,
+        controller_quat: tuple[float, float, float, float],
+    ) -> list[float]:
+        assert self._robot_start_pose is not None
+        if not self._enable_orientation_control:
+            return self._robot_start_pose.rpy()
+
+        assert self._controller_orientation_start is not None
+        xr_delta_quat = _quaternion_multiply(
+            controller_quat,
+            _quaternion_conjugate(self._controller_orientation_start),
+        )
+        xr_delta_matrix = _quaternion_to_matrix(xr_delta_quat)
+        matrix = self._xr_to_robot_matrix
+        robot_delta_matrix = _matrix_multiply(
+            _matrix_multiply(matrix, xr_delta_matrix),
+            _matrix_transpose(matrix),
+        )
+        robot_start_matrix = _euler_to_matrix(
+            self._robot_start_pose.rx,
+            self._robot_start_pose.ry,
+            self._robot_start_pose.rz,
+        )
+        target_matrix = _matrix_multiply(robot_delta_matrix, robot_start_matrix)
+        target_rpy = list(_matrix_to_euler(target_matrix))
+        start_rpy = self._robot_start_pose.rpy()
+        return [
+            target_rpy[i] if self._enable_orientation_axes[i] else start_rpy[i]
+            for i in range(3)
+        ]
+
     def _apply_deadband(self, target: list[float]) -> list[float]:
         if self._deadband_m <= 0.0 or self._last_sent_target is None:
             return target
@@ -457,6 +660,51 @@ class SingleArmVelocityTeleop(Node):
             for i in range(3)
         ], True
 
+    def _apply_orientation_deadband(self, target_rpy: list[float]) -> list[float]:
+        if self._orientation_deadband_rad <= 0.0 or self._last_sent_orientation is None:
+            return [_wrap_angle(value) for value in target_rpy]
+        delta = [
+            _angle_delta(target_rpy[i], self._last_sent_orientation[i])
+            for i in range(3)
+        ]
+        if _norm(delta) < self._orientation_deadband_rad:
+            return list(self._last_sent_orientation)
+        return [_wrap_angle(value) for value in target_rpy]
+
+    def _filter_orientation_target(self, target_rpy: list[float]) -> list[float]:
+        if self._filtered_orientation_target is None:
+            self._filtered_orientation_target = [_wrap_angle(value) for value in target_rpy]
+            return list(self._filtered_orientation_target)
+
+        self._filtered_orientation_target = [
+            _wrap_angle(
+                self._filtered_orientation_target[i]
+                + self._orientation_filter_alpha
+                * _angle_delta(target_rpy[i], self._filtered_orientation_target[i])
+            )
+            for i in range(3)
+        ]
+        return list(self._filtered_orientation_target)
+
+    def _limit_orientation_step(self, target_rpy: list[float]) -> tuple[list[float], bool]:
+        if self._last_sent_orientation is None:
+            return [_wrap_angle(value) for value in target_rpy], False
+
+        delta = [
+            _angle_delta(target_rpy[i], self._last_sent_orientation[i])
+            for i in range(3)
+        ]
+        distance = _norm(delta)
+        max_step = self._max_orientation_speed * self._dt
+        if distance <= max_step or distance <= 1e-9:
+            return [_wrap_angle(value) for value in target_rpy], False
+
+        scale = max_step / distance
+        return [
+            _wrap_angle(self._last_sent_orientation[i] + delta[i] * scale)
+            for i in range(3)
+        ], True
+
     def _clamp_workspace_with_flag(self, target: list[float]) -> tuple[list[float], bool]:
         clamped = [
             _clamp(target[i], self._workspace_min[i], self._workspace_max[i])
@@ -487,8 +735,13 @@ class SingleArmVelocityTeleop(Node):
 
         return target
 
-    def _estimate_command_velocity(self, target: list[float], now: Time) -> list[float]:
+    def _estimate_command_velocity(
-        if self._last_sent_target is None:
+        self,
+        target_xyz: list[float],
+        target_rpy: list[float],
+        now: Time,
+    ) -> list[float]:
+        if self._last_sent_target is None or self._last_sent_orientation is None:
             return [0.0] * 6
 
         dt = self._dt
@@ -497,9 +750,12 @@ class SingleArmVelocityTeleop(Node):
             if measured_dt > 1e-6:
                 dt = measured_dt
         return [
-            (target[i] - self._last_sent_target[i]) / dt
+            (target_xyz[i] - self._last_sent_target[i]) / dt
             for i in range(3)
-        ] + [0.0, 0.0, 0.0]
+        ] + [
+            _angle_delta(target_rpy[i], self._last_sent_orientation[i]) / dt
+            for i in range(3)
+        ]
 
     def _read_current_pose_for_control(self, now: Time) -> ArmPose:
         pose = self._adapter.get_current_pose()
@@ -529,9 +785,12 @@ class SingleArmVelocityTeleop(Node):
         if reset_active:
             self._active = False
             self._controller_start = None
+            self._controller_orientation_start = None
             self._robot_start_pose = None
             self._filtered_target = None
+            self._filtered_orientation_target = None
             self._last_sent_target = None
+            self._last_sent_orientation = None
             self._last_command_time = None
         self._publish_stop_debug()
 
@@ -549,7 +808,7 @@ class SingleArmVelocityTeleop(Node):
         pose = self._debug_pose_fallback()
         if pose is None:
             return
-        self._publish_debug(pose.xyz(), pose.xyz(), [0.0] * 6, False)
+        self._publish_debug(pose, pose, [0.0] * 6, False)
 
     def _debug_pose_fallback(self) -> ArmPose | None:
         if self._last_current_pose is not None:
@@ -557,7 +816,8 @@ class SingleArmVelocityTeleop(Node):
         if self._robot_start_pose is not None:
             return self._robot_start_pose
         if self._last_sent_target is not None:
-            return ArmPose(*self._last_sent_target)
+            rpy = self._last_sent_orientation or [0.0, 0.0, 0.0]
+            return ArmPose(*self._last_sent_target, *rpy)
         return None
 
     def _send_cartesian_target(self, pose: ArmPose) -> bool:
@@ -575,38 +835,18 @@ class SingleArmVelocityTeleop(Node):
 
     def _publish_debug(
         self,
-        raw_target_xyz: list[float],
+        raw_target_pose: ArmPose,
-        target_xyz: list[float],
+        target_pose: ArmPose,
         command_velocity: list[float],
         target_clamped: bool,
     ) -> None:
         stamp = self.get_clock().now().to_msg()
         current_pose = self._debug_pose_fallback()
         if current_pose is None:
-            current_pose = ArmPose(*target_xyz)
+            current_pose = target_pose
 
-        raw_pose = self._pose_msg(
+        raw_pose = self._pose_msg(stamp, raw_target_pose)
-            stamp,
+        target_msg = self._pose_msg(stamp, target_pose)
-            ArmPose(
-                x=raw_target_xyz[0],
-                y=raw_target_xyz[1],
-                z=raw_target_xyz[2],
-                rx=current_pose.rx,
-                ry=current_pose.ry,
-                rz=current_pose.rz,
-            ),
-        )
-        target_msg = self._pose_msg(
-            stamp,
-            ArmPose(
-                x=target_xyz[0],
-                y=target_xyz[1],
-                z=target_xyz[2],
-                rx=current_pose.rx,
-                ry=current_pose.ry,
-                rz=current_pose.rz,
-            ),
-        )
         velocity_msg = TwistStamped()
         velocity_msg.header.stamp = stamp
         velocity_msg.header.frame_id = "rm_base"
@@ -628,7 +868,7 @@ class SingleArmVelocityTeleop(Node):
 
     @staticmethod
     def _pose_msg(stamp, pose: ArmPose) -> PoseStamped:
-        qx, qy, qz, qw = SingleArmVelocityTeleop._euler_to_quaternion(pose.rx, pose.ry, pose.rz)
+        qx, qy, qz, qw = _euler_to_quaternion(pose.rx, pose.ry, pose.rz)
         msg = PoseStamped()
         msg.header.stamp = stamp
         msg.header.frame_id = "rm_base"
@@ -641,21 +881,6 @@ class SingleArmVelocityTeleop(Node):
         msg.pose.orientation.w = qw
         return msg
 
-    @staticmethod
-    def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> tuple[float, float, float, float]:
-        cy = math.cos(yaw * 0.5)
-        sy = math.sin(yaw * 0.5)
-        cp = math.cos(pitch * 0.5)
-        sp = math.sin(pitch * 0.5)
-        cr = math.cos(roll * 0.5)
-        sr = math.sin(roll * 0.5)
-
-        qw = cr * cp * cy + sr * sp * sy
-        qx = sr * cp * cy - cr * sp * sy
-        qy = cr * sp * cy + sr * cp * sy
-        qz = cr * cp * sy - sr * sp * cy
-        return qx, qy, qz, qw
-
     def _float_list_parameter(self, name: str, expected_length: int) -> list[float]:
         values = [float(value) for value in self.get_parameter(name).value]
         if len(values) != expected_length:
@@ -696,6 +921,12 @@ class SingleArmVelocityTeleop(Node):
             raise ValueError("max_linear_speed must be > 0")
         if self._current_pose_poll_hz < 0.0:
             raise ValueError("current_pose_poll_hz must be >= 0")
+        if self._orientation_deadband_rad < 0.0:
+            raise ValueError("orientation_deadband_rad must be >= 0")
+        if not 0.0 <= self._orientation_filter_alpha <= 1.0:
+            raise ValueError("orientation_filter_alpha must be in [0, 1]")
+        if self._max_orientation_speed <= 0.0:
+            raise ValueError("max_orientation_speed must be > 0")
         if not 0.0 <= self._trigger_close_threshold <= 1.0:
             raise ValueError("trigger_close_threshold must be in [0, 1]")
         for axis, (low, high) in enumerate(zip(self._workspace_min, self._workspace_max)):