核心模块 API

场景

场景配置加载与 MjSpec 运行时组装器（阶段 A1 新增，暂未接入现有仿真回路）。

把"机械臂 + 公头 + 母头 + 环境"从单一硬编码 XML 拆成可配置的 MJCF 片段组合：

• 机械臂 MJCF（assets/iiwa14/iiwa14_arm.xml）：纯机械臂，不含工具/环境元素；
• 工具片段（公头）：根 body 必须命名为 dock（挂载位姿由场景 YAML 的 tool.pose 提供且相对 ee_site），并且片段内必须提供名为 sensor_site 的 site（力/力矩传感器的锚点，attach 后自动加前缀）；
• 目标片段（母头）：根 body 必须命名为 dock，组装时经 worldbody frame 固定于世界系。

组装配方（MjSpec attach 机制）与 legacy 全量 XML（assets/iiwa14/ iiwa14_dock_updated.xml）物理逐位等价，等价性测试见 tests/test_scene.py。

类

Scene dataclass

完整对接场景：机械臂 + 公头 + 母头 + 物理 + 任务初始条件。

源代码位于： src/compliant_docking/scene.py

@dataclass(frozen=True)
class Scene:
    """完整对接场景：机械臂 + 公头 + 母头 + 物理 + 任务初始条件。"""

    name: str
    robot: RobotSpec
    tool: ToolSpec
    physics: PhysicsSpec
    task: TaskSpec
    path: Path  # 场景 YAML 的绝对路径
    target: TargetSpec | None = None  # 可选：跟踪测试场景不挂母头
    trajectory: TrajectorySpec | None = None  # 可选轨迹段（两段式对接 / 圆+8字跟踪；缺省走单段五次）
    tracking_thresholds: TrackingThresholds | None = None  # 跟踪门禁阈值（只在 type=tracking 时使用）
    impedance: ImpedanceOverride | None = None  # 可选阻抗增益覆盖（缺省走 ImpedanceConfig）
    friction_comp: str = "torque"  # 摩擦前馈模式："torque"（默认，力矩方向，治零速死区） | "velocity"
    hqp: HQPOverride | None = None  # 可选 HQP-AC 参数覆盖（外力源/预紧力）
    se3_impedance: SE3ImpedanceOverride | None = None  # 可选 SE(3) Lie 阻抗覆盖

    # ---- 解析后的名称属性（下阶段接线时使用） ----

    @property
    def eef_body(self) -> str:
        """公头挂载后的根 body 名（framepos 传感器跟踪对象）。"""
        return f"{self.tool.prefix}dock"

    @property
    def sensor_site(self) -> str:
        """公头片段提供的力/力矩传感器锚点 site 名（attach 后带前缀）。"""
        return f"{self.tool.prefix}sensor_site"

    @property
    def camera(self) -> str:
        """组装器统一添加的跟随相机名。"""
        return "track_cam"

    def build_mjmodel(self) -> mujoco.MjModel:
        """按已验证配方组装 MjSpec 并编译为 MjModel。

        配方顺序敏感（与 legacy XML 物理逐位等价，勿改 attach 语义与数值）：
        先设公头根 body 位姿再 attach（attach 把 body 位姿解释为相对 site 系）；
        母头经 worldbody frame 挂载；胶水与传感器在 attach 之后添加。
        """
        # 1) 机械臂基底
        arm = mujoco.MjSpec.from_file(str(self.robot.mjcf))

        # 2) 公头：先给根 body 设挂载位姿（相对 ee_site），再挂到法兰 site
        tool = mujoco.MjSpec.from_file(str(self.tool.mjcf))
        tool_root = tool.body("dock")
        tool_root.pos = self.tool.pose_pos
        tool_root.quat = self.tool.pose_quat
        arm.site(self.robot.ee_site).attach_body(tool_root, prefix=self.tool.prefix)

        # 3) 母头：worldbody 加 frame，母头根 body 挂到 frame（世界系固定）。
        #    target 为 None（跟踪测试场景）时跳过母头挂载，其余流程不变 /
        #    Skip the female-side attach entirely for tracking scenes (target=None)
        if self.target is not None:
            female = mujoco.MjSpec.from_file(str(self.target.mjcf))
            frame = arm.worldbody.add_frame(
                name="target_frame", pos=self.target.pos, quat=self.target.quat
            )
            frame.attach_body(female.body("dock"), prefix=self.target.prefix)

        # 4) 环境胶水：地面/平行光/可视化 site/跟随相机
        arm.worldbody.add_geom(
            name="floor",
            type=mujoco.mjtGeom.mjGEOM_PLANE,
            size=[0, 0, 0.05],
            material="groundplane",
        )
        arm.worldbody.add_light(
            pos=[0, 0, 1.5],
            dir=[0, 0, -1],
            type=mujoco.mjtLightType.mjLIGHT_DIRECTIONAL,
        )
        arm.worldbody.add_site(
            name="eef_marker",
            type=mujoco.mjtGeom.mjGEOM_SPHERE,
            size=[0.015, 0.015, 0.015],
            pos=[0, 0, 0],
            rgba=[1, 0, 0, 0.6],
        )
        arm.worldbody.add_site(
            name="vis",
            type=mujoco.mjtGeom.mjGEOM_SPHERE,
            size=[0.015, 0.015, 0.015],
            pos=[0, 0, 0],
            rgba=[0, 0, 1, 0.6],
        )
        # 实测结论：mode 须用 mjtCamLight 枚举 int（不是字符串）
        arm.worldbody.add_camera(
            name="track_cam",
            pos=[0.5, 1.3, 0.8],
            mode=int(mujoco.mjtCamLight.mjCAMLIGHT_TARGETBODY),
            targetbody=f"{self.tool.prefix}rev",
        )

        # 5) 传感器：framepos 跟踪公头根 body，force/torque 锚在公头 sensor_site
        arm.add_sensor(
            name="body1_position",
            type=mujoco.mjtSensor.mjSENS_FRAMEPOS,
            objtype=mujoco.mjtObj.mjOBJ_BODY,
            objname=f"{self.tool.prefix}dock",
        )
        arm.add_sensor(
            name="force_sensor",
            type=mujoco.mjtSensor.mjSENS_FORCE,
            objtype=mujoco.mjtObj.mjOBJ_SITE,
            objname=f"{self.tool.prefix}sensor_site",
        )
        arm.add_sensor(
            name="torque_sensor",
            type=mujoco.mjtSensor.mjSENS_TORQUE,
            objtype=mujoco.mjtObj.mjOBJ_SITE,
            objname=f"{self.tool.prefix}sensor_site",
        )

        # 6) 物理参数（integrator/cone 从 YAML 字符串映射为枚举 int）
        opt = arm.option
        opt.timestep = self.physics.timestep
        opt.gravity = list(self.physics.gravity)
        opt.integrator = _enum_value(_INTEGRATORS, "integrator", self.physics.integrator)
        opt.cone = _enum_value(_CONES, "cone", self.physics.cone)
        opt.sdf_iterations = self.physics.sdf_iterations
        opt.sdf_initpoints = self.physics.sdf_initpoints

        return arm.compile()

属性

eef_body property

eef_body: str

公头挂载后的根 body 名（framepos 传感器跟踪对象）。

sensor_site property

sensor_site: str

公头片段提供的力/力矩传感器锚点 site 名（attach 后带前缀）。

camera property

camera: str

组装器统一添加的跟随相机名。

方法：

build_mjmodel

build_mjmodel() -> MjModel

按已验证配方组装 MjSpec 并编译为 MjModel。

配方顺序敏感（与 legacy XML 物理逐位等价，勿改 attach 语义与数值）： 先设公头根 body 位姿再 attach（attach 把 body 位姿解释为相对 site 系）； 母头经 worldbody frame 挂载；胶水与传感器在 attach 之后添加。

源代码位于： src/compliant_docking/scene.py

def build_mjmodel(self) -> mujoco.MjModel:
    """按已验证配方组装 MjSpec 并编译为 MjModel。

    配方顺序敏感（与 legacy XML 物理逐位等价，勿改 attach 语义与数值）：
    先设公头根 body 位姿再 attach（attach 把 body 位姿解释为相对 site 系）；
    母头经 worldbody frame 挂载；胶水与传感器在 attach 之后添加。
    """
    # 1) 机械臂基底
    arm = mujoco.MjSpec.from_file(str(self.robot.mjcf))

    # 2) 公头：先给根 body 设挂载位姿（相对 ee_site），再挂到法兰 site
    tool = mujoco.MjSpec.from_file(str(self.tool.mjcf))
    tool_root = tool.body("dock")
    tool_root.pos = self.tool.pose_pos
    tool_root.quat = self.tool.pose_quat
    arm.site(self.robot.ee_site).attach_body(tool_root, prefix=self.tool.prefix)

    # 3) 母头：worldbody 加 frame，母头根 body 挂到 frame（世界系固定）。
    #    target 为 None（跟踪测试场景）时跳过母头挂载，其余流程不变 /
    #    Skip the female-side attach entirely for tracking scenes (target=None)
    if self.target is not None:
        female = mujoco.MjSpec.from_file(str(self.target.mjcf))
        frame = arm.worldbody.add_frame(
            name="target_frame", pos=self.target.pos, quat=self.target.quat
        )
        frame.attach_body(female.body("dock"), prefix=self.target.prefix)

    # 4) 环境胶水：地面/平行光/可视化 site/跟随相机
    arm.worldbody.add_geom(
        name="floor",
        type=mujoco.mjtGeom.mjGEOM_PLANE,
        size=[0, 0, 0.05],
        material="groundplane",
    )
    arm.worldbody.add_light(
        pos=[0, 0, 1.5],
        dir=[0, 0, -1],
        type=mujoco.mjtLightType.mjLIGHT_DIRECTIONAL,
    )
    arm.worldbody.add_site(
        name="eef_marker",
        type=mujoco.mjtGeom.mjGEOM_SPHERE,
        size=[0.015, 0.015, 0.015],
        pos=[0, 0, 0],
        rgba=[1, 0, 0, 0.6],
    )
    arm.worldbody.add_site(
        name="vis",
        type=mujoco.mjtGeom.mjGEOM_SPHERE,
        size=[0.015, 0.015, 0.015],
        pos=[0, 0, 0],
        rgba=[0, 0, 1, 0.6],
    )
    # 实测结论：mode 须用 mjtCamLight 枚举 int（不是字符串）
    arm.worldbody.add_camera(
        name="track_cam",
        pos=[0.5, 1.3, 0.8],
        mode=int(mujoco.mjtCamLight.mjCAMLIGHT_TARGETBODY),
        targetbody=f"{self.tool.prefix}rev",
    )

    # 5) 传感器：framepos 跟踪公头根 body，force/torque 锚在公头 sensor_site
    arm.add_sensor(
        name="body1_position",
        type=mujoco.mjtSensor.mjSENS_FRAMEPOS,
        objtype=mujoco.mjtObj.mjOBJ_BODY,
        objname=f"{self.tool.prefix}dock",
    )
    arm.add_sensor(
        name="force_sensor",
        type=mujoco.mjtSensor.mjSENS_FORCE,
        objtype=mujoco.mjtObj.mjOBJ_SITE,
        objname=f"{self.tool.prefix}sensor_site",
    )
    arm.add_sensor(
        name="torque_sensor",
        type=mujoco.mjtSensor.mjSENS_TORQUE,
        objtype=mujoco.mjtObj.mjOBJ_SITE,
        objname=f"{self.tool.prefix}sensor_site",
    )

    # 6) 物理参数（integrator/cone 从 YAML 字符串映射为枚举 int）
    opt = arm.option
    opt.timestep = self.physics.timestep
    opt.gravity = list(self.physics.gravity)
    opt.integrator = _enum_value(_INTEGRATORS, "integrator", self.physics.integrator)
    opt.cone = _enum_value(_CONES, "cone", self.physics.cone)
    opt.sdf_iterations = self.physics.sdf_iterations
    opt.sdf_initpoints = self.physics.sdf_initpoints

    return arm.compile()

