Refine the guard control to eliminate jitter

src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py

@@ -25,6 +25,7 @@ INIT_SYNC_JOINT_VELOCITY_RAD = 0.2
 ROBOT_RESET_SPEED_DEG = 60
 TCP_Z_CLAMP_TOLERANCE_MM = 1e-3
 TCP_Z_LOG_INTERVAL_S = 1.0
+TCP_Z_MAX_IK_JOINT_STEP_RAD = math.radians(10.0)
 
 CARTESIAN_OBS_KEYS = [
     "pose.x", "pose.y", "pose.z", "pose.rx", "pose.ry", "pose.rz",
@@ -102,6 +103,7 @@ class UFRobot(Robot, Thread):
         self._max_linear_velocity = self.config.max_linear_velocity
 
         self._min_tcp_z_mm = self.config.min_tcp_z_mm
+        self._tcp_z_guard_activation_margin_mm = self.config.tcp_z_guard_activation_margin_mm
         self._last_safe_joint_target = None
         self._last_safe_cartesian_target = None
         self._tcp_z_is_clamped = False
@@ -111,7 +113,11 @@ class UFRobot(Robot, Thread):
         self.report_stop_event = Event()
         self._rt_report_normal = False
         self._update_lock = Lock()
-        self._use_rt_report = (self._control_space == "cartesian") # Cartesian observations must utilize rt_report
+        # The TCP z guard uses the asynchronous RT report to avoid a blocking
+        # FK request on every GELLO servo cycle.
+        self._use_rt_report = (
+            self._control_space == "cartesian" or self._min_tcp_z_mm is not None
+        )
         self._cart_obs_has_vel = any('velo.' in key for key in CARTESIAN_OBS_KEYS)
         self._jnt_obs_has_vel = self.config.observe_joint_vel
 
@@ -323,6 +329,15 @@ class UFRobot(Robot, Thread):
             if desired.shape != (self._dof,) or not np.all(np.isfinite(desired)):
                 raise ValueError("joint target has invalid shape or contains NaN/Inf")
 
+            # Far above the floor, the current TCP height is available from
+            # the RT report. Bypass the synchronous controller FK call so the
+            # normal GELLO path keeps a stable command cadence. A large
+            # activation margin absorbs ordinary per-cycle motion changes.
+            if self._rt_actual_tcp_is_far_above_floor():
+                self._last_safe_joint_target = desired.copy()
+                self._log_tcp_z_clamp(False)
+                return desired
+
             code, pose = self.real_arm.get_forward_kinematics(
                 desired.tolist(), input_is_radian=True, return_is_radian=True
             )
@@ -332,24 +347,27 @@ class UFRobot(Robot, Thread):
 
             requested_z = float(pose[2])
             if requested_z >= self._min_tcp_z_mm:
+                self._validate_guard_joint_target(desired, fallback, "GELLO target")
                 self._last_safe_joint_target = desired.copy()
                 self._log_tcp_z_clamp(False)
                 return desired
 
             clamped_pose = pose[:6].copy()
             clamped_pose[2] = self._min_tcp_z_mm
+            ik_reference = fallback if fallback is not None else desired
             code, inverse = self.real_arm.get_inverse_kinematics(
                 clamped_pose.tolist(),
                 input_is_radian=True,
                 return_is_radian=True,
                 limited=True,
-                ref_angles=desired.tolist(),
+                ref_angles=np.asarray(ik_reference, dtype=np.float64).tolist(),
             )
             inverse = np.asarray(inverse, dtype=np.float64)
             if code != 0 or inverse.shape[0] < self._dof or not np.all(np.isfinite(inverse)):
                 raise RuntimeError(f"inverse kinematics failed, code={code}")
 
             safe_target = inverse[:self._dof].copy()
+            self._validate_guard_joint_target(safe_target, fallback, "clamped IK target")
             code, verified_pose = self.real_arm.get_forward_kinematics(
                 safe_target.tolist(), input_is_radian=True, return_is_radian=True
             )
@@ -371,6 +389,48 @@ class UFRobot(Robot, Thread):
                 return None
             return np.asarray(fallback, dtype=np.float64).copy()
 
+    def _validate_guard_joint_target(
+        self,
+        target: np.ndarray,
+        previous_safe_target: np.ndarray | None,
+        label: str,
+    ) -> None:
+        code, is_limited = self.real_arm.is_joint_limit(target.tolist(), is_radian=True)
+        if code != 0 or is_limited is not False:
+            raise RuntimeError(
+                f"{label} violates a joint limit, code={code}, limited={is_limited}, "
+                f"target={target.tolist()}"
+            )
+
+        if previous_safe_target is None:
+            return
+        previous = np.asarray(previous_safe_target, dtype=np.float64)
+        if previous.shape != target.shape or not np.all(np.isfinite(previous)):
+            raise RuntimeError("previous safe joint target is invalid")
+        delta = (target - previous + math.pi) % (2 * math.pi) - math.pi
+        max_delta = float(np.max(np.abs(delta)))
+        if max_delta > TCP_Z_MAX_IK_JOINT_STEP_RAD:
+            raise RuntimeError(
+                f"{label} jumps {math.degrees(max_delta):.1f} deg from the previous safe target"
+            )
+
+    def _rt_actual_tcp_is_far_above_floor(self) -> bool:
+        if self._min_tcp_z_mm is None or not getattr(self, "_rt_report_normal", False):
+            return False
+        update_lock = getattr(self, "_update_lock", None)
+        if update_lock is None:
+            return False
+        with update_lock:
+            pose = getattr(self, "rt_actual_tcp_pose", None)
+            if pose is None or len(pose) < 3:
+                return False
+            actual_z = float(pose[2])
+        activation_margin = getattr(self, "_tcp_z_guard_activation_margin_mm", 100.0)
+        return (
+            math.isfinite(actual_z)
+            and actual_z > self._min_tcp_z_mm + activation_margin
+        )
+
     def _guard_cartesian_target(self, command: list[float]) -> np.ndarray | None:
         """Clamp a Cartesian target without changing its other five components."""
         target = np.asarray(command, dtype=np.float64)

src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py

@@ -32,6 +32,10 @@ class UFRobotConfig(RobotConfig):
     # Optional TCP height floor in the xArm base coordinate system (mm).
     # The value should include any desired safety margin above the table.
     min_tcp_z_mm: float | None = None
+    # Skip synchronous FK while the actual TCP is this far above the floor.
+    # The RT report keeps this fast path asynchronous and avoids jitter during
+    # normal teleoperation; FK/IK remains active near the configured floor.
+    tcp_z_guard_activation_margin_mm: float = 100.0
 
     def __post_init__(self):
         super().__post_init__()
@@ -49,3 +53,8 @@ class UFRobotConfig(RobotConfig):
             raise ValueError("joint_command_mode must be 1 or 6 for joint control")
         if self.min_tcp_z_mm is not None and not math.isfinite(self.min_tcp_z_mm):
             raise ValueError("min_tcp_z_mm must be finite when provided")
+        if (
+            not math.isfinite(self.tcp_z_guard_activation_margin_mm)
+            or self.tcp_z_guard_activation_margin_mm < 0
+        ):
+            raise ValueError("tcp_z_guard_activation_margin_mm must be finite and non-negative")

tests/test_tcp_z_safety.py

@@ -1,5 +1,6 @@
 from pathlib import Path
 from types import SimpleNamespace
+from threading import Lock
 
 import numpy as np
 import pytest
@@ -13,10 +14,11 @@ class FakeKinematicsArm:
     def __init__(self, requested_pose=None, safe_pose=None):
         self.requested_pose = requested_pose or [300.0, 10.0, 90.0, 0.1, 0.2, 0.3]
         self.safe_pose = safe_pose or [300.0, 10.0, 100.0, 0.1, 0.2, 0.3]
-        self.inverse_result = [0.9] * 7
+        self.inverse_result = [0.3] * 7
         self.inverse_calls = []
         self.fail_forward = False
         self.fail_inverse = False
+        self.joint_limit = False
 
     def get_forward_kinematics(self, angles, **kwargs):
         if self.fail_forward:
@@ -31,6 +33,9 @@ class FakeKinematicsArm:
             return 2, []
         return 0, self.inverse_result.copy()
 
+    def is_joint_limit(self, target, **kwargs):
+        return 0, self.joint_limit
+
 
 def make_guard_robot(arm, min_tcp_z_mm=100.0, control_space="joint"):
     robot = UFRobot.__new__(UFRobot)
@@ -58,6 +63,20 @@ def test_joint_target_above_floor_is_unchanged():
     assert np.allclose(robot._last_safe_joint_target, requested)
 
 
+def test_joint_guard_uses_rt_report_fast_path_far_above_floor():
+    arm = FakeKinematicsArm()
+    robot = make_guard_robot(arm)
+    robot._rt_report_normal = True
+    robot._update_lock = Lock()
+    robot.rt_actual_tcp_pose = [0.0, 0.0, 200.0, 0.0, 0.0, 0.0]
+    robot._tcp_z_guard_activation_margin_mm = 50.0
+
+    result = robot._guard_joint_target([0.1] * 7)
+
+    assert np.allclose(result, [0.1] * 7)
+    assert arm.inverse_calls == []
+
+
 def test_joint_target_below_floor_clamps_only_tcp_z_before_inverse_kinematics():
     arm = FakeKinematicsArm()
     robot = make_guard_robot(arm)
@@ -69,10 +88,23 @@ def test_joint_target_below_floor_clamps_only_tcp_z_before_inverse_kinematics():
     inverse_pose, inverse_kwargs = arm.inverse_calls[0]
     assert inverse_pose == pytest.approx([300.0, 10.0, 100.0, 0.1, 0.2, 0.3])
     assert inverse_kwargs["limited"] is True
-    assert inverse_kwargs["ref_angles"] == requested
+    assert inverse_kwargs["ref_angles"] == pytest.approx([0.25] * 7)
     assert np.allclose(robot._last_safe_joint_target, arm.inverse_result)
 
 
+def test_successive_clamped_ik_uses_last_accepted_solution_as_reference():
+    arm = FakeKinematicsArm()
+    robot = make_guard_robot(arm)
+
+    first_result = robot._guard_joint_target([0.1] * 7)
+    arm.inverse_result = [0.32] * 7
+    second_result = robot._guard_joint_target([0.05] * 7)
+
+    assert first_result == pytest.approx([0.3] * 7)
+    assert second_result == pytest.approx([0.32] * 7)
+    assert arm.inverse_calls[1][1]["ref_angles"] == pytest.approx([0.3] * 7)
+
+
 @pytest.mark.parametrize("failed_stage", ["forward", "inverse", "verification"])
 def test_joint_guard_holds_last_safe_target_when_kinematics_fails(failed_stage):
     arm = FakeKinematicsArm()
@@ -89,6 +121,27 @@ def test_joint_guard_holds_last_safe_target_when_kinematics_fails(failed_stage):
     assert np.allclose(result, [0.25] * 7)
 
 
+def test_joint_guard_holds_last_safe_target_when_ik_hits_joint_limit():
+    arm = FakeKinematicsArm()
+    arm.joint_limit = True
+    robot = make_guard_robot(arm)
+
+    result = robot._guard_joint_target([0.1] * 7)
+
+    assert np.allclose(result, [0.25] * 7)
+
+
+def test_joint_guard_rejects_discontinuous_ik_solution():
+    arm = FakeKinematicsArm()
+    arm.inverse_result = [1.5] * 7
+    arm.safe_pose[2] = 100.0
+    robot = make_guard_robot(arm)
+
+    result = robot._guard_joint_target([0.1] * 7)
+
+    assert np.allclose(result, [0.25] * 7)
+
+
 def test_cartesian_guard_preserves_other_axes_and_clamps_z():
     robot = make_guard_robot(object(), control_space="cartesian")