diff --git a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py index 2a623fb..5166696 100644 --- a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py +++ b/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) diff --git a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py index cb49214..1e17d3f 100644 --- a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py +++ b/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") diff --git a/tests/test_tcp_z_safety.py b/tests/test_tcp_z_safety.py index a4a024c..cb9b488 100644 --- a/tests/test_tcp_z_safety.py +++ b/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")