RobotSpec dataclass

机械臂描述：MJCF（组装基底）+ Pinocchio 模型 + 末端锚点/frame 名。

pin_model 是 Pinocchio 侧的模型路径，可以是 .urdf（URDF 解析）或 .xml（MJCF，经 buildModelFromMJCF 直读），load_pin_model 按后缀分发。

源代码位于： src/compliant_docking/scene.py

@dataclass(frozen=True)
class RobotSpec:
    """机械臂描述：MJCF（组装基底）+ Pinocchio 模型 + 末端锚点/frame 名。

    pin_model 是 Pinocchio 侧的模型路径，可以是 ``.urdf``（URDF 解析）或
    ``.xml``（MJCF，经 buildModelFromMJCF 直读），load_pin_model 按后缀分发。
    """

    mjcf: Path
    pin_model: Path
    ee_site: str
    ee_frame: str

ToolSpec dataclass

公头工具片段：根 body 必须叫 dock，须含 sensor_site site。

源代码位于： src/compliant_docking/scene.py

@dataclass(frozen=True)
class ToolSpec:
    """公头工具片段：根 body 必须叫 ``dock``，须含 ``sensor_site`` site。"""

    mjcf: Path
    prefix: str
    pose_pos: np.ndarray  # 相对 robot.ee_site 的平移
    pose_quat: np.ndarray  # 相对 robot.ee_site 的旋转（wxyz）
    pin_inertia: ToolInertiaSpec | None = None

ToolInertiaSpec dataclass

需追加入 Pinocchio 的固定工具惯量（MuJoCo 工具片段已有同一惯量）。

源代码位于： src/compliant_docking/scene.py

@dataclass(frozen=True)
class ToolInertiaSpec:
    """需追加入 Pinocchio 的固定工具惯量（MuJoCo 工具片段已有同一惯量）。"""

    mass: float
    com: np.ndarray
    diaginertia: np.ndarray

TargetSpec dataclass

母头片段：根 body 必须叫 dock，经 worldbody frame 固定于世界系。

源代码位于： src/compliant_docking/scene.py

@dataclass(frozen=True)
class TargetSpec:
    """母头片段：根 body 必须叫 ``dock``，经 worldbody frame 固定于世界系。"""

    mjcf: Path
    prefix: str
    pos: np.ndarray  # 世界系平移
    quat: np.ndarray  # 世界系旋转（wxyz）

PhysicsSpec dataclass

物理参数（与历史 XML option 对应）。

源代码位于： src/compliant_docking/scene.py

@dataclass(frozen=True)
class PhysicsSpec:
    """物理参数（与历史 XML option 对应）。"""

    timestep: float
    gravity: np.ndarray
    integrator: str  # YAML 字符串，编译时映射为 mujoco.mjtIntegrator
    cone: str  # YAML 字符串，编译时映射为 mujoco.mjtCone
    sdf_iterations: int
    sdf_initpoints: int

TaskSpec dataclass

对接任务初始条件（历史硬编码值收编）。

源代码位于： src/compliant_docking/scene.py

@dataclass(frozen=True)
class TaskSpec:
    """对接任务初始条件（历史硬编码值收编）。"""

    init_pos: np.ndarray
    init_ori: np.ndarray  # (3, 3) 旋转矩阵
    ik_guess: np.ndarray
    stroke: np.ndarray

ImpedanceOverride dataclass

可选的任务空间阻抗增益覆盖（覆盖 ImpedanceConfig 对应字段）。

典型用途：带关节摩擦的机械臂（如 FR3 上游真实摩擦）需要更高刚度 压小静摩擦死区（死区 ≈ 摩擦阈值/k）。缺省段则完全沿用 ImpedanceConfig。

源代码位于： src/compliant_docking/scene.py

@dataclass(frozen=True)
class ImpedanceOverride:
    """可选的任务空间阻抗增益覆盖（覆盖 ImpedanceConfig 对应字段）。

    典型用途：带关节摩擦的机械臂（如 FR3 上游真实摩擦）需要更高刚度
    压小静摩擦死区（死区 ≈ 摩擦阈值/k）。缺省段则完全沿用 ImpedanceConfig。
    """

    k: float | None = None      # 平动刚度 [N/m]
    d: float | None = None      # 平动阻尼 [N·s/m]
    k_rot: float | None = None  # 姿态刚度 [N·m/rad]
    d_rot: float | None = None  # 姿态阻尼 [N·m·s/rad]

HQPOverride dataclass

HQP-AC 可选参数覆盖（场景 YAML 的可选 hqp 段，缺省走控制器默认）。

.. code-block:: yaml

hqp:
  force_source: sensor      # "sensor"（F/T 传感器）| "observer"（PI 动量观测器，无传感器）
  observer_kp: 20.0         # 观测器比例增益 [1/s]
  observer_ki: 40.0         # 观测器积分增益 [1/s²]
  preload_force: 0.0        # 接触预紧力目标 [N]（世界系沿 stroke 方向，0=关闭）
  preload_ramp_s: 1.5       # 预紧力斜坡时间 [s]

源代码位于： src/compliant_docking/scene.py

@dataclass(frozen=True)
class HQPOverride:
    """HQP-AC 可选参数覆盖（场景 YAML 的可选 ``hqp`` 段，缺省走控制器默认）。

    .. code-block:: yaml

        hqp:
          force_source: sensor      # "sensor"（F/T 传感器）| "observer"（PI 动量观测器，无传感器）
          observer_kp: 20.0         # 观测器比例增益 [1/s]
          observer_ki: 40.0         # 观测器积分增益 [1/s²]
          preload_force: 0.0        # 接触预紧力目标 [N]（世界系沿 stroke 方向，0=关闭）
          preload_ramp_s: 1.5       # 预紧力斜坡时间 [s]
    """

    force_source: str = "sensor"
    observer_kp: float = 20.0
    observer_ki: float = 40.0
    preload_force: float = 0.0
    preload_ramp_s: float = 1.5
    contact_deadband: float = 0.0

SE3ImpedanceOverride dataclass

SE(3) Lie 阻抗可选参数覆盖（场景 YAML 的可选 se3_impedance 段）。

缺省字段沿用 SE3ImpedanceConfig 默认（ImpedanceConfig 基线映射值）。 字段为 None 表示不覆盖。所有 *_diag 为 6 维列表（平动 3 + 姿态 3）。

.. code-block:: yaml

se3_impedance:
  a_diag: [10.0, 10.0, 10.0, 1.0, 1.0, 1.0]   # 期望惯量对角
  d_diag: [80.0, 80.0, 80.0, 10.0, 10.0, 10.0]
  k_diag: [50.0, 50.0, 50.0, 25.0, 25.0, 25.0]
  null_damping: 10.0

源代码位于： src/compliant_docking/scene.py

@dataclass(frozen=True)
class SE3ImpedanceOverride:
    """SE(3) Lie 阻抗可选参数覆盖（场景 YAML 的可选 ``se3_impedance`` 段）。

    缺省字段沿用 SE3ImpedanceConfig 默认（ImpedanceConfig 基线映射值）。
    字段为 None 表示不覆盖。所有 *_diag 为 6 维列表（平动 3 + 姿态 3）。

    .. code-block:: yaml

        se3_impedance:
          a_diag: [10.0, 10.0, 10.0, 1.0, 1.0, 1.0]   # 期望惯量对角
          d_diag: [80.0, 80.0, 80.0, 10.0, 10.0, 10.0]
          k_diag: [50.0, 50.0, 50.0, 25.0, 25.0, 25.0]
          null_damping: 10.0
    """

    a_diag: list[float] | None = None
    d_diag: list[float] | None = None
    k_diag: list[float] | None = None
    null_damping: float | None = None

TrajectorySpec dataclass

轨迹段参数（可选）：两段式对接 或 圆+8字跟踪测试。

对应场景 YAML 的可选 trajectory 扁平段；type 区分两类规划器 （缺省 "twophase"，既有 YAML 不写 type 时行为不变）：

.. code-block:: yaml

trajectory:
  type: twophase       # 可选 "twophase" | "tracking"（缺省 twophase）
  # ---- twophase 专用 ----
  standoff: 0.06        # 预对接点沿接近轴的后撤距离 [m]
  v_max_approach: 0.10  # 接近段线速度上限 [m/s]
  a_max_approach: 0.20  # 接近段线加速度上限 [m/s^2]
  v_max_docking: 0.02   # 对接段线速度上限 [m/s]
  a_max_docking: 0.05   # 对接段线加速度上限 [m/s^2]
  # ---- tracking 专用（圆+8字跟踪测试） ----
  transition_duration: 1.5   # 过渡段时长 [s]
  circle_duration: 5.0       # 圆周段时长 [s]
  circle_radius: 0.10        # 圆周半径 [m]
  circle_frequency: 0.2      # 圆周频率 [Hz]
  circle_center_offset: -0.06  # 圆心相对起点的 z 偏移 [m]
  figure8_duration: 5.0      # 8 字段时长 [s]
  figure8_radius_x: 0.10     # 8 字 x 半幅值 [m]
  figure8_radius_y: 0.07     # 8 字 y 半幅值 [m]
  figure8_frequency: 0.2     # 8 字频率 [Hz]

所有字段带默认值：tracking 场景只写 type: tracking 即可（twophase 字段 用默认值占位），twophase 场景沿用既有五参数写法（tracking 字段用默认值）。

源代码位于： src/compliant_docking/scene.py

