feat: Implement RM75 joint feedback and fault recovery design
This commit is contained in:
@@ -0,0 +1,964 @@
|
||||
# RM75 关节反馈与故障恢复实施计划
|
||||
|
||||
> **执行要求：** 使用 `superpowers:executing-plans` 按任务逐项实施。只有用户明确授权 subagent 后，才允许使用 `superpowers:subagent-driven-development`、独立 worktree 或本地分支。所有步骤使用复选框跟踪。
|
||||
|
||||
**目标：** 启动时用 `rm_get_joint_degree()` 初始化 RM75 QP；运行时以 UDP `joint_position` 作为实际反馈；短暂丢包时保持最后安全目标；持续丢包或 CANFD 错误时安全同步、停止或等待人工重新使能。
|
||||
|
||||
**实现方式：** 继续复用现有唯一 `RealManAdapter` 连接，只增加一个同步读取关节角的方法。恢复决策仍放在 `SingleArmVelocityTeleop`，用少量布尔状态复用现有 slow-stop、Grip 重新使能和关节限速逻辑，不新增状态机类、线程、连接或依赖。
|
||||
|
||||
**技术栈：** Python 3.10、ROS2 Humble `rclpy`、睿尔曼 Python API2、Placo、pytest、ament/colcon。
|
||||
|
||||
**设计文档：** `docs/superpowers/specs/2026-07-29-rm75-feedback-recovery-design.md`
|
||||
|
||||
**厂商接口依据：**
|
||||
|
||||
- `rm_get_joint_degree() -> tuple[int, list[float]]`：<https://develop.realman-robotics.com/robot/apipython/classes/armState/>
|
||||
- `rm_movej_canfd(..., follow=False, ...)` 为低跟随：<https://develop.realman-robotics.com/robot/apipython/classes/movePlan/>
|
||||
|
||||
## 仓库约束
|
||||
|
||||
- 所有构建、测试和启动命令均在工作空间根目录 `/home/robot/WS_xr` 执行。
|
||||
- 所有 Git 命令均在仓库根目录 `/home/robot/WS_xr/src` 执行。
|
||||
- 每次 ROS2 构建、测试或启动前先执行 `source /opt/ros/humble/setup.bash`。
|
||||
- 不自动提交、推送、创建分支或 worktree；只有用户明确要求后才执行。
|
||||
- 验证只通过 `xr_rm_bringup/launch/arm_debug.launch.py use_mock:=true`。
|
||||
- 不连接真机，不发送真实 CANFD，不移动机械臂，不操作夹爪或末端外设。
|
||||
- 不修改依赖、锁文件、CI、格式化配置、公开入口和无关代码。
|
||||
|
||||
## 文件范围
|
||||
|
||||
- 修改 `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
|
||||
- 增加同步关节查询。
|
||||
- 复用 UDP 与同步查询的角度校验。
|
||||
- 修改 `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
||||
- 启动同步、UDP保持/恢复、CANFD恢复和故障锁存。
|
||||
- 修改 `src/xr_rm_teleop/test/test_initial_joint_pose.py`
|
||||
- 适配器同步查询测试。
|
||||
- 修改 `src/xr_rm_teleop/test/test_joint_control.py`
|
||||
- 启动、超时、恢复和锁存测试。
|
||||
- 同步修改：
|
||||
- `src/xr_rm_bringup/config/dual_arm_rm75.yaml`
|
||||
- `src/xr_rm_bringup/config/left_arm_rm75.yaml`
|
||||
- `src/xr_rm_bringup/config/right_arm_rm75.yaml`
|
||||
|
||||
---
|
||||
|
||||
## 任务一：给现有适配器增加同步关节查询
|
||||
|
||||
**修改文件：**
|
||||
|
||||
- `src/xr_rm_teleop/test/test_initial_joint_pose.py`
|
||||
- `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
|
||||
|
||||
- [x] **步骤1：先写失败测试**
|
||||
|
||||
在 `test_initial_joint_pose.py` 增加：
|
||||
|
||||
```python
|
||||
def test_joint_degree_query_returns_validated_radians() -> 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]
|
||||
|
||||
adapter = RealManAdapter(
|
||||
"127.0.0.1",
|
||||
8080,
|
||||
0,
|
||||
"127.0.0.1",
|
||||
8090,
|
||||
)
|
||||
adapter._arm = FakeArm()
|
||||
|
||||
snapshot = adapter.read_joint_state()
|
||||
|
||||
assert snapshot.positions == pytest.approx(
|
||||
[math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
|
||||
)
|
||||
assert snapshot.read_duration_ms is not None
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"result",
|
||||
[
|
||||
(7, [0.0] * 7),
|
||||
(0, [0.0] * 6),
|
||||
(0, [0.0, 0.0, 0.0, math.nan, 0.0, 0.0, 0.0]),
|
||||
],
|
||||
)
|
||||
def test_joint_degree_query_rejects_sdk_errors_and_invalid_values(result) -> None:
|
||||
adapter = RealManAdapter(
|
||||
"127.0.0.1",
|
||||
8080,
|
||||
0,
|
||||
"127.0.0.1",
|
||||
8090,
|
||||
)
|
||||
adapter._arm = SimpleNamespace(rm_get_joint_degree=lambda: result)
|
||||
|
||||
with pytest.raises((RuntimeError, ValueError)):
|
||||
adapter.read_joint_state()
|
||||
|
||||
|
||||
def test_mock_joint_query_uses_current_mock_positions() -> None:
|
||||
adapter = MockRealManAdapter([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])
|
||||
|
||||
snapshot = adapter.read_joint_state()
|
||||
|
||||
assert snapshot.positions == pytest.approx(
|
||||
[math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
|
||||
)
|
||||
```
|
||||
|
||||
把现有 `test_invalid_udp_feedback_does_not_replace_snapshot` 的最终断言改为：
|
||||
|
||||
```python
|
||||
after = adapter.get_latest_joint_state()
|
||||
assert after is not None
|
||||
assert before is not None
|
||||
assert after.positions == before.positions
|
||||
assert after.received_at == before.received_at
|
||||
assert after.motion_ready is False
|
||||
```
|
||||
|
||||
该断言要求无效 UDP 帧保留最后已知角度，但立即禁止这些角度继续参与运动。
|
||||
|
||||
- [x] **步骤2：运行测试并确认 RED**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py \
|
||||
-k 'joint_degree_query or mock_joint_query or invalid_udp_feedback' -v
|
||||
```
|
||||
|
||||
预期：测试失败；原因是适配器还没有 `read_joint_state()`，且无效 UDP 帧仍被标记为可运动。
|
||||
|
||||
- [x] **步骤3：实现最小同步查询**
|
||||
|
||||
给 `MockRealManAdapter` 增加：
|
||||
|
||||
```python
|
||||
def read_joint_state(self) -> JointStateSnapshot:
|
||||
return self.get_latest_joint_state()
|
||||
```
|
||||
|
||||
给 `RealManAdapter` 增加：
|
||||
|
||||
```python
|
||||
def read_joint_state(self) -> JointStateSnapshot:
|
||||
self._require_arm()
|
||||
started_at = time.monotonic()
|
||||
result = self._arm.rm_get_joint_degree()
|
||||
finished_at = time.monotonic()
|
||||
if not isinstance(result, tuple) or len(result) != 2:
|
||||
raise RuntimeError(
|
||||
f"rm_get_joint_degree returned invalid result: {result!r}"
|
||||
)
|
||||
self._check_return(result, "rm_get_joint_degree")
|
||||
return JointStateSnapshot(
|
||||
self._joint_positions_from_degrees(
|
||||
result[1],
|
||||
"rm_get_joint_degree",
|
||||
),
|
||||
finished_at,
|
||||
(finished_at - started_at) * 1000.0,
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def _joint_positions_from_degrees(
|
||||
values: Any,
|
||||
source: str,
|
||||
) -> list[float]:
|
||||
try:
|
||||
degrees = list(values)
|
||||
except TypeError as exc:
|
||||
raise ValueError(f"{source} must contain 7 numeric joints") from exc
|
||||
if len(degrees) != 7 or not all(
|
||||
isinstance(value, Number) for value in degrees
|
||||
):
|
||||
raise ValueError(f"{source} 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(f"{source} contains NaN/Inf")
|
||||
return positions
|
||||
```
|
||||
|
||||
把 UDP 回调中重复的角度转换替换为：
|
||||
|
||||
```python
|
||||
positions = self._joint_positions_from_degrees(
|
||||
data.joint_status.joint_position,
|
||||
"RM75 UDP feedback",
|
||||
)
|
||||
```
|
||||
|
||||
在 UDP 回调的异常分支中，保留最后角度但标记为不可运动：
|
||||
|
||||
```python
|
||||
with self._joint_state_lock:
|
||||
if self._latest_joint_state is not None:
|
||||
current = self._latest_joint_state
|
||||
self._latest_joint_state = JointStateSnapshot(
|
||||
list(current.positions),
|
||||
current.received_at,
|
||||
current.read_duration_ms,
|
||||
current.update_interval_ms,
|
||||
False,
|
||||
)
|
||||
```
|
||||
|
||||
- [x] **步骤4：运行适配器测试并确认 GREEN**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -v
|
||||
```
|
||||
|
||||
预期：该文件全部测试通过。
|
||||
|
||||
- [x] **步骤5：检查本任务差异**
|
||||
|
||||
```bash
|
||||
git diff --check
|
||||
git diff -- \
|
||||
xr_rm_teleop/xr_rm_teleop/realman_adapter.py \
|
||||
xr_rm_teleop/test/test_initial_joint_pose.py
|
||||
```
|
||||
|
||||
预期：只有同步查询、共用角度校验、无效反馈安全标记及对应测试。
|
||||
|
||||
---
|
||||
|
||||
## 任务二：用启动查询结果初始化 QP 和关节命令历史
|
||||
|
||||
**修改文件：**
|
||||
|
||||
- `src/xr_rm_teleop/test/test_joint_control.py`
|
||||
- `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
||||
|
||||
- [x] **步骤1：先写启动同步失败测试**
|
||||
|
||||
增加：
|
||||
|
||||
```python
|
||||
def test_startup_joint_query_initializes_qp_and_command_history() -> None:
|
||||
positions = [0.1] * 7
|
||||
pose = np.eye(4)
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._arm_name = "right_rm75"
|
||||
teleop._adapter = SimpleNamespace(
|
||||
read_joint_state=lambda: JointStateSnapshot(
|
||||
positions,
|
||||
time.monotonic(),
|
||||
)
|
||||
)
|
||||
teleop._ik_solver = SimpleNamespace(
|
||||
update_joint_state=lambda joints: pose
|
||||
)
|
||||
teleop.get_logger = lambda: FakeLogger()
|
||||
|
||||
teleop._initialize_joint_state()
|
||||
|
||||
assert teleop._latest_joint_positions == positions
|
||||
assert teleop._last_valid_joint_target == positions
|
||||
assert teleop._last_joint_command_target == positions
|
||||
assert teleop._last_joint_command_velocity == [0.0] * 7
|
||||
assert teleop._last_current_pose is pose
|
||||
|
||||
|
||||
def test_startup_joint_query_failure_closes_adapter() -> None:
|
||||
class FailingAdapter:
|
||||
def __init__(self):
|
||||
self.close_calls = 0
|
||||
|
||||
def read_joint_state(self):
|
||||
raise RuntimeError("rm_get_joint_degree failed with code 7")
|
||||
|
||||
def close(self):
|
||||
self.close_calls += 1
|
||||
|
||||
errors = []
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._arm_name = "left_rm75"
|
||||
teleop._adapter = FailingAdapter()
|
||||
teleop.get_logger = lambda: SimpleNamespace(
|
||||
error=lambda message: errors.append(message)
|
||||
)
|
||||
|
||||
with pytest.raises(RuntimeError, match="code 7"):
|
||||
teleop._initialize_joint_state()
|
||||
|
||||
assert teleop._adapter.close_calls == 1
|
||||
assert "left_rm75" in errors[0]
|
||||
assert "启动关节同步失败" in errors[0]
|
||||
```
|
||||
|
||||
- [x] **步骤2：运行测试并确认 RED**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
|
||||
-k 'startup_joint_query' -v
|
||||
```
|
||||
|
||||
预期：测试因 `_initialize_joint_state()` 尚不存在而失败。
|
||||
|
||||
- [x] **步骤3：增加启动同步**
|
||||
|
||||
增加：
|
||||
|
||||
```python
|
||||
def _initialize_joint_state(self) -> None:
|
||||
try:
|
||||
self._reset_joint_state(self._adapter.read_joint_state())
|
||||
except Exception as exc:
|
||||
self.get_logger().error(
|
||||
f"{self._arm_name} 启动关节同步失败：{exc}"
|
||||
)
|
||||
self._adapter.close()
|
||||
raise
|
||||
|
||||
def _reset_joint_state(
|
||||
self,
|
||||
snapshot: JointStateSnapshot,
|
||||
) -> np.ndarray:
|
||||
current_pose = self._ik_solver.update_joint_state(snapshot.positions)
|
||||
positions = list(snapshot.positions)
|
||||
self._latest_joint_positions = positions
|
||||
self._last_current_pose = current_pose
|
||||
self._last_valid_joint_target = list(positions)
|
||||
self._last_joint_command_target = list(positions)
|
||||
self._last_joint_command_velocity = [0.0] * 7
|
||||
return current_pose
|
||||
```
|
||||
|
||||
在现有适配器连接之后、外设初始化之前调用：
|
||||
|
||||
```python
|
||||
self._adapter = self._make_adapter()
|
||||
self._adapter.connect()
|
||||
self._initialize_joint_state()
|
||||
self._setup_tool_control()
|
||||
```
|
||||
|
||||
同步查询结果不得写入 `RealManAdapter._latest_joint_state`；该缓存继续只代表 UDP 反馈。
|
||||
|
||||
- [x] **步骤4：运行启动与适配器测试**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
|
||||
-k 'startup_joint_query or first_feedback' -v
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -v
|
||||
```
|
||||
|
||||
预期：所选控制测试和全部适配器测试通过。
|
||||
|
||||
- [x] **步骤5：检查本任务差异**
|
||||
|
||||
```bash
|
||||
git diff --check
|
||||
git diff -- \
|
||||
xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \
|
||||
xr_rm_teleop/test/test_joint_control.py
|
||||
```
|
||||
|
||||
预期：启动阶段只增加一次同步读取，并初始化现有 QP/关节命令字段。
|
||||
|
||||
---
|
||||
|
||||
## 任务三：UDP 短暂超时保持，持续超时重新同步
|
||||
|
||||
**修改文件：**
|
||||
|
||||
- `src/xr_rm_teleop/test/test_joint_control.py`
|
||||
- `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
||||
|
||||
- [x] **步骤1：先写超时行为测试**
|
||||
|
||||
在测试文件增加最小构造器：
|
||||
|
||||
```python
|
||||
def _timeout_teleop(adapter) -> SingleArmVelocityTeleop:
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._adapter = adapter
|
||||
teleop._arm_name = "right_rm75"
|
||||
teleop._follow = False
|
||||
teleop._active = True
|
||||
teleop._joint_feedback_ready = True
|
||||
teleop._grip_rearm_required = False
|
||||
teleop._feedback_resync_attempted = False
|
||||
teleop._control_fault_latched = False
|
||||
teleop._last_joint_command_target = [0.1] * 7
|
||||
teleop._last_joint_command_velocity = [0.0] * 7
|
||||
teleop._latest_joint_positions = [0.1] * 7
|
||||
teleop._last_valid_joint_target = [0.1] * 7
|
||||
teleop._last_current_pose = np.eye(4)
|
||||
teleop._controller_start = None
|
||||
teleop._controller_orientation_start = None
|
||||
teleop._robot_start_transform = None
|
||||
teleop._filtered_target = None
|
||||
teleop._filtered_orientation_target = None
|
||||
teleop._last_sent_target = None
|
||||
teleop._last_sent_orientation = None
|
||||
teleop._last_command_time = None
|
||||
teleop._ik_solver = SimpleNamespace(
|
||||
update_joint_state=lambda joints: np.eye(4)
|
||||
)
|
||||
teleop._stop_sent = False
|
||||
teleop._feedback_resync_timeout_sec = 0.5
|
||||
teleop._publish_stop_debug = lambda: None
|
||||
teleop.get_logger = lambda: FakeLogger()
|
||||
return teleop
|
||||
```
|
||||
|
||||
增加：
|
||||
|
||||
```python
|
||||
def test_missing_or_disabled_joint_snapshot_is_not_motion_ready() -> None:
|
||||
assert not SingleArmVelocityTeleop._joint_snapshot_is_motion_ready(None)
|
||||
assert not SingleArmVelocityTeleop._joint_snapshot_is_motion_ready(
|
||||
JointStateSnapshot(
|
||||
[0.0] * 7,
|
||||
time.monotonic(),
|
||||
motion_ready=False,
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
def test_short_udp_timeout_repeats_last_limited_target_without_query() -> None:
|
||||
sends = []
|
||||
adapter = SimpleNamespace(
|
||||
send_joint_target=lambda joints, follow: sends.append(
|
||||
(list(joints), follow)
|
||||
),
|
||||
read_joint_state=lambda: pytest.fail("query must not run"),
|
||||
stop=lambda: pytest.fail("stop must not run"),
|
||||
)
|
||||
teleop = _timeout_teleop(adapter)
|
||||
|
||||
teleop._handle_stale_joint_feedback(0.2)
|
||||
|
||||
assert sends == [([0.1] * 7, False)]
|
||||
assert teleop._last_joint_command_target == [0.1] * 7
|
||||
assert teleop._grip_rearm_required
|
||||
|
||||
|
||||
def test_short_udp_timeout_without_active_target_stays_stopped() -> None:
|
||||
stop_calls = []
|
||||
adapter = SimpleNamespace(
|
||||
send_joint_target=lambda joints, follow: pytest.fail(
|
||||
"inactive control must not start CANFD output"
|
||||
),
|
||||
read_joint_state=lambda: pytest.fail("query must not run"),
|
||||
stop=lambda: stop_calls.append(True),
|
||||
)
|
||||
teleop = _timeout_teleop(adapter)
|
||||
teleop._active = False
|
||||
|
||||
teleop._handle_stale_joint_feedback(0.2)
|
||||
|
||||
assert len(stop_calls) == 1
|
||||
|
||||
|
||||
def test_persistent_udp_timeout_queries_once_and_holds_actual_position() -> None:
|
||||
sends = []
|
||||
query_calls = []
|
||||
adapter = SimpleNamespace(
|
||||
send_joint_target=lambda joints, follow: sends.append(list(joints)),
|
||||
read_joint_state=lambda: (
|
||||
query_calls.append(True)
|
||||
or JointStateSnapshot([0.2] * 7, time.monotonic())
|
||||
),
|
||||
stop=lambda: None,
|
||||
)
|
||||
teleop = _timeout_teleop(adapter)
|
||||
|
||||
teleop._handle_stale_joint_feedback(0.5)
|
||||
teleop._handle_stale_joint_feedback(0.6)
|
||||
|
||||
assert len(query_calls) == 1
|
||||
assert sends == [[0.2] * 7, [0.2] * 7]
|
||||
assert teleop._last_valid_joint_target == [0.2] * 7
|
||||
assert teleop._last_joint_command_velocity == [0.0] * 7
|
||||
|
||||
|
||||
def test_persistent_udp_timeout_query_failure_latches_control() -> None:
|
||||
stop_calls = []
|
||||
adapter = SimpleNamespace(
|
||||
send_joint_target=lambda joints, follow: pytest.fail(
|
||||
"CANFD must stop after query failure"
|
||||
),
|
||||
read_joint_state=lambda: (_ for _ in ()).throw(
|
||||
RuntimeError("rm_get_joint_degree failed with code 7")
|
||||
),
|
||||
stop=lambda: stop_calls.append(True),
|
||||
)
|
||||
teleop = _timeout_teleop(adapter)
|
||||
|
||||
teleop._handle_stale_joint_feedback(0.5)
|
||||
teleop._handle_stale_joint_feedback(0.6)
|
||||
|
||||
assert teleop._control_fault_latched
|
||||
assert len(stop_calls) == 1
|
||||
```
|
||||
|
||||
删除旧的 `_fresh_joint_state()` 直接测试，并用
|
||||
`test_short_udp_timeout_without_active_target_stays_stopped` 替换旧的
|
||||
`test_stale_feedback_stops_before_active_control`。
|
||||
|
||||
在 `test_feedback_fault_blocks_grip_until_release` 中补齐：
|
||||
|
||||
```python
|
||||
teleop._control_fault_latched = False
|
||||
teleop._feedback_resync_attempted = False
|
||||
```
|
||||
|
||||
- [x] **步骤2：运行测试并确认 RED**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
|
||||
-k 'udp_timeout or joint_snapshot' -v
|
||||
```
|
||||
|
||||
预期：测试因超时处理和锁存状态尚不存在而失败。
|
||||
|
||||
- [x] **步骤3：增加参数与最小状态**
|
||||
|
||||
参数默认值：
|
||||
|
||||
```python
|
||||
self.declare_parameter("control_rate_hz", 90.0)
|
||||
self.declare_parameter("command_timeout_sec", 0.12)
|
||||
self.declare_parameter("feedback_resync_timeout_sec", 0.5)
|
||||
```
|
||||
|
||||
读取并初始化：
|
||||
|
||||
```python
|
||||
self._feedback_resync_timeout_sec = float(
|
||||
self.get_parameter("feedback_resync_timeout_sec").value
|
||||
)
|
||||
self._feedback_resync_attempted = False
|
||||
self._control_fault_latched = False
|
||||
```
|
||||
|
||||
在 `_validate_parameters()` 中增加：
|
||||
|
||||
```python
|
||||
if self._feedback_resync_timeout_sec <= self._command_timeout_sec:
|
||||
raise ValueError(
|
||||
"feedback_resync_timeout_sec must be greater than command_timeout_sec"
|
||||
)
|
||||
```
|
||||
|
||||
- [x] **步骤4：增加保持、重新同步和锁存逻辑**
|
||||
|
||||
增加：
|
||||
|
||||
```python
|
||||
def _handle_stale_joint_feedback(self, age: float) -> None:
|
||||
if self._control_fault_latched:
|
||||
return
|
||||
self._grip_rearm_required = True
|
||||
if self._joint_feedback_ready:
|
||||
self.get_logger().warn(
|
||||
f"{self._arm_name} UDP关节反馈超时，保持最后安全目标。"
|
||||
)
|
||||
self._joint_feedback_ready = False
|
||||
|
||||
if (
|
||||
age >= self._feedback_resync_timeout_sec
|
||||
and not self._feedback_resync_attempted
|
||||
):
|
||||
self._feedback_resync_attempted = True
|
||||
self.get_logger().warn(
|
||||
f"{self._arm_name} UDP关节反馈持续超时，"
|
||||
"尝试rm_get_joint_degree重新同步。"
|
||||
)
|
||||
try:
|
||||
self._reset_joint_state(self._adapter.read_joint_state())
|
||||
except Exception as exc:
|
||||
self._latch_control_fault(
|
||||
f"UDP关节反馈持续超时且重新同步失败：{exc}"
|
||||
)
|
||||
return
|
||||
self.get_logger().info(
|
||||
f"{self._arm_name} 已通过rm_get_joint_degree重新同步，"
|
||||
"继续保持并等待UDP恢复。"
|
||||
)
|
||||
|
||||
if self._active and self._last_joint_command_target is not None:
|
||||
self._repeat_last_joint_target()
|
||||
else:
|
||||
self._safe_stop(reset_active=True)
|
||||
|
||||
def _repeat_last_joint_target(self) -> None:
|
||||
target = self._last_joint_command_target
|
||||
if target is None:
|
||||
return
|
||||
self._adapter.send_joint_target(list(target), self._follow)
|
||||
self._stop_sent = False
|
||||
|
||||
def _latch_control_fault(self, message: str) -> None:
|
||||
if self._control_fault_latched:
|
||||
return
|
||||
self._control_fault_latched = True
|
||||
self._grip_rearm_required = True
|
||||
self.get_logger().error(
|
||||
f"{self._arm_name} 控制故障已锁存：{message}"
|
||||
)
|
||||
self._safe_stop(reset_active=True)
|
||||
```
|
||||
|
||||
- [x] **步骤5：在 QP 之前处理反馈状态**
|
||||
|
||||
在 `_control_tick()` 开头用以下逻辑替换现有 `_fresh_joint_state()` 分支：
|
||||
|
||||
```python
|
||||
if self._control_fault_latched:
|
||||
return
|
||||
|
||||
snapshot = self._adapter.get_latest_joint_state()
|
||||
if not self._joint_snapshot_is_motion_ready(snapshot):
|
||||
self._grip_rearm_required = True
|
||||
if self._joint_feedback_ready:
|
||||
self.get_logger().warn(
|
||||
f"{self._arm_name} 关节反馈无效或机械臂未就绪，机械臂停止。"
|
||||
)
|
||||
self._joint_feedback_ready = False
|
||||
self._safe_stop(reset_active=True)
|
||||
return
|
||||
|
||||
feedback_age = time.monotonic() - snapshot.received_at
|
||||
if feedback_age < 0.0:
|
||||
self._grip_rearm_required = True
|
||||
self._joint_feedback_ready = False
|
||||
self._safe_stop(reset_active=True)
|
||||
return
|
||||
if feedback_age > self._command_timeout_sec:
|
||||
self._handle_stale_joint_feedback(feedback_age)
|
||||
return
|
||||
|
||||
self._feedback_resync_attempted = False
|
||||
```
|
||||
|
||||
在成功执行 `_sync_joint_feedback(snapshot)` 后，用以下逻辑替换现有首次反馈日志：
|
||||
|
||||
```python
|
||||
if not self._joint_feedback_ready:
|
||||
if self._grip_rearm_required:
|
||||
message = (
|
||||
f"{self._arm_name} UDP关节反馈已恢复，"
|
||||
"等待Grip松开后重新使能。"
|
||||
)
|
||||
else:
|
||||
message = (
|
||||
f"{self._arm_name} 已收到首帧有效关节反馈，QP可以启用。"
|
||||
)
|
||||
self.get_logger().info(message)
|
||||
self._joint_feedback_ready = True
|
||||
```
|
||||
|
||||
用以下静态校验替换 `_fresh_joint_state()`：
|
||||
|
||||
```python
|
||||
@staticmethod
|
||||
def _joint_snapshot_is_motion_ready(
|
||||
snapshot: JointStateSnapshot | None,
|
||||
) -> bool:
|
||||
return (
|
||||
snapshot is not None
|
||||
and len(snapshot.positions) == 7
|
||||
and all(math.isfinite(value) for value in snapshot.positions)
|
||||
and snapshot.motion_ready
|
||||
)
|
||||
```
|
||||
|
||||
XR手柄消息超时、Grip逻辑、工作空间/圆柱限位、姿态限速和关节限速保持原样。
|
||||
|
||||
- [x] **步骤6：运行超时及反馈安全测试**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
|
||||
-k 'udp_timeout or feedback_fault or joint_snapshot' -v
|
||||
```
|
||||
|
||||
预期：所选测试通过；短暂超时不调用 QP 和同步查询，机械臂未就绪仍立即停止。
|
||||
|
||||
- [x] **步骤7：检查本任务差异**
|
||||
|
||||
```bash
|
||||
git diff --check
|
||||
git diff --stat
|
||||
```
|
||||
|
||||
预期：没有新增状态机类、线程、依赖、连接或常态轮询。
|
||||
|
||||
---
|
||||
|
||||
## 任务四：CANFD 错误后停止、查询并等待人工恢复
|
||||
|
||||
**修改文件：**
|
||||
|
||||
- `src/xr_rm_teleop/test/test_joint_control.py`
|
||||
- `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
||||
|
||||
- [x] **步骤1：先写 CANFD 恢复测试**
|
||||
|
||||
用以下测试替换旧的 `test_joint_send_failure_requests_slow_stop_and_resets_control`：
|
||||
|
||||
```python
|
||||
def test_canfd_error_stops_queries_and_requires_grip_rearm() -> None:
|
||||
class RecoveringAdapter:
|
||||
def __init__(self):
|
||||
self.stop_calls = 0
|
||||
self.read_calls = 0
|
||||
|
||||
def send_joint_target(self, joints, follow):
|
||||
raise RuntimeError("rm_movej_canfd failed with code 9")
|
||||
|
||||
def stop(self):
|
||||
self.stop_calls += 1
|
||||
|
||||
def read_joint_state(self):
|
||||
self.read_calls += 1
|
||||
return JointStateSnapshot([0.2] * 7, time.monotonic())
|
||||
|
||||
teleop = _timeout_teleop(RecoveringAdapter())
|
||||
teleop._joint_command_max_speed = math.radians(180.0)
|
||||
teleop._joint_command_max_acceleration = math.radians(300.0)
|
||||
teleop._dt = 1.0 / 90.0
|
||||
|
||||
sent = teleop._send_joint_target([0.3] * 7)
|
||||
|
||||
assert not sent
|
||||
assert teleop._adapter.stop_calls == 1
|
||||
assert teleop._adapter.read_calls == 1
|
||||
assert not teleop._control_fault_latched
|
||||
assert teleop._grip_rearm_required
|
||||
assert teleop._last_joint_command_target == [0.2] * 7
|
||||
|
||||
|
||||
def test_canfd_error_latches_when_joint_query_also_fails() -> None:
|
||||
class FailingAdapter:
|
||||
def __init__(self):
|
||||
self.stop_calls = 0
|
||||
|
||||
def send_joint_target(self, joints, follow):
|
||||
raise RuntimeError("rm_movej_canfd failed with code 9")
|
||||
|
||||
def stop(self):
|
||||
self.stop_calls += 1
|
||||
|
||||
def read_joint_state(self):
|
||||
raise RuntimeError("rm_get_joint_degree failed with code 7")
|
||||
|
||||
teleop = _timeout_teleop(FailingAdapter())
|
||||
teleop._joint_command_max_speed = math.radians(180.0)
|
||||
teleop._joint_command_max_acceleration = math.radians(300.0)
|
||||
teleop._dt = 1.0 / 90.0
|
||||
|
||||
assert not teleop._send_joint_target([0.3] * 7)
|
||||
assert teleop._control_fault_latched
|
||||
assert teleop._adapter.stop_calls == 1
|
||||
```
|
||||
|
||||
- [x] **步骤2：运行测试并确认 RED**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
|
||||
-k 'canfd_error' -v
|
||||
```
|
||||
|
||||
预期：测试失败；当前发送错误只会 slow-stop，不会查询实际关节角或锁存查询失败。
|
||||
|
||||
- [x] **步骤3：增加统一 CANFD 恢复路径**
|
||||
|
||||
增加：
|
||||
|
||||
```python
|
||||
def _recover_from_canfd_error(self, send_error: Exception) -> None:
|
||||
self.get_logger().error(
|
||||
f"{self._arm_name} rm_movej_canfd发送失败：{send_error}"
|
||||
)
|
||||
self._grip_rearm_required = True
|
||||
self._send_stop_once()
|
||||
self._safe_stop(reset_active=True)
|
||||
try:
|
||||
snapshot = self._adapter.read_joint_state()
|
||||
self._reset_joint_state(snapshot)
|
||||
except Exception as query_error:
|
||||
self._latch_control_fault(
|
||||
"CANFD错误后关节同步失败："
|
||||
f"send={send_error}; query={query_error}"
|
||||
)
|
||||
return
|
||||
self.get_logger().info(
|
||||
f"{self._arm_name} CANFD错误后已同步实际关节角，"
|
||||
"等待UDP恢复及Grip重新使能。"
|
||||
)
|
||||
```
|
||||
|
||||
把 `_send_joint_target()` 的异常分支替换为：
|
||||
|
||||
```python
|
||||
except Exception as exc:
|
||||
self._recover_from_canfd_error(exc)
|
||||
return False
|
||||
```
|
||||
|
||||
让短暂超时重发也走相同错误恢复：
|
||||
|
||||
```python
|
||||
def _repeat_last_joint_target(self) -> None:
|
||||
target = self._last_joint_command_target
|
||||
if target is None:
|
||||
return
|
||||
try:
|
||||
self._adapter.send_joint_target(list(target), self._follow)
|
||||
self._stop_sent = False
|
||||
except Exception as exc:
|
||||
self._recover_from_canfd_error(exc)
|
||||
```
|
||||
|
||||
- [x] **步骤4：运行全部关节控制测试**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -v
|
||||
```
|
||||
|
||||
预期：全部关节控制测试通过。
|
||||
|
||||
- [x] **步骤5：检查日志与差异**
|
||||
|
||||
```bash
|
||||
rg -n "rm_movej_canfd发送失败|控制故障已锁存|rm_get_joint_degree" \
|
||||
src/xr_rm_teleop/xr_rm_teleop
|
||||
git diff --check
|
||||
```
|
||||
|
||||
预期：
|
||||
|
||||
- CANFD错误和查询错误都包含机械臂名称及失败阶段。
|
||||
- 锁存分支不会每周期重复打印错误。
|
||||
- 正常QP输出和超时保持使用同一个CANFD恢复入口。
|
||||
|
||||
---
|
||||
|
||||
## 任务五：同步90 Hz配置并完成mock验证
|
||||
|
||||
**修改文件：**
|
||||
|
||||
- `src/xr_rm_bringup/config/dual_arm_rm75.yaml`
|
||||
- `src/xr_rm_bringup/config/left_arm_rm75.yaml`
|
||||
- `src/xr_rm_bringup/config/right_arm_rm75.yaml`
|
||||
|
||||
- [x] **步骤1：只修改请求中的控制参数**
|
||||
|
||||
三份配置的每个机械臂条目统一为：
|
||||
|
||||
```yaml
|
||||
control_rate_hz: 90.0
|
||||
command_timeout_sec: 0.12
|
||||
feedback_resync_timeout_sec: 0.5
|
||||
```
|
||||
|
||||
保留低跟随：
|
||||
|
||||
```yaml
|
||||
follow: false
|
||||
```
|
||||
|
||||
不得修改：
|
||||
|
||||
- 工作空间与圆柱限位。
|
||||
- TCP线速度、角速度及关节速度/加速度限制。
|
||||
- `configure_safety_limits: true`。
|
||||
- `move_to_initial_pose_on_connect: false`。
|
||||
- 机械臂IP、端口、初始位姿和末端工具配置。
|
||||
- 双臂节点名 `left_arm_teleop`、`right_arm_teleop`。
|
||||
|
||||
- [x] **步骤2：机械检查三份配置**
|
||||
|
||||
```bash
|
||||
rg -n "control_rate_hz|command_timeout_sec|feedback_resync_timeout_sec|follow:" \
|
||||
src/xr_rm_bringup/config/dual_arm_rm75.yaml \
|
||||
src/xr_rm_bringup/config/left_arm_rm75.yaml \
|
||||
src/xr_rm_bringup/config/right_arm_rm75.yaml
|
||||
```
|
||||
|
||||
预期：共四个机械臂配置条目，每个条目均为90.0、0.12、0.5和`follow: false`；三份文件不再出现125.0。
|
||||
|
||||
- [x] **步骤3：运行相关测试**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -v
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -v
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_orientation_control.py -v
|
||||
```
|
||||
|
||||
预期：三个命令均以0退出且无失败。
|
||||
|
||||
- [x] **步骤4：构建ROS2工作空间**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
colcon build --symlink-install
|
||||
```
|
||||
|
||||
预期：`xr_rm_interfaces`、`xr_rm_input`、`xr_rm_teleop`、`xr_rm_bringup` 构建成功。
|
||||
|
||||
- [x] **步骤5：通过统一入口进行mock启动验证**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
if timeout --signal=INT 10s ros2 launch xr_rm_bringup arm_debug.launch.py \
|
||||
arm:=right use_mock:=true udp_port:=15123
|
||||
then
|
||||
true
|
||||
else
|
||||
launch_status=$?
|
||||
test "$launch_status" -eq 124
|
||||
fi
|
||||
```
|
||||
|
||||
预期：
|
||||
|
||||
- `udp_controller_receiver` 与 `single_arm_velocity_teleop` 正常启动。
|
||||
- 遥操作节点报告90 Hz、低跟随，并完成mock关节状态初始化。
|
||||
- 不导入厂商SDK，不建立RealMan连接，不发送CANFD，不移动机械臂，不操作夹爪。
|
||||
- 10秒后由`timeout`结束；仅该超时允许退出码124。
|
||||
|
||||
- [x] **步骤6：最终范围与安全审计**
|
||||
|
||||
```bash
|
||||
git diff --check
|
||||
git status --short
|
||||
git diff --stat
|
||||
git diff -- \
|
||||
xr_rm_teleop/xr_rm_teleop/realman_adapter.py \
|
||||
xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \
|
||||
xr_rm_teleop/test/test_initial_joint_pose.py \
|
||||
xr_rm_teleop/test/test_joint_control.py \
|
||||
xr_rm_bringup/config/dual_arm_rm75.yaml \
|
||||
xr_rm_bringup/config/left_arm_rm75.yaml \
|
||||
xr_rm_bringup/config/right_arm_rm75.yaml
|
||||
```
|
||||
|
||||
逐项确认：
|
||||
|
||||
- 没有无关文件或格式化改动。
|
||||
- 没有提交、推送、分支、worktree、锁文件、CI、格式化规则或依赖变化。
|
||||
- 没有新增线程、ROS包、launch入口、并发RealMan连接或常态SDK轮询。
|
||||
- mock模式不导入、不依赖厂商SDK。
|
||||
- `configure_safety_limits` 默认仍为开启。
|
||||
- `move_to_initial_pose_on_connect` 默认仍为关闭。
|
||||
- `left_arm_teleop`、`right_arm_teleop` 节点名不变。
|
||||
- 工作空间/圆柱限位、TCP与关节限速、XR命令超时和slow-stop逻辑仍保留。
|
||||
- 验证期间未连接真机、移动机械臂或操作夹爪。
|
||||
@@ -0,0 +1,151 @@
|
||||
# RM75 关节反馈与故障恢复设计
|
||||
|
||||
## 目标
|
||||
|
||||
将当前 RM75 QP 遥操作链路调整为：
|
||||
|
||||
- 启动时使用 `rm_get_joint_degree()` 获取实际关节角并初始化 QP。
|
||||
- 运行时只把 RealMan UDP `joint_position` 作为连续实际关节反馈。
|
||||
- UDP 短暂超时时保持最后一次已限速的安全关节目标，不生成新运动。
|
||||
- UDP 持续超时或 CANFD 发送错误时，按明确的同步、停止和人工恢复流程处理。
|
||||
- `rm_movej_canfd()` 保持低跟随，控制频率使用 `xr_rm_teleop` 分支的 90 Hz。
|
||||
|
||||
不改变现有工作空间/圆柱限位、TCP与关节速度和加速度限制、XR命令超时、安全停止、外设控制及双臂节点名。
|
||||
|
||||
## 控制数据源
|
||||
|
||||
启动初始化与运行反馈使用不同的数据源：
|
||||
|
||||
1. `RealManAdapter` 建立现有唯一厂商连接。
|
||||
2. 节点同步调用一次 `rm_get_joint_degree()`。
|
||||
3. 查询成功后把7个角度转换为弧度，用于初始化 Placo QP、最后安全目标和关节限速历史。
|
||||
4. 查询失败时关闭适配器、打印错误并使节点启动失败，不发送 CANFD。
|
||||
5. 正常运行后，QP 的连续实际状态只来自已校验且运动状态正常的 UDP `joint_position`。
|
||||
|
||||
同步查询只用于启动、持续反馈超时恢复和 CANFD 错误恢复，不新增连接，不进行常态轮询。
|
||||
|
||||
## 状态与转换
|
||||
|
||||
### 正常运行
|
||||
|
||||
控制定时器以 90 Hz 执行。每个周期读取最新 UDP 关节快照，同步 QP，执行现有目标生成、安全限位、单步 QP、关节速度/加速度限制，然后调用：
|
||||
|
||||
```text
|
||||
rm_movej_canfd(target_degrees, follow=false, ...)
|
||||
```
|
||||
|
||||
只有已经通过关节限速并成功发送的目标才能成为“最后安全目标”。
|
||||
|
||||
### UDP 短暂超时
|
||||
|
||||
UDP 快照年龄超过现有 `command_timeout_sec=0.12` 秒、但未达到 `feedback_resync_timeout_sec=0.5` 秒时：
|
||||
|
||||
- 不使用过期反馈同步 QP。
|
||||
- 不运行目标生成和 QP。
|
||||
- 不更新任何目标、滤波器或限速历史。
|
||||
- 若超时前正在遥操作且已有成功发送的安全目标，以 90 Hz 原样重发该目标。
|
||||
- 若超时前未在遥操作或没有成功发送的目标，保持停止，不开始 CANFD 输出。
|
||||
- 首次进入时打印节流后的警告。
|
||||
|
||||
机械臂报警、关节掉使能、关节错误或非有限关节值不是普通超时，仍立即执行安全停止。
|
||||
|
||||
### UDP 持续超时
|
||||
|
||||
UDP 快照年龄达到 0.5 秒时，每次中断只同步调用一次 `rm_get_joint_degree()`：
|
||||
|
||||
- 查询成功：用实际角度重置 QP、最后安全目标和关节限速历史；不生成新运动，继续保持并等待 UDP 恢复。
|
||||
- 查询失败：调用 slow-stop，停止 CANFD，进入锁存故障并打印错误。
|
||||
|
||||
同一次中断不会反复查询。收到新的有效 UDP 反馈后，查询标志才复位。
|
||||
|
||||
### UDP 恢复
|
||||
|
||||
UDP 恢复后先持续同步实际关节状态，但不能直接恢复运动：
|
||||
|
||||
1. 当前 Grip 必须松开。
|
||||
2. 节点清除重新使能要求。
|
||||
3. 操作者再次按下 Grip，节点以新的手柄和机械臂实际位姿建立相对控制起点。
|
||||
|
||||
### CANFD 错误
|
||||
|
||||
`rm_movej_canfd()` 返回错误或抛出异常时：
|
||||
|
||||
1. 立即停止后续 CANFD 发送。
|
||||
2. 调用 slow-stop。
|
||||
3. 打印包含机械臂名称、命令名称和原始错误的终端错误日志。
|
||||
4. 调用 `rm_get_joint_degree()` 查询实际关节角。
|
||||
5. 查询成功时重置 QP和关节命令历史，但不再发送保持命令；等待有效 UDP 和 Grip 松开后重新按下。
|
||||
6. 查询失败时进入锁存故障并打印查询错误。
|
||||
|
||||
### 锁存故障
|
||||
|
||||
锁存故障只作用于发生错误的机械臂节点：
|
||||
|
||||
- 控制定时器不再查询、运行 QP或发送 CANFD。
|
||||
- slow-stop 只发送一次。
|
||||
- 后续 UDP 恢复或 Grip 操作不能自动解锁。
|
||||
- 终端保留明确错误信息，但不在每个周期重复刷屏。
|
||||
- 操作者检查后必须重启对应遥操作节点才能恢复。
|
||||
|
||||
## 代码边界
|
||||
|
||||
### `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
|
||||
|
||||
- 给真实与 mock 适配器增加同步关节角查询能力。
|
||||
- 复用现有厂商连接。
|
||||
- 校验返回码、数量和有限值，统一返回弧度。
|
||||
- 保留 UDP 回调作为运行时快照来源。
|
||||
|
||||
### `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
||||
|
||||
- 启动时查询并初始化 QP。
|
||||
- 增加 `feedback_resync_timeout_sec` 参数，默认 0.5 秒。
|
||||
- 校验 `feedback_resync_timeout_sec > command_timeout_sec > 0`。
|
||||
- 在现有控制周期内加入保持、一次性重新同步、等待 Grip 重使能和锁存判断。
|
||||
- 复用现有 `_safe_stop()`、Grip 重使能和关节限速逻辑，不新增状态机类。
|
||||
|
||||
### 配置
|
||||
|
||||
以下配置的 `control_rate_hz` 从 125 Hz 改为 90 Hz，并增加相同的 0.5 秒持续超时参数：
|
||||
|
||||
- `xr_rm_bringup/config/dual_arm_rm75.yaml`
|
||||
- `xr_rm_bringup/config/left_arm_rm75.yaml`
|
||||
- `xr_rm_bringup/config/right_arm_rm75.yaml`
|
||||
|
||||
三份配置继续使用 `follow: false`，双臂节点名保持 `left_arm_teleop` 和 `right_arm_teleop`。
|
||||
|
||||
## 错误日志
|
||||
|
||||
以下转换必须写入 ROS2 终端日志：
|
||||
|
||||
- 启动关节查询失败：`error`。
|
||||
- 首次进入 UDP 短暂超时：`warn`。
|
||||
- 持续超时查询开始及成功：`warn`/`info`。
|
||||
- 持续超时查询失败并锁存：`error`。
|
||||
- CANFD 发送失败：`error`。
|
||||
- CANFD 后关节查询失败并锁存：`error`。
|
||||
- UDP 恢复并等待 Grip 人工重使能：`info`。
|
||||
|
||||
日志包含机械臂名称和失败阶段；周期性路径使用状态转换或节流避免刷屏。
|
||||
|
||||
## 测试与验证
|
||||
|
||||
使用现有 mock 和单元测试完成，不连接真机：
|
||||
|
||||
1. 适配器正确解析 `rm_get_joint_degree()` 成功结果，并拒绝错误码、错误数量和 NaN/Inf。
|
||||
2. 启动查询结果初始化 QP 和安全目标；查询失败时节点不能进入控制。
|
||||
3. 0.12～0.5 秒反馈超时期间不调用 QP，只重发同一安全目标。
|
||||
4. 0.5 秒持续超时只查询一次；成功后等待 UDP 与 Grip，失败后锁存。
|
||||
5. CANFD 错误后停止发送并查询；查询成功要求 Grip 重使能，查询失败锁存。
|
||||
6. 机械臂报警或掉使能仍立即停止，不能进入保持路径。
|
||||
7. 三份配置均使用 90 Hz、0.5 秒持续超时和低跟随。
|
||||
|
||||
在工作空间根目录 `/home/robot/WS_xr` 执行：
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
pytest src/xr_rm_teleop/test/test_initial_joint_pose.py
|
||||
pytest src/xr_rm_teleop/test/test_joint_control.py
|
||||
pytest src/xr_rm_teleop/test/test_orientation_control.py
|
||||
colcon build --symlink-install
|
||||
```
|
||||
@@ -0,0 +1,192 @@
|
||||
# RM75 QP 收敛与低跟随稳定性优化设计
|
||||
|
||||
## 背景
|
||||
|
||||
右臂真机以90 Hz、`follow: false`运行时，用户观察到：
|
||||
|
||||
- 手柄移动约10 cm后，`target_pose`很快稳定；
|
||||
- `current_pose`仍需约3秒缓慢追赶；
|
||||
- 运动过程中机械臂存在肉眼可见的轻微晃动。
|
||||
|
||||
现场 timing 日志同时表明：
|
||||
|
||||
- 控制周期约11.111 ms；
|
||||
- 控制回调平均约2.6 ms，最大约6.0 ms；
|
||||
- QP平均约0.39 ms；
|
||||
- CANFD发送平均约0.21 ms；
|
||||
- UDP实际关节反馈平均约25 ms一帧，即约40 Hz。
|
||||
|
||||
因此，控制线程、QP单次计算和CANFD调用本身没有耗尽90 Hz周期；慢速发生在
|
||||
`target_pose`生成之后。
|
||||
|
||||
## 根因
|
||||
|
||||
当前 `PlacoIkSolver.solve()` 每次只调用一次：
|
||||
|
||||
```python
|
||||
self._solver.solve(True)
|
||||
```
|
||||
|
||||
该调用把一次QP增量应用为 `q + Δq`。与此同时，90 Hz控制循环每次都会先用
|
||||
最新实际关节反馈重置Placo模型。由于实际反馈约40 Hz，同一帧反馈通常会被重复
|
||||
使用两到三次。
|
||||
|
||||
结果是每次下发的关节目标只位于实际关节角前方一小步，而不是当前TCP目标对应的
|
||||
收敛关节解。低跟随控制器持续追逐这个短距离移动点，表现为：
|
||||
|
||||
- 对稳定TCP目标呈缓慢的渐近追赶；
|
||||
- 实际反馈每约25 ms更新一次时，关节目标随反馈发生台阶式修正；
|
||||
- 低跟随内部平滑与台阶式关节目标叠加，形成轻微晃动。
|
||||
|
||||
本地RM75模型对照结果支持该判断：从右臂初始姿态求解7 cm平移目标时，单次QP
|
||||
只产生约7.6 mm TCP位移；在同一次逆解中连续迭代30次后，目标误差可降至接近
|
||||
零，计算耗时约3.56 ms。
|
||||
|
||||
## 目标
|
||||
|
||||
保持现有安全基线并实现：
|
||||
|
||||
- 手柄移动10 cm后，机械臂约1秒内稳定到位；
|
||||
- 运动和到位后无持续肉眼可见晃动；
|
||||
- 控制频率保持90 Hz；
|
||||
- `rm_movej_canfd()`保持低跟随；
|
||||
- 运行时仍以UDP `joint_position`作为实际关节反馈；
|
||||
- 保留工作空间、圆柱、TCP速度、姿态速度、关节速度与关节加速度限制；
|
||||
- 保留反馈超时、CANFD错误恢复、Grip重新使能和安全停止逻辑。
|
||||
|
||||
## 不在本次范围
|
||||
|
||||
- 不启用高跟随；
|
||||
- 不提高TCP或关节安全上限；
|
||||
- 不修改XR手柄滤波和坐标映射；
|
||||
- 不修改UDP反馈周期或增加反馈预测器；
|
||||
- 不新增线程、RealMan连接、依赖或状态机；
|
||||
- 不处理双臂碰撞检测。
|
||||
|
||||
## 方案比较
|
||||
|
||||
### 方案一：有限次数迭代QP
|
||||
|
||||
每个控制周期仍从实际关节角开始，但在一次 `solve()` 调用内部迭代QP，直到TCP
|
||||
目标收敛或达到固定迭代上限。得到的完整关节目标继续经过现有关节速度与加速度
|
||||
限幅后才发送。
|
||||
|
||||
优点：
|
||||
|
||||
- 直接修复单步QP只生成近距离移动点的根因；
|
||||
- 不需要预测状态，不会在反馈中断时继续外推；
|
||||
- 复用现有限速、错误回退和CANFD发送路径；
|
||||
- 本地测量表明计算量可放入90 Hz周期。
|
||||
|
||||
缺点：
|
||||
|
||||
- 单周期QP耗时会高于当前单步求解；
|
||||
- 不可达目标需要明确的未收敛处理。
|
||||
|
||||
### 方案二：反馈帧之间维护预测关节状态
|
||||
|
||||
仅在新UDP反馈到达时校正模型，其余90 Hz周期从上一条关节命令继续积分QP。
|
||||
|
||||
优点：
|
||||
|
||||
- 每周期仍只求解一次QP；
|
||||
- 可避免同一反馈帧反复重置模型。
|
||||
|
||||
缺点：
|
||||
|
||||
- 引入预测状态、反馈校正和漂移处理；
|
||||
- 反馈与预测偏差可能在校正时产生新的关节跳动；
|
||||
- 超时与恢复逻辑需要同时管理实际状态和预测状态。
|
||||
|
||||
### 方案三：只调整滤波、速度或高跟随参数
|
||||
|
||||
`target_pose`已经快速稳定，继续提高 `max_linear_speed` 或减小目标滤波不能解决
|
||||
下游渐近追赶。启用高跟随则违反本次低跟随约束。
|
||||
|
||||
## 决策
|
||||
|
||||
采用方案一。它在不引入预测状态的情况下直接修复根因，改动范围只涉及Placo
|
||||
求解器及其测试。
|
||||
|
||||
## 控制数据流
|
||||
|
||||
正常运行时的数据流调整为：
|
||||
|
||||
```text
|
||||
UDP实际关节反馈
|
||||
→ 更新Placo实际关节状态和current_pose
|
||||
→ 现有XR相对位姿、工作空间、圆柱、滤波和TCP限速
|
||||
→ 有限次数迭代QP，得到收敛关节目标
|
||||
→ 现有关节速度与加速度限幅
|
||||
→ rm_movej_canfd(..., follow=false)
|
||||
```
|
||||
|
||||
反馈短暂超时仍只以90 Hz重发最后一次已通过限速的关节目标，不运行QP。反馈持续
|
||||
超时和CANFD错误仍沿用现有同步、停止与故障锁存逻辑。
|
||||
|
||||
## QP迭代规则
|
||||
|
||||
`PlacoIkSolver.solve()`按以下规则执行：
|
||||
|
||||
1. 校验目标变换。
|
||||
2. 记录本次内部迭代前的关节状态。
|
||||
3. 调用一次 `self._solver.solve(True)`。
|
||||
4. 更新Placo运动学。
|
||||
5. 校验本次候选关节状态：
|
||||
- 7个有限数值；
|
||||
- 不违反RM75关节位置限制；
|
||||
- 本次数值迭代步长不超过Placo按 `dt=1/90` 应用的URDF关节速度限制。
|
||||
6. 使用位置任务与姿态任务的 `error_norm()`检查收敛：
|
||||
- 位置误差不超过1 mm；
|
||||
- 姿态误差不超过0.005 rad。
|
||||
7. 未收敛则继续迭代，最多30次。
|
||||
|
||||
30次后仍未收敛，或任一迭代产生非法结果时，抛出异常。节点复用现有
|
||||
`_solve_joint_target()`错误路径，在终端限频打印QP失败原因，并保持上一组安全
|
||||
关节目标。
|
||||
|
||||
内部迭代得到的是逆解目标，不会直接绕过发送限速。最终下发仍必须经过
|
||||
`_limit_joint_command_step()`，因此每个90 Hz真实命令继续满足现有
|
||||
`joint_max_speed` 和 `joint_max_acc`。
|
||||
|
||||
## 晃动抑制
|
||||
|
||||
本次不再叠加新的低通滤波器。晃动通过两层现有机制抑制：
|
||||
|
||||
1. QP先收敛到当前TCP目标对应的关节解，避免关节目标随40 Hz反馈只前进一小步；
|
||||
2. 最终关节目标由现有关节速度与加速度限幅器生成连续90 Hz命令。
|
||||
|
||||
若真机仍存在晃动，再根据“目标关节角与实际关节角误差”追加诊断；本次不预先
|
||||
引入预测器或额外滤波。
|
||||
|
||||
## 性能与安全验收
|
||||
|
||||
自动验证：
|
||||
|
||||
- 7 cm可达TCP平移目标在一次 `solve()` 后位置误差不超过1 mm；
|
||||
- 姿态误差满足0.005 rad阈值；
|
||||
- 非法结果和未收敛目标继续触发现有安全回退；
|
||||
- 关节命令速度与加速度限幅测试继续通过；
|
||||
- `xr_rm_teleop`全部pytest通过；
|
||||
- `colcon build --symlink-install`通过；
|
||||
- `arm_debug.launch.py arm:=right use_mock:=true`正常启动。
|
||||
|
||||
真机由用户验证：
|
||||
|
||||
- 手柄快速移动10 cm并保持不动，机械臂约1秒内稳定；
|
||||
- 无持续肉眼可见晃动；
|
||||
- 连续四个5秒 timing 窗口中 `total` 最大值低于11.111 ms；
|
||||
- 无QP失败、反馈超时、CANFD错误或故障锁存日志；
|
||||
- 松开Grip后仍立即退出遥操作并执行安全停止。
|
||||
|
||||
若单周期 `total` 达到或超过11.111 ms，停止真机运动并降低最大QP迭代次数，
|
||||
不得通过提高控制频率或关闭安全检查规避计算超时。
|
||||
|
||||
## 文件范围
|
||||
|
||||
- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`。
|
||||
- 修改 `xr_rm_teleop/test/test_placo_transforms.py`。
|
||||
- 如现有QP失败测试需要补充未收敛原因断言，只精确修改
|
||||
`xr_rm_teleop/test/test_joint_control.py`。
|
||||
|
||||
不修改三份机械臂YAML、RealMan适配器、launch、UI、依赖或公开入口。
|
||||
Reference in New Issue
Block a user