YikaiFu-cart/acRealman_xr
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
d7ff769109d070c19388cea2a04f7db822fe3986
acRealman_xr/docs/superpowers/plans/2026-08-24-rm75-ik-method-comparison.md
T

50 KiB
Raw Blame History

RM75 三种逆运动学方法离线对比实验实施计划

For agentic workers: REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (- [ ]) syntax for tracking.

Goal: 使用 episode_0.hdf5 的右臂目标位姿，在统一 90 Hz 离线运动学环境中比较 Jacobian 伪逆、固定阻尼 DLS 和当前优化 Placo QP，并生成三张报告图、逐采样数据、汇总指标和中文分析。

Architecture: 新增一个仅供离线实验使用的脚本，复用现有 PlacoIkSolver、双臂 URDF、右臂 YAML 参数和关节命令限速函数。脚本只读加载 episode，将 30 Hz 位姿重采样到 90 Hz，然后让三种方法从同一初始关节状态独立复放，最后统一计算指标并输出 CSV、JSON、SVG、PNG 和 Markdown。

Tech Stack: Python 3.10、NumPy 2.2.6、h5py 3.16.0、Matplotlib 3.10.9、PyYAML、Placo 0.9.4、pytest、ROS2 Humble/colcon。

文件结构

新增文件

  • xr_rm_teleop/test/ik_method_comparison.py
    • episode 读取和验证；
    • 位姿重采样；
    • 伪逆、DLS、现有 QP 三路求解；
    • 共同安全输出和状态推进；
    • 指标、CSV、JSON、绘图和中文结果分析；
    • 命令行入口。
  • xr_rm_teleop/test/test_ik_method_comparison.py
    • 只覆盖本实验新增的重采样、指标、DLS 选择、安全保持和真实模型冒烟逻辑。

只读复用文件

  • xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py
  • xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
  • xr_rm_teleop/models/dual_rm75/Dual_arm.urdf
  • xr_rm_bringup/config/right_arm_rm75.yaml
  • /home/robot/ACT_Data/tomato_pick/episode_0.hdf5

生产节点、URDF、YAML、launch 和消息定义均不修改。

执行约定

所有命令从 /home/robot/WS_xr 执行：

source /opt/ros/humble/setup.bash
source install/setup.bash

涉及 Placo、h5py 和 Matplotlib 的命令固定使用：

/home/robot/miniconda3/envs/xr/bin/python

Task 1：建立位姿类型、四元数插值和 90 Hz 重采样

Files:

  • Create: xr_rm_teleop/test/ik_method_comparison.py

  • Create: xr_rm_teleop/test/test_ik_method_comparison.py

  • Step 1：写重采样失败测试

在 test_ik_method_comparison.py 中加入脚本目录导入和以下测试：

from __future__ import annotations

import math
import sys
from pathlib import Path

import numpy as np
import pytest

TEST_DIR = Path(__file__).resolve().parent
if str(TEST_DIR) not in sys.path:
    sys.path.insert(0, str(TEST_DIR))

import ik_method_comparison as comparison


def test_slerp_uses_shortest_arc_and_returns_unit_quaternion() -> None:
    start = np.asarray([0.0, 0.0, 0.0, 1.0])
    end = -np.asarray([0.0, 0.0, math.sin(0.1), math.cos(0.1)])

    actual = comparison._slerp_quaternion(start, end, 0.5)

    assert np.linalg.norm(actual) == pytest.approx(1.0)
    assert actual == pytest.approx(
        [0.0, 0.0, math.sin(0.05), math.cos(0.05)]
    )