@dataclass(frozen=True)
class TrajectorySpec:
    """轨迹段参数（可选）：两段式对接 或 圆+8字跟踪测试。

    对应场景 YAML 的可选 ``trajectory`` 扁平段；``type`` 区分两类规划器
    （缺省 ``"twophase"``，既有 YAML 不写 type 时行为不变）：

    .. code-block:: yaml

        trajectory:
          type: twophase       # 可选 "twophase" | "tracking"（缺省 twophase）
          # ---- twophase 专用 ----
          standoff: 0.06        # 预对接点沿接近轴的后撤距离 [m]
          v_max_approach: 0.10  # 接近段线速度上限 [m/s]
          a_max_approach: 0.20  # 接近段线加速度上限 [m/s^2]
          v_max_docking: 0.02   # 对接段线速度上限 [m/s]
          a_max_docking: 0.05   # 对接段线加速度上限 [m/s^2]
          # ---- tracking 专用（圆+8字跟踪测试） ----
          transition_duration: 1.5   # 过渡段时长 [s]
          circle_duration: 5.0       # 圆周段时长 [s]
          circle_radius: 0.10        # 圆周半径 [m]
          circle_frequency: 0.2      # 圆周频率 [Hz]
          circle_center_offset: -0.06  # 圆心相对起点的 z 偏移 [m]
          figure8_duration: 5.0      # 8 字段时长 [s]
          figure8_radius_x: 0.10     # 8 字 x 半幅值 [m]
          figure8_radius_y: 0.07     # 8 字 y 半幅值 [m]
          figure8_frequency: 0.2     # 8 字频率 [Hz]

    所有字段带默认值：tracking 场景只写 ``type: tracking`` 即可（twophase 字段
    用默认值占位），twophase 场景沿用既有五参数写法（tracking 字段用默认值）。
    """

    # ---- 两段式对接（既有字段，默认值取自 iiwa14_docking_twophase.yaml） ----
    standoff: float = 0.10
    v_max_approach: float = 0.10
    a_max_approach: float = 0.20
    v_max_docking: float = 0.02
    a_max_docking: float = 0.05

    # ---- 类型开关 + 圆+8字跟踪测试 ----
    type: str = "twophase"  # "twophase" | "tracking" | "se3topp"（load_scene 校验取值）
    # ---- se3topp 专用（角速度/角加速度限幅，论文 Table 8） ----
    omega_max_approach: float = 0.20  # 接近段角速度上限 [rad/s]
    omega_max_docking: float = 0.05   # 对接段角速度上限 [rad/s]
    alpha_max_approach: float = 0.20  # 接近段角加速度上限 [rad/s^2]
    alpha_max_docking: float = 0.05   # 对接段角加速度上限 [rad/s^2]
    transition_duration: float = 1.5
    circle_duration: float = 5.0
    circle_radius: float = 0.10
    circle_frequency: float = 0.2
    circle_center_offset: float = -0.06
    figure8_duration: float = 5.0
    figure8_radius_x: float = 0.10
    figure8_radius_y: float = 0.07
    figure8_frequency: float = 0.2

函数：

load_scene

load_scene(path: str | Path) -> Scene

加载场景 YAML 并解析为 Scene。

YAML 相对路径相对仓库根解析（与 models.py 的 ASSETS_DIR 同口径）； 场景文件自身传相对路径时也按仓库根解析。

源代码位于： src/compliant_docking/scene.py

def load_scene(path: str | Path) -> Scene:
    """加载场景 YAML 并解析为 Scene。

    YAML 相对路径相对仓库根解析（与 models.py 的 ASSETS_DIR 同口径）；
    场景文件自身传相对路径时也按仓库根解析。
    """
    scene_path = Path(path)
    if not scene_path.is_absolute():
        scene_path = REPO_ROOT / scene_path
    raw = yaml.safe_load(scene_path.read_text(encoding="utf-8"))

    def asset(rel: str) -> Path:
        p = Path(rel)
        return p if p.is_absolute() else REPO_ROOT / p

    scene = raw["scene"]
    robot = raw["robot"]
    tool = raw["tool"]
    physics = raw["physics"]
    task = raw["task"]
    target = raw.get("target")  # 跟踪测试场景无母头段（target=None）
    impedance = ImpedanceOverride(**raw["impedance"]) if "impedance" in raw else None
    hqp = HQPOverride(**raw["hqp"]) if "hqp" in raw else None
    se3_impedance = SE3ImpedanceOverride(**raw["se3_impedance"]) if "se3_impedance" in raw else None
    if hqp is not None and hqp.force_source not in ("sensor", "observer"):
        raise ValueError(
            f"scene 配置 hqp.force_source 不支持 {hqp.force_source!r}，可选值: sensor, observer")
    friction_comp = str(raw.get("friction_comp", "torque"))
    if friction_comp not in ("velocity", "torque"):
        raise ValueError(
            f"scene 配置 friction_comp 不支持 {friction_comp!r}，可选值: velocity, torque")

    # trajectory 段解析后校验 type 合法取值，再构造 TrajectorySpec /
    # Validate trajectory.type against the legal values before constructing the spec
    trajectory = None
    tracking_thresholds = None
    if "trajectory" in raw:
        traj_type = str(raw["trajectory"].get("type", "twophase"))
        if traj_type not in _TRAJECTORY_TYPES:
            options = ", ".join(sorted(_TRAJECTORY_TYPES))
            raise ValueError(
                f"scene 配置 trajectory.type 不支持 {traj_type!r}，可选值: {options}")
        trajectory = TrajectorySpec(**raw["trajectory"])
        if traj_type == "tracking":
            if target is not None:
                raise ValueError(
                    "tracking 场景不得配置 target；自由空间跟踪必须与对接接触隔离")
            # 允许顶层 tracking_thresholds；缺省严格采用对接前门禁默认阈值。
            # 显式拒绝未知键，避免 YAML 拼写错误悄悄放宽验收。
            threshold_raw = raw.get("tracking_thresholds", {})
            tracking_thresholds = TrackingThresholds(**threshold_raw)

    return Scene(
        name=str(scene["name"]),
        robot=RobotSpec(
            mjcf=asset(robot["mjcf"]),
            pin_model=asset(robot["pin_model"]),
            ee_site=str(robot["ee_site"]),
            ee_frame=str(robot["ee_frame"]),
        ),
        tool=ToolSpec(
            mjcf=asset(tool["mjcf"]),
            prefix=str(tool["prefix"]),
            pose_pos=np.asarray(tool["pose"]["pos"], dtype=float),
            pose_quat=np.asarray(tool["pose"]["quat"], dtype=float),
            pin_inertia=ToolInertiaSpec(
                mass=float(tool["pin_inertia"]["mass"]),
                com=np.asarray(tool["pin_inertia"]["com"], dtype=float),
                diaginertia=np.asarray(tool["pin_inertia"]["diaginertia"], dtype=float),
            ) if "pin_inertia" in tool else None,
        ),
        target=TargetSpec(
            mjcf=asset(target["mjcf"]),
            prefix=str(target["prefix"]),
            pos=np.asarray(target["pos"], dtype=float),
            quat=np.asarray(target["quat"], dtype=float),
        ) if target is not None else None,
        physics=PhysicsSpec(
            timestep=float(physics["timestep"]),
            gravity=np.asarray(physics["gravity"], dtype=float),
            integrator=str(physics["integrator"]),
            cone=str(physics["cone"]),
            sdf_iterations=int(physics["sdf_iterations"]),
            sdf_initpoints=int(physics["sdf_initpoints"]),
        ),
        task=TaskSpec(
            init_pos=np.asarray(task["init_pos"], dtype=float),
            init_ori=np.asarray(task["init_ori"], dtype=float).reshape(3, 3),
            ik_guess=np.asarray(task["ik_guess"], dtype=float),
            stroke=np.asarray(task["stroke"], dtype=float),
        ),
        path=scene_path,
        trajectory=trajectory,
        tracking_thresholds=tracking_thresholds,
        impedance=impedance,
    friction_comp=friction_comp,
    hqp=hqp,
    se3_impedance=se3_impedance,
    )

控制配置

仿真与控制参数配置：集中管理，避免魔法数字散落各处。

类

ImpedanceConfig dataclass

操作空间阻抗参数（平动 m/d/k + 姿态 m_rot/d_rot/k_rot）与零空间阻尼。

源代码位于： src/compliant_docking/config.py

@dataclass(frozen=True)
class ImpedanceConfig:
    """操作空间阻抗参数（平动 m/d/k + 姿态 m_rot/d_rot/k_rot）与零空间阻尼。"""

    m: float = 10.0
    # 保守的平动阻抗：在修正操作空间动力学项后，降低接触瞬态峰值。
    d: float = 80.0
    k: float = 50.0
    m_rot: float = 1.0
    d_rot: float = 10.0
    k_rot: float = 25.0
    null_damping: float = 10.0

DockingConfig dataclass

对接任务与仿真设置。

源代码位于： src/compliant_docking/config.py

@dataclass(frozen=True)
class DockingConfig:
    """对接任务与仿真设置。"""

    dt: float = 0.001
    traj_duration: float = 15.0
    duration: float = 18.0
    max_torque: float = 10.0  # 关节力矩限幅 [N·m]
    impedance: ImpedanceConfig = field(default_factory=ImpedanceConfig)

HQPConfig dataclass

HQP-AC（分层二次规划自适应控制）参数。

取值参照 Ren & Shan 2026 (Acta Astronautica) 第 3.2 节与 Table D.12； 供 control.hqp_ac.HQPAdaptiveController 使用。

字段 / Fields: K0: 初始参考刚度对角向量（6 维：平动 3 + 姿态 3），Eq.(27) K_min_ratio: K_min = K_min_ratio·K0（逐元素），Eq.(27) 下界 k_alpha: 自适应刚度 sigmoid 增益，Eq.(26) omega_th: 可操作度奇异性阈值，Eq.(39) k_sa: 奇异性规避任务权重，Eq.(42) K_ji / D_ji: 关节位姿阻抗刚度/阻尼（7×7），Eq.(41) k_ji: 关节位姿阻抗任务权重，Eq.(42) dt_p: ZOH 短时域预测步长 [s]，Eq.(34)-(36) 约束预测用 torque_limit: 关节力矩约束幅值 [N·m]；None 时取 model.effortLimit eps_abs: ProxQP 求解绝对精度

源代码位于： src/compliant_docking/config.py

@dataclass(frozen=True)
class HQPConfig:
    """HQP-AC（分层二次规划自适应控制）参数。

    取值参照 Ren & Shan 2026 (Acta Astronautica) 第 3.2 节与 Table D.12；
    供 control.hqp_ac.HQPAdaptiveController 使用。

    字段 / Fields:
        K0: 初始参考刚度对角向量（6 维：平动 3 + 姿态 3），Eq.(27)
        K_min_ratio: K_min = K_min_ratio·K0（逐元素），Eq.(27) 下界
        k_alpha: 自适应刚度 sigmoid 增益，Eq.(26)
        omega_th: 可操作度奇异性阈值，Eq.(39)
        k_sa: 奇异性规避任务权重，Eq.(42)
        K_ji / D_ji: 关节位姿阻抗刚度/阻尼（7×7），Eq.(41)
        k_ji: 关节位姿阻抗任务权重，Eq.(42)
        dt_p: ZOH 短时域预测步长 [s]，Eq.(34)-(36) 约束预测用
        torque_limit: 关节力矩约束幅值 [N·m]；None 时取 model.effortLimit
        eps_abs: ProxQP 求解绝对精度
    """

    K0: np.ndarray = field(
        default_factory=lambda: np.array([300.0, 300.0, 300.0, 50.0, 50.0, 50.0]))
    K_min_ratio: float = 0.3
    k_alpha: float = 0.5
    omega_th: float = 0.08
    k_sa: float = 500.0
    K_ji: np.ndarray = field(default_factory=lambda: 5.0 * np.eye(7))
    D_ji: np.ndarray = field(default_factory=lambda: 10.0 * np.eye(7))
    k_ji: float = 1.0
    dt_p: float = 0.05
    torque_limit: float | None = None
    eps_abs: float = 1e-5

