Enhance orientation control for RM75 arms in XR teleoperation

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,
-            0.0,
-            0.0,
+            _angle_delta(pose.rx, self._pose.rx) / self._dt,
+            _angle_delta(pose.ry, self._pose.ry) / self._dt,
+            _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)
-        workspace_target, workspace_clamped = self._clamp_workspace_with_flag(raw_target)
+        raw_target_xyz = self._raw_target_from_controller(controller_now)
+        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)
-        target_clamped = workspace_clamped or step_limited or final_clamped
+        velocity = self._estimate_command_velocity(sent_target, sent_orientation, now)
+        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,
-            ry=self._robot_start_pose.ry,
-            rz=self._robot_start_pose.rz,
+            rx=sent_orientation[0],
+            ry=sent_orientation[1],
+            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]:
-        if self._last_sent_target is None:
+    def _estimate_command_velocity(
+        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],
-        target_xyz: list[float],
+        raw_target_pose: ArmPose,
+        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(
-            stamp,
-            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,
-            ),
-        )
+        raw_pose = self._pose_msg(stamp, raw_target_pose)
+        target_msg = self._pose_msg(stamp, target_pose)
         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)):