From ede2b4bed9f0730514d2ceb0e82c9aa57f2485ef Mon Sep 17 00:00:00 2001 From: ChenYuhan <2514158309@qq.com> Date: Sun, 16 Aug 2026 15:55:59 +0800 Subject: [PATCH] Add safety height guard in control --- .gitignore | 1 + README.md | 14 ++ README_ZH.md | 22 +- config/gello/xarm7_gello_record_config.yaml | 4 +- .../xarm7_manual_record_config.yaml | 4 +- pyproject.toml | 1 + .../robots/uf_robot/uf_robot.py | 175 +++++++++++++++- .../robots/uf_robot/uf_robot_config.py | 6 + .../scripts/uf_lerobot_record.py | 4 + .../scripts/uf_read_tcp_z.py | 92 +++++++++ tests/test_tcp_z_safety.py | 193 ++++++++++++++++++ 11 files changed, 498 insertions(+), 18 deletions(-) create mode 100644 src/lerobot_robot_ufactory/scripts/uf_read_tcp_z.py create mode 100644 tests/test_tcp_z_safety.py diff --git a/.gitignore b/.gitignore index c48b1d7..3f7fe02 100644 --- a/.gitignore +++ b/.gitignore @@ -90,6 +90,7 @@ models/ ufactory_usage/ .history/ datasets/ +tmp/ # Local checkout used for GELLO hardware development. Runtime fixes live in # the main package so the repository does not depend on a modified submodule. diff --git a/README.md b/README.md index b02e4a8..c2a6aec 100644 --- a/README.md +++ b/README.md @@ -125,6 +125,20 @@ uv run uf-robot-teleop --config_path config/gello/xarm7_gello_record_config.yaml `Space` reset & start, `←` reset, `Esc` exit. +#### Configure the GELLO TCP height floor + +Stop other robot control processes, move the TCP to its lowest safe pose, then read its height: + +```bash +uv run uf-read-tcp-z \ + --config-path config/gello/xarm7_gello_record_config.yaml \ + --margin-mm 5 +``` + +The command does not move the arm. Put the recommended `min_tcp_z_mm` value in the GELLO YAML. Teleop, recording, and other control entry points using that robot configuration will then enforce the floor immediately before sending each command. For joint control, targets below the floor retain their TCP x/y position and orientation while z is clamped. + +> This protects the TCP against crossing a horizontal plane. It does not detect collisions involving links, the elbow, or the gripper body, and it does not replace the emergency stop. Measure again after changing the tool, TCP offset, robot base, or table position. + ### 2. GELLO data collection ```bash diff --git a/README_ZH.md b/README_ZH.md index 07b6984..3cf61b4 100644 --- a/README_ZH.md +++ b/README_ZH.md @@ -113,17 +113,23 @@ uv run uf-camera-view -l -T realsense # 列出每台相机的序列号 ## 使用 -### 1. GELLO 遥操作测试 - -不录制数据，仅测试 GELLO 与机械臂的联动： - -```bash -uv run uf-robot-teleop --config_path config/gello/xarm7_gello_record_config.yaml -uv run uf-robot-teleop --config_path config/gello/xarm7_gello_record_config.yaml --fps 60 # 可选，指定循环频率 -``` `Space` 复位并开始，`←` 复位，`Esc` 退出。 +#### 设置 GELLO TCP 最低高度 + +先停止其他控制程序，将机械臂 TCP 移到最低安全位置，然后只读当前高度： + +```bash +uv run uf-read-tcp-z \ + --config-path config/gello/xarm7_gello_record_config.yaml \ + --margin-mm 5 +``` + +该命令不会移动机械臂。把输出的 `min_tcp_z_mm` 建议值填入 GELLO YAML；测试遥操作、数据采集及其他使用该机器人配置的控制入口都会在最终下发前启用保护。关节控制下，低于下限的目标会保留 TCP 的 x/y 和姿态，只把 z 钳制到下限。 + +> 该限制只保护 TCP 不低于一个水平面，不能检测机械臂连杆、肘部或夹爪外形与桌子的碰撞，也不能替代急停。更换工具、TCP 偏置、底座或桌面位置后必须重新测量。 + ### 2. GELLO 数据采集 ```bash diff --git a/config/gello/xarm7_gello_record_config.yaml b/config/gello/xarm7_gello_record_config.yaml index 51458af..94c4bf6 100644 --- a/config/gello/xarm7_gello_record_config.yaml +++ b/config/gello/xarm7_gello_record_config.yaml @@ -8,6 +8,8 @@ robot: # Use the high-frequency servo interface for lower-latency GELLO tracking. joint_command_mode: 1 max_joint_velocity: 120 + # TCP z floor in the xArm base coordinate system (mm). + min_tcp_z_mm: -2.0 # Append gripper initialization/read/write failures here. gripper_error_log_path: "logs/xarm7_gripper_errors.log" # Redundant args, indicating the initial pose of xarm7. Set by 192.168.1.245:18333 @@ -27,7 +29,7 @@ teleop: dataset: # Dataset path relative to the directory where the command is started. root: "datasets/xarm7_gello_datas" - repo_id: "ufactory/xarm6_gello_datas" + repo_id: "ufactory/xarm7_gello_datas" single_task: "Pick up the purple grape and drop into the box on the left." fps: 60 episode_time_s: 60 # max duration for one episode diff --git a/config/manual_mode/xarm7_manual_record_config.yaml b/config/manual_mode/xarm7_manual_record_config.yaml index a14f396..0ebf89c 100644 --- a/config/manual_mode/xarm7_manual_record_config.yaml +++ b/config/manual_mode/xarm7_manual_record_config.yaml @@ -11,14 +11,14 @@ robot: # Normalized gripper position change per second while holding C/O. manual_gripper_speed: 0.5 # Teaching sensitivity, valid range is 1-5, affecting vel of the robot. - teach_sensitivity: 3 + teach_sensitivity: 5 # Whether to record joint velocities in observations. observe_joint_vel: false cameras: camera: type: intelrealsense - serial_number_or_name: "148522072685" + serial_number_or_name: "242622070583" width: 640 height: 480 fps: 30 diff --git a/pyproject.toml b/pyproject.toml index 28081ce..0ba0fc3 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -41,6 +41,7 @@ uf-vive-calibrate = "lerobot_robot_ufactory.scripts.vive_calibrate:main" uf-camera-view = "lerobot_robot_ufactory.scripts.uf_camera_view:main" uf-realsense-view = "lerobot_robot_ufactory.scripts.uf_realsense_view:main" uf-camera-test = "lerobot_robot_ufactory.scripts.uf_camera_test:main" +uf-read-tcp-z = "lerobot_robot_ufactory.scripts.uf_read_tcp_z:main" [project.optional-dependencies] # GELLO 遥操作 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 b5101de..2a623fb 100644 --- a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py +++ b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py @@ -17,9 +17,14 @@ from .uf_robot_config import UFRobotConfig from xarm.wrapper import XArmAPI from xarm.core.utils import convert + +logger = logging.getLogger(__name__) + ## Configurations: 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 CARTESIAN_OBS_KEYS = [ "pose.x", "pose.y", "pose.z", "pose.rx", "pose.ry", "pose.rz", @@ -96,6 +101,13 @@ class UFRobot(Robot, Thread): self._max_joint_velocity = math.radians(self.config.max_joint_velocity) self._max_linear_velocity = self.config.max_linear_velocity + self._min_tcp_z_mm = self.config.min_tcp_z_mm + self._last_safe_joint_target = None + self._last_safe_cartesian_target = None + self._tcp_z_is_clamped = False + self._tcp_z_last_log_time = 0.0 + self._tcp_z_last_error_log_time = 0.0 + self.report_stop_event = Event() self._rt_report_normal = False self._update_lock = Lock() @@ -214,6 +226,8 @@ class UFRobot(Robot, Thread): self.configure() else: self.reset_to_initial() + if self._min_tcp_z_mm is not None and self.config.manual_mode: + self._initialize_tcp_z_guard() if calibrate: self.calibrate() @@ -247,6 +261,139 @@ class UFRobot(Robot, Thread): raise RuntimeError(f"Failed to move to xArm initial point, code={code}") self.configure() + if self._min_tcp_z_mm is not None: + self._initialize_tcp_z_guard() + + def _initialize_tcp_z_guard(self) -> None: + """Initialize guard state from the robot's current physical target.""" + if self._min_tcp_z_mm is None or self.real_arm is None: + return + + if self._control_space == "joint": + code, states = self.real_arm.get_joint_states(is_radian=True, num=1) + if code != 0 or not states or len(states[0]) < self._dof: + raise RuntimeError(f"Unable to initialize TCP z guard from joint state, code={code}") + target = np.asarray(states[0][:self._dof], dtype=np.float64) + if not np.all(np.isfinite(target)): + raise RuntimeError("Unable to initialize TCP z guard from non-finite joint state") + self._last_safe_joint_target = target + else: + code, pose = self.real_arm.get_position_aa(is_radian=True) + if code != 0 or len(pose) < 6: + raise RuntimeError(f"Unable to initialize TCP z guard from TCP pose, code={code}") + target = np.asarray(pose[:6], dtype=np.float64) + if not np.all(np.isfinite(target)): + raise RuntimeError("Unable to initialize TCP z guard from non-finite TCP pose") + self._last_safe_cartesian_target = target + + self._tcp_z_is_clamped = False + self._tcp_z_last_log_time = 0.0 + self._tcp_z_last_error_log_time = 0.0 + + def _log_tcp_z_clamp(self, clamped: bool, requested_z: float | None = None) -> None: + """Log clamp state changes while avoiding per-cycle console spam.""" + now = time.monotonic() + if clamped: + should_log = not self._tcp_z_is_clamped or now - self._tcp_z_last_log_time >= TCP_Z_LOG_INTERVAL_S + if should_log: + logger.warning( + "TCP z safety clamp active: requested %.2f mm, limiting to %.2f mm", + requested_z if requested_z is not None else float("nan"), + self._min_tcp_z_mm, + ) + self._tcp_z_last_log_time = now + elif self._tcp_z_is_clamped: + logger.info("TCP z safety clamp released") + self._tcp_z_is_clamped = clamped + + def _log_tcp_z_guard_error(self, message: str) -> None: + now = time.monotonic() + if now - self._tcp_z_last_error_log_time >= TCP_Z_LOG_INTERVAL_S: + logger.error("TCP z safety guard rejected target: %s", message) + self._tcp_z_last_error_log_time = now + + def _guard_joint_target(self, command: list[float]) -> np.ndarray | None: + """Return a safe joint target, or None when motion must be skipped.""" + desired = np.asarray(command, dtype=np.float64) + if self._min_tcp_z_mm is None: + return desired + + fallback = self._last_safe_joint_target + try: + if desired.shape != (self._dof,) or not np.all(np.isfinite(desired)): + raise ValueError("joint target has invalid shape or contains NaN/Inf") + + code, pose = self.real_arm.get_forward_kinematics( + desired.tolist(), input_is_radian=True, return_is_radian=True + ) + pose = np.asarray(pose, dtype=np.float64) + if code != 0 or pose.shape[0] < 6 or not np.all(np.isfinite(pose)): + raise RuntimeError(f"forward kinematics failed, code={code}") + + requested_z = float(pose[2]) + if requested_z >= self._min_tcp_z_mm: + 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 + 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(), + ) + 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() + code, verified_pose = self.real_arm.get_forward_kinematics( + safe_target.tolist(), input_is_radian=True, return_is_radian=True + ) + verified_pose = np.asarray(verified_pose, dtype=np.float64) + if ( + code != 0 + or verified_pose.shape[0] < 3 + or not np.all(np.isfinite(verified_pose)) + or verified_pose[2] < self._min_tcp_z_mm - TCP_Z_CLAMP_TOLERANCE_MM + ): + raise RuntimeError(f"inverse-kinematics result is below the TCP z floor, code={code}") + + self._last_safe_joint_target = safe_target + self._log_tcp_z_clamp(True, requested_z) + return safe_target + except Exception as exc: + self._log_tcp_z_guard_error(str(exc)) + if fallback is None: + return None + return np.asarray(fallback, dtype=np.float64).copy() + + 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) + if self._min_tcp_z_mm is None: + return target + + fallback = self._last_safe_cartesian_target + try: + if target.shape != (6,) or not np.all(np.isfinite(target)): + raise ValueError("Cartesian target has invalid shape or contains NaN/Inf") + requested_z = float(target[2]) + if requested_z < self._min_tcp_z_mm: + target[2] = self._min_tcp_z_mm + self._log_tcp_z_clamp(True, requested_z) + else: + self._log_tcp_z_clamp(False) + self._last_safe_cartesian_target = target.copy() + return target + except Exception as exc: + self._log_tcp_z_guard_error(str(exc)) + if fallback is None: + return None + return np.asarray(fallback, dtype=np.float64).copy() def configure(self) -> None: self.real_arm.motion_enable() @@ -530,6 +677,7 @@ class UFRobot(Robot, Thread): return action before_write_t = time.perf_counter() + safe_action = dict(action) if self._control_space == "joint": # first sync with gello or other control device SLOWLY! jnt_spd = INIT_SYNC_JOINT_VELOCITY_RAD if self._cmd_cnt < 20 else self._max_joint_velocity @@ -538,8 +686,16 @@ class UFRobot(Robot, Thread): cmd_list = [0]*(self._dof) for i in range(self._dof): cmd_list[i] = action[f"{self.prefix}J{i+1}.pos"] + safe_cmd = self._guard_joint_target(cmd_list) + if safe_cmd is None: + # Do not send an unverified arm target. Gripper handling below + # remains independent and can continue safely. + safe_cmd = None + else: + for i in range(self._dof): + safe_action[f"{self.prefix}J{i+1}.pos"] = float(safe_cmd[i]) - if self.config.joint_command_mode == 1: + if safe_cmd is not None and self.config.joint_command_mode == 1: # set_servo_angle_j is an absolute target command. It is the # SDK's high-frequency interface and executes only the latest # target, so it must be used with servo motion mode (1). @@ -550,10 +706,10 @@ class UFRobot(Robot, Thread): self._check_motion_code("set_state(0)", code) time.sleep(0.1) code = self.real_arm.set_servo_angle_j( - cmd_list[:self._dof], speed=jnt_spd, is_radian=True + safe_cmd[:self._dof].tolist(), speed=jnt_spd, is_radian=True ) self._check_motion_code("set_servo_angle_j", code) - else: + elif safe_cmd is not None: # The legacy mode-6 path uses the absolute move_joint API. # The first blocking command must be sent in position mode. if wait_ == False and self.real_arm.mode != 6: @@ -570,7 +726,7 @@ class UFRobot(Robot, Thread): time.sleep(0.1) code = self.real_arm.set_servo_angle( - angle=cmd_list[:self._dof], + angle=safe_cmd[:self._dof].tolist(), speed=jnt_spd, is_radian=True, wait=wait_, @@ -582,16 +738,21 @@ class UFRobot(Robot, Thread): if not self._rt_report_normal: raise ConnectionError("RT Report for target robot NOT READY! ") cmd_list = [action[f"{self.prefix}pose.x"], action[f"{self.prefix}pose.y"], action[f"{self.prefix}pose.z"], action[f"{self.prefix}pose.rx"], action[f"{self.prefix}pose.ry"], action[f"{self.prefix}pose.rz"]] - self.real_arm.set_position_aa(axis_angle_pose=cmd_list, speed=lin_spd, is_radian=True, wait=False) + safe_cmd = self._guard_cartesian_target(cmd_list) + if safe_cmd is not None: + safe_cmd = safe_cmd.tolist() + for i, key in enumerate(("x", "y", "z", "rx", "ry", "rz")): + safe_action[f"{self.prefix}pose.{key}"] = float(safe_cmd[i]) + self.real_arm.set_position_aa(axis_angle_pose=safe_cmd, speed=lin_spd, is_radian=True, wait=False) # self.real_arm.set_position(*cmd_list, radius=0, speed=lin_spd, is_radian=True, wait=False) if self._cmd_cnt < 99999: self._cmd_cnt += 1 # CHECK!! possibility of overflow? if self._gripper_type > GripperType.NoGripper: - self._send_gripper_action(action[f"{self.prefix}gripper.pos"]) + self._send_gripper_action(safe_action[f"{self.prefix}gripper.pos"]) self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t - return action + return safe_action def print_logs(self) -> None: pass 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 0b79b96..cb49214 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 @@ -1,3 +1,4 @@ +import math from dataclasses import dataclass, field from lerobot.cameras import CameraConfig from lerobot.robots import RobotConfig @@ -28,6 +29,9 @@ class UFRobotConfig(RobotConfig): max_joint_velocity: int = 90 # °/s, only effective in joint control mode max_linear_velocity: int = 200 # mm/s, only effective in cartesian control mode no_action: bool = False # only for debug + # 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 def __post_init__(self): super().__post_init__() @@ -43,3 +47,5 @@ class UFRobotConfig(RobotConfig): raise ValueError("gripper_command_threshold must be between 0 and 1") if self.control_space == "joint" and self.joint_command_mode not in (1, 6): 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") diff --git a/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py b/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py index 2a86b77..e12dac1 100644 --- a/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py +++ b/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py @@ -445,6 +445,10 @@ def record_loop( # so action actually sent is saved in the dataset. action = postprocessor.process(action) # TODO(steven, pepijn, adil): we should use a pipeline step to clip the action, so the sent action is the action that we input to the robot. _sent_action = robot.send_action(robot_action_to_send) + # Robots may clamp or otherwise sanitize a command before sending it. + # Store that effective command so demonstrations match the motion. + if isinstance(_sent_action, dict): + action_values = _sent_action # Write to dataset if dataset is not None: diff --git a/src/lerobot_robot_ufactory/scripts/uf_read_tcp_z.py b/src/lerobot_robot_ufactory/scripts/uf_read_tcp_z.py new file mode 100644 index 0000000..faa9814 --- /dev/null +++ b/src/lerobot_robot_ufactory/scripts/uf_read_tcp_z.py @@ -0,0 +1,92 @@ +import argparse +import math +from pathlib import Path +from typing import Callable + +import numpy as np +import yaml +from xarm.wrapper import XArmAPI + + +def _load_robot_config(config_path: Path) -> tuple[str, int]: + with config_path.open(encoding="utf-8") as stream: + config = yaml.safe_load(stream) + + robot_config = config.get("robot") if isinstance(config, dict) else None + if not isinstance(robot_config, dict): + raise ValueError(f"{config_path} does not contain a robot configuration") + + robot_ip = robot_config.get("robot_ip") + robot_dof = robot_config.get("robot_dof") + if not isinstance(robot_ip, str) or not robot_ip: + raise ValueError(f"{config_path} does not define robot.robot_ip") + if robot_dof not in (5, 6, 7): + raise ValueError(f"{config_path} has invalid robot.robot_dof: {robot_dof}") + return robot_ip, int(robot_dof) + + +def read_tcp_z( + config_path: Path, + margin_mm: float = 5.0, + arm_factory: Callable[[str], object] = XArmAPI, +) -> tuple[float, float]: + """Read the current TCP z using the same FK API as the runtime guard.""" + if not math.isfinite(margin_mm) or margin_mm < 0: + raise ValueError("margin-mm must be a finite, non-negative number") + + robot_ip, robot_dof = _load_robot_config(config_path) + arm = arm_factory(robot_ip) + try: + if not arm.connected: + raise ConnectionError(f"Unable to connect to xArm at {robot_ip}") + if arm.axis != robot_dof: + raise RuntimeError( + f"Connected xArm has {arm.axis} axes, but config specifies {robot_dof}" + ) + + code, states = arm.get_joint_states(is_radian=True, num=1) + if code != 0 or not states or len(states[0]) < robot_dof: + raise RuntimeError(f"get_joint_states failed, code={code}") + joints = np.asarray(states[0][:robot_dof], dtype=np.float64) + if not np.all(np.isfinite(joints)): + raise RuntimeError("get_joint_states returned NaN/Inf") + + code, pose = arm.get_forward_kinematics( + joints.tolist(), input_is_radian=True, return_is_radian=True + ) + pose = np.asarray(pose, dtype=np.float64) + if code != 0 or pose.shape[0] < 3 or not np.all(np.isfinite(pose)): + raise RuntimeError(f"get_forward_kinematics failed, code={code}") + + tcp_z_mm = float(pose[2]) + return tcp_z_mm, tcp_z_mm + margin_mm + finally: + arm.disconnect() + + +def main() -> None: + parser = argparse.ArgumentParser( + description="Read the current xArm TCP z without moving the robot." + ) + parser.add_argument( + "--config-path", + type=Path, + required=True, + help="GELLO YAML configuration containing robot.robot_ip and robot.robot_dof", + ) + parser.add_argument( + "--margin-mm", + type=float, + default=5.0, + help="safety margin added to the measured z (default: 5 mm)", + ) + args = parser.parse_args() + + tcp_z_mm, recommended_mm = read_tcp_z(args.config_path, args.margin_mm) + print(f"Current TCP z: {tcp_z_mm:.3f} mm") + print(f"Safety margin: {args.margin_mm:.3f} mm") + print(f"Recommended YAML value: min_tcp_z_mm: {recommended_mm:.3f}") + + +if __name__ == "__main__": + main() diff --git a/tests/test_tcp_z_safety.py b/tests/test_tcp_z_safety.py new file mode 100644 index 0000000..a4a024c --- /dev/null +++ b/tests/test_tcp_z_safety.py @@ -0,0 +1,193 @@ +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest + +from lerobot_robot_ufactory.robots.uf_robot.uf_robot import UFRobot +from lerobot_robot_ufactory.robots.uf_robot.uf_robot_config import UFRobotConfig +from lerobot_robot_ufactory.scripts.uf_read_tcp_z import read_tcp_z + + +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_calls = [] + self.fail_forward = False + self.fail_inverse = False + + def get_forward_kinematics(self, angles, **kwargs): + if self.fail_forward: + return 1, [] + if np.allclose(angles, self.inverse_result): + return 0, self.safe_pose.copy() + return 0, self.requested_pose.copy() + + def get_inverse_kinematics(self, pose, **kwargs): + self.inverse_calls.append((pose.copy(), kwargs)) + if self.fail_inverse: + return 2, [] + return 0, self.inverse_result.copy() + + +def make_guard_robot(arm, min_tcp_z_mm=100.0, control_space="joint"): + robot = UFRobot.__new__(UFRobot) + robot._dof = 7 + robot._control_space = control_space + robot._min_tcp_z_mm = min_tcp_z_mm + robot._last_safe_joint_target = np.asarray([0.25] * 7) + robot._last_safe_cartesian_target = np.asarray([250.0, 0.0, 120.0, 0.0, 0.0, 0.0]) + robot._tcp_z_is_clamped = False + robot._tcp_z_last_log_time = 0.0 + robot._tcp_z_last_error_log_time = 0.0 + robot.real_arm = arm + return robot + + +def test_joint_target_above_floor_is_unchanged(): + arm = FakeKinematicsArm(requested_pose=[300.0, 10.0, 101.0, 0.1, 0.2, 0.3]) + robot = make_guard_robot(arm) + requested = [0.1] * 7 + + result = robot._guard_joint_target(requested) + + assert np.allclose(result, requested) + assert arm.inverse_calls == [] + assert np.allclose(robot._last_safe_joint_target, requested) + + +def test_joint_target_below_floor_clamps_only_tcp_z_before_inverse_kinematics(): + arm = FakeKinematicsArm() + robot = make_guard_robot(arm) + requested = [0.1] * 7 + + result = robot._guard_joint_target(requested) + + assert np.allclose(result, arm.inverse_result) + 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 np.allclose(robot._last_safe_joint_target, arm.inverse_result) + + +@pytest.mark.parametrize("failed_stage", ["forward", "inverse", "verification"]) +def test_joint_guard_holds_last_safe_target_when_kinematics_fails(failed_stage): + arm = FakeKinematicsArm() + if failed_stage == "forward": + arm.fail_forward = True + elif failed_stage == "inverse": + arm.fail_inverse = True + else: + arm.safe_pose[2] = 99.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") + + result = robot._guard_cartesian_target([300.0, 20.0, 90.0, 0.1, 0.2, 0.3]) + + assert result == pytest.approx([300.0, 20.0, 100.0, 0.1, 0.2, 0.3]) + + +def test_send_action_sends_and_returns_clamped_joint_target(): + arm = FakeKinematicsArm() + arm.error_code = 0 + arm.mode = 1 + arm.sent_joint_targets = [] + arm.set_servo_angle_j = lambda target, **kwargs: arm.sent_joint_targets.append(target) or 0 + robot = make_guard_robot(arm) + robot._is_connected = True + robot._last_logged_controller_error = 0 + robot._cmd_cnt = 20 + robot._max_joint_velocity = 1.0 + robot._gripper_type = 0 + robot.prefix = "" + robot.logs = {} + robot.config = SimpleNamespace( + manual_mode=False, + no_action=False, + joint_command_mode=1, + gripper_error_log_path=None, + ) + action = {f"J{i + 1}.pos": 0.1 for i in range(7)} + + sent_action = robot.send_action(action) + + assert arm.sent_joint_targets == [pytest.approx(arm.inverse_result)] + assert [sent_action[f"J{i + 1}.pos"] for i in range(7)] == pytest.approx( + arm.inverse_result + ) + assert [action[f"J{i + 1}.pos"] for i in range(7)] == pytest.approx([0.1] * 7) + + +def test_non_finite_tcp_floor_is_rejected(): + with pytest.raises(ValueError, match="min_tcp_z_mm"): + UFRobotConfig(robot_dof=7, min_tcp_z_mm=float("nan")) + + +class FakeMeasurementArm: + def __init__(self, robot_ip): + self.robot_ip = robot_ip + self.connected = True + self.axis = 7 + self.disconnected = False + self.forward_calls = [] + + def get_joint_states(self, **kwargs): + return 0, [[0.1] * 7] + + def get_forward_kinematics(self, joints, **kwargs): + self.forward_calls.append((joints, kwargs)) + return 0, [300.0, 0.0, 87.25, 0.0, 0.0, 0.0] + + def disconnect(self): + self.disconnected = True + + +def write_measurement_config(path: Path): + path.write_text( + "robot:\n robot_ip: '192.168.1.245'\n robot_dof: 7\n", + encoding="utf-8", + ) + + +def test_read_tcp_z_adds_margin_and_disconnects(tmp_path): + config_path = tmp_path / "gello.yaml" + write_measurement_config(config_path) + arms = [] + + def arm_factory(robot_ip): + arm = FakeMeasurementArm(robot_ip) + arms.append(arm) + return arm + + measured, recommended = read_tcp_z(config_path, margin_mm=5.0, arm_factory=arm_factory) + + assert measured == pytest.approx(87.25) + assert recommended == pytest.approx(92.25) + assert arms[0].robot_ip == "192.168.1.245" + assert arms[0].disconnected is True + assert arms[0].forward_calls[0][1] == { + "input_is_radian": True, + "return_is_radian": True, + } + + +def test_read_tcp_z_disconnects_when_fk_fails(tmp_path): + config_path = tmp_path / "gello.yaml" + write_measurement_config(config_path) + arm = FakeMeasurementArm("192.168.1.245") + arm.get_forward_kinematics = lambda *args, **kwargs: (1, []) + + with pytest.raises(RuntimeError, match="get_forward_kinematics"): + read_tcp_z(config_path, arm_factory=lambda _: arm) + + assert arm.disconnected is True