SE3ImpedanceConfig dataclass

SE(3) Lie 群阻抗参数（Kim et al. 2025 T-RO §III-A，Eq. 55-61）。

论文阻抗模型 A·V̇̃ + D·Ṽ + dexp⁻ᵀKλ = F̃ 的期望惯量/阻尼/刚度。 默认对角向量由既有 ImpedanceConfig 基线数值映射而来 （m=10, d=80, k=50；m_rot=1, d_rot=10, k_rot=25），仅作控制器间 公平对比的兼容性初值，不声称是论文最优参数。控制器内部以一般 6×6 矩阵持有 A/D/K（对角配置只是特例）。

字段 / Fields: A_diag: 期望惯量对角（平动质量 kg ×3，转动惯量 kg·m² ×3） D_diag: 期望阻尼对角（N·s/m ×3，N·m·s/rad ×3） K_diag: 期望刚度对角（N/m ×3，N·m/rad ×3） null_damping: 冗余零空间速度阻尼 [N·m·s/rad]（仅稳定用， 不改变主任务；7-DoF 广义逆适配见控制器 Eq. 66 扩展） condition_threshold: 任务空间矩阵 Λ=(J M⁻¹ Jᵀ)⁻¹ 的条件数告警 阈值；超过时降级为带阈值的阻尼 pinv 并记录诊断

源代码位于： src/compliant_docking/config.py

@dataclass(frozen=True)
class SE3ImpedanceConfig:
    """SE(3) Lie 群阻抗参数（Kim et al. 2025 T-RO §III-A，Eq. 55-61）。

    论文阻抗模型 ``A·V̇̃ + D·Ṽ + dexp⁻ᵀKλ = F̃`` 的期望惯量/阻尼/刚度。
    默认对角向量由既有 ImpedanceConfig 基线数值映射而来
    （m=10, d=80, k=50；m_rot=1, d_rot=10, k_rot=25），**仅作控制器间
    公平对比的兼容性初值**，不声称是论文最优参数。控制器内部以一般
    6×6 矩阵持有 A/D/K（对角配置只是特例）。

    字段 / Fields:
        A_diag: 期望惯量对角（平动质量 kg ×3，转动惯量 kg·m² ×3）
        D_diag: 期望阻尼对角（N·s/m ×3，N·m·s/rad ×3）
        K_diag: 期望刚度对角（N/m ×3，N·m/rad ×3）
        null_damping: 冗余零空间速度阻尼 [N·m·s/rad]（仅稳定用，
            不改变主任务；7-DoF 广义逆适配见控制器 Eq. 66 扩展）
        condition_threshold: 任务空间矩阵 Λ=(J M⁻¹ Jᵀ)⁻¹ 的条件数告警
            阈值；超过时降级为带阈值的阻尼 pinv 并记录诊断
    """

    A_diag: np.ndarray = field(
        default_factory=lambda: np.array([10.0, 10.0, 10.0, 1.0, 1.0, 1.0]))
    D_diag: np.ndarray = field(
        default_factory=lambda: np.array([80.0, 80.0, 80.0, 10.0, 10.0, 10.0]))
    K_diag: np.ndarray = field(
        default_factory=lambda: np.array([50.0, 50.0, 50.0, 25.0, 25.0, 25.0]))
    null_damping: float = 10.0
    condition_threshold: float = 1e8

指标与门禁

对接性能指标套件（对标 Ren & Shan 2026, Acta Astronautica, Table 10）。

三层指标： 1. 接触安全：峰值轴向力、峰值广义力范数、稳态轴向力、稳态广义力范数； 2. 内部安全：最大关节角度/速度占模型限值百分比、最小可操作度； 3. 跟踪精度：位置跟踪 RMS、姿态跟踪 RMS、末端稳态横向误差、稳态姿态误差。

指标定义（与论文表 10 的对应关系）： - 峰值轴向力：全程 |f_ext · axis| 的最大值，f_ext 为世界系外力，axis 为对接轴 单位向量（世界系，取轨迹推进方向）；峰值取绝对值，接触反力沿轴反向时同样捕捉； - 峰值/稳态广义力范数：每步 6 维广义力 [f; τ] 的 2-范数的最大值/稳态均值。 注意：广义力范数包含力矩分量 τ，数值不等于纯接触力大小； - 稳态窗口：t ≥ t_end - steady_window 的采样段；稳态轴向力、稳态广义力范数、 稳态横向误差、稳态姿态误差均为该窗口内的时间均值； - 关节角度占比：|q - 限位区间中点| / 半量程 × 100%（到达任一限位时为 100%）， 使用 pin_model.lowerPositionLimit/upperPositionLimit； - 关节速度占比：|v| / velocityLimit × 100%；某轴 velocityLimit ≤ 0（或非有限） 时该轴跳过，全部无效则该项为 None；均报告全程最大百分比与对应关节编号； - 最小可操作度：min sqrt(det(J Jᵀ))，J 为末端 frame 的世界系雅可比 （pin.computeFrameJacobian(..., pin.ReferenceFrame.WORLD)，后处理逐帧计算）； - 位置跟踪 RMS：sqrt(mean(error²))，error 为每步末端位置误差范数（log.error）； - 姿态跟踪 RMS / 稳态姿态误差：基于世界系姿态误差向量 log(R_d Rᵀ) 的范数； 仅当 Log.orientation_errors 非空且长度与时间序列一致时计算，否则为 None。

所有数值字段均为 float | None，None 表示数据不足无法计算。

类

DockingMetrics dataclass

对接性能指标集合（None = 数据不足无法计算）。

源代码位于： src/compliant_docking/metrics.py

@dataclass(frozen=True)
class DockingMetrics:
    """对接性能指标集合（None = 数据不足无法计算）。"""

    # ---- 1) 接触安全 ----
    peak_axial_force_N: float | None = None
    peak_wrench_norm_N: float | None = None
    steady_axial_force_N: float | None = None
    steady_wrench_norm_N: float | None = None

    # ---- 2) 内部安全 ----
    max_joint_pos_pct: float | None = None
    max_joint_vel_pct: float | None = None
    min_manipulability: float | None = None

    # ---- 3) 跟踪精度 ----
    pos_tracking_rms_m: float | None = None
    ori_tracking_rms_rad: float | None = None
    final_lateral_error_m: float | None = None
    final_orientation_error_rad: float | None = None

    # 达到最大占比的关节编号（0 起索引）
    max_joint_pos_joint: int | None = None
    max_joint_vel_joint: int | None = None

TrackingThresholds dataclass

自由空间跟踪通过柔顺对接前的默认门槛。

源代码位于： src/compliant_docking/metrics.py

@dataclass(frozen=True)
class TrackingThresholds:
    """自由空间跟踪通过柔顺对接前的默认门槛。"""

    circle_position_rms_m: float = 0.005
    figure8_position_rms_m: float = 0.005
    position_peak_m: float = 0.015
    orientation_rms_rad: float = float(np.deg2rad(0.5))
    torque_saturation_ratio: float = 0.01
    max_contacts: int = 0

函数：

compute_metrics

compute_metrics(
    log: Log,
    pin_model: Model,
    *,
    axis: ndarray,
    ee_frame: str,
    steady_window: float = 2.0,
) -> DockingMetrics

从仿真 Log 后处理计算对接性能指标（不进热循环）。

参数：

名称	类型	描述	默认
log	Log	仿真日志（store_data 产出；可选含 orientation_errors 姿态误差序列）	必需
pin_model	Model	Pinocchio 模型（提供关节限值/速度上限/雅可比）	必需
axis	ndarray	对接轴方向（世界系；内部归一化，非单位向量也可）	必需
ee_frame	str	末端 frame 名（可操作度雅可比取自该 frame）	必需
steady_window	float	稳态窗口长度 [s]（取 t ≥ t_end - steady_window）	2.0

源代码位于： src/compliant_docking/metrics.py