def test_resample_trajectory_keeps_endpoints_and_uses_requested_rate() -> None:
    trajectory = comparison.EpisodeTrajectory(
        source_path=Path("episode.hdf5"),
        times_s=np.asarray([0.0, 0.5, 1.0]),
        target_poses=np.asarray(
            [
                [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
                [0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
                [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
            ]
        ),
        initial_joints=np.zeros(7),
    )

    actual = comparison.resample_trajectory(trajectory, 4.0)

    assert actual.times_s == pytest.approx([0.0, 0.25, 0.5, 0.75, 1.0])
    assert actual.target_poses[0] == pytest.approx(trajectory.target_poses[0])
    assert actual.target_poses[-1] == pytest.approx(trajectory.target_poses[-1])
    assert actual.target_poses[:, 0] == pytest.approx(actual.times_s)
  • Step 2：运行测试并确认失败

Run:

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
source install/setup.bash
/home/robot/miniconda3/envs/xr/bin/python -m pytest \
  src/xr_rm_teleop/test/test_ik_method_comparison.py -q

Expected: FAIL，提示 ik_method_comparison 不存在或缺少 EpisodeTrajectory。

  • Step 3：实现最小重采样逻辑

在 ik_method_comparison.py 中加入：

from __future__ import annotations

from dataclasses import dataclass
from pathlib import Path

import numpy as np


@dataclass(frozen=True)
class EpisodeTrajectory:
    source_path: Path
    times_s: np.ndarray
    target_poses: np.ndarray
    initial_joints: np.ndarray


def _normalized_quaternion(values: np.ndarray) -> np.ndarray:
    quaternion = np.asarray(values, dtype=float)
    if quaternion.shape != (4,) or not np.isfinite(quaternion).all():
        raise ValueError("quaternion must contain 4 finite values")
    norm = float(np.linalg.norm(quaternion))
    if norm <= 1e-12:
        raise ValueError("quaternion norm must be positive")
    return quaternion / norm


def _slerp_quaternion(
    start: np.ndarray,
    end: np.ndarray,
    fraction: float,
) -> np.ndarray:
    first = _normalized_quaternion(start)
    second = _normalized_quaternion(end)
    dot = float(np.dot(first, second))
    if dot < 0.0:
        second = -second
        dot = -dot
    dot = float(np.clip(dot, -1.0, 1.0))
    if dot > 1.0 - 1e-8:
        return _normalized_quaternion(
            first + float(fraction) * (second - first)
        )
    angle = float(np.arccos(dot))
    sine = float(np.sin(angle))
    return _normalized_quaternion(
        np.sin((1.0 - fraction) * angle) / sine * first
        + np.sin(fraction * angle) / sine * second
    )


def resample_trajectory(
    trajectory: EpisodeTrajectory,
    sample_rate_hz: float,
) -> EpisodeTrajectory:
    if not np.isfinite(sample_rate_hz) or sample_rate_hz <= 0.0:
        raise ValueError("sample_rate_hz must be finite and positive")
    source_times = np.asarray(trajectory.times_s, dtype=float)
    poses = np.asarray(trajectory.target_poses, dtype=float)
    if source_times.ndim != 1 or poses.shape != (source_times.size, 7):
        raise ValueError("trajectory must contain N timestamps and N x 7 poses")
    if source_times.size < 2 or np.any(np.diff(source_times) <= 0.0):
        raise ValueError("trajectory timestamps must be strictly increasing")

    duration = float(source_times[-1] - source_times[0])
    count = int(round(duration * sample_rate_hz)) + 1
    target_times = np.linspace(source_times[0], source_times[-1], count)
    target_poses = np.empty((count, 7), dtype=float)
    for axis in range(3):
        target_poses[:, axis] = np.interp(
            target_times,
            source_times,
            poses[:, axis],
        )
    for index, timestamp in enumerate(target_times):
        right = int(np.searchsorted(source_times, timestamp, side="right"))
        right = min(max(right, 1), source_times.size - 1)
        left = right - 1
        interval = source_times[right] - source_times[left]
        fraction = float((timestamp - source_times[left]) / interval)
        target_poses[index, 3:] = _slerp_quaternion(
            poses[left, 3:], poses[right, 3:], fraction
        )
    target_poses[0] = poses[0]
    target_poses[-1] = poses[-1]
    return EpisodeTrajectory(
        source_path=trajectory.source_path,
        times_s=target_times - target_times[0],
        target_poses=target_poses,
        initial_joints=np.asarray(trajectory.initial_joints, dtype=float).copy(),
    )
  • Step 4：运行重采样测试

Run: 上一步的聚焦 pytest 命令。

Expected: 2 passed。

  • Step 5：提交任务 1
cd /home/robot/WS_xr/src
git add xr_rm_teleop/test/ik_method_comparison.py \
  xr_rm_teleop/test/test_ik_method_comparison.py
git commit -m "feat: 添加逆运动学轨迹重采样"

Task 2：只读加载并验证 episode_0

Files:

  • Modify: xr_rm_teleop/test/ik_method_comparison.py

  • Modify: xr_rm_teleop/test/test_ik_method_comparison.py

  • Step 1：写 HDF5 读取失败测试

在测试文件中加入：

import h5py


def _write_episode(path: Path) -> None:
    poses = np.asarray(
        [
            [0.1, -0.2, 0.3, 0.0, 0.0, 0.0, 1.0],
            [0.2, -0.2, 0.3, 0.0, 0.0, 0.0, 1.0],
            [0.3, -0.2, 0.3, 0.0, 0.0, 0.0, 1.0],
            [0.4, -0.2, 0.3, 0.0, 0.0, 0.0, 1.0],
        ],
        dtype=np.float32,
    )
    with h5py.File(path, "w") as handle:
        handle.attrs["arm"] = "right_rm75"
        handle.attrs["pose_order"] = "x,y,z,qx,qy,qz,qw"
        handle.create_dataset("debug/tcp/final_target_pose", data=poses)
        handle.create_dataset(
            "debug/timestamps/control_monotonic_ns",
            data=np.asarray([0, 33_000_000, 66_000_000, 99_000_000]),
        )
        handle.create_dataset(
            "debug/control/teleop_active", data=[0, 1, 1, 0]
        )
        handle.create_dataset(
            "debug/control/action_valid", data=[1, 1, 1, 1]
        )
        handle.create_dataset(
            "debug/control/command_sent", data=[0, 1, 1, 0]
        )
        qpos = np.zeros((4, 8), dtype=np.float32)
        qpos[1, :7] = np.arange(7) * 0.1
        handle.create_dataset("observations/qpos", data=qpos)


def test_load_episode_uses_longest_valid_run_and_first_valid_qpos(
    tmp_path: Path,
) -> None:
    path = tmp_path / "episode.hdf5"
    _write_episode(path)

    actual = comparison.load_episode(path)

    assert actual.times_s == pytest.approx([0.0, 0.033])
    assert actual.target_poses[:, 0] == pytest.approx([0.2, 0.3])
    assert actual.initial_joints == pytest.approx(np.arange(7) * 0.1)


def test_load_episode_rejects_wrong_arm(tmp_path: Path) -> None:
    path = tmp_path / "episode.hdf5"
    _write_episode(path)
    with h5py.File(path, "r+") as handle:
        handle.attrs.modify("arm", "left_rm75")

    with pytest.raises(ValueError, match="right_rm75"):
        comparison.load_episode(path)
  • Step 2：运行测试并确认失败

Run: 聚焦 pytest 命令。

Expected: FAIL，提示 load_episode 不存在。

  • Step 3：实现数据读取和最长连续区间选择

在脚本中加入 import h5py 和：

def _longest_true_run(mask: np.ndarray) -> slice:
    values = np.asarray(mask, dtype=bool)
    best_start = best_stop = start = 0
    for index, enabled in enumerate(np.r_[values, False]):
        if enabled:
            continue
        if index - start > best_stop - best_start:
            best_start, best_stop = start, index
        start = index + 1
    if best_stop - best_start < 2:
        raise ValueError("episode has no valid teleoperation run")
    return slice(best_start, best_stop)


def load_episode(path: Path) -> EpisodeTrajectory:
    source = Path(path).expanduser().resolve()
    if not source.is_file() or source.suffix.lower() not in (".h5", ".hdf5"):
        raise FileNotFoundError(f"episode not found: {source}")
    with h5py.File(source, "r") as handle:
        if str(handle.attrs.get("arm", "")) != "right_rm75":
            raise ValueError("episode arm must be right_rm75")
        if str(handle.attrs.get("pose_order", "")) != "x,y,z,qx,qy,qz,qw":
            raise ValueError("episode pose_order is unsupported")
        required = (
            "debug/tcp/final_target_pose",
            "debug/timestamps/control_monotonic_ns",
            "debug/control/teleop_active",
            "debug/control/action_valid",
            "debug/control/command_sent",
            "observations/qpos",
        )
        missing = [name for name in required if name not in handle]
        if missing:
            raise ValueError(f"episode datasets missing: {missing}")
        poses = np.asarray(handle[required[0]], dtype=float)
        timestamps = np.asarray(handle[required[1]], dtype=np.int64)
        mask = np.logical_and.reduce(
            [
                np.asarray(handle[required[2]], dtype=bool),
                np.asarray(handle[required[3]], dtype=bool),
                np.asarray(handle[required[4]], dtype=bool),
            ]
        )
        qpos = np.asarray(handle[required[5]], dtype=float)
    if poses.shape != (timestamps.size, 7) or qpos.shape != (timestamps.size, 8):
        raise ValueError("episode arrays have inconsistent shapes")
    if not np.isfinite(poses).all() or not np.isfinite(qpos).all():
        raise ValueError("episode contains NaN/Inf")
    selected = _longest_true_run(mask)
    selected_times = timestamps[selected]
    if np.any(np.diff(selected_times) <= 0):
        raise ValueError("episode timestamps must be strictly increasing")
    selected_poses = poses[selected].copy()
    selected_poses[:, 3:] = np.asarray(
        [_normalized_quaternion(value) for value in selected_poses[:, 3:]]
    )
    return EpisodeTrajectory(
        source_path=source,
        times_s=(selected_times - selected_times[0]) * 1e-9,
        target_poses=selected_poses,
        initial_joints=qpos[selected.start, :7].copy(),
    )
  • Step 4：运行 episode 读取测试

Run: 聚焦 pytest 命令。

Expected: 4 passed。

  • Step 5：只读检查真实 episode

Run:

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
source install/setup.bash
/home/robot/miniconda3/envs/xr/bin/python -c \
  "import sys; sys.path.insert(0, 'src/xr_rm_teleop/test'); \
from pathlib import Path; from ik_method_comparison import load_episode; \
t=load_episode(Path('/home/robot/ACT_Data/tomato_pick/episode_0.hdf5')); \
print(len(t.times_s), t.times_s[-1], t.initial_joints.tolist())"

Expected: 输出 483 个有效样本、约 16.07 s 时长和 7 个有限初始关节角。

  • Step 6：提交任务 2
cd /home/robot/WS_xr/src
git add xr_rm_teleop/test/ik_method_comparison.py \
  xr_rm_teleop/test/test_ik_method_comparison.py
git commit -m "feat: 读取番茄采摘目标轨迹"

Task 3：实现指标、DLS 选择和共同命令限速

Files:

  • Modify: xr_rm_teleop/test/ik_method_comparison.py

  • Modify: xr_rm_teleop/test/test_ik_method_comparison.py

  • Step 1：写指标和选择规则失败测试

在测试文件中加入：

def test_orientation_error_and_joint_margin_match_definitions() -> None:
    identity = np.eye(3)
    quarter_turn = comparison._rotation_z(math.pi / 2.0)
    joints = np.asarray([0.0, -0.5])
    lower = np.asarray([-1.0, -1.0])
    upper = np.asarray([1.0, 3.0])

    assert comparison.orientation_error_rad(identity, quarter_turn) \
        == pytest.approx(math.pi / 2.0)
    assert comparison.normalized_joint_margin(joints, lower, upper) \
        == pytest.approx(0.125)


def test_choose_dls_damping_is_lexicographic() -> None:
    candidates = [
        comparison.MethodSummary("dls", 0.01, 0.90, 0.004, 0.01, 50.0),
        comparison.MethodSummary("dls", 0.03, 0.95, 0.006, 0.02, 30.0),
        comparison.MethodSummary("dls", 0.10, 0.95, 0.004, 0.01, 40.0),
    ]

    assert comparison.choose_dls_damping(candidates) == pytest.approx(0.10)


def test_limit_joint_command_reuses_production_limiter() -> None:
    target, velocity, limited = comparison.limit_joint_command(
        target=np.full(7, 1.0),
        previous_target=np.zeros(7),
        previous_velocity=np.zeros(7),
        max_speed=1.0,
        max_acceleration=10.0,
        dt=0.1,
    )

    assert target == pytest.approx([0.1] * 7)
    assert velocity == pytest.approx([1.0] * 7)
    assert limited
  • Step 2：运行测试并确认失败

Run: 聚焦 pytest 命令。

Expected: FAIL，提示指标函数或 MethodSummary 不存在。

  • Step 3：实现指标和 DLS 选择

在脚本中导入现有旋转、限速工具，并加入：

import math

from xr_rm_teleop.single_arm_velocity_teleop import (
    SingleArmVelocityTeleop,
    _matrix_to_quaternion,
    _quaternion_to_matrix,
    _so3_log,
)


@dataclass(frozen=True)
class MethodSummary:
    method: str
    damping: float | None
    success_rate: float
    position_rmse_m: float
    orientation_rmse_rad: float
    max_joint_speed_deg_s: float


def _rotation_z(angle: float) -> np.ndarray:
    cosine, sine = math.cos(angle), math.sin(angle)
    return np.asarray(
        [[cosine, -sine, 0.0], [sine, cosine, 0.0], [0.0, 0.0, 1.0]]
    )


def orientation_error_rad(actual: np.ndarray, target: np.ndarray) -> float:
    delta = np.asarray(target) @ np.asarray(actual).T
    cosine = float(np.clip((np.trace(delta) - 1.0) * 0.5, -1.0, 1.0))
    return float(math.acos(cosine))


def normalized_joint_margin(
    joints: np.ndarray,
    lower: np.ndarray,
    upper: np.ndarray,
) -> float:
    values = np.asarray(joints, dtype=float)
    lower_values = np.asarray(lower, dtype=float)
    upper_values = np.asarray(upper, dtype=float)
    span = upper_values - lower_values
    if np.any(span <= 0.0):
        raise ValueError("joint limits must have positive spans")
    margins = np.minimum(
        values - lower_values,
        upper_values - values,
    ) / span
    return float(np.min(margins))


def choose_dls_damping(candidates: list[MethodSummary]) -> float:
    if not candidates or any(value.damping is None for value in candidates):
        raise ValueError("DLS candidates must contain damping values")
    selected = min(
        candidates,
        key=lambda value: (
            -value.success_rate,
            value.position_rmse_m / 0.002
            + value.orientation_rmse_rad / 0.005,
            value.max_joint_speed_deg_s,
        ),
    )
    return float(selected.damping)


def limit_joint_command(
    *,
    target: np.ndarray,
    previous_target: np.ndarray,
    previous_velocity: np.ndarray,
    max_speed: float,
    max_acceleration: float,
    dt: float,
) -> tuple[np.ndarray, np.ndarray, bool]:
    limited_target, limited_velocity = (
        SingleArmVelocityTeleop._limit_joint_command_step(
            target=np.asarray(target, dtype=float).tolist(),
            previous_target=np.asarray(previous_target, dtype=float).tolist(),
            previous_velocity=np.asarray(previous_velocity, dtype=float).tolist(),
            max_speed=max_speed,
            max_acceleration=max_acceleration,
            dt=dt,
        )
    )
    target_array = np.asarray(limited_target, dtype=float)
    velocity_array = np.asarray(limited_velocity, dtype=float)
    return (
        target_array,
        velocity_array,
        not np.allclose(target_array, target, atol=1e-12, rtol=0.0),
    )
  • Step 4：运行指标测试

Run: 聚焦 pytest 命令。

Expected: 7 passed。

  • Step 5：提交任务 3
cd /home/robot/WS_xr/src
git add xr_rm_teleop/test/ik_method_comparison.py \
  xr_rm_teleop/test/test_ik_method_comparison.py
git commit -m "feat: 添加逆运动学对比指标"

Task 4：实现伪逆、DLS、当前 QP 和安全离线复放

Files:

  • Modify: xr_rm_teleop/test/ik_method_comparison.py

  • Modify: xr_rm_teleop/test/test_ik_method_comparison.py

  • Step 1：写失败保持和真实模型冒烟测试

在测试文件中加入：

class _FakeSolver:
    def __init__(self, fail: bool) -> None:
        self.fail = fail
        self.joint_limits = np.asarray([[-2.0, 2.0]] * 7)

    def update_joint_state(self, joints: list[float]) -> np.ndarray:
        pose = np.eye(4)
        pose[0, 3] = joints[0]
        return pose

    def solve(self, target: np.ndarray) -> list[float]:
        if self.fail:
            raise RuntimeError("not converged")
        return [float(target[0, 3])] + [0.0] * 6


def test_replay_holds_previous_state_on_solver_failure() -> None:
    trajectory = comparison.EpisodeTrajectory(
        source_path=Path("episode.hdf5"),
        times_s=np.asarray([0.0, 0.1]),
        target_poses=np.asarray(
            [
                [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
                [0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
            ]
        ),
        initial_joints=np.zeros(7),
    )

    result = comparison.run_replay(
        "fake",
        _FakeSolver(fail=True),
        trajectory,
        max_speed=1.0,
        max_acceleration=10.0,
        measure_time=False,
    )

    assert result.joints == pytest.approx(np.zeros((2, 7)))
    assert not result.success.any()
    assert result.velocities == pytest.approx(np.zeros((2, 7)))


def test_real_urdf_solvers_return_finite_safe_outputs() -> None:
    pytest.importorskip("placo")
    urdf = TEST_DIR.parent / "models" / "dual_rm75" / "Dual_arm.urdf"
    joints = np.radians(
        [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
    )
    solvers = [
        comparison.DifferentialIkSolver(urdf, 1.0 / 90.0, "pinv"),
        comparison.DifferentialIkSolver(urdf, 1.0 / 90.0, "dls", 0.03),
        comparison.make_qp_solver(urdf, 1.0 / 90.0),
    ]
    for solver in solvers:
        target = solver.update_joint_state(joints.tolist())
        target = target.copy()
        target[0, 3] += 0.004
        result = np.asarray(solver.solve(target), dtype=float)
        assert result.shape == (7,)
        assert np.isfinite(result).all()
        assert np.all(result >= solver.joint_limits[:, 0] - 1e-9)
        assert np.all(result <= solver.joint_limits[:, 1] + 1e-9)
  • Step 2：运行测试并确认失败

Run: 聚焦 pytest 命令。

Expected: FAIL，提示 run_replay 或求解器适配器不存在。

  • Step 3：实现位姿转换和差分 IK 求解器

在脚本中导入现有 QP 常量和校验函数：

from xr_rm_teleop.placo_ik_solver import (
    QP_MAX_ITERATIONS,
    QP_ORIENTATION_TOLERANCE_RAD,
    QP_POSITION_TOLERANCE_M,
    PlacoIkSolver,
    _validated_transform,
)

加入以下最小适配器；不新增抽象基类：

def _pose_to_transform(pose: np.ndarray) -> np.ndarray:
    values = np.asarray(pose, dtype=float)
    transform = np.eye(4)
    transform[:3, 3] = values[:3]
    transform[:3, :3] = _quaternion_to_matrix(tuple(values[3:]))
    return transform


def _transform_to_pose(transform: np.ndarray) -> np.ndarray:
    quaternion = _matrix_to_quaternion(transform[:3, :3])
    return np.asarray([*transform[:3, 3], *quaternion], dtype=float)


class DifferentialIkSolver:
    def __init__(
        self,
        urdf_path: Path,
        dt: float,
        method: str,
        damping: float = 0.0,
    ) -> None:
        if method not in ("pinv", "dls"):
            raise ValueError("method must be pinv or dls")
        if method == "dls" and damping <= 0.0:
            raise ValueError("DLS damping must be positive")
        self._kinematics = PlacoIkSolver(str(urdf_path), dt, "right")
        self._dt = dt
        self._method = method
        self._damping = float(damping)
        self._actual_joints: np.ndarray | None = None

    @property
    def joint_limits(self) -> np.ndarray:
        return self._kinematics._joint_limits.copy()

    def update_joint_state(self, joints: list[float]) -> np.ndarray:
        self._actual_joints = np.asarray(joints, dtype=float).copy()
        return self._kinematics.update_joint_state(joints)

    def _set_internal_joints(self, joints: np.ndarray) -> None:
        robot = self._kinematics._robot
        robot.state.q[self._kinematics._q_offsets] = joints
        robot.update_kinematics()

    def _errors(self, target: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
        robot = self._kinematics._robot
        world_base = robot.get_T_world_frame(self._kinematics._base_frame)
        world_tcp = robot.get_T_world_frame(self._kinematics._tcp_frame)
        world_target = world_base @ target
        position = world_target[:3, 3] - world_tcp[:3, 3]
        orientation = _so3_log(
            world_target[:3, :3] @ world_tcp[:3, :3].T
        )
        return position, orientation

    def solve(self, target: np.ndarray) -> list[float]:
        if self._actual_joints is None:
            raise RuntimeError("joint state must be initialized before IK solve")
        target = _validated_transform(target)
        actual = self._actual_joints.copy()
        result = actual.copy()
        try:
            for _ in range(QP_MAX_ITERATIONS):
                self._set_internal_joints(result)
                position, orientation = self._errors(target)
                if (
                    np.linalg.norm(position) <= QP_POSITION_TOLERANCE_M
                    and np.linalg.norm(orientation)
                    <= QP_ORIENTATION_TOLERANCE_RAD
                ):
                    return result.tolist()
                jacobian = self._kinematics._active_tcp_jacobian()
                desired_twist = np.r_[position, orientation] / self._dt
                if self._method == "pinv":
                    joint_velocity = np.linalg.pinv(jacobian) @ desired_twist
                else:
                    system = (
                        jacobian @ jacobian.T
                        + self._damping**2 * np.eye(6)
                    )
                    joint_velocity = jacobian.T @ np.linalg.solve(
                        system, desired_twist
                    )
                candidate = result + joint_velocity * self._dt
                self._kinematics._validate_result(candidate, result)
                result = candidate
            raise RuntimeError(
                f"{self._method} did not converge after {QP_MAX_ITERATIONS} iterations"
            )
        except Exception:
            self._set_internal_joints(actual)
            raise


class QpSolverAdapter:
    def __init__(self, solver: PlacoIkSolver) -> None:
        self._solver = solver

    @property
    def joint_limits(self) -> np.ndarray:
        return self._solver._joint_limits.copy()

    def update_joint_state(self, joints: list[float]) -> np.ndarray:
        return self._solver.update_joint_state(joints)

    def solve(self, target: np.ndarray) -> list[float]:
        return self._solver.solve(target)


def make_qp_solver(urdf_path: Path, dt: float) -> QpSolverAdapter:
    return QpSolverAdapter(
        PlacoIkSolver(
            str(urdf_path),
            dt,
            "right",
            j3_reference_deg=-89.57,
            j3_weight=1e-4,
            j4_min_deg=10.0,
            j4_warn_deg=25.0,
            j4_weight=1e-4,
            manipulability_sigma_stop=0.01,
            manipulability_sigma_warn=0.04,
            manipulability_weight=1e-4,
        )
    )

make_qp_solver 的硬编码参数只用于先让冒烟测试变绿；Task 5 必须改为从右臂 YAML 读取，避免配置重复。

  • Step 4：实现逐周期复放和失败保持

在脚本中加入：

import time


@dataclass(frozen=True)
class ReplayResult:
    method: str
    times_s: np.ndarray
    target_poses: np.ndarray
    actual_poses: np.ndarray
    joints: np.ndarray
    velocities: np.ndarray
    position_errors_m: np.ndarray
    orientation_errors_rad: np.ndarray
    joint_margins: np.ndarray
    solve_durations_ms: np.ndarray
    success: np.ndarray
    command_limited: np.ndarray


def run_replay(
    method: str,
    solver,
    trajectory: EpisodeTrajectory,
    *,
    max_speed: float,
    max_acceleration: float,
    measure_time: bool = True,
) -> ReplayResult:
    count = trajectory.times_s.size
    dt = float(np.median(np.diff(trajectory.times_s)))
    joints = np.empty((count, 7))
    velocities = np.zeros((count, 7))
    actual_poses = np.empty((count, 7))
    position_errors = np.empty(count)
    orientation_errors = np.empty(count)
    margins = np.empty(count)
    durations = np.zeros(count)
    success = np.zeros(count, dtype=bool)
    command_limited = np.zeros(count, dtype=bool)
    current = trajectory.initial_joints.copy()
    previous_velocity = np.zeros(7)
    lower, upper = solver.joint_limits.T

    for index, pose in enumerate(trajectory.target_poses):
        solver.update_joint_state(current.tolist())
        started = time.perf_counter_ns()
        try:
            candidate = np.asarray(
                solver.solve(_pose_to_transform(pose)), dtype=float
            )
            success[index] = True
        except Exception:
            candidate = current.copy()
            previous_velocity = np.zeros(7)
        durations[index] = (
            (time.perf_counter_ns() - started) * 1e-6 if measure_time else 0.0
        )
        if success[index]:
            current, previous_velocity, command_limited[index] = (
                limit_joint_command(
                    target=candidate,
                    previous_target=current,
                    previous_velocity=previous_velocity,
                    max_speed=max_speed,
                    max_acceleration=max_acceleration,
                    dt=dt,
                )
            )
        actual_transform = solver.update_joint_state(current.tolist())
        actual_pose = _transform_to_pose(actual_transform)
        joints[index] = current
        velocities[index] = previous_velocity
        actual_poses[index] = actual_pose
        position_errors[index] = np.linalg.norm(pose[:3] - actual_pose[:3])
        orientation_errors[index] = orientation_error_rad(
            _quaternion_to_matrix(tuple(actual_pose[3:])),
            _quaternion_to_matrix(tuple(pose[3:])),
        )
        margins[index] = normalized_joint_margin(current, lower, upper)

    return ReplayResult(
        method=method,
        times_s=trajectory.times_s.copy(),
        target_poses=trajectory.target_poses.copy(),
        actual_poses=actual_poses,
        joints=joints,
        velocities=velocities,
        position_errors_m=position_errors,
        orientation_errors_rad=orientation_errors,
        joint_margins=margins,
        solve_durations_ms=durations,
        success=success,
        command_limited=command_limited,
    )
  • Step 5：运行复放和真实模型测试

Run: 聚焦 pytest 命令。

Expected: 9 passed；真实 Placo 不可用时只跳过真实模型用例，其余测试通过。

  • Step 6：提交任务 4
cd /home/robot/WS_xr/src
git add xr_rm_teleop/test/ik_method_comparison.py \
  xr_rm_teleop/test/test_ik_method_comparison.py
git commit -m "feat: 添加三种逆运动学离线复放"

Task 5：从 YAML 读取 QP 参数并完成汇总、计时与命令行

Files:

  • Modify: xr_rm_teleop/test/ik_method_comparison.py

  • Modify: xr_rm_teleop/test/test_ik_method_comparison.py

  • Step 1：写配置读取、汇总和确定性失败测试

在测试文件中加入：

def test_load_right_config_returns_qp_and_command_limits() -> None:
    config = TEST_DIR.parents[1] / "xr_rm_bringup" / "config" / "right_arm_rm75.yaml"

    actual = comparison.load_right_config(config)

    assert actual["qp_j3_reference_deg"] == pytest.approx(-89.57)
    assert actual["qp_j4_min_deg"] == pytest.approx(10.0)
    assert actual["qp_manipulability_weight"] == pytest.approx(1e-4)
    assert actual["joint_max_speed"] == pytest.approx(180.0)
    assert actual["joint_max_acc"] == pytest.approx(300.0)


def test_summarize_result_uses_report_metrics() -> None:
    result = comparison.ReplayResult(
        method="pinv",
        times_s=np.asarray([0.0, 0.1]),
        target_poses=np.zeros((2, 7)),
        actual_poses=np.zeros((2, 7)),
        joints=np.zeros((2, 7)),
        velocities=np.asarray([[0.0] * 7, [math.pi] + [0.0] * 6]),
        position_errors_m=np.asarray([0.003, 0.004]),
        orientation_errors_rad=np.asarray([0.01, 0.02]),
        joint_margins=np.asarray([0.2, 0.1]),
        solve_durations_ms=np.asarray([1.0, 2.0]),
        success=np.asarray([True, False]),
        command_limited=np.asarray([False, True]),
    )

    actual = comparison.summarize_result(result)

    assert actual.success_rate == pytest.approx(0.5)
    assert actual.position_rmse_m == pytest.approx(0.0035355339)
    assert actual.orientation_rmse_rad == pytest.approx(0.0158113883)
    assert actual.max_joint_speed_deg_s == pytest.approx(180.0)
  • Step 2：运行测试并确认失败

Run: 聚焦 pytest 命令。

Expected: FAIL，提示配置或汇总函数不存在。

  • Step 3：实现 YAML 配置、汇总和 QP 工厂

在脚本中加入 import yaml，扩展 MethodSummary：

@dataclass(frozen=True)
class MethodSummary:
    method: str
    damping: float | None
    success_rate: float
    position_rmse_m: float
    orientation_rmse_rad: float
    max_joint_speed_deg_s: float
    min_joint_margin: float = 0.0
    mean_solve_ms: float = 0.0
    max_solve_ms: float = 0.0
    failure_count: int = 0
    longest_failure_streak: int = 0
    command_limited_count: int = 0

加入：

def load_right_config(path: Path) -> dict[str, float]:
    with Path(path).open("r", encoding="utf-8") as stream:
        document = yaml.safe_load(stream)
    parameters = document["single_arm_velocity_teleop"]["ros__parameters"]
    names = (
        "qp_j3_reference_deg",
        "qp_j3_weight",
        "qp_j4_min_deg",
        "qp_j4_warn_deg",
        "qp_j4_weight",
        "qp_manipulability_sigma_stop",
        "qp_manipulability_sigma_warn",
        "qp_manipulability_weight",
        "joint_max_speed",
        "joint_max_acc",
    )
    return {name: float(parameters[name]) for name in names}


def _longest_failure_streak(success: np.ndarray) -> int:
    longest = current = 0
    for value in np.asarray(success, dtype=bool):
        current = 0 if value else current + 1
        longest = max(longest, current)
    return longest


def summarize_result(
    result: ReplayResult,
    damping: float | None = None,
) -> MethodSummary:
    return MethodSummary(
        method=result.method,
        damping=damping,
        success_rate=float(np.mean(result.success)),
        position_rmse_m=float(
            np.sqrt(np.mean(result.position_errors_m**2))
        ),
        orientation_rmse_rad=float(
            np.sqrt(np.mean(result.orientation_errors_rad**2))
        ),
        max_joint_speed_deg_s=float(
            np.max(np.abs(np.degrees(result.velocities)))
        ),
        min_joint_margin=float(np.min(result.joint_margins)),
        mean_solve_ms=float(np.mean(result.solve_durations_ms)),
        max_solve_ms=float(np.max(result.solve_durations_ms)),
        failure_count=int(np.count_nonzero(~result.success)),
        longest_failure_streak=_longest_failure_streak(result.success),
        command_limited_count=int(np.count_nonzero(result.command_limited)),
    )


def make_qp_solver(
    urdf_path: Path,
    dt: float,
    config: dict[str, float],
) -> QpSolverAdapter:
    return QpSolverAdapter(
        PlacoIkSolver(
            str(urdf_path),
            dt,
            "right",
            j3_reference_deg=config["qp_j3_reference_deg"],
            j3_weight=config["qp_j3_weight"],
            j4_min_deg=config["qp_j4_min_deg"],
            j4_warn_deg=config["qp_j4_warn_deg"],
            j4_weight=config["qp_j4_weight"],
            manipulability_sigma_stop=config[
                "qp_manipulability_sigma_stop"
            ],
            manipulability_sigma_warn=config[
                "qp_manipulability_sigma_warn"
            ],
            manipulability_weight=config["qp_manipulability_weight"],
        )
    )

同时更新 Task 4 的真实模型测试，将 make_qp_solver(urdf, dt) 改为传入 load_right_config(...)，删除硬编码参数工厂。

  • Step 4：实现 DLS 扫描、预热和 10 次计时

加入单一编排函数；计时重复只替换 solve_durations_ms 汇总，不改变首次复放的轨迹：

DLS_DAMPING_CANDIDATES = (0.001, 0.003, 0.01, 0.03, 0.1, 0.3)


def evaluate_methods(
    trajectory: EpisodeTrajectory,
    urdf_path: Path,
    config: dict[str, float],
    timing_repeats: int,
) -> tuple[dict[str, ReplayResult], dict[str, MethodSummary], float]:
    dt = float(np.median(np.diff(trajectory.times_s)))
    speed = math.radians(config["joint_max_speed"])
    acceleration = math.radians(config["joint_max_acc"])
    damping_results = []
    for damping in DLS_DAMPING_CANDIDATES:
        replay = run_replay(
            "dls",
            DifferentialIkSolver(urdf_path, dt, "dls", damping),
            trajectory,
            max_speed=speed,
            max_acceleration=acceleration,
            measure_time=False,
        )
        damping_results.append(summarize_result(replay, damping))
    selected_damping = choose_dls_damping(damping_results)

    factories = {
        "pinv": lambda: DifferentialIkSolver(urdf_path, dt, "pinv"),
        "dls": lambda: DifferentialIkSolver(
            urdf_path, dt, "dls", selected_damping
        ),
        "qp": lambda: make_qp_solver(urdf_path, dt, config),
    }
    results = {}
    summaries = {}
    for name, factory in factories.items():
        run_replay(
            name,
            factory(),
            trajectory,
            max_speed=speed,
            max_acceleration=acceleration,
        )
        result = run_replay(
            name,
            factory(),
            trajectory,
            max_speed=speed,
            max_acceleration=acceleration,
        )
        timed = []
        for _ in range(timing_repeats):
            repeated = run_replay(
                name,
                factory(),
                trajectory,
                max_speed=speed,
                max_acceleration=acceleration,
            )
            if not np.allclose(
                repeated.joints,
                result.joints,
                atol=1e-10,
                rtol=0.0,
            ):
                raise RuntimeError(f"{name} replay is not deterministic")
            timed.append(repeated.solve_durations_ms)
        timing_matrix = np.stack(timed)
        result = replace(
            result,
            solve_durations_ms=np.mean(timing_matrix, axis=0),
        )
        results[name] = result
        summaries[name] = replace(
            summarize_result(
                result,
                selected_damping if name == "dls" else None,
            ),
            mean_solve_ms=float(np.mean(timing_matrix)),
            max_solve_ms=float(np.max(timing_matrix)),
        )
    return results, summaries, selected_damping

在脚本顶部使用 from dataclasses import dataclass, replace，不要导入整个 dataclasses 模块。逐采样耗时保存 10 次重复的逐点平均值；汇总最大耗时则取全部 重复中的最大值，保证 CSV 行数仍与统一时间轴一致。

  • Step 5：运行聚焦测试

Run: 聚焦 pytest 命令。

Expected: 11 passed，或 10 passed、1 skipped（Placo 不可用时）。

  • Step 6：提交任务 5
cd /home/robot/WS_xr/src
git add xr_rm_teleop/test/ik_method_comparison.py \
  xr_rm_teleop/test/test_ik_method_comparison.py
git commit -m "feat: 汇总逆运动学对比结果"

Task 6：生成 CSV、JSON、三张论文图和中文分析

Files:

  • Modify: xr_rm_teleop/test/ik_method_comparison.py

  • Modify: xr_rm_teleop/test/test_ik_method_comparison.py

  • Create at runtime: output/ik_comparison/episode_0/*

  • Step 1：写产物失败测试

在测试文件中用两个样本的 ReplayResult 调用产物函数：

def test_write_outputs_creates_consistent_files(tmp_path: Path) -> None:
    result = comparison.ReplayResult(
        method="qp",
        times_s=np.asarray([0.0, 0.1]),
        target_poses=np.tile([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], (2, 1)),
        actual_poses=np.tile([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], (2, 1)),
        joints=np.zeros((2, 7)),
        velocities=np.zeros((2, 7)),
        position_errors_m=np.asarray([0.001, 0.002]),
        orientation_errors_rad=np.asarray([0.001, 0.002]),
        joint_margins=np.asarray([0.2, 0.2]),
        solve_durations_ms=np.asarray([0.5, 0.6]),
        success=np.asarray([True, True]),
        command_limited=np.asarray([False, False]),
    )
    summary = comparison.summarize_result(result)

    comparison.write_outputs(
        tmp_path,
        {"pinv": result, "dls": result, "qp": result},
        {"pinv": summary, "dls": summary, "qp": summary},
        selected_damping=0.03,
        source_path=Path("episode_0.hdf5"),
        git_commit="abc1234",
    )

    expected = {
        "samples.csv",
        "summary.json",
        "figure_2_11_tracking_error.svg",
        "figure_2_11_tracking_error.png",
        "figure_2_12_joint_constraints.svg",
        "figure_2_12_joint_constraints.png",
        "figure_2_13_summary.svg",
        "figure_2_13_summary.png",
        "analysis_2.3.4.md",
    }
    assert expected == {path.name for path in tmp_path.iterdir()}
    assert all((tmp_path / name).stat().st_size > 0 for name in expected)
  • Step 2：运行测试并确认失败

Run: 聚焦 pytest 命令。

Expected: FAIL，提示 write_outputs 不存在。

  • Step 3：实现 CSV 和 JSON

使用标准库 csv、json 和 subprocess。CSV 每行写一个“方法 + 时间点”，字段固定为：

method,time_s,target_x,target_y,target_z,target_qx,target_qy,target_qz,target_qw,
actual_x,actual_y,actual_z,actual_qx,actual_qy,actual_qz,actual_qw,
position_error_m,orientation_error_rad,q1,q2,q3,q4,q5,q6,q7,
qd1,qd2,qd3,qd4,qd5,qd6,qd7,joint_margin,solve_ms,success,command_limited

JSON 使用：

payload = {
    "source_episode": str(source_path),
    "git_commit": git_commit,
    "sample_rate_hz": 90.0,
    "selected_dls_damping": selected_damping,
    "methods": {
        name: asdict(summary)
        for name, summary in summaries.items()
    },
}

脚本顶部使用 from dataclasses import asdict, dataclass, replace，不要额外导入整个 dataclasses 模块。JSON 采用 ensure_ascii=False, indent=2。

  • Step 4：实现三张图

使用 Matplotlib 的 Noto Sans CJK SC 字体和固定色盲友好样式：

METHOD_STYLE = {
    "pinv": ("Jacobian 伪逆", "#D55E00", "-"),
    "dls": ("DLS", "#0072B2", "--"),
    "qp": ("优化 QP", "#009E73", "-.")
}
plt.rcParams.update(
    {
        "font.family": "sans-serif",
        "font.sans-serif": ["Noto Sans CJK SC", "Droid Sans Fallback"],
        "axes.unicode_minus": False,
        "figure.dpi": 120,
        "savefig.dpi": 300,
    }
)

实现三个独立函数：

plot_tracking_error(results, output_dir)
plot_joint_constraints(results, output_dir)
plot_summary(summaries, output_dir)

每个函数用 fig.savefig(...svg, bbox_inches="tight") 和 fig.savefig(...png, dpi=300, bbox_inches="tight") 保存，并在完成后 plt.close(fig)。

图 2-11：上下两图分别绘制 position_errors_m * 1000 和 degrees(orientation_errors_rad)；失败点用稀疏 x 标记。

图 2-12：上下两图分别绘制每行 degrees(abs(velocities)).max(axis=1) 和 joint_margins；速度图绘制 180 的红色虚线。

图 2-13：2 x 3 六面板柱状图，依次使用位置 RMSE、姿态 RMSE、最大速度、最小 裕度、平均/最大耗时、成功率，柱顶标注保留 2--3 位有效小数的精确值。

  • Step 5：生成中文分析文件

analysis_2.3.4.md 必须由汇总数据格式化生成，包含：

  1. “基于真实遥操作目标轨迹的离线运动学对比”限定；
  2. episode 路径、90 Hz 重采样和同一初始关节状态；
  3. DLS 候选集合、选定阻尼和同轨迹选优限制；
  4. 三张图题和图注；
  5. 三种方法的 RMSE、最大速度、最小裕度、耗时和成功率；
  6. 当前优化 QP 的 J3、J4 和动态可操作度补充说明；
  7. 只依据实际数值生成的客观比较，不使用固定“显著优于”等结论模板。
  • Step 6：运行产物测试

Run: 聚焦 pytest 命令。

Expected: 12 passed，或 11 passed、1 skipped。

  • Step 7：提交任务 6
cd /home/robot/WS_xr/src
git add xr_rm_teleop/test/ik_method_comparison.py \
  xr_rm_teleop/test/test_ik_method_comparison.py
git commit -m "feat: 生成逆运动学论文对比图"

Task 7：加入 CLI，执行完整 episode 并检查正式产物

Files:

  • Modify: xr_rm_teleop/test/ik_method_comparison.py

  • Create at runtime: output/ik_comparison/episode_0/*

  • Step 1：实现命令行入口

在脚本中加入：

import argparse
import subprocess

SOURCE_ROOT = Path(__file__).resolve().parents[2]
DEFAULT_EPISODE = Path("/home/robot/ACT_Data/tomato_pick/episode_0.hdf5")
DEFAULT_URDF = (
    SOURCE_ROOT / "xr_rm_teleop" / "models" / "dual_rm75" / "Dual_arm.urdf"
)
DEFAULT_CONFIG = (
    SOURCE_ROOT / "xr_rm_bringup" / "config" / "right_arm_rm75.yaml"
)
DEFAULT_OUTPUT = SOURCE_ROOT / "output" / "ik_comparison" / "episode_0"


def main() -> None:
    parser = argparse.ArgumentParser(
        description="离线比较 RM75 伪逆、DLS 和当前优化 QP"
    )
    parser.add_argument("--episode", type=Path, default=DEFAULT_EPISODE)
    parser.add_argument("--urdf", type=Path, default=DEFAULT_URDF)
    parser.add_argument("--config", type=Path, default=DEFAULT_CONFIG)
    parser.add_argument("--output-dir", type=Path, default=DEFAULT_OUTPUT)
    parser.add_argument("--timing-repeats", type=int, default=10)
    args = parser.parse_args()
    if args.timing_repeats <= 0:
        parser.error("--timing-repeats must be positive")

    source = load_episode(args.episode)
    trajectory = resample_trajectory(source, 90.0)
    config = load_right_config(args.config)
    results, summaries, damping = evaluate_methods(
        trajectory,
        args.urdf,
        config,
        args.timing_repeats,
    )
    commit = subprocess.run(
        ["git", "rev-parse", "HEAD"],
        cwd=SOURCE_ROOT,
        check=True,
        capture_output=True,
        text=True,
    ).stdout.strip()
    write_outputs(
        args.output_dir,
        results,
        summaries,
        selected_damping=damping,
        source_path=source.source_path,
        git_commit=commit,
    )
    print(f"results written to {args.output_dir}")


if __name__ == "__main__":
    main()
  • Step 2：检查 CLI 帮助

Run:

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
source install/setup.bash
/home/robot/miniconda3/envs/xr/bin/python \
  src/xr_rm_teleop/test/ik_method_comparison.py --help

Expected: 显示 episode、URDF、config、output-dir 和 timing-repeats 参数，不连接真机。

  • Step 3：执行完整对比实验

Run:

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
source install/setup.bash
/home/robot/miniconda3/envs/xr/bin/python \
  src/xr_rm_teleop/test/ik_method_comparison.py

Expected: 成功选择一个 DLS 固定阻尼，在 src/output/ik_comparison/episode_0/ 生成 9 个非空产物；终端无真机连接日志。

  • Step 4：核对 CSV、JSON 和图片元数据

Run:

cd /home/robot/WS_xr
/home/robot/miniconda3/envs/xr/bin/python - <<'PY'
import csv
import json
from pathlib import Path
from PIL import Image

root = Path("src/output/ik_comparison/episode_0")
with (root / "samples.csv").open(newline="", encoding="utf-8") as stream:
    rows = list(csv.DictReader(stream))
with (root / "summary.json").open(encoding="utf-8") as stream:
    summary = json.load(stream)
assert len(rows) > 3 * 1400
assert set(summary["methods"]) == {"pinv", "dls", "qp"}
assert summary["selected_dls_damping"] in [0.001, 0.003, 0.01, 0.03, 0.1, 0.3]
for name in (
    "figure_2_11_tracking_error.png",
    "figure_2_12_joint_constraints.png",
    "figure_2_13_summary.png",
):
    with Image.open(root / name) as image:
        dpi = image.info.get("dpi", (0.0, 0.0))
        assert min(dpi) >= 299.0
print(len(rows), summary["selected_dls_damping"])
PY

Expected: 输出超过 4200 行逐采样记录和选定阻尼；断言全部通过。

  • Step 5：目视检查三张 PNG

使用 view_image 分别检查：

/home/robot/WS_xr/src/output/ik_comparison/episode_0/figure_2_11_tracking_error.png
/home/robot/WS_xr/src/output/ik_comparison/episode_0/figure_2_12_joint_constraints.png
/home/robot/WS_xr/src/output/ik_comparison/episode_0/figure_2_13_summary.png

Expected: 中文无方框、标题和图例不重叠、曲线可区分、坐标轴单位完整、柱顶数值未裁切。 如有视觉缺陷，只调整绘图函数后重新运行 Task 6 聚焦测试和完整实验。

  • Step 6：提交 CLI 与正式实验产物
cd /home/robot/WS_xr/src
git add xr_rm_teleop/test/ik_method_comparison.py \
  output/ik_comparison/episode_0
git commit -m "docs: 生成逆运动学对比实验结果"

Task 8：完整回归验证和最终检查

Files:

  • Verify only: all files above

  • Step 1：运行聚焦测试

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
source install/setup.bash
/home/robot/miniconda3/envs/xr/bin/python -m pytest \
  src/xr_rm_teleop/test/test_ik_method_comparison.py -q

Expected: 全部通过，不跳过真实 Placo 冒烟测试。

  • Step 2：运行遥操作姿态回归测试
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
source install/setup.bash
/home/robot/miniconda3/envs/xr/bin/python -m pytest \
  src/xr_rm_teleop/test/test_orientation_control.py -q

Expected: 全部通过。

  • Step 3：构建工作空间
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
colcon build --symlink-install

Expected: xr_rm_input、xr_rm_interfaces、xr_rm_teleop、xr_rm_mujoco 和 xr_rm_bringup 构建成功。

  • Step 4：检查工作树和差异
cd /home/robot/WS_xr/src
git status --short
git diff --check HEAD~1..HEAD

Expected: 只包含本实验脚本、测试、规格、计划和正式结果；无 .superpowers/、临时 渲染、HDF5 副本、真机配置改动或无关格式化。

  • Step 5：核对报告文字边界

打开 output/ik_comparison/episode_0/analysis_2.3.4.md，确认明确写出：

  • 离线运动学对比，不是新的真机在线实验；

  • 使用 episode_0.hdf5 的相同目标轨迹和初始状态；

  • DLS 在本轨迹上扫描固定阻尼；

  • 当前 QP 包含 J3、J4 和动态六维可操作度任务；

  • 分析结论与 summary.json 数值一致。

  • Step 6：如最后检查产生修正，创建一个准确的收尾提交

仅在 Task 8 实际修改文件时执行：

cd /home/robot/WS_xr/src
git add xr_rm_teleop/test/ik_method_comparison.py \
  xr_rm_teleop/test/test_ik_method_comparison.py \
  output/ik_comparison/episode_0
git commit -m "fix: 修正逆运动学对比实验输出"

如果没有文件变化，不创建空提交。

Reference in New Issue View Git Blame Copy Permalink