wangshuxun/Xarm-DataCollection
1
0
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add safety height guard in control

Browse Source
This commit is contained in:
ChenYuhan 2026-08-16 15:55:59 +08:00
parent 6288ac7d7d
commit ede2b4bed9
11 changed files with 498 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -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.

14
README.md
View File

@ -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

22
README_ZH.md
View File

@ -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

4
config/gello/xarm7_gello_record_config.yaml
View File

@ -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

4
config/manual_mode/xarm7_manual_record_config.yaml
View File

@ -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

1
pyproject.toml
View File

@ -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 遥操作

175
src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py
View File

@ -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

6
src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py
View File

@ -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")

4
src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py
View File

@ -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:

92
src/lerobot_robot_ufactory/scripts/uf_read_tcp_z.py Normal file
View File

@ -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()

193
tests/test_tcp_z_safety.py Normal file
View File

@ -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