def compute_metrics(log: Log, pin_model: pin.Model, *, axis: np.ndarray,
                    ee_frame: str, steady_window: float = 2.0) -> DockingMetrics:
    """从仿真 Log 后处理计算对接性能指标（不进热循环）。

    Args:
        log: 仿真日志（store_data 产出；可选含 orientation_errors 姿态误差序列）
        pin_model: Pinocchio 模型（提供关节限值/速度上限/雅可比）
        axis: 对接轴方向（世界系；内部归一化，非单位向量也可）
        ee_frame: 末端 frame 名（可操作度雅可比取自该 frame）
        steady_window: 稳态窗口长度 [s]（取 t ≥ t_end - steady_window）
    """
    axis = np.asarray(axis, dtype=float).reshape(3)
    axis_norm = float(np.linalg.norm(axis))
    if axis_norm == 0.0:
        raise ValueError("axis 不能为零向量")
    axis = axis / axis_norm

    n_steps = len(log.t_list)
    if n_steps == 0:
        return DockingMetrics()

    t_arr = np.asarray(log.t_list, dtype=float)
    steady_mask = t_arr >= t_arr[-1] - steady_window

    # ---- 1) 接触安全 ----
    forces = _stack(log.force_externals, 3)  # 世界系外力（run 循环里 current_ori @ sensor）
    torques = _stack(log.torque_externals, 3)  # 世界系外力矩
    peak_axial = steady_axial = peak_wrench = steady_wrench = None
    if forces is not None:
        axial = forces @ axis
        peak_axial = float(np.max(np.abs(axial)))
        steady_axial = float(np.mean(axial[steady_mask]))
    if forces is not None and torques is not None:
        wrench_norm = np.linalg.norm(np.hstack([forces, torques]), axis=1)
        peak_wrench = float(np.max(wrench_norm))
        steady_wrench = float(np.mean(wrench_norm[steady_mask]))

    # ---- 2) 内部安全 ----
    q_arr = _stack(log.joint_angles, pin_model.nq)
    v_arr = _stack(log.joint_velocities, pin_model.nq)
    max_pos_pct, max_pos_joint = (None, None)
    max_vel_pct, max_vel_joint = (None, None)
    min_manip = None
    if q_arr is not None:
        max_pos_pct, max_pos_joint = _joint_pct_max(
            q_arr, np.asarray(pin_model.lowerPositionLimit, dtype=float),
            np.asarray(pin_model.upperPositionLimit, dtype=float))
        if pin_model.getFrameId(ee_frame) < len(pin_model.frames):
            frame_id = pin_model.getFrameId(ee_frame)
            pin_data = pin_model.createData()
            w = np.empty(q_arr.shape[0])
            for k, q_k in enumerate(q_arr):
                J = pin.computeFrameJacobian(pin_model, pin_data, q_k, frame_id,
                                             pin.ReferenceFrame.WORLD)
                w[k] = np.sqrt(max(float(np.linalg.det(J @ J.T)), 0.0))
            min_manip = float(np.min(w))
    if v_arr is not None:
        max_vel_pct, max_vel_joint = _vel_pct_max(
            v_arr, np.asarray(pin_model.velocityLimit, dtype=float))

    # ---- 3) 跟踪精度 ----
    pos_rms = None
    if len(log.error) == n_steps:
        pos_rms = float(np.sqrt(np.mean(np.asarray(log.error, dtype=float) ** 2)))

    ori_rms = final_ori_err = None
    if len(log.orientation_errors) == n_steps:
        ori_arr = np.asarray(log.orientation_errors, dtype=float).reshape(n_steps, 3)
        ori_norms = np.linalg.norm(ori_arr, axis=1)
        ori_rms = float(np.sqrt(np.mean(ori_norms**2)))
        final_ori_err = float(np.mean(ori_norms[steady_mask]))

    final_lateral = None
    pos_act = _stack(log.pos_actual, 3)
    pos_des = _stack(log.pos_desired, 3)
    if pos_act is not None and pos_des is not None:
        e_vec = pos_act - pos_des
        e_perp = e_vec - (e_vec @ axis)[:, None] * axis
        final_lateral = float(np.mean(np.linalg.norm(e_perp, axis=1)[steady_mask]))

    return DockingMetrics(
        peak_axial_force_N=peak_axial,
        peak_wrench_norm_N=peak_wrench,
        steady_axial_force_N=steady_axial,
        steady_wrench_norm_N=steady_wrench,
        max_joint_pos_pct=max_pos_pct,
        max_joint_vel_pct=max_vel_pct,
        min_manipulability=min_manip,
        pos_tracking_rms_m=pos_rms,
        ori_tracking_rms_rad=ori_rms,
        final_lateral_error_m=final_lateral,
        final_orientation_error_rad=final_ori_err,
        max_joint_pos_joint=max_pos_joint,
        max_joint_vel_joint=max_vel_joint,
    )

format_metrics

format_metrics(m: DockingMetrics) -> str

把指标渲染为对齐的中文表格文本（三段，与 Table 10 分层一致；None 显示 n/a）。

源代码位于： src/compliant_docking/metrics.py

def format_metrics(m: DockingMetrics) -> str:
    """把指标渲染为对齐的中文表格文本（三段，与 Table 10 分层一致；None 显示 n/a）。"""

    def num(value: float | None, unit: str = "", spec: str = ".4f") -> str:
        if value is None:
            return "n/a"
        text = f"{value:{spec}}"
        return f"{text} {unit}" if unit else text

    def pct_line(pct: float | None, joint: int | None) -> str:
        if pct is None:
            return "n/a"
        where = f"（关节 {joint + 1}）" if joint is not None else ""
        return f"{pct:.2f}%{where}"

    def row(label: str, value: str) -> str:
        return f"  {_label_pad(label)}: {value}"

    lines = [
        "=" * 64,
        "对接性能指标（Ren & Shan 2026, Acta Astronautica, Table 10）",
        "=" * 64,
        "[接触安全]",
        row("峰值轴向力", num(m.peak_axial_force_N, "N")),
        row("峰值广义力范数", num(m.peak_wrench_norm_N, "N")),
        row("稳态轴向力", num(m.steady_axial_force_N, "N")),
        row("稳态广义力范数", num(m.steady_wrench_norm_N, "N")),
        "[内部安全]",
        row("最大关节角度占比", pct_line(m.max_joint_pos_pct, m.max_joint_pos_joint)),
        row("最大关节速度占比", pct_line(m.max_joint_vel_pct, m.max_joint_vel_joint)),
        row("最小可操作度", num(m.min_manipulability, spec=".6g")),
        "[跟踪精度]",
        row("位置跟踪 RMS", num(m.pos_tracking_rms_m, "m", ".6f")),
        row("姿态跟踪 RMS", num(m.ori_tracking_rms_rad, "rad", ".6f")),
        row("稳态横向误差", num(m.final_lateral_error_m, "m", ".6f")),
        row("稳态姿态误差", num(m.final_orientation_error_rad, "rad", ".6f")),
    ]
    return "\n".join(lines)

tracking_summary

tracking_summary(
    log: Log,
    segments: Sequence[tuple[str, float, float]],
) -> str

圆+8字跟踪测试的分段误差统计（位置误差 RMS/峰值，单位 mm）。

按 segments 给出的时间窗 [t0, t1) 切片 log.error（每步末端位置误差范数，m）， 计算每段 RMS 与峰值并换算为 mm；再加总全时程（全部采样点，含段外保持段）的 RMS/峰值。输出多行中文文本，打印风格与 format_metrics 对齐。

参数：

名称	类型	描述	默认
log	Log	仿真日志（t_list 与 error 逐 step 对齐）	必需
segments	Sequence[tuple[str, float, float]]	[(名称, t_start, t_end), ...]，与 CircleFigure8Trajectory.segments 同构	必需

源代码位于： src/compliant_docking/metrics.py

def tracking_summary(log: Log, segments: Sequence[tuple[str, float, float]]) -> str:
    """圆+8字跟踪测试的分段误差统计（位置误差 RMS/峰值，单位 mm）。

    按 segments 给出的时间窗 [t0, t1) 切片 log.error（每步末端位置误差范数，m），
    计算每段 RMS 与峰值并换算为 mm；再加总全时程（全部采样点，含段外保持段）的
    RMS/峰值。输出多行中文文本，打印风格与 format_metrics 对齐。

    Args:
        log: 仿真日志（t_list 与 error 逐 step 对齐）
        segments: [(名称, t_start, t_end), ...]，与
            CircleFigure8Trajectory.segments 同构
    """
    t_arr = np.asarray(log.t_list, dtype=float)
    err = np.asarray(log.error, dtype=float)
    if t_arr.size != err.size:
        err = err[:0]  # 时间与误差不对齐时不做统计（全部 n/a）

    def stats_mm(values: np.ndarray) -> tuple[float, float] | None:
        """(RMS, 峰值)，单位 mm；空切片返回 None。"""
        if values.size == 0:
            return None
        rms = float(np.sqrt(np.mean(values**2)) * 1e3)
        peak = float(np.max(values) * 1e3)
        return rms, peak

    lines = [
        "=" * 64,
        "轨迹跟踪统计（圆+8字，按段位置误差）",
        "=" * 64,
        "[分段统计]",
    ]
    for name, t0, t1 in segments:
        seg = stats_mm(err[(t_arr >= t0) & (t_arr < t1)]) if err.size else None
        if seg is None:
            lines.append(f"  {_label_pad(name)}: n/a")
        else:
            lines.append(f"  {_label_pad(name)}: RMS {seg[0]:.4f} mm, 峰值 {seg[1]:.4f} mm")

    lines.append("[全时程]")
    total = stats_mm(err)
    if total is None:
        lines.append(f"  {_label_pad('位置跟踪 RMS')}: n/a")
        lines.append(f"  {_label_pad('峰值误差')}: n/a")
    else:
        lines.append(f"  {_label_pad('位置跟踪 RMS')}: {total[0]:.4f} mm")
        lines.append(f"  {_label_pad('峰值误差')}: {total[1]:.4f} mm")
    return "\n".join(lines)

compute_tracking_metrics

compute_tracking_metrics(
    log: Log,
    segments: Sequence[tuple[str, float, float]],
) -> TrackingMetrics

从 Log 计算圆形/8 字跟踪门禁指标。

segments 使用轨迹规划器公开的 (name, start, end) 结构。姿态误差、 力矩限幅标记和接触数均是可选的向后兼容遥测字段；长度不匹配时相应指标为 None，由门禁作为数据不足处理。

源代码位于： src/compliant_docking/metrics.py

def compute_tracking_metrics(
        log: Log, segments: Sequence[tuple[str, float, float]]) -> TrackingMetrics:
    """从 ``Log`` 计算圆形/8 字跟踪门禁指标。

    ``segments`` 使用轨迹规划器公开的 ``(name, start, end)`` 结构。姿态误差、
    力矩限幅标记和接触数均是可选的向后兼容遥测字段；长度不匹配时相应指标为
    ``None``，由门禁作为数据不足处理。
    """
    n_steps = len(log.t_list)
    t_arr = np.asarray(log.t_list, dtype=float)
    position_errors = np.asarray(log.error, dtype=float)
    if t_arr.size != n_steps or position_errors.size != n_steps:
        position_errors = np.empty(0)
        t_arr = np.empty(0)

    # 门禁的总指标只描述实际测试窗口，而非轨迹结束后的保持段。分段仍保持
    # [start, end) 语义；总窗口按调用方给定的最早开始与最晚结束确定。
    window_mask: np.ndarray | None = None
    if position_errors.size and segments:
        bounds = np.asarray([(start, end) for _, start, end in segments], dtype=float)
        if bounds.shape == (len(segments), 2) and np.all(np.isfinite(bounds)) \
                and np.all(bounds[:, 1] > bounds[:, 0]):
            window_start = float(np.min(bounds[:, 0]))
            window_end = float(np.max(bounds[:, 1]))
            window_mask = (t_arr >= window_start) & (t_arr < window_end)

    orientation_norms = None
    if len(log.orientation_errors) == n_steps:
        orientation_array = np.asarray(log.orientation_errors, dtype=float)
        if orientation_array.shape == (n_steps, 3):
            orientation_norms = np.linalg.norm(orientation_array, axis=1)

    segment_metrics = tuple(
        (name, _tracking_error_metrics(
            position_errors, orientation_norms,
            (t_arr >= start) & (t_arr < end),
        ))
        for name, start, end in segments
    ) if position_errors.size else tuple(
        (name, TrackingErrorMetrics()) for name, _, _ in segments
    )
    overall = _tracking_error_metrics(
        position_errors, orientation_norms, window_mask,
    ) if window_mask is not None else TrackingErrorMetrics()

    tau = np.asarray(log.tau_hist, dtype=float)
    peak_torque = None
    if window_mask is not None and tau.ndim == 2 and tau.shape[0] == n_steps and tau.size:
        tau_window = tau[window_mask]
        if tau_window.size:
            peak_torque = float(np.max(np.abs(tau_window)))

    saturation = getattr(log, "torque_saturated", [])
    saturation_ratio = None
    if window_mask is not None and len(saturation) == n_steps and np.any(window_mask):
        saturation_ratio = float(np.mean(np.asarray(saturation, dtype=bool)[window_mask]))

    contacts = getattr(log, "contact_counts", [])
    max_contacts = None
    if window_mask is not None and len(contacts) == n_steps and np.any(window_mask):
        max_contacts = int(np.max(np.asarray(contacts, dtype=int)[window_mask]))

    return TrackingMetrics(
        segments=segment_metrics,
        overall=overall,
        peak_joint_torque_Nm=peak_torque,
        torque_saturation_ratio=saturation_ratio,
        max_contacts=max_contacts,
    )

