YikaiFu-cart/acRealman_xr
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: Implement UDP feedback for RM75 robot arms

Browse Source
This commit is contained in:
YikaiFu-cart
2026-07-29 15:26:59 +08:00
parent 687a0b401a
commit 08996434e5
16 changed files with 1600 additions and 329 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xr_rm_teleop/test/placo_ik_smoke.py
+3
View File
@@ -50,6 +50,9 @@ def main() -> None:
drift_degrees = float(
np.max(np.abs(np.rad2deg(np.asarray(joints) - initial_joints)))
)
assert drift_degrees <= 0.05, (
f"{arm} stationary target drifted {drift_degrees:.3f}deg"
)
solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
joints = initial_joints.tolist()
xr_rm_teleop/test/test_initial_joint_pose.py
+293 -59
View File
@@ -1,4 +1,6 @@
import math
import sys
from types import ModuleType, SimpleNamespace
import pytest
@@ -18,7 +20,14 @@ def test_initial_pose_uses_joint_move_only() -> None:
return 0
joints = [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
adapter = RealManAdapter("127.0.0.1", 8080, 0, 1, initial_joint_pose=joints)
adapter = RealManAdapter(
"127.0.0.1",
8080,
0,
"127.0.0.1",
8090,
initial_joint_pose=joints,
)
adapter._arm = FakeArm()
adapter._move_to_initial_pose()
@@ -111,80 +120,299 @@ def test_tool_frame_sdk_failures_are_reported(existing, failure, operation) -> N
_configure_tool_frame(FakeArm(), object(), "omnipic")
def test_joint_feedback_is_cached_in_radians(monkeypatch) -> None:
class FakeArm:
def rm_get_joint_degree(self):
return 0, [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
perf_counter_ns = iter(
[1_000_000_000, 1_002_000_000, 2_000_000_000, 2_003_000_000]
)
monotonic = iter([10.0, 10.011])
monkeypatch.setattr(
realman_adapter,
"time",
type(
"FakeTime",
(),
{
"perf_counter_ns": staticmethod(lambda: next(perf_counter_ns)),
"monotonic": staticmethod(lambda: next(monotonic)),
},
def _udp_state(
*,
robot_ip: str = "127.0.0.1",
joints=None,
error_code: int = 0,
joint_enabled=None,
joint_error_codes=None,
arm_error_codes=None,
arm_current_status: int = 0,
):
if joints is None:
joints = [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
if joint_enabled is None:
joint_enabled = [True] * 7
if joint_error_codes is None:
joint_error_codes = [0] * 7
if arm_error_codes is None:
arm_error_codes = []
return SimpleNamespace(
errCode=error_code,
arm_ip=robot_ip.encode(),
joint_status=SimpleNamespace(
joint_position=joints,
joint_en_flag=joint_enabled,
joint_err_code=joint_error_codes,
),
err=SimpleNamespace(
err_len=len(arm_error_codes),
err=list(arm_error_codes),
),
arm_current_status=arm_current_status,
)
adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
adapter._arm = FakeArm()
adapter._read_joint_state_once()
def _install_fake_sdk(monkeypatch, *, push_return=0, send_feedback=True):
class FakeThreadMode:
RM_TRIPLE_MODE_E = 2
class FakePushConfig:
def __init__(self, *args):
self.args = args
class FakeArm:
instance = None
def __init__(self, mode):
self.mode = mode
self.callback = None
self.config = None
self.delete_calls = 0
FakeArm.instance = self
def rm_create_robot_arm(self, robot_ip, robot_port):
self.robot_ip = robot_ip
self.robot_port = robot_port
return SimpleNamespace(id=1)
def rm_realtime_arm_state_call_back(self, callback):
self.callback = callback
def rm_set_realtime_push(self, config):
self.config = config
if push_return == 0 and send_feedback:
self.callback(_udp_state())
return push_return
def rm_delete_robot_arm(self):
self.delete_calls += 1
return 0
sdk = ModuleType("Robotic_Arm.rm_robot_interface")
sdk.RoboticArm = FakeArm
sdk.rm_thread_mode_e = FakeThreadMode
sdk.rm_realtime_push_config_t = FakePushConfig
sdk.rm_realtime_arm_state_callback_ptr = lambda callback: callback
package = ModuleType("Robotic_Arm")
package.rm_robot_interface = sdk
monkeypatch.setitem(sys.modules, "Robotic_Arm", package)
monkeypatch.setitem(sys.modules, "Robotic_Arm.rm_robot_interface", sdk)
return SimpleNamespace(RoboticArm=FakeArm)
def test_udp_feedback_is_cached_in_radians(monkeypatch) -> None:
monotonic = iter([10.0, 10.005])
monkeypatch.setattr(realman_adapter.time, "monotonic", lambda: next(monotonic))
adapter = RealManAdapter(
"127.0.0.1",
8080,
0,
"127.0.0.1",
8090,
)
adapter._accept_realtime_feedback = True
adapter._on_realtime_arm_state(_udp_state())
first = adapter.get_latest_joint_state()
adapter._read_joint_state_once()
adapter._on_realtime_arm_state(_udp_state())
second = adapter.get_latest_joint_state()
assert first is not None
assert first.positions == pytest.approx(
[math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
)
assert first.read_duration_ms == pytest.approx(2.0)
assert first.read_duration_ms is None
assert first.update_interval_ms is None
assert second is not None
assert second.read_duration_ms == pytest.approx(3.0)
assert second.update_interval_ms == pytest.approx(11.0)
assert second.read_duration_ms is None
assert second.update_interval_ms == pytest.approx(5.0)
def test_feedback_loop_uses_absolute_schedule_without_catch_up(monkeypatch) -> None:
class FakeStopEvent:
def __init__(self) -> None:
self.checks = 0
self.waits = []
def is_set(self):
self.checks += 1
return self.checks > 3
def wait(self, timeout):
self.waits.append(timeout)
return False
monotonic = iter([0.0, 0.005, 0.018, 0.018, 0.023])
monkeypatch.setattr(
realman_adapter,
"time",
type(
"FakeTime",
(),
{"monotonic": staticmethod(lambda: next(monotonic))},
),
@pytest.mark.parametrize(
"state",
[
_udp_state(error_code=-3),
_udp_state(robot_ip="192.168.192.18"),
_udp_state(joints=[0.0] * 6),
_udp_state(joints=[0.0, 0.0, 0.0, math.nan, 0.0, 0.0, 0.0]),
],
)
def test_invalid_udp_feedback_does_not_replace_snapshot(state) -> None:
adapter = RealManAdapter(
"127.0.0.1",
8080,
0,
"127.0.0.1",
8090,
)
adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.008)
stop_event = FakeStopEvent()
reads = []
adapter._feedback_stop = stop_event
adapter._read_joint_state_once = lambda: reads.append(None)
adapter._accept_realtime_feedback = True
adapter._on_realtime_arm_state(_udp_state(joints=[1.0] * 7))
before = adapter.get_latest_joint_state()
adapter._feedback_loop()
adapter._on_realtime_arm_state(state)
assert len(reads) == 3
assert stop_event.waits == pytest.approx([0.003, 0.003])
assert adapter.get_latest_joint_state() == before
def test_udp_joint_fault_marks_snapshot_unready() -> None:
adapter = RealManAdapter(
"127.0.0.1",
8080,
0,
"127.0.0.1",
8090,
)
adapter._accept_realtime_feedback = True
adapter._on_realtime_arm_state(
_udp_state(
joint_enabled=[True, True, False, True, True, True, True],
joint_error_codes=[0, 0, 17, 0, 0, 0, 0],
arm_error_codes=[42],
arm_current_status=9,
)
)
snapshot = adapter.get_latest_joint_state()
assert snapshot is not None
assert snapshot.motion_ready is False
def test_udp_stop_status_marks_snapshot_unready_without_joint_error() -> None:
adapter = RealManAdapter(
"127.0.0.1",
8080,
0,
"127.0.0.1",
8090,
)
adapter._accept_realtime_feedback = True
adapter._on_realtime_arm_state(_udp_state(arm_current_status=9))
snapshot = adapter.get_latest_joint_state()
assert snapshot is not None
assert snapshot.motion_ready is False
def test_udp_zero_arm_error_code_is_motion_ready() -> None:
adapter = RealManAdapter(
"127.0.0.1",
8080,
0,
"127.0.0.1",
8090,
)
adapter._accept_realtime_feedback = True
adapter._on_realtime_arm_state(_udp_state(arm_error_codes=[0]))
snapshot = adapter.get_latest_joint_state()
assert snapshot is not None
assert snapshot.motion_ready is True
def test_udp_fault_and_recovery_are_logged_once_per_transition() -> None:
class FakeLogger:
def __init__(self) -> None:
self.infos = []
self.warnings = []
def info(self, message):
self.infos.append(message)
def warn(self, message):
self.warnings.append(message)
logger = FakeLogger()
adapter = RealManAdapter(
"127.0.0.1",
8080,
0,
"127.0.0.1",
8090,
logger=logger,
)
adapter._accept_realtime_feedback = True
adapter._on_realtime_arm_state(_udp_state())
fault = _udp_state(
joint_enabled=[False] * 7,
joint_error_codes=[17, 0, 0, 0, 0, 0, 0],
arm_error_codes=[42],
arm_current_status=9,
)
adapter._on_realtime_arm_state(fault)
adapter._on_realtime_arm_state(fault)
adapter._on_realtime_arm_state(_udp_state())
assert len(logger.warnings) == 1
assert "joint_errors=[17, 0, 0, 0, 0, 0, 0]" in logger.warnings[0]
assert len([message for message in logger.infos if "恢复正常" in message]) == 1
def test_connect_configures_udp_feedback_and_waits_for_first_frame(monkeypatch) -> None:
fake_sdk = _install_fake_sdk(monkeypatch)
adapter = RealManAdapter(
"127.0.0.1",
8080,
0,
"192.168.192.148",
8090,
configure_safety_limits=False,
)
adapter.connect()
arm = fake_sdk.RoboticArm.instance
assert arm is not None
assert arm.config.args == (5, True, 8090, 0, "192.168.192.148")
assert arm.callback is adapter._realtime_callback
assert adapter.get_latest_joint_state() is not None
assert not hasattr(adapter, "_feedback_thread")
def test_udp_configuration_failure_cleans_up_robot_handle(monkeypatch) -> None:
fake_sdk = _install_fake_sdk(monkeypatch, push_return=1)
adapter = RealManAdapter(
"127.0.0.1",
8080,
0,
"192.168.192.148",
8090,
configure_safety_limits=False,
)
with pytest.raises(RuntimeError, match="rm_set_realtime_push"):
adapter.connect()
assert fake_sdk.RoboticArm.instance.delete_calls == 1
assert adapter._arm is None
def test_udp_first_frame_timeout_cleans_up_robot_handle(monkeypatch) -> None:
fake_sdk = _install_fake_sdk(monkeypatch, send_feedback=False)
adapter = RealManAdapter(
"127.0.0.1",
8080,
0,
"192.168.192.148",
8090,
configure_safety_limits=False,
)
adapter._feedback_ready = SimpleNamespace(
clear=lambda: None,
set=lambda: None,
wait=lambda timeout: False,
)
with pytest.raises(RuntimeError, match="within 2 seconds"):
adapter.connect()
assert fake_sdk.RoboticArm.instance.delete_calls == 1
assert adapter._arm is None
def test_joint_target_uses_movej_canfd_in_degrees() -> None:
@@ -196,7 +424,13 @@ def test_joint_target_uses_movej_canfd_in_degrees() -> None:
self.calls.append(args)
return 0
adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
adapter = RealManAdapter(
"127.0.0.1",
8080,
0,
"127.0.0.1",
8090,
)
adapter._arm = FakeArm()
target = [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
xr_rm_teleop/test/test_joint_control.py
+85
View File
@@ -1,3 +1,4 @@
import math
import time
from types import SimpleNamespace
@@ -45,6 +46,85 @@ def test_missing_or_stale_feedback_does_not_enable_qp() -> None:
assert teleop._fresh_joint_state() is None
def test_disabled_joint_feedback_does_not_enable_qp() -> None:
teleop = object.__new__(SingleArmVelocityTeleop)
teleop._command_timeout_sec = 0.12
teleop._adapter = SimpleNamespace(
get_latest_joint_state=lambda: JointStateSnapshot(
[0.0] * 7,
time.monotonic(),
motion_ready=False,
)
)
assert teleop._fresh_joint_state() is None
def test_joint_command_step_limits_acceleration_from_rest() -> None:
dt = 1.0 / 125.0
target, velocity = SingleArmVelocityTeleop._limit_joint_command_step(
target=[0.2] * 7,
previous_target=[0.0] * 7,
previous_velocity=[0.0] * 7,
max_speed=math.radians(180.0),
max_acceleration=math.radians(300.0),
dt=dt,
)
assert velocity == pytest.approx([math.radians(2.4)] * 7)
assert target == pytest.approx([math.radians(0.0192)] * 7)
def test_feedback_fault_blocks_grip_until_release() -> None:
class FakeClock:
def now(self):
return FakeTime()
teleop = object.__new__(SingleArmVelocityTeleop)
teleop._adapter = SimpleNamespace(
get_latest_joint_state=lambda: JointStateSnapshot(
[0.1] * 7,
time.monotonic(),
)
)
teleop._command_timeout_sec = 0.12
teleop._joint_feedback_ready = True
teleop._arm_name = "right_rm75"
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=1.0),
),
)
teleop._last_msg_time = FakeTime()
teleop._active = False
teleop._enable_orientation_control = False
teleop._last_valid_joint_target = None
teleop._last_current_pose = None
teleop._ik_solver = SimpleNamespace(
update_joint_state=lambda joints: np.eye(4)
)
teleop._grip_rearm_required = True
teleop.get_clock = lambda: FakeClock()
teleop.get_logger = lambda: FakeLogger()
stopped = []
entered = []
teleop._safe_stop = lambda reset_active: stopped.append(reset_active)
teleop._enter_active_control = lambda *args: entered.append(args)
teleop._control_tick()
assert entered == []
teleop._last_msg.grip = False
teleop._control_tick()
assert teleop._grip_rearm_required is False
teleop._last_msg.grip = True
teleop._control_tick()
assert len(entered) == 1
def test_stale_feedback_stops_before_active_control() -> None:
stopped = []
entered = []
@@ -256,6 +336,11 @@ def test_joint_send_failure_requests_slow_stop_and_resets_control() -> None:
teleop._follow = False
teleop._arm_name = "left_rm75"
teleop._stop_sent = False
teleop._last_joint_command_target = [0.0] * 7
teleop._last_joint_command_velocity = [0.0] * 7
teleop._joint_command_max_speed = math.radians(180.0)
teleop._joint_command_max_acceleration = math.radians(300.0)
teleop._dt = 1.0 / 125.0
teleop.get_logger = lambda: FakeLogger()
teleop._safe_stop = lambda reset_active: reset_calls.append(reset_active)
xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py
-6
View File
@@ -99,12 +99,6 @@ class PlacoIkSolver:
np.eye(4),
)
self._frame_task.configure("rm75_frame", "soft", 1.0)
manipulability = self._solver.add_manipulability_task(
"link_7",
"both",
1.0,
)
manipulability.configure("rm75_manipulability", "soft", 5e-2)
self._solver.add_kinetic_energy_regularization_task(1e-6)
@property
xr_rm_teleop/xr_rm_teleop/realman_adapter.py
+188 -79
View File
@@ -2,6 +2,7 @@
from __future__ import annotations
import ipaddress
import math
import threading
import time
@@ -32,6 +33,7 @@ class JointStateSnapshot:
received_at: float
read_duration_ms: float | None = None
update_interval_ms: float | None = None
motion_ready: bool = True
class MockRealManAdapter:
@@ -85,7 +87,9 @@ class RealManAdapter:
robot_ip: str,
robot_port: int,
avoid_singularity: int,
feedback_period: float,
realtime_push_host_ip: str,
realtime_push_port: int,
realtime_push_cycle_ms: int = 5,
logger: Any | None = None,
configure_safety_limits: bool = True,
max_line_speed: float = 1.0,
@@ -103,9 +107,22 @@ class RealManAdapter:
self._robot_ip = robot_ip
self._robot_port = robot_port
self._avoid_singularity = avoid_singularity
if feedback_period <= 0.0:
raise ValueError("feedback_period must be positive")
self._feedback_period = feedback_period
try:
self._realtime_push_host_ip = str(
ipaddress.IPv4Address(realtime_push_host_ip)
)
except ipaddress.AddressValueError as exc:
raise ValueError(
"realtime_push_host_ip must be a valid IPv4 address"
) from exc
if not 1 <= realtime_push_port <= 65535:
raise ValueError("realtime_push_port must be between 1 and 65535")
if realtime_push_cycle_ms <= 0 or realtime_push_cycle_ms % 5 != 0:
raise ValueError(
"realtime_push_cycle_ms must be a positive multiple of 5"
)
self._realtime_push_port = realtime_push_port
self._realtime_push_cycle_ms = realtime_push_cycle_ms
self._logger = logger
self._configure_safety_limits = configure_safety_limits
self._max_line_speed = max_line_speed
@@ -123,38 +140,81 @@ class RealManAdapter:
self._arm: Any | None = None
self._joint_state_lock = threading.Lock()
self._latest_joint_state: JointStateSnapshot | None = None
self._feedback_stop = threading.Event()
self._feedback_thread: threading.Thread | None = None
self._feedback_ready = threading.Event()
self._realtime_callback: Any | None = None
self._accept_realtime_feedback = False
self._feedback_fault_logged = False
self._last_motion_status: tuple[Any, ...] | None = None
def connect(self) -> None:
try:
from Robotic_Arm.rm_robot_interface import RoboticArm, rm_thread_mode_e
from Robotic_Arm.rm_robot_interface import (
RoboticArm,
rm_realtime_arm_state_callback_ptr,
rm_realtime_push_config_t,
rm_thread_mode_e,
)
except ImportError as exc:
raise RuntimeError(
"未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。"
) from exc
self._arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
handle = self._arm.rm_create_robot_arm(self._robot_ip, self._robot_port)
self._check_robot_handle(handle)
self._log_info(
"RealMan connected: "
f"ip={self._robot_ip}, port={self._robot_port}, "
f"avoid_singularity={self._avoid_singularity}, "
"command=rm_movej_canfd"
)
if self._configure_safety_limits:
self._apply_safety_limits()
if self._move_to_initial_pose_on_connect:
self._move_to_initial_pose()
self._feedback_stop.clear()
self._feedback_thread = threading.Thread(
target=self._feedback_loop,
name=f"rm75_feedback_{self._robot_ip}",
daemon=True,
)
self._feedback_thread.start()
try:
handle = self._arm.rm_create_robot_arm(
self._robot_ip,
self._robot_port,
)
self._check_robot_handle(handle)
self._log_info(
"RealMan connected: "
f"ip={self._robot_ip}, port={self._robot_port}, "
f"avoid_singularity={self._avoid_singularity}, "
"command=rm_movej_canfd"
)
if self._configure_safety_limits:
self._apply_safety_limits()
if self._move_to_initial_pose_on_connect:
self._move_to_initial_pose()
self._feedback_ready.clear()
self._accept_realtime_feedback = True
self._realtime_callback = rm_realtime_arm_state_callback_ptr(
self._on_realtime_arm_state
)
self._arm.rm_realtime_arm_state_call_back(
self._realtime_callback
)
config = rm_realtime_push_config_t(
self._realtime_push_cycle_ms,
True,
self._realtime_push_port,
0,
self._realtime_push_host_ip,
)
self._check_return(
self._arm.rm_set_realtime_push(config),
"rm_set_realtime_push",
)
if not self._feedback_ready.wait(timeout=2.0):
raise RuntimeError(
"RealMan UDP realtime feedback did not receive a valid "
"frame within 2 seconds"
)
self._log_info(
"RealMan UDP realtime feedback ready: "
f"host={self._realtime_push_host_ip}:"
f"{self._realtime_push_port}, "
f"cycle={self._realtime_push_cycle_ms} ms"
)
except Exception:
self._accept_realtime_feedback = False
try:
self._arm.rm_delete_robot_arm()
except Exception:
pass
self._arm = None
self._realtime_callback = None
raise
def get_latest_joint_state(self) -> JointStateSnapshot | None:
with self._joint_state_lock:
@@ -165,6 +225,7 @@ class RealManAdapter:
self._latest_joint_state.received_at,
self._latest_joint_state.read_duration_ms,
self._latest_joint_state.update_interval_ms,
self._latest_joint_state.motion_ready,
)
def send_joint_target(self, joints: list[float], follow: bool) -> None:
@@ -219,70 +280,118 @@ class RealManAdapter:
def close(self) -> None:
if self._arm is None:
return
self._accept_realtime_feedback = False
self.stop()
self._feedback_stop.set()
if self._feedback_thread is not None:
self._feedback_thread.join(timeout=3.0)
if self._feedback_thread.is_alive():
self._log_warn("RealMan 关节反馈线程未在 3 秒内退出。")
self._feedback_thread = None
try:
self._arm.rm_delete_robot_arm()
finally:
self._arm = None
self._realtime_callback = None
def _require_arm(self) -> None:
if self._arm is None:
raise RuntimeError("睿尔曼机械臂尚未连接")
def _feedback_loop(self) -> None:
next_read_at = time.monotonic()
while not self._feedback_stop.is_set():
try:
self._read_joint_state_once()
self._feedback_fault_logged = False
except Exception as exc:
if not self._feedback_fault_logged:
self._log_warn(f"RealMan 关节反馈读取失败：{exc}")
self._feedback_fault_logged = True
next_read_at += self._feedback_period
remaining = next_read_at - time.monotonic()
if remaining <= 0.0:
next_read_at = time.monotonic()
continue
self._feedback_stop.wait(remaining)
def _on_realtime_arm_state(self, data: Any) -> None:
if not self._accept_realtime_feedback:
return
try:
if data is None or int(data.errCode) != 0:
raise ValueError("invalid realtime feedback error code")
arm_ip = data.arm_ip
if isinstance(arm_ip, bytes):
arm_ip = arm_ip.decode("utf-8").split("\x00", 1)[0]
if str(arm_ip) != self._robot_ip:
raise ValueError(
f"unexpected realtime feedback source: {arm_ip}"
)
degrees = list(data.joint_status.joint_position)
if (
len(degrees) != 7
or not all(isinstance(value, Number) for value in degrees)
):
raise ValueError(
"RM75 UDP feedback must contain 7 numeric joints"
)
positions = [math.radians(float(value)) for value in degrees]
if not all(math.isfinite(value) for value in positions):
raise ValueError("RM75 UDP feedback contains NaN/Inf")
joint_enabled = [
bool(value) for value in data.joint_status.joint_en_flag
]
joint_errors = [
int(value) for value in data.joint_status.joint_err_code
]
if len(joint_enabled) != 7 or len(joint_errors) != 7:
raise ValueError(
"RM75 UDP feedback must contain 7 joint states"
)
arm_error_count = int(data.err.err_len)
arm_errors = [
int(value) for value in list(data.err.err)[:arm_error_count]
]
arm_errors = [code for code in arm_errors if code != 0]
arm_current_status = int(data.arm_current_status)
motion_ready = (
0 <= arm_current_status <= 8
and all(joint_enabled)
and not any(joint_errors)
and not arm_errors
)
motion_status = (
arm_current_status,
tuple(joint_enabled),
tuple(joint_errors),
tuple(arm_errors),
motion_ready,
)
received_at = time.monotonic()
with self._joint_state_lock:
update_interval_ms = (
None
if self._latest_joint_state is None
else (
received_at
- self._latest_joint_state.received_at
)
* 1000.0
)
self._latest_joint_state = JointStateSnapshot(
positions,
received_at,
None,
update_interval_ms,
motion_ready,
)
self._log_motion_status_transition(motion_status)
self._feedback_fault_logged = False
self._feedback_ready.set()
except Exception as exc:
if not self._feedback_fault_logged:
self._log_warn(
f"RealMan UDP realtime feedback invalid: {exc}"
)
self._feedback_fault_logged = True
def _read_joint_state_once(self) -> None:
self._require_arm()
read_started_ns = time.perf_counter_ns()
result = self._arm.rm_get_joint_degree()
read_duration_ms = (time.perf_counter_ns() - read_started_ns) * 1e-6
self._check_return(result, "rm_get_joint_degree")
if not isinstance(result, tuple) or len(result) < 2:
raise RuntimeError(f"rm_get_joint_degree 返回格式错误：{result!r}")
degrees = result[1]
if (
not isinstance(degrees, (list, tuple))
or len(degrees) != 7
or not all(isinstance(value, Number) for value in degrees)
):
raise RuntimeError(f"RM75 关节反馈必须包含 7 个数值：{degrees!r}")
positions = [math.radians(float(value)) for value in degrees]
if not all(math.isfinite(value) for value in positions):
raise RuntimeError("RM75 关节反馈包含 NaN/Inf")
received_at = time.monotonic()
with self._joint_state_lock:
update_interval_ms = (
None
if self._latest_joint_state is None
else (received_at - self._latest_joint_state.received_at) * 1000.0
)
self._latest_joint_state = JointStateSnapshot(
positions,
received_at,
read_duration_ms,
update_interval_ms,
)
def _log_motion_status_transition(
self,
status: tuple[Any, ...],
) -> None:
previous = self._last_motion_status
if status == previous:
return
self._last_motion_status = status
arm_status, joint_enabled, joint_errors, arm_errors, ready = status
details = (
f"arm_status={arm_status}, "
f"joint_enabled={list(joint_enabled)}, "
f"joint_errors={list(joint_errors)}, "
f"arm_errors={list(arm_errors)}"
)
if not ready:
self._log_warn(f"RealMan UDP 报警或掉使能：{details}")
elif previous is not None and not previous[-1]:
self._log_info(f"RealMan UDP 运动状态恢复正常：{details}")
def _apply_safety_limits(self) -> None:
# 真机安全限幅尽量下发到控制器；不支持的 SDK 接口会在 _try_call 中降级为警告。
xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
+91 -3
View File
@@ -201,6 +201,9 @@ class SingleArmVelocityTeleop(Node):
self.declare_parameter("robot_urdf_path", "")
self.declare_parameter("robot_ip", "192.168.1.18")
self.declare_parameter("robot_port", 8080)
self.declare_parameter("realtime_push_host_ip", "")
self.declare_parameter("realtime_push_port", 0)
self.declare_parameter("realtime_push_cycle_ms", 5)
self.declare_parameter("avoid_singularity", 1)
self.declare_parameter("follow", False)
self.declare_parameter("configure_safety_limits", True)
@@ -255,6 +258,12 @@ class SingleArmVelocityTeleop(Node):
self._enable_tool_control = self._bool_parameter("enable_tool_control")
self._enable_trigger_gripper_control = self._bool_parameter("enable_trigger_gripper_control")
self._trigger_close_threshold = float(self.get_parameter("trigger_close_threshold").value)
self._joint_command_max_speed = math.radians(
float(self.get_parameter("joint_max_speed").value)
)
self._joint_command_max_acceleration = math.radians(
float(self.get_parameter("joint_max_acc").value)
)
self._debug_topic_prefix = str(self.get_parameter("debug_topic_prefix").value).rstrip("/")
if not self._debug_topic_prefix:
self._debug_topic_prefix = "/xr_rm"
@@ -273,7 +282,11 @@ class SingleArmVelocityTeleop(Node):
self._last_command_time: Time | None = None
self._last_current_pose: np.ndarray | None = None
self._last_valid_joint_target: list[float] | None = None
self._latest_joint_positions: list[float] | None = None
self._last_joint_command_target: list[float] | None = None
self._last_joint_command_velocity: list[float] | None = None
self._joint_feedback_ready = False
self._grip_rearm_required = False
self._stop_sent = True
self._trigger_tool_open = True
self._last_trigger_pressed: bool | None = None
@@ -337,7 +350,15 @@ class SingleArmVelocityTeleop(Node):
robot_ip=self.get_parameter("robot_ip").value,
robot_port=int(self.get_parameter("robot_port").value),
avoid_singularity=int(self.get_parameter("avoid_singularity").value),
feedback_period=self._dt,
realtime_push_host_ip=str(
self.get_parameter("realtime_push_host_ip").value
),
realtime_push_port=int(
self.get_parameter("realtime_push_port").value
),
realtime_push_cycle_ms=int(
self.get_parameter("realtime_push_cycle_ms").value
),
logger=self.get_logger(),
configure_safety_limits=self._bool_parameter("configure_safety_limits"),
max_line_speed=float(self.get_parameter("max_line_speed").value),
@@ -496,9 +517,10 @@ class SingleArmVelocityTeleop(Node):
now = self.get_clock().now()
snapshot = self._fresh_joint_state()
if snapshot is None:
self._grip_rearm_required = True
if self._joint_feedback_ready:
self.get_logger().warn(
f"{self._arm_name} 关节反馈缺失或过期，机械臂停止。",
f"{self._arm_name} 关节反馈缺失、过期或机械臂未就绪，机械臂停止。",
throttle_duration_sec=1.0,
)
self._joint_feedback_ready = False
@@ -512,6 +534,7 @@ class SingleArmVelocityTeleop(Node):
throttle_duration_sec=1.0,
)
self._joint_feedback_ready = False
self._grip_rearm_required = True
self._safe_stop(reset_active=True)
return
if not self._joint_feedback_ready:
@@ -537,10 +560,18 @@ class SingleArmVelocityTeleop(Node):
return
if not self._last_msg.grip:
self._grip_rearm_required = False
if self._active:
self.get_logger().info(f"{self._arm_name} Grip 松开，退出相对位姿遥操。")
self._safe_stop(reset_active=True)
return
if getattr(self, "_grip_rearm_required", False):
self.get_logger().warn(
f"{self._arm_name} 反馈故障后等待 Grip 松开，禁止自动恢复运动。",
throttle_duration_sec=1.0,
)
self._safe_stop(reset_active=True)
return
controller_now = self._controller_xyz(self._last_msg)
try:
@@ -957,6 +988,7 @@ class SingleArmVelocityTeleop(Node):
if (
len(snapshot.positions) != 7
or not all(math.isfinite(value) for value in snapshot.positions)
or not snapshot.motion_ready
):
return None
return snapshot
@@ -968,6 +1000,7 @@ class SingleArmVelocityTeleop(Node):
current_pose = self._ik_solver.update_joint_state(
snapshot.positions
)
self._latest_joint_positions = list(snapshot.positions)
self._last_current_pose = current_pose
if not self._active or self._last_valid_joint_target is None:
self._last_valid_joint_target = list(snapshot.positions)
@@ -1000,6 +1033,8 @@ class SingleArmVelocityTeleop(Node):
self._last_sent_target = None
self._last_sent_orientation = None
self._last_command_time = None
self._last_joint_command_target = None
self._last_joint_command_velocity = None
self._publish_stop_debug()
def _send_stop_once(self) -> None:
@@ -1034,8 +1069,22 @@ class SingleArmVelocityTeleop(Node):
return None
def _send_joint_target(self, joints: list[float]) -> bool:
previous_target = self._last_joint_command_target
if previous_target is None:
previous_target = self._latest_joint_positions
if previous_target is None:
raise RuntimeError("valid joint feedback has not been initialized")
previous_velocity = self._last_joint_command_velocity or [0.0] * 7
limited_target, limited_velocity = self._limit_joint_command_step(
target=joints,
previous_target=previous_target,
previous_velocity=previous_velocity,
max_speed=self._joint_command_max_speed,
max_acceleration=self._joint_command_max_acceleration,
dt=self._dt,
)
try:
self._adapter.send_joint_target(joints, self._follow)
self._adapter.send_joint_target(limited_target, self._follow)
except Exception as exc:
self.get_logger().error(
f"{self._arm_name} 发送关节透传命令失败：{exc}",
@@ -1044,8 +1093,43 @@ class SingleArmVelocityTeleop(Node):
self._send_stop_once()
self._safe_stop(reset_active=True)
return False
self._last_joint_command_target = limited_target
self._last_joint_command_velocity = limited_velocity
return True
@staticmethod
def _limit_joint_command_step(
target: list[float],
previous_target: list[float],
previous_velocity: list[float],
max_speed: float,
max_acceleration: float,
dt: float,
) -> tuple[list[float], list[float]]:
if (
len(target) != 7
or len(previous_target) != 7
or len(previous_velocity) != 7
):
raise ValueError("joint command state must contain 7 values")
if max_speed <= 0.0 or max_acceleration <= 0.0 or dt <= 0.0:
raise ValueError("joint command limits and dt must be positive")
desired_velocity = np.clip(
(np.asarray(target) - np.asarray(previous_target)) / dt,
-max_speed,
max_speed,
)
velocity_step = max_acceleration * dt
velocity = np.clip(
desired_velocity,
np.asarray(previous_velocity) - velocity_step,
np.asarray(previous_velocity) + velocity_step,
)
limited_target = np.asarray(previous_target) + velocity * dt
if not np.isfinite(limited_target).all():
raise ValueError("joint command contains NaN/Inf")
return limited_target.tolist(), velocity.tolist()
def _publish_debug(
self,
raw_target_pose: np.ndarray,
@@ -1151,6 +1235,10 @@ class SingleArmVelocityTeleop(Node):
raise ValueError("cyl_radius_limit[1] must be > cyl_radius_limit[0]")
if self._low_z_min_radius < 0.0:
raise ValueError("low_z_min_radius must be >= 0")
if self._joint_command_max_speed <= 0.0:
raise ValueError("joint_max_speed must be > 0")
if self._joint_command_max_acceleration <= 0.0:
raise ValueError("joint_max_acc must be > 0")
def _shutdown_tool_worker(self) -> None:
if self._tool_worker_thread is None or self._tool_command_queue is None: