ZhengLiu-cart/acRealman_xr_from_Yikai
1
0
Fork 0
forked from YikaiFu-cart/acRealman_xr
Code Pull Requests Activity

Refactor and enhance XR-RM teleoperation functionality

Browse Source
This commit is contained in:
YikaiFu-cart
2026-05-31 20:06:07 +08:00
parent 3f48468f63
commit 948d50cab4
13 changed files with 409 additions and 354 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
xr_rm_teleop/setup.py
View File

@ -1,6 +1,6 @@
"""xr_rm_teleop 包安装配置。
该包提供基于 XR 相对位移的 RM75 笛卡尔速度遥操作节点。
该包提供基于 XR 相对位移的 RM75 笛卡尔位姿透传遥操作节点。
"""
from setuptools import setup
@ -19,7 +19,7 @@ setup(
zip_safe=True,
maintainer="Yikai Fu",
maintainer_email="user@example.com",
description="XR relative-motion teleoperation controllers for RealMan RM75 arms.",
description="XR relative-pose teleoperation controllers for RealMan RM75 arms.",
license="Apache-2.0",
tests_require=["pytest"],
entry_points={

2
xr_rm_teleop/xr_rm_teleop/__init__.py
View File

@ -1,4 +1,4 @@
"""XR 到 RM75 遥操作节点包。
核心模块包括单臂速度遥操作、RealMan SDK 适配层和夹爪 trigger 桥接。
核心模块包括单臂位姿透传遥操作、RealMan SDK 适配层和夹爪 trigger 桥接。
"""

91
xr_rm_teleop/xr_rm_teleop/realman_adapter.py
View File

@ -1,12 +1,11 @@
"""RM75 机械臂适配层。
对上提供统一的当前位姿读取、笛卡尔速度发送和停止接口；对下根据配置
对上提供统一的当前位姿读取、笛卡尔位姿目标发送和停止接口；对下根据配置
选择 mock 积分模拟器或睿尔曼 Python API2 真机通信。
"""
from __future__ import annotations
import time
from dataclasses import dataclass
from numbers import Number
from typing import Any
@ -40,16 +39,17 @@ class MockRealManAdapter:
def get_current_pose(self) -> ArmPose:
return self._pose
def send_cartesian_velocity(self, velocity: list[float], follow: bool) -> None:
def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
del follow
self.last_velocity = velocity
# 模拟模式只做简单积分，便于观察控制器是否在按预期更新末端位置。
self._pose.x += velocity[0] * self._dt
self._pose.y += velocity[1] * self._dt
self._pose.z += velocity[2] * self._dt
self._pose.rx += velocity[3] * self._dt
self._pose.ry += velocity[4] * self._dt
self._pose.rz += velocity[5] * self._dt
self.last_velocity = [
(pose.x - self._pose.x) / self._dt,
(pose.y - self._pose.y) / self._dt,
(pose.z - self._pose.z) / self._dt,
0.0,
0.0,
0.0,
]
self._pose = pose
def stop(self) -> None:
self.last_velocity = [0.0] * 6
@ -65,13 +65,12 @@ class MockRealManAdapter:
class RealManAdapter:
"""睿尔曼 Python API2 的笛卡尔速度透传适配层。"""
"""睿尔曼 Python API2 的笛卡尔位姿透传适配层。"""
def __init__(
self,
robot_ip: str,
robot_port: int,
dt: float,
avoid_singularity: int,
frame_type: int,
logger: Any | None = None,
@ -86,13 +85,11 @@ class RealManAdapter:
initial_joint_pose: list[float] | None = None,
initial_tcp_pose: list[float] | None = None,
init_move_speed: int = 20,
command_mode: str = "velocity",
canfd_trajectory_mode: int = 2,
canfd_radio: int = 0,
) -> None:
self._robot_ip = robot_ip
self._robot_port = robot_port
self._dt_ms = int(round(dt * 1000.0))
self._avoid_singularity = avoid_singularity
self._frame_type = frame_type
self._logger = logger
@ -107,13 +104,10 @@ class RealManAdapter:
self._initial_joint_pose = initial_joint_pose
self._initial_tcp_pose = initial_tcp_pose
self._init_move_speed = init_move_speed
self._command_mode = command_mode
self._canfd_trajectory_mode = canfd_trajectory_mode
self._canfd_radio = canfd_radio
self._scissorgripper: int | None = None
self._arm: Any | None = None
if self._command_mode not in ("velocity", "pose_canfd"):
raise ValueError("command_mode must be one of: velocity, pose_canfd")
def connect(self) -> None:
try:
@ -130,17 +124,12 @@ class RealManAdapter:
"RealMan connected: "
f"ip={self._robot_ip}, port={self._robot_port}, "
f"avoid_singularity={self._avoid_singularity}, "
f"frame_type={self._frame_type}, dt_ms={self._dt_ms}, "
f"command_mode={self._command_mode}"
f"frame_type={self._frame_type}, command=rm_movep_canfd"
)
if self._configure_safety_limits:
self._apply_safety_limits()
if self._move_to_initial_pose_on_connect:
self._move_to_initial_pose()
if self._command_mode == "velocity":
self._init_movev_canfd()
else:
self._log_info("command_mode=pose_canfd，跳过 rm_set_movev_canfd_init。")
def get_current_pose(self) -> ArmPose:
self._require_arm()
@ -150,17 +139,8 @@ class RealManAdapter:
raise RuntimeError(f"无法从睿尔曼状态中解析当前 TCP 位姿：{state!r}")
return ArmPose(*pose[:6])
def send_cartesian_velocity(self, velocity: list[float], follow: bool) -> None:
self._require_arm()
if self._command_mode == "pose_canfd":
return
ret = self._arm.rm_movev_canfd(velocity, follow, 0, 0)
self._check_return(ret, "rm_movev_canfd")
def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
self._require_arm()
if self._command_mode != "pose_canfd":
raise RuntimeError("send_cartesian_target requires command_mode=pose_canfd")
ret = self._arm.rm_movep_canfd(
[pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz],
follow,
@ -169,9 +149,6 @@ class RealManAdapter:
)
self._check_return(ret, "rm_movep_canfd")
def uses_pose_targets(self) -> bool:
return self._command_mode == "pose_canfd"
def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
self._require_arm()
from .fun_peripheral import load_peripheral_config, peripheral_cfg
@ -205,8 +182,6 @@ class RealManAdapter:
if self._arm is None:
return
try:
if self._command_mode == "velocity":
self._arm.rm_movev_canfd([0.0] * 6, False, 0, 0)
self._arm.rm_set_arm_slow_stop()
except Exception:
pass
@ -244,46 +219,6 @@ class RealManAdapter:
ret = self._arm.rm_movel(self._initial_tcp_pose, self._init_move_speed, 0, 0, 1)
self._check_return(ret, "rm_movel(initial_tcp_pose)")
def _init_movev_canfd(self) -> None:
attempts = 3
candidates = [self._avoid_singularity]
if self._avoid_singularity != 0:
candidates.append(0)
# 某些现场控制器初始化避奇异模式会超时，失败后自动降级到 0 再重试。
last_ret: Any = None
for avoid_singularity in candidates:
if avoid_singularity != self._avoid_singularity:
self._log_warn("rm_set_movev_canfd_init 降级为 avoid_singularity=0 后重试。")
for attempt in range(1, attempts + 1):
ret = self._arm.rm_set_movev_canfd_init(
avoid_singularity,
self._frame_type,
self._dt_ms,
)
last_ret = ret
code = self._return_code(ret)
if code == 0:
self._avoid_singularity = avoid_singularity
self._log_info(
"rm_set_movev_canfd_init 成功："
f"avoid_singularity={avoid_singularity}, "
f"frame_type={self._frame_type}, dt_ms={self._dt_ms}"
)
return
self._log_warn(
"rm_set_movev_canfd_init 失败："
f"avoid_singularity={avoid_singularity}, "
f"frame_type={self._frame_type}, dt_ms={self._dt_ms}, "
f"attempt={attempt}/{attempts}, ret={ret!r}"
)
if attempt < attempts:
time.sleep(0.2)
self._check_return(last_ret, "rm_set_movev_canfd_init")
def _try_call(self, name: str, *args: Any) -> None:
func = getattr(self._arm, name, None)
if func is None:

419
xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
View File

@ -1,7 +1,7 @@
"""RM75 单臂 XR 相对位移遥操作节点。
"""RM75 单臂 XR 相对位姿透传遥操作节点。
节点订阅左/右手柄位姿，在 grip 按下时锁定手柄和 TCP 起点，把手柄相对位移
映射到机器人坐标系中的目标 TCP，并通过笛卡尔速度命令跟随该目标。
映射成机器人坐标系中的目标 TCP，并通过 rm_movep_canfd 持续下发目标位姿。
"""
from __future__ import annotations
@ -12,6 +12,7 @@ from typing import Iterable
import rclpy
from geometry_msgs.msg import PoseStamped, TwistStamped
from rclpy.node import Node
from rclpy.time import Time
from std_msgs.msg import Bool
from xr_rm_interfaces.msg import XrController
@ -28,21 +29,24 @@ def _clamp(value: float, low: float, high: float) -> float:
class SingleArmVelocityTeleop(Node):
"""基于 XR 手柄相对位移的 RM75 单臂笛卡尔速度遥操节点。"""
"""基于 XR 手柄相对位移的 RM75 单臂位姿透传遥操节点。
类名和可执行入口沿用旧名称，避免已有 launch、UI 和命令失效。
"""
def __init__(self) -> None:
super().__init__("single_arm_velocity_teleop")
self.declare_parameter("arm_name", "rm75")
self.declare_parameter("controller_topic", "/xr/right_controller")
self.declare_parameter("control_rate_hz", 50.0)
self.declare_parameter("control_rate_hz", 90.0)
self.declare_parameter("command_timeout_sec", 0.12)
self.declare_parameter("scale", 1.0)
self.declare_parameter("kp_linear", 2.0)
self.declare_parameter("deadband_m", 0.002)
self.declare_parameter("low_pass_alpha", 0.35)
self.declare_parameter("velocity_zero_epsilon", 1e-6)
self.declare_parameter("max_linear_speed", 0.05)
self.declare_parameter("deadband_m", 0.001)
self.declare_parameter("target_filter_alpha", 0.65)
self.declare_parameter("target_filter_alpha_fast", 0.9)
self.declare_parameter("target_filter_fast_threshold_m", 0.03)
self.declare_parameter("max_linear_speed", 0.25)
self.declare_parameter("enable_position_axes", [True, True, True])
self.declare_parameter("workspace_min", [0.20, -0.35, 0.10])
self.declare_parameter("workspace_max", [0.65, 0.35, 0.60])
@ -50,6 +54,7 @@ class SingleArmVelocityTeleop(Node):
self.declare_parameter("low_z_threshold", 0.20)
self.declare_parameter("low_z_min_radius", 0.21)
self.declare_parameter("xr_to_robot_matrix", [0.0, 0.0, -1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0])
self.declare_parameter("current_pose_poll_hz", 10.0)
self.declare_parameter("use_mock", True)
self.declare_parameter("mock_initial_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0])
self.declare_parameter("robot_ip", "192.168.1.18")
@ -68,7 +73,6 @@ class SingleArmVelocityTeleop(Node):
self.declare_parameter("initial_joint_pose", [0.0] * 7)
self.declare_parameter("initial_tcp_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0])
self.declare_parameter("init_move_speed", 20)
self.declare_parameter("command_mode", "velocity")
self.declare_parameter("canfd_trajectory_mode", 2)
self.declare_parameter("canfd_radio", 0)
self.declare_parameter("enable_tool_control", True)
@ -86,10 +90,12 @@ class SingleArmVelocityTeleop(Node):
self._dt = 1.0 / control_rate_hz
self._command_timeout_sec = float(self.get_parameter("command_timeout_sec").value)
self._scale = float(self.get_parameter("scale").value)
self._kp_linear = float(self.get_parameter("kp_linear").value)
self._deadband_m = float(self.get_parameter("deadband_m").value)
self._low_pass_alpha = float(self.get_parameter("low_pass_alpha").value)
self._velocity_zero_epsilon = float(self.get_parameter("velocity_zero_epsilon").value)
self._target_filter_alpha = float(self.get_parameter("target_filter_alpha").value)
self._target_filter_alpha_fast = float(self.get_parameter("target_filter_alpha_fast").value)
self._target_filter_fast_threshold_m = float(
self.get_parameter("target_filter_fast_threshold_m").value
)
self._max_linear_speed = float(self.get_parameter("max_linear_speed").value)
self._enable_position_axes = self._bool_list_parameter("enable_position_axes", 3)
self._workspace_min = self._float_list_parameter("workspace_min", 3)
@ -98,6 +104,7 @@ class SingleArmVelocityTeleop(Node):
self._low_z_threshold = float(self.get_parameter("low_z_threshold").value)
self._low_z_min_radius = float(self.get_parameter("low_z_min_radius").value)
self._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9)
self._current_pose_poll_hz = float(self.get_parameter("current_pose_poll_hz").value)
self._follow = self._bool_parameter("follow")
self._debug_topic_prefix = str(self.get_parameter("debug_topic_prefix").value).rstrip("/")
if not self._debug_topic_prefix:
@ -105,11 +112,16 @@ class SingleArmVelocityTeleop(Node):
self._validate_parameters()
self._last_msg: XrController | None = None
self._last_msg_time = None
self._last_msg_time: Time | None = None
self._active = False
self._controller_start: list[float] | None = None
self._robot_start: list[float] | None = None
self._filtered_velocity = [0.0, 0.0, 0.0]
self._robot_start_pose: ArmPose | None = None
self._filtered_target: list[float] | None = None
self._last_sent_target: list[float] | None = None
self._last_command_time: Time | None = None
self._last_current_pose: ArmPose | None = None
self._last_current_pose_time: Time | None = None
self._stop_sent = True
self._adapter = self._make_adapter()
self._adapter.connect()
@ -117,15 +129,20 @@ class SingleArmVelocityTeleop(Node):
debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
self._current_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/current_pose", 10)
self._raw_target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/raw_target_pose", 10)
self._target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/target_pose", 10)
self._cmd_vel_pub = self.create_publisher(TwistStamped, f"{debug_ns}/cmd_vel", 10)
self._target_clamped_pub = self.create_publisher(Bool, f"{debug_ns}/target_clamped", 10)
self.create_subscription(XrController, topic, self._on_controller, 10)
self.create_timer(self._dt, self._control_tick)
self.get_logger().info(f"{self._arm_name} 速度遥操节点已启动，监听话题：{topic}")
self.get_logger().info(
f"{self._arm_name} 位姿透传遥操节点已启动，监听话题：{topic}, "
f"dt={self._dt:.4f}s, follow={self._follow}"
)
def _make_adapter(self):
# mock 和真机共享同一控制逻辑，只在适配层切换运动命令的执行方式。
# mock 和真机共享同一位姿目标链路，只在适配层切换执行方式。
if self._bool_parameter("use_mock"):
return MockRealManAdapter(
[float(v) for v in self.get_parameter("mock_initial_pose").value],
@ -135,7 +152,6 @@ class SingleArmVelocityTeleop(Node):
return RealManAdapter(
robot_ip=self.get_parameter("robot_ip").value,
robot_port=int(self.get_parameter("robot_port").value),
dt=self._dt,
avoid_singularity=int(self.get_parameter("avoid_singularity").value),
frame_type=int(self.get_parameter("frame_type").value),
logger=self.get_logger(),
@ -150,7 +166,6 @@ class SingleArmVelocityTeleop(Node):
initial_joint_pose=self._float_list_parameter("initial_joint_pose", 7),
initial_tcp_pose=self._float_list_parameter("initial_tcp_pose", 6),
init_move_speed=int(self.get_parameter("init_move_speed").value),
command_mode=str(self.get_parameter("command_mode").value),
canfd_trajectory_mode=int(self.get_parameter("canfd_trajectory_mode").value),
canfd_radio=int(self.get_parameter("canfd_radio").value),
)
@ -196,98 +211,100 @@ class SingleArmVelocityTeleop(Node):
self._last_msg_time = self.get_clock().now()
def _control_tick(self) -> None:
now = self.get_clock().now()
if self._last_msg is None or self._last_msg_time is None:
self._safe_stop()
self._safe_stop(reset_active=True)
return
age = (self.get_clock().now() - self._last_msg_time).nanoseconds * 1e-9
age = (now - self._last_msg_time).nanoseconds * 1e-9
if age > self._command_timeout_sec:
self.get_logger().warn(f"{self._arm_name} XR 手柄数据超时，机械臂停止。", throttle_duration_sec=1.0)
self._safe_stop()
self._active = False
if self._active or not self._stop_sent:
self.get_logger().warn(
f"{self._arm_name} XR 手柄数据超时，机械臂停止。",
throttle_duration_sec=1.0,
)
self._safe_stop(reset_active=True)
self._last_msg = None
self._last_msg_time = None
return
if not self._last_msg.grip:
if self._active:
self.get_logger().info(f"{self._arm_name} Grip 松开，退出相对位移遥操。")
self._safe_stop()
self._active = False
self._controller_start = None
self._robot_start = None
self.get_logger().info(f"{self._arm_name} Grip 松开，退出相对位姿遥操。")
self._safe_stop(reset_active=True)
return
controller_now = self._controller_xyz(self._last_msg)
if not self._active:
self._enter_active_control(controller_now, now)
return
assert self._controller_start is not None
assert self._robot_start_pose is not None
self._maybe_refresh_current_pose(now)
raw_target = self._raw_target_from_controller(controller_now)
workspace_target, workspace_clamped = self._clamp_workspace_with_flag(raw_target)
desired_target = self._apply_deadband(workspace_target)
filtered_target = self._filter_target(desired_target)
sent_target, step_limited = self._limit_target_step(filtered_target)
sent_target, final_clamped = self._clamp_workspace_with_flag(sent_target)
velocity = self._estimate_command_velocity(sent_target, now)
target_clamped = workspace_clamped or step_limited or final_clamped
target_pose = ArmPose(
x=sent_target[0],
y=sent_target[1],
z=sent_target[2],
rx=self._robot_start_pose.rx,
ry=self._robot_start_pose.ry,
rz=self._robot_start_pose.rz,
)
self._publish_debug(raw_target, sent_target, velocity, target_clamped)
if self._send_cartesian_target(target_pose):
self._last_sent_target = sent_target
self._last_command_time = now
self._stop_sent = False
def _enter_active_control(self, controller_now: list[float], now: Time) -> None:
try:
robot_pose = self._adapter.get_current_pose()
robot_now = robot_pose.xyz()
robot_pose = self._read_current_pose_for_control(now)
except Exception as exc:
self.get_logger().error(
f"{self._arm_name} 读取 TCP 位姿失败，停止输出：{exc}",
throttle_duration_sec=1.0,
)
self._safe_stop()
self._active = False
self._safe_stop(reset_active=True)
return
if not self._active:
# 握持键按下的第一帧只锁定手柄和 TCP 起点，不立即运动，避免启停跳变。
self._active = True
self._controller_start = controller_now
self._robot_start = robot_now
self._filtered_velocity = [0.0, 0.0, 0.0]
self.get_logger().info(f"{self._arm_name} Grip 按下，已锁定手柄和机械臂初始位姿。")
self._safe_stop()
return
assert self._controller_start is not None
assert self._robot_start is not None
# 核心控制：手柄相对位移 -> 机器人坐标系相对位移 -> 目标 TCP -> 速度命令。
controller_delta = [controller_now[i] - self._controller_start[i] for i in range(3)]
robot_delta = self._map_xr_delta_to_robot(controller_delta)
target = [self._robot_start[i] + self._scale * robot_delta[i] for i in range(3)]
target = self._clamp_workspace(target)
error = [target[i] - robot_now[i] for i in range(3)]
error = [error[i] if self._enable_position_axes[i] else 0.0 for i in range(3)]
if _norm(error) < self._deadband_m:
velocity = [0.0, 0.0, 0.0]
else:
velocity = [self._kp_linear * value for value in error]
velocity = self._clamp_vector_norm(velocity, self._max_linear_speed)
# 低通滤波可以削弱 XR 追踪抖动，真实机械臂测试时不要关掉。
alpha = self._low_pass_alpha
self._filtered_velocity = [
alpha * velocity[i] + (1.0 - alpha) * self._filtered_velocity[i]
for i in range(3)
]
self._filtered_velocity = self._zero_tiny_velocity(self._filtered_velocity)
cartesian_velocity = self._filtered_velocity + [0.0, 0.0, 0.0]
self._publish_debug(robot_pose, target, cartesian_velocity)
if self._adapter_uses_pose_targets():
step_target = [
robot_now[i] + cartesian_velocity[i] * self._dt
for i in range(3)
]
step_target = self._clamp_workspace(step_target)
self._send_cartesian_target(
ArmPose(
x=step_target[0],
y=step_target[1],
z=step_target[2],
rx=robot_pose.rx,
ry=robot_pose.ry,
rz=robot_pose.rz,
)
)
else:
self._send_cartesian_velocity(cartesian_velocity)
robot_xyz = robot_pose.xyz()
self._active = True
self._controller_start = controller_now
self._robot_start_pose = robot_pose
self._filtered_target = robot_xyz
self._last_sent_target = robot_xyz
self._last_command_time = now
self._stop_sent = True
self.get_logger().info(f"{self._arm_name} Grip 按下，已锁定手柄和机械臂初始位姿。")
self._publish_debug(robot_xyz, robot_xyz, [0.0] * 6, False)
@staticmethod
def _controller_xyz(msg: XrController) -> list[float]:
return [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]
def _raw_target_from_controller(self, controller_now: list[float]) -> list[float]:
assert self._controller_start is not None
assert self._robot_start_pose is not None
controller_delta = [controller_now[i] - self._controller_start[i] for i in range(3)]
robot_delta = self._map_xr_delta_to_robot(controller_delta)
target = [
self._robot_start_pose.x + self._scale * robot_delta[0],
self._robot_start_pose.y + self._scale * robot_delta[1],
self._robot_start_pose.z + self._scale * robot_delta[2],
]
return [target[i] if self._enable_position_axes[i] else self._robot_start_pose.xyz()[i] for i in range(3)]
def _map_xr_delta_to_robot(self, delta: list[float]) -> list[float]:
matrix = self._xr_to_robot_matrix
return [
@ -296,13 +313,61 @@ class SingleArmVelocityTeleop(Node):
matrix[6] * delta[0] + matrix[7] * delta[1] + matrix[8] * delta[2],
]
def _clamp_workspace(self, target: list[float]) -> list[float]:
# 先做盒状工作空间限幅，再叠加底座圆柱半径限制。
def _apply_deadband(self, target: list[float]) -> list[float]:
if self._deadband_m <= 0.0 or self._last_sent_target is None:
return target
delta = [target[i] - self._last_sent_target[i] for i in range(3)]
if _norm(delta) < self._deadband_m:
return list(self._last_sent_target)
return target
def _filter_target(self, target: list[float]) -> list[float]:
if self._filtered_target is None:
self._filtered_target = list(target)
return list(target)
delta = [target[i] - self._filtered_target[i] for i in range(3)]
distance = _norm(delta)
alpha = self._adaptive_filter_alpha(distance)
self._filtered_target = [
alpha * target[i] + (1.0 - alpha) * self._filtered_target[i]
for i in range(3)
]
return list(self._filtered_target)
def _adaptive_filter_alpha(self, distance: float) -> float:
if self._target_filter_fast_threshold_m <= 1e-9:
return self._target_filter_alpha_fast
ratio = _clamp(distance / self._target_filter_fast_threshold_m, 0.0, 1.0)
return (
self._target_filter_alpha
+ (self._target_filter_alpha_fast - self._target_filter_alpha) * ratio
)
def _limit_target_step(self, target: list[float]) -> tuple[list[float], bool]:
if self._last_sent_target is None:
return target, False
delta = [target[i] - self._last_sent_target[i] for i in range(3)]
distance = _norm(delta)
max_step = self._max_linear_speed * self._dt
if distance <= max_step or distance <= 1e-9:
return target, False
scale = max_step / distance
return [
self._last_sent_target[i] + delta[i] * scale
for i in range(3)
], True
def _clamp_workspace_with_flag(self, target: list[float]) -> tuple[list[float], bool]:
clamped = [
_clamp(target[i], self._workspace_min[i], self._workspace_max[i])
for i in range(3)
]
return self._clamp_cylinder_radius(clamped)
clamped = self._clamp_cylinder_radius(clamped)
was_clamped = any(abs(clamped[i] - target[i]) > 1e-9 for i in range(3))
return clamped, was_clamped
def _clamp_cylinder_radius(self, target: list[float]) -> list[float]:
min_radius, max_radius = self._cyl_radius_limit
@ -325,40 +390,80 @@ class SingleArmVelocityTeleop(Node):
return target
@staticmethod
def _clamp_vector_norm(vector: list[float], max_norm: float) -> list[float]:
norm = _norm(vector)
if norm <= max_norm or norm <= 1e-9:
return vector
scale = max_norm / norm
return [value * scale for value in vector]
def _estimate_command_velocity(self, target: list[float], now: Time) -> list[float]:
if self._last_sent_target is None:
return [0.0] * 6
def _safe_stop(self) -> None:
# 所有断连、松手、退出路径都走这里，确保发送零速度。
self._filtered_velocity = [0.0, 0.0, 0.0]
self._publish_stop_debug()
if self._adapter_uses_pose_targets():
dt = self._dt
if self._last_command_time is not None:
measured_dt = (now - self._last_command_time).nanoseconds * 1e-9
if measured_dt > 1e-6:
dt = measured_dt
return [
(target[i] - self._last_sent_target[i]) / dt
for i in range(3)
] + [0.0, 0.0, 0.0]
def _read_current_pose_for_control(self, now: Time) -> ArmPose:
pose = self._adapter.get_current_pose()
self._last_current_pose = pose
self._last_current_pose_time = now
return pose
def _maybe_refresh_current_pose(self, now: Time) -> None:
if self._current_pose_poll_hz <= 0.0:
return
self._send_cartesian_velocity([0.0] * 6)
if self._last_current_pose_time is not None:
age = (now - self._last_current_pose_time).nanoseconds * 1e-9
if age < 1.0 / self._current_pose_poll_hz:
return
def _publish_stop_debug(self) -> None:
try:
current_pose = self._adapter.get_current_pose()
except Exception:
return
self._publish_debug(current_pose, current_pose.xyz(), [0.0] * 6)
def _send_cartesian_velocity(self, velocity: list[float]) -> None:
try:
self._adapter.send_cartesian_velocity(velocity, self._follow)
self._read_current_pose_for_control(now)
except Exception as exc:
self.get_logger().error(
f"{self._arm_name} 发送速度命令失败：{exc}",
self.get_logger().warn(
f"{self._arm_name} 低频读取 TCP 位姿失败，继续透传目标：{exc}",
throttle_duration_sec=1.0,
)
self._active = False
def _send_cartesian_target(self, pose: ArmPose) -> None:
def _safe_stop(self, reset_active: bool) -> None:
if not self._stop_sent:
self._send_stop_once()
if reset_active:
self._active = False
self._controller_start = None
self._robot_start_pose = None
self._filtered_target = None
self._last_sent_target = None
self._last_command_time = None
self._publish_stop_debug()
def _send_stop_once(self) -> None:
try:
self._adapter.stop()
except Exception as exc:
self.get_logger().error(
f"{self._arm_name} 停止机械臂失败：{exc}",
throttle_duration_sec=1.0,
)
self._stop_sent = True
def _publish_stop_debug(self) -> None:
pose = self._debug_pose_fallback()
if pose is None:
return
self._publish_debug(pose.xyz(), pose.xyz(), [0.0] * 6, False)
def _debug_pose_fallback(self) -> ArmPose | None:
if self._last_current_pose is not None:
return self._last_current_pose
if self._robot_start_pose is not None:
return self._robot_start_pose
if self._last_sent_target is not None:
return ArmPose(*self._last_sent_target)
return None
def _send_cartesian_target(self, pose: ArmPose) -> bool:
try:
self._adapter.send_cartesian_target(pose, self._follow)
except Exception as exc:
@ -367,22 +472,33 @@ class SingleArmVelocityTeleop(Node):
throttle_duration_sec=1.0,
)
self._active = False
self._send_stop_once()
return False
return True
def _adapter_uses_pose_targets(self) -> bool:
uses_pose_targets = getattr(self._adapter, "uses_pose_targets", None)
return bool(uses_pose_targets is not None and uses_pose_targets())
def _zero_tiny_velocity(self, velocity: list[float]) -> list[float]:
if self._velocity_zero_epsilon <= 0.0:
return velocity
return [
0.0 if abs(value) < self._velocity_zero_epsilon else value
for value in velocity
]
def _publish_debug(self, current_pose: ArmPose, target_xyz: list[float], velocity: list[float]) -> None:
def _publish_debug(
self,
raw_target_xyz: list[float],
target_xyz: list[float],
command_velocity: list[float],
target_clamped: bool,
) -> None:
stamp = self.get_clock().now().to_msg()
current_msg = self._pose_msg(stamp, current_pose)
current_pose = self._debug_pose_fallback()
if current_pose is None:
current_pose = ArmPose(*target_xyz)
raw_pose = self._pose_msg(
stamp,
ArmPose(
x=raw_target_xyz[0],
y=raw_target_xyz[1],
z=raw_target_xyz[2],
rx=current_pose.rx,
ry=current_pose.ry,
rz=current_pose.rz,
),
)
target_msg = self._pose_msg(
stamp,
ArmPose(
@ -394,20 +510,24 @@ class SingleArmVelocityTeleop(Node):
rz=current_pose.rz,
),
)
velocity = self._zero_tiny_velocity(velocity)
velocity_msg = TwistStamped()
velocity_msg.header.stamp = stamp
velocity_msg.header.frame_id = "rm_base"
velocity_msg.twist.linear.x = float(velocity[0])
velocity_msg.twist.linear.y = float(velocity[1])
velocity_msg.twist.linear.z = float(velocity[2])
velocity_msg.twist.angular.x = float(velocity[3])
velocity_msg.twist.angular.y = float(velocity[4])
velocity_msg.twist.angular.z = float(velocity[5])
velocity_msg.twist.linear.x = float(command_velocity[0])
velocity_msg.twist.linear.y = float(command_velocity[1])
velocity_msg.twist.linear.z = float(command_velocity[2])
velocity_msg.twist.angular.x = float(command_velocity[3])
velocity_msg.twist.angular.y = float(command_velocity[4])
velocity_msg.twist.angular.z = float(command_velocity[5])
self._current_pose_pub.publish(current_msg)
clamped_msg = Bool()
clamped_msg.data = bool(target_clamped)
self._current_pose_pub.publish(self._pose_msg(stamp, current_pose))
self._raw_target_pose_pub.publish(raw_pose)
self._target_pose_pub.publish(target_msg)
self._cmd_vel_pub.publish(velocity_msg)
self._target_clamped_pub.publish(clamped_msg)
@staticmethod
def _pose_msg(stamp, pose: ArmPose) -> PoseStamped:
@ -467,12 +587,18 @@ class SingleArmVelocityTeleop(Node):
raise ValueError("command_timeout_sec must be > 0")
if self._deadband_m < 0.0:
raise ValueError("deadband_m must be >= 0")
if not 0.0 <= self._low_pass_alpha <= 1.0:
raise ValueError("low_pass_alpha must be in [0, 1]")
if self._velocity_zero_epsilon < 0.0:
raise ValueError("velocity_zero_epsilon must be >= 0")
if self._max_linear_speed < 0.0:
raise ValueError("max_linear_speed must be >= 0")
if not 0.0 <= self._target_filter_alpha <= 1.0:
raise ValueError("target_filter_alpha must be in [0, 1]")
if not 0.0 <= self._target_filter_alpha_fast <= 1.0:
raise ValueError("target_filter_alpha_fast must be in [0, 1]")
if self._target_filter_alpha_fast < self._target_filter_alpha:
raise ValueError("target_filter_alpha_fast must be >= target_filter_alpha")
if self._target_filter_fast_threshold_m < 0.0:
raise ValueError("target_filter_fast_threshold_m must be >= 0")
if self._max_linear_speed <= 0.0:
raise ValueError("max_linear_speed must be > 0")
if self._current_pose_poll_hz < 0.0:
raise ValueError("current_pose_poll_hz must be >= 0")
for axis, (low, high) in enumerate(zip(self._workspace_min, self._workspace_max)):
if low >= high:
raise ValueError(f"workspace_min[{axis}] must be smaller than workspace_max[{axis}]")
@ -498,7 +624,8 @@ def main(args: list[str] | None = None) -> None:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if rclpy.ok():
rclpy.shutdown()
if __name__ == "__main__":