evaluate_tracking_gate

evaluate_tracking_gate(
    metrics: TrackingMetrics,
    thresholds: TrackingThresholds,
    *,
    complete: bool,
) -> TrackingGateResult

按门槛判定跟踪测试；未完整覆盖轨迹只报告 INCOMPLETE。

源代码位于： src/compliant_docking/metrics.py

def evaluate_tracking_gate(metrics: TrackingMetrics, thresholds: TrackingThresholds,
                           *, complete: bool) -> TrackingGateResult:
    """按门槛判定跟踪测试；未完整覆盖轨迹只报告 ``INCOMPLETE``。"""
    if not complete:
        return TrackingGateResult(metrics, thresholds, "INCOMPLETE")

    failures: list[str] = []

    def is_finite_scalar(value: object) -> bool:
        """门禁只接受有限标量；NaN/inf/非标量都必须 fail closed。"""
        try:
            return bool(np.isscalar(value) and np.isfinite(value))
        except TypeError:
            return False

    def nonfinite(label: str, value: object) -> None:
        if value is not None and not is_finite_scalar(value):
            failures.append(f"{label}={value!r}（非有限或非标量）")

    # 即使某项当前没有阈值，也不能让非有限遥测借由未参与比较而通过门禁。
    for segment_name, stats in metrics.segments:
        nonfinite(f"{segment_name}位置 RMS[m]", stats.position_rms_m)
        nonfinite(f"{segment_name}位置峰值[m]", stats.position_peak_m)
        nonfinite(f"{segment_name}姿态 RMS[rad]", stats.orientation_rms_rad)
        nonfinite(f"{segment_name}姿态峰值[rad]", stats.orientation_peak_rad)
    nonfinite("全程位置 RMS[m]", metrics.overall.position_rms_m)
    nonfinite("全程位置峰值[m]", metrics.overall.position_peak_m)
    nonfinite("全程姿态 RMS[rad]", metrics.overall.orientation_rms_rad)
    nonfinite("全程姿态峰值[rad]", metrics.overall.orientation_peak_rad)
    nonfinite("峰值关节力矩[Nm]", metrics.peak_joint_torque_Nm)
    nonfinite("力矩限幅比例", metrics.torque_saturation_ratio)
    nonfinite("最大接触数", metrics.max_contacts)

    def upper_bound(label: str, value: float | int | None, limit: float | int) -> None:
        if value is None:
            failures.append(f"{label}=n/a（缺少数据）")
        elif not is_finite_scalar(value):
            return
        elif not is_finite_scalar(limit):
            failures.append(f"{label} 阈值={limit!r}（非有限或非标量）")
        elif value > limit:
            failures.append(f"{label}={value:.6g} > {limit:.6g}")

    circle = metrics.segment("圆周")
    figure8 = metrics.segment("8字")
    upper_bound("圆周位置 RMS[m]", None if circle is None else circle.position_rms_m,
                thresholds.circle_position_rms_m)
    upper_bound("8字位置 RMS[m]", None if figure8 is None else figure8.position_rms_m,
                thresholds.figure8_position_rms_m)
    upper_bound("全程位置峰值[m]", metrics.overall.position_peak_m,
                thresholds.position_peak_m)
    upper_bound("全程姿态 RMS[rad]", metrics.overall.orientation_rms_rad,
                thresholds.orientation_rms_rad)
    upper_bound("力矩限幅比例", metrics.torque_saturation_ratio,
                thresholds.torque_saturation_ratio)
    upper_bound("最大接触数", metrics.max_contacts, thresholds.max_contacts)
    return TrackingGateResult(
        metrics, thresholds, "PASS" if not failures else "FAIL", tuple(failures))

format_tracking_gate

format_tracking_gate(
    result: TrackingGateResult,
) -> str

以可审计的 PASS/FAIL/INCOMPLETE 文本格式输出跟踪门禁。

源代码位于： src/compliant_docking/metrics.py

def format_tracking_gate(result: TrackingGateResult) -> str:
    """以可审计的 PASS/FAIL/INCOMPLETE 文本格式输出跟踪门禁。"""
    def metric(value: float | None, scale: float = 1.0, unit: str = "") -> str:
        if value is None:
            return "n/a"
        return f"{value * scale:.4f}{unit}"

    lines = ["=" * 64, f"自由空间跟踪门禁: {result.status}", "=" * 64]
    for name, stats in result.metrics.segments:
        lines.append(
            f"  {name}: pos RMS {metric(stats.position_rms_m, 1e3, ' mm')}, "
            f"peak {metric(stats.position_peak_m, 1e3, ' mm')}; "
            f"ori RMS {metric(stats.orientation_rms_rad, 180.0 / np.pi, ' deg')}, "
            f"peak {metric(stats.orientation_peak_rad, 180.0 / np.pi, ' deg')}")
    overall = result.metrics.overall
    lines.extend([
        f"  全程: pos RMS {metric(overall.position_rms_m, 1e3, ' mm')}, "
        f"peak {metric(overall.position_peak_m, 1e3, ' mm')}; "
        f"ori RMS {metric(overall.orientation_rms_rad, 180.0 / np.pi, ' deg')}, "
        f"peak {metric(overall.orientation_peak_rad, 180.0 / np.pi, ' deg')}",
        f"  峰值关节力矩: {metric(result.metrics.peak_joint_torque_Nm, 1.0, ' N m')}",
        f"  力矩限幅比例: {metric(result.metrics.torque_saturation_ratio, 100.0, '%')}",
        f"  最大接触数: {result.metrics.max_contacts if result.metrics.max_contacts is not None else 'n/a'}",
    ])
    if result.status == "INCOMPLETE":
        lines.append("  结果: INCOMPLETE（仿真时长不足，未作为门禁失败）")
    elif result.failures:
        lines.append("  失败项: " + "; ".join(result.failures))
    else:
        lines.append("  结果: PASS（可进入柔顺力控对接测试）")
    return "\n".join(lines)

遥测

源代码位于： src/compliant_docking/telemetry.py

class Log:
    def __init__(self):
        # 关节数量（KUKA iiwa14 为 7 自由度）
        self.nq = 7

    def reset_logs(self):
        """重置记录数据：在每次仿真开始时调用"""

        self.t_list = []

        self.joint_angles = []
        self.joint_velocities = []

        self.pos_actual = []
        self.vel_actual = []

        self.error =[]

        self.pos_desired = []
        self.vel_desired = []
        self.acc_desired = []

        self.tau_hist = []
        # 每步控制输入是否触及软件力矩限幅、MuJoCo 当前接触对数量。
        # 保留为独立时序，便于自由空间跟踪作为对接前门禁时审计。
        self.torque_saturated = []
        self.contact_counts = []
        self.force_externals = []
        self.torque_externals = []

        # 末端姿态误差向量（世界系，log(R_d R^T)）；仅在调用方提供时记录，
        # 列表长度可能短于其他列表（metrics 侧按非空判断）
        self.orientation_errors = []

        # SE(3) Lie 控制器逐步诊断（仅 --controller se3_lie 时填充；每元素为
        # 控制器 latest_diagnostics 的标量子集 + torque_saturated，见
        # experiments/run_docking.py）。旧控制器路径不触碰该列表。
        self.se3_diagnostics = []



    def store_data(self, t: float, q: np.ndarray,
                   v: np.ndarray, pos_actual: np.ndarray,
                   vel_actual: np.ndarray, error: float,
                   pos_desired: np.ndarray, vel_desired: np.ndarray,
                   acc_desired: np.ndarray, tau: np.ndarray,
                   external_force: np.ndarray, external_torque: np.ndarray,
                   *, orientation_error: np.ndarray | None = None,
                   torque_saturated: bool = False, contact_count: int = 0):
        """
        存储数据（单步）：时间、关节状态、末端状态、期望轨迹、力矩及外力

        Args:
            t: 时间戳（秒）
            q: 当前关节角
            v: 当前关节角速度
            pos_actual: 当前末端位置
            vel_actual: 当前末端速度
            error: 末端位置跟踪误差范数
            pos_desired: 期望末端位置
            vel_desired: 期望末端速度
            acc_desired: 期望末端加速度
            tau: 控制器计算的关节力矩
            external_force: 传感器外力（控制参考系）
            external_torque: 传感器外力矩（控制参考系）
            orientation_error: 末端姿态误差向量（世界系，可选；None 时不记录）
            torque_saturated: 本步控制量是否触及软件力矩限幅（可选，默认 False）
            contact_count: 本步 MuJoCo 接触对数量（可选，默认 0）
        """

        self.t_list.append(t)

        # q/v 可能是 MuJoCo data.qpos/qvel 的视图（缓冲区随步进原地改写），
        # 必须存副本，否则整列事后读到的都是末步值 /
        # q/v may be views into MuJoCo's data.qpos/qvel buffers (mutated in
        # place each step); store copies or every entry reads as the last step
        self.joint_angles.append(np.array(q, copy=True))
        self.joint_velocities.append(np.array(v, copy=True))

        self.pos_actual.append(pos_actual)
        self.vel_actual.append(vel_actual)

        self.error.append(error)

        self.pos_desired.append(pos_desired)
        self.vel_desired.append(vel_desired)
        self.acc_desired.append(acc_desired)

        self.tau_hist.append(tau)
        self.torque_saturated.append(bool(torque_saturated))
        self.contact_counts.append(int(contact_count))
        self.force_externals.append(external_force)
        self.torque_externals.append(external_torque)

        if orientation_error is not None:
            self.orientation_errors.append(orientation_error)



    def plot_results(self, save_path: str = "figure/",
                     *, scene_name: str | None = None) -> list[Path]:
        """绘制仿真结果（SciencePlots IEEE 中文风格，委托 plotting 模块）/
        Plot simulation results (SciencePlots IEEE CJK style; delegates to plotting)."""
        # 惰性导入：telemetry 在无 scienceplots 的环境下仍可独立 import，不连累 CLI /
        # Lazy import: telemetry stays importable without scienceplots installed
        from compliant_docking.plotting import plot_docking_log

        # 每个场景一个子目录，避免多场景图件在 figure/ 根下混放 /
        # One subdirectory per scene keeps multi-scene figures out of figure/ root
        out_dir = Path(save_path) / scene_name if scene_name is not None else Path(save_path)
        return plot_docking_log(self, out_dir, scene_name=scene_name)

方法：

reset_logs

reset_logs()

重置记录数据：在每次仿真开始时调用

源代码位于： src/compliant_docking/telemetry.py

def reset_logs(self):
    """重置记录数据：在每次仿真开始时调用"""

    self.t_list = []

    self.joint_angles = []
    self.joint_velocities = []

    self.pos_actual = []
    self.vel_actual = []

    self.error =[]

    self.pos_desired = []
    self.vel_desired = []
    self.acc_desired = []

    self.tau_hist = []
    # 每步控制输入是否触及软件力矩限幅、MuJoCo 当前接触对数量。
    # 保留为独立时序，便于自由空间跟踪作为对接前门禁时审计。
    self.torque_saturated = []
    self.contact_counts = []
    self.force_externals = []
    self.torque_externals = []

    # 末端姿态误差向量（世界系，log(R_d R^T)）；仅在调用方提供时记录，
    # 列表长度可能短于其他列表（metrics 侧按非空判断）
    self.orientation_errors = []

    # SE(3) Lie 控制器逐步诊断（仅 --controller se3_lie 时填充；每元素为
    # 控制器 latest_diagnostics 的标量子集 + torque_saturated，见
    # experiments/run_docking.py）。旧控制器路径不触碰该列表。
    self.se3_diagnostics = []

store_data

store_data(
    t: float,
    q: ndarray,
    v: ndarray,
    pos_actual: ndarray,
    vel_actual: ndarray,
    error: float,
    pos_desired: ndarray,
    vel_desired: ndarray,
    acc_desired: ndarray,
    tau: ndarray,
    external_force: ndarray,
    external_torque: ndarray,
    *,
    orientation_error: ndarray | None = None,
    torque_saturated: bool = False,
    contact_count: int = 0,
)

存储数据（单步）：时间、关节状态、末端状态、期望轨迹、力矩及外力

参数：

名称	类型	描述	默认
t	float	时间戳（秒）	必需
q	ndarray	当前关节角	必需
v	ndarray	当前关节角速度	必需
pos_actual	ndarray	当前末端位置	必需
vel_actual	ndarray	当前末端速度	必需
error	float	末端位置跟踪误差范数	必需
pos_desired	ndarray	期望末端位置	必需
vel_desired	ndarray	期望末端速度	必需
acc_desired	ndarray	期望末端加速度	必需
tau	ndarray	控制器计算的关节力矩	必需
external_force	ndarray	传感器外力（控制参考系）	必需
external_torque	ndarray	传感器外力矩（控制参考系）	必需
orientation_error	ndarray | None	末端姿态误差向量（世界系，可选；None 时不记录）	None
torque_saturated	bool	本步控制量是否触及软件力矩限幅（可选，默认 False）	False
contact_count	int	本步 MuJoCo 接触对数量（可选，默认 0）	0

源代码位于： src/compliant_docking/telemetry.py

def store_data(self, t: float, q: np.ndarray,
               v: np.ndarray, pos_actual: np.ndarray,
               vel_actual: np.ndarray, error: float,
               pos_desired: np.ndarray, vel_desired: np.ndarray,
               acc_desired: np.ndarray, tau: np.ndarray,
               external_force: np.ndarray, external_torque: np.ndarray,
               *, orientation_error: np.ndarray | None = None,
               torque_saturated: bool = False, contact_count: int = 0):
    """
    存储数据（单步）：时间、关节状态、末端状态、期望轨迹、力矩及外力

    Args:
        t: 时间戳（秒）
        q: 当前关节角
        v: 当前关节角速度
        pos_actual: 当前末端位置
        vel_actual: 当前末端速度
        error: 末端位置跟踪误差范数
        pos_desired: 期望末端位置
        vel_desired: 期望末端速度
        acc_desired: 期望末端加速度
        tau: 控制器计算的关节力矩
        external_force: 传感器外力（控制参考系）
        external_torque: 传感器外力矩（控制参考系）
        orientation_error: 末端姿态误差向量（世界系，可选；None 时不记录）
        torque_saturated: 本步控制量是否触及软件力矩限幅（可选，默认 False）
        contact_count: 本步 MuJoCo 接触对数量（可选，默认 0）
    """

    self.t_list.append(t)

    # q/v 可能是 MuJoCo data.qpos/qvel 的视图（缓冲区随步进原地改写），
    # 必须存副本，否则整列事后读到的都是末步值 /
    # q/v may be views into MuJoCo's data.qpos/qvel buffers (mutated in
    # place each step); store copies or every entry reads as the last step
    self.joint_angles.append(np.array(q, copy=True))
    self.joint_velocities.append(np.array(v, copy=True))

    self.pos_actual.append(pos_actual)
    self.vel_actual.append(vel_actual)

    self.error.append(error)

    self.pos_desired.append(pos_desired)
    self.vel_desired.append(vel_desired)
    self.acc_desired.append(acc_desired)

    self.tau_hist.append(tau)
    self.torque_saturated.append(bool(torque_saturated))
    self.contact_counts.append(int(contact_count))
    self.force_externals.append(external_force)
    self.torque_externals.append(external_torque)

    if orientation_error is not None:
        self.orientation_errors.append(orientation_error)

plot_results

plot_results(
    save_path: str = "figure/",
    *,
    scene_name: str | None = None,
) -> list[Path]

绘制仿真结果（SciencePlots IEEE 中文风格，委托 plotting 模块）/ Plot simulation results (SciencePlots IEEE CJK style; delegates to plotting).

源代码位于： src/compliant_docking/telemetry.py

def plot_results(self, save_path: str = "figure/",
                 *, scene_name: str | None = None) -> list[Path]:
    """绘制仿真结果（SciencePlots IEEE 中文风格，委托 plotting 模块）/
    Plot simulation results (SciencePlots IEEE CJK style; delegates to plotting)."""
    # 惰性导入：telemetry 在无 scienceplots 的环境下仍可独立 import，不连累 CLI /
    # Lazy import: telemetry stays importable without scienceplots installed
    from compliant_docking.plotting import plot_docking_log

    # 每个场景一个子目录，避免多场景图件在 figure/ 根下混放 /
    # One subdirectory per scene keeps multi-scene figures out of figure/ root
    out_dir = Path(save_path) / scene_name if scene_name is not None else Path(save_path)
    return plot_docking_log(self, out_dir, scene_name=scene_name)

模型加载

加载 Pinocchio 模型；gravity=False 时置零重力（与 MuJoCo 模型保持一致）。

参数：

名称	类型	描述	默认
urdf_path	str | Path	模型文件路径（.urdf 或 .xml；默认取 PIN_URDF）	PIN_URDF
gravity	bool	True 时保留模型自带重力；False（默认）时把重力线性分量置零	False
tool_frame	str | None	附加固定工具惯量的末端 frame 名（tool_* 参数须成组提供）	None
tool_mount_pos	ndarray | None	工具挂载平移（相对 tool_frame）	None
tool_mount_quat	ndarray | None	工具挂载四元数（wxyz）	None
tool_mass	float | None	工具质量（须为正）	None
tool_com	ndarray | None	工具质心位置	None
tool_diaginertia	ndarray | None	工具转动惯量对角项（逐轴为正）	None

返回：

类型	描述
Model	加载（并按需附加工具惯量）后的 Pinocchio 模型

Note

按文件后缀分发解析器：.urdf 走 URDF 解析，.xml（MJCF，如 FR3 的 Menagerie 模型变体）走 buildModelFromMJCF。两条路线同样置零重力。

源代码位于： src/compliant_docking/models.py

def load_pin_model(urdf_path=PIN_URDF, gravity: bool = False, *,
                   tool_frame: str | None = None,
                   tool_mount_pos: np.ndarray | None = None,
                   tool_mount_quat: np.ndarray | None = None,
                   tool_mass: float | None = None,
                   tool_com: np.ndarray | None = None,
                   tool_diaginertia: np.ndarray | None = None):
    """加载 Pinocchio 模型；gravity=False 时置零重力（与 MuJoCo 模型保持一致）。

    Args:
        urdf_path (str | Path): 模型文件路径（``.urdf`` 或 ``.xml``；默认取 PIN_URDF）
        gravity: True 时保留模型自带重力；False（默认）时把重力线性分量置零
        tool_frame: 附加固定工具惯量的末端 frame 名（tool_* 参数须成组提供）
        tool_mount_pos: 工具挂载平移（相对 tool_frame）
        tool_mount_quat: 工具挂载四元数（``wxyz``）
        tool_mass: 工具质量（须为正）
        tool_com: 工具质心位置
        tool_diaginertia: 工具转动惯量对角项（逐轴为正）

    Returns:
        (pin.Model): 加载（并按需附加工具惯量）后的 Pinocchio 模型

    Note:
        按文件后缀分发解析器：``.urdf`` 走 URDF 解析，``.xml``（MJCF，如 FR3 的
        Menagerie 模型变体）走 buildModelFromMJCF。两条路线同样置零重力。
    """
    if Path(urdf_path).suffix.lower() == ".xml":
        model = pin.buildModelFromMJCF(str(urdf_path))
    else:
        model = pin.buildModelFromUrdf(str(urdf_path))
    tool_args = (tool_frame, tool_mount_pos, tool_mount_quat,
                 tool_mass, tool_com, tool_diaginertia)
    if any(arg is not None for arg in tool_args):
        if any(arg is None for arg in tool_args):
            raise ValueError("附加工具惯量需要完整的 frame、挂载位姿和惯量参数")
        _append_tool_inertia(
            model,
            tool_frame=tool_frame,
            tool_mount_pos=tool_mount_pos,
            tool_mount_quat=tool_mount_quat,
            tool_mass=tool_mass,
            tool_com=tool_com,
            tool_diaginertia=tool_diaginertia,
        )
    if not gravity:
        # 注意：此版本 pinocchio 的 gravity.linear 需要 Eigen 向量，不能传 Python 元组 /
        # Note: this pinocchio build requires an Eigen vector, not a Python tuple
        model.gravity.linear = np.array([0.0, 0.0, 0.0])
    return model

绘图

plotting.py - SciencePlots IEEE 中文绘图 / SciencePlots IEEE plotting with CJK support

项目统一绘图入口：基于 SciencePlots 的 ["science", "ieee", "no-latex"] 风格 （IEEE 单栏、不依赖 LaTeX），叠加中文字体回退（Noto CJK）与 axes.unicode_minus=False（中文字体缺 U+2212 负号），每张图同时输出 PNG（位图）与 PDF（矢量）。

Unified plotting entry point for the project. Built on SciencePlots' ["science", "ieee", "no-latex"] style (IEEE single column, no LaTeX) with a CJK font fallback (Noto) and axes.unicode_minus=False (CJK fonts lack the U+2212 minus sign). Every figure is saved as both PNG (raster) and PDF (vector).

Author: langxin11 Date: 2025

函数：

apply_style

apply_style() -> None

应用 SciencePlots IEEE + 中文字体回退样式（幂等，可重复调用）/ Apply the SciencePlots IEEE style with CJK font fallback (idempotent).

源代码位于： src/compliant_docking/plotting.py

def apply_style() -> None:
    """应用 SciencePlots IEEE + 中文字体回退样式（幂等，可重复调用）/
    Apply the SciencePlots IEEE style with CJK font fallback (idempotent)."""
    # 导入 scienceplots 即向 matplotlib 注册样式表（此处无需直接引用）/
    # Importing scienceplots registers its stylesheets with matplotlib
    import scienceplots  # noqa: F401

    # 顺序固定：science → ieee → no-latex，确保 text.usetex 关闭 /
    # Order matters: science → ieee → no-latex, guaranteeing text.usetex = False
    plt.style.use(["science", "ieee", "no-latex"])

    # 中文字体回退（覆盖在 style.use 之后）：具体字体列表必须放在 font.family——
    # matplotlib 仅对 font.family 列表构建逐字形回退链；font.serif 这类泛型别名
    # 只会解析出单个最优字体（本机装有真实 Times New Roman，中文将无回退而变方框）。
    # Times New Roman 渲染西文，CJK 字形落到 Noto；mathtext 用 STIX 与衬线体一致；
    # 中文字体普遍缺 U+2212，必须改用 ASCII 负号 /
    # CJK fallback applied after style.use: the concrete list must live in
    # font.family -- matplotlib only builds a per-glyph fallback chain from
    # font.family entries; a generic alias like "serif" resolves to a single
    # best-match font (real Times New Roman here), leaving CJK glyphs without
    # fallback (tofu). Times New Roman renders Latin, CJK falls to Noto; STIX
    # mathtext matches the serif look; CJK fonts lack U+2212 so the ASCII
    # minus is required
    mpl.rcParams["font.family"] = ["Times New Roman", "Noto Serif CJK SC",
                                   "Noto Sans CJK SC", "DejaVu Serif"]
    mpl.rcParams["font.serif"] = ["Times New Roman", "Noto Serif CJK SC",
                                  "Noto Sans CJK SC", "DejaVu Serif"]
    mpl.rcParams["mathtext.fontset"] = "stix"
    mpl.rcParams["axes.unicode_minus"] = False

plot_docking_log

plot_docking_log(
    log: Log,
    out_dir: str | Path,
    *,
    scene_name: str | None = None,
    docking_axis: int = 2,
    dpi: int = 600,
) -> list[Path]

绘制对接仿真结果图（SciencePlots IEEE 中文风格，PNG + PDF 双格式）/ Plot docking simulation results (SciencePlots IEEE style, PNG + PDF).

参数：

名称	类型	描述	默认
log	Log	已灌入时序数据的 telemetry.Log（须先 reset_logs + store_data）/ Populated telemetry.Log (reset_logs + store_data first)	必需
out_dir	str | Path	图件输出目录（不存在则创建）/ Output directory (created if missing)	必需
scene_name	str | None	文件名前缀；None 时用 "docking_" / Filename prefix; "docking_" if None	None
docking_axis	int	对接轴索引（默认 2 = 世界 Z）/ Docking axis index (default 2 = world Z)	2
dpi	int	PNG 输出分辨率 / PNG resolution	600

返回：

类型	描述
list[Path]	生成的全部文件路径列表 / List of all generated file paths

源代码位于： src/compliant_docking/plotting.py

def plot_docking_log(log: "Log", out_dir: str | Path, *,
                     scene_name: str | None = None,
                     docking_axis: int = 2,
                     dpi: int = 600) -> list[Path]:
    """
    绘制对接仿真结果图（SciencePlots IEEE 中文风格，PNG + PDF 双格式）/
    Plot docking simulation results (SciencePlots IEEE style, PNG + PDF).

    Args:
        log: 已灌入时序数据的 telemetry.Log（须先 reset_logs + store_data）/
            Populated telemetry.Log (reset_logs + store_data first)
        out_dir: 图件输出目录（不存在则创建）/ Output directory (created if missing)
        scene_name: 文件名前缀；None 时用 "docking_" / Filename prefix; "docking_" if None
        docking_axis: 对接轴索引（默认 2 = 世界 Z）/ Docking axis index (default 2 = world Z)
        dpi: PNG 输出分辨率 / PNG resolution

    Returns:
        生成的全部文件路径列表 / List of all generated file paths
    """
    apply_style()

    if len(log.t_list) == 0:
        raise ValueError("日志为空，请先仿真并记录数据 / empty log: run the simulation first")

    prefix = f"{scene_name}_" if scene_name is not None else "docking_"
    out = Path(out_dir)
    out.mkdir(parents=True, exist_ok=True)

    # 时间轴与各时序列表统一转 ndarray / Convert all logged series to ndarrays
    t = np.asarray(log.t_list, dtype=float)
    pos_des = np.asarray(log.pos_desired, dtype=float).reshape(-1, 3)
    pos_act = np.asarray(log.pos_actual, dtype=float).reshape(-1, 3)
    forces = np.asarray(log.force_externals, dtype=float).reshape(-1, 3)
    tau = np.atleast_2d(np.asarray(log.tau_hist, dtype=float))

    saved: list[Path] = []

    def _save(fig: plt.Figure, stem: str) -> None:
        # 每张图同时落 PNG（dpi）与 PDF（矢量）/ Save each figure as PNG (dpi) and PDF (vector)
        for suffix in (".png", ".pdf"):
            path = out / f"{stem}{suffix}"
            fig.savefig(path, dpi=dpi)
            saved.append(path)
        plt.close(fig)

    # 1. 末端三轴位置跟踪（期望虚线 vs 实际实线）/
    # 1. EE per-axis position tracking (desired dashed vs actual solid)
    fig, axes = plt.subplots(3, 1, sharex=True,
                             figsize=(_FIG_WIDTH, 4.6), constrained_layout=True)
    for i, ax in enumerate(axes):
        ax.plot(t, pos_des[:, i], "--", label="期望")
        ax.plot(t, pos_act[:, i], "-", label="实际")
        ax.set_ylabel(f"{'XYZ'[i]} 位置 [m]")
    axes[0].legend(ncols=2)
    axes[-1].set_xlabel("时间 [s]")
    _save(fig, f"{prefix}ee_tracking")

    # 2. 跟踪误差范数（对数轴；clip 防 log(0)）/
    # 2. Tracking error norm (log axis; clip guards log(0))
    err_mm = np.clip(np.asarray(log.error, dtype=float) * 1000.0, 1e-9, None)
    fig, ax = plt.subplots(figsize=(_FIG_WIDTH, 2.2), constrained_layout=True)
    ax.semilogy(t, err_mm)
    ax.set_ylabel("跟踪误差范数 [mm]")
    ax.set_xlabel("时间 [s]")
    _save(fig, f"{prefix}tracking_error")

    # 3. 接触力：合力范数与对接轴向分量 /
    # 3. Contact force: resultant norm and docking-axis component
    fig, ax = plt.subplots(figsize=(_FIG_WIDTH, 2.2), constrained_layout=True)
    ax.plot(t, np.linalg.norm(forces, axis=1), label="合力范数")
    ax.plot(t, forces[:, docking_axis], label="对接轴向分量")
    ax.set_ylabel("接触力 [N]")
    ax.set_xlabel("时间 [s]")
    ax.legend(ncols=2)
    _save(fig, f"{prefix}contact_force")

    # 4. 各关节力矩（4x2 栅格，第 8 格置空）/
    # 4. Joint torques (4x2 grid, 8th cell hidden)
    fig, axes = plt.subplots(4, 2, sharex=True,
                             figsize=(_FIG_WIDTH, 4.6), constrained_layout=True)
    for i, ax in enumerate(axes.flat):
        if i < tau.shape[1]:
            ax.plot(t, tau[:, i])
            ax.set_ylabel(f"J{i + 1} [N·m]")
        else:
            ax.set_visible(False)
    for ax in axes[-1]:
        if ax.get_visible():
            ax.set_xlabel("时间 [s]")
    _save(fig, f"{prefix}joint_torques")

    # 5. 姿态误差范数（弧度→度；仅在记录了姿态误差时生成，
    #    列表可能短于时间轴，按其自身长度取时间切片）/
    # 5. Orientation error norm (rad→deg); only when logged. The list may be
    #    shorter than the time axis, so slice time to its own length.
    if len(log.orientation_errors) > 0:
        ori = np.asarray(log.orientation_errors, dtype=float).reshape(-1, 3)
        ori_deg = np.linalg.norm(ori, axis=1) * 180.0 / np.pi
        fig, ax = plt.subplots(figsize=(_FIG_WIDTH, 2.2), constrained_layout=True)
        ax.plot(t[: len(ori_deg)], ori_deg)
        ax.set_ylabel("姿态误差范数 [°]")
        ax.set_xlabel("时间 [s]")
        _save(fig, f"{prefix}orientation_error")

    # 6. 规划轨迹本体：上图为 3D 末端路径（规划虚线 vs 实际实线），
    #    下图为速度剖面（两段式轨迹的对接段限速平台在此直接可见）。
    #    Axes3D 的刻度/轴标签不参与 constrained_layout 的占位计算，会裁切进
    #    下方子图，故本图关闭 constrained layout，改用手动边距 /
    # 6. The planned trajectory itself: top panel is the 3D EE path
    #    (planned dashed vs actual solid), bottom panel the speed profile,
    #    where the slow docking-phase plateau is directly visible. Axes3D
    #    tick/axis labels are not measured by constrained_layout (they would
    #    be clipped under the lower panel), so manual margins are used here.
    vel_des = np.asarray(log.vel_desired, dtype=float).reshape(-1, 3)
    vel_act = np.asarray(log.vel_actual, dtype=float).reshape(-1, 3)
    fig = plt.figure(figsize=(_FIG_WIDTH, 5.0))
    ax3d = fig.add_subplot(2, 1, 1, projection="3d")
    ax3d.plot(pos_des[:, 0], pos_des[:, 1], pos_des[:, 2], "--", label="规划")
    ax3d.plot(pos_act[:, 0], pos_act[:, 1], pos_act[:, 2], "-", label="实际")
    for axis in (ax3d.xaxis, ax3d.yaxis, ax3d.zaxis):
        axis.set_major_locator(mpl.ticker.MaxNLocator(3))
    ax3d.set_xlabel("X [m]")
    ax3d.set_ylabel("Y [m]")
    ax3d.set_zlabel("Z [m]")
    ax3d.legend(ncols=2)
    ax_sp = fig.add_subplot(2, 1, 2)
    ax_sp.plot(t, np.linalg.norm(vel_des, axis=1), "--", label="规划")
    ax_sp.plot(t, np.linalg.norm(vel_act, axis=1), "-", label="实际")
    ax_sp.set_ylabel("末端速度 [m/s]")
    ax_sp.set_xlabel("时间 [s]")
    ax_sp.legend(ncols=2)
    fig.subplots_adjust(left=0.15, right=0.84, top=0.97, bottom=0.10, hspace=0.42)
    _save(fig, f"{prefix}planned_trajectory")

    return saved