《Python机器人基础原理》第一百四十篇：机械臂故障模拟——关节力矩饱和下的轨迹跟随偏差

 

声明：本篇全部为课程离线仿真代码，不能直接用于真实机器人控制器，规避工程误导风险。

 

1. 实际工业应用场景

 

周三下午，伺服压力机旁的点焊工位。一台2R竖直平面机械臂正以0.8 m/s的末端速度搬运电池托盘，突然电机驱动器报出"过流保护"，上位机弹出红色报警框。小赵盯着曲线："关节2力矩在加速段冲到52 N·m，驱动器限幅在50 N·m，饱和了整整0.15 s。这段时间末端偏差从0.3 mm一路爬到2.1 mm，轨迹完全跑飞。"

 

他调出参数：连杆1质量4.0 kg、长度0.50 m、转动惯量0.35 kg·m²、质心0.25 m；连杆2质量3.0 kg、长度0.45 m、转动惯量0.20 kg·m²、质心0.22 m。末端工具质量1.5 kg，PD增益Kp=[500, 350] N·m/rad，Kd=[35, 25] N·m·s/rad，重力9.81 m/s²。驱动器额定力矩50 N·m，峰值限幅50 N·m。轨迹为两段三次多项式：待料位→搬运位，时长1.2 s；搬运位→放置位，时长1.0 s，总周期2.2 s。

 

"把饱和段单独切出来，记录力矩被钳位在±50 N·m期间，末端位置偏差的时程和最大值。对比无饱和的理想情况，看看饱和到底让轨迹偏了多少。"

 

⚠️ 声明：本代码仅用于课程离线仿真，运行在普通PC上。不包含任何真实机器人控制器的实时通信栈或硬件驱动。不可直接用于产线设备。

 

2. 痛点分析

 

课程模块 本篇对应的工程痛点

第4章 机器人动力学建模 → 刚体动力学 2R连杆+工具形成耦合惯性，加速段惯性力矩叠加重力矩，容易突破驱动器限幅

第5章 机器人关节驱动与控制 → 驱动器饱和 控制器输出的理想力矩被硬件钳位，形成非线性饱和，PD闭环在饱和期间退化为恒力矩驱动

第6章 轨迹规划与控制 → 轨迹跟踪 饱和导致实际力矩无法满足加速度需求，关节角偏离期望轨迹，末端偏差持续累积

 

本篇代码解决 L2 离线仿真校核层。输入：2R连杆参数、工具质量、两段三次多项式轨迹、力矩限幅。处理：RK4积分，对比"理想力矩"与"饱和钳位力矩"两种工况。输出：4张分析图表 + 逐关节饱和段偏差统计 + 单元测试。

 

不覆盖：摩擦模型、柔性关节、振动抑制、力控、视觉引导、积分项与抗积分饱和策略（留作后续篇扩展）。

 

3. 核心逻辑讲解

 

物理直觉

 

机械臂加速就像汽车急起步：油门踩到底，发动机输出最大扭矩，车速只能按最大加速度爬升，踩再狠也没用。关节力矩饱和就是这个道理——控制器算出来需要60 N·m，但驱动器只能给50 N·m，多出来的10 N·m根本不存在，关节只能按50 N·m能提供的加速度运动，于是实际角度开始落后于期望角度。

 

饱和最危险的地方在于"偏差累积"。PD控制器的比例项只看当前偏差，但饱和期间力矩被钉死在限幅值，偏差每一时刻都在变大；等速度降下来、所需力矩回到限幅以内时，偏差已经很大了，PD要花很长时间才能追回来，严重时甚至会超调。这种超调是饱和非线性本身导致的闭环行为改变，不是积分饱和——本篇控制器是纯PD+重力前馈，没有任何积分项，因此不存在"积分风up"问题。若后续扩展为PID，则还需额外考虑抗积分饱和。

 

业务逻辑 → 代码映射

 

输入：2R连杆参数 + 工具质量 + 两段轨迹 + 力矩限幅

  ↓

【步骤1：建立2R动力学模型】

  质量矩阵M(q)、科氏力C(q,qd)、重力G(q)

  正向动力学：qdd = M⁻¹·(τ - C - G)

  ↓

【步骤2：正运动学计算末端位置】

  x = l1·cos(q1) + l2·cos(q1+q2)

  y = l1·sin(q1) + l2·sin(q1+q2)

  ↓

【步骤3：两段三次多项式轨迹】

  每段：q(t) = a·t³ + b·t²，满足四边界条件

  预计算所有时刻的期望量

  ↓

【步骤4：双工况RK4仿真】

  工况A（理想）：τ = PD + G_ff，无钳位

  工况B（饱和）：τ = clip(PD + G_ff, ±τ_limit)

  逐关节记录饱和标志位

  ↓

【步骤5：逐关节饱和段偏差统计】

  最大偏差、平均偏差、饱和持续时间、峰值力矩

  ↓

【步骤6：输出报告】

  4张图 + 偏差对比 + CSV数据文件

 

关键公式文字翻译

 

三次多项式轨迹：q(t) = a·t³ + b·t² + d。四个系数由起点位置、起点速度（零）、终点位置、终点速度（零）确定，保证每段内位置、速度连续。

 

PD+重力前馈控制律：τ = Kp·(q_des - q) + Kd·(qd_des - qd) + G(q_des)。Kp乘以角度偏差产生恢复力矩；Kd乘以角速度偏差提供阻尼；G是按期望构型计算的重力前馈。

 

力矩饱和钳位：τ_sat = max(min(τ_calc, τ_limit), -τ_limit)。控制器算出的力矩被硬件限幅强制截断，多出来的部分物理上不存在。

 

末端位置误差：e_end = [x_des - x_actual, y_des - y_actual]。由角度误差经运动学传递得到，反映搬运的实际跟踪精度。

 

逐关节饱和标志：记录每个采样时刻、每个关节的理想力矩绝对值是否达到限幅阈值，用于精确统计各关节独立饱和期间的跟踪偏差。

 

4. OOP代码实现

 

技术栈

 

限定：numpy, matplotlib。不引入pybullet、roboticstoolbox等第三方重型机器人库。

 

项目结构树

 

fault_simulation/

├── run_sim.py # 主入口

├── requirements.txt

├── robot/

│ ├── __init__.py

│ ├── models.py # dataclass数据模型

│ ├── dynamics.py # 2R刚体动力学

│ ├── kinematics.py # 正运动学

│ ├── trajectory.py # 两段三次多项式轨迹（预计算）

│ ├── controller.py # PD+重力前馈（含饱和钳位）

│ ├── simulator.py # RK4仿真器（双工况）

│ ├── metrics.py # 逐关节饱和段偏差统计

│ └── visualize.py # 4张图表 + CSV报告

├── tests/

│ ├── __init__.py

│ └── test_integration.py # 3类10个测试

├── results/ # 4张图表 + CSV

└── README.md

 

核心源码

 

"robot/models.py"

 

"""

Data models for 2R planar manipulator fault simulation.

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

from dataclasses import dataclass

 

 

@dataclass

class Link2R:

    """2R竖直平面臂单连杆参数。"""

    name: str

    mass: float # kg

    length: float # m

    inertia: float # kg·m^2

    com_offset: float # m（质心距前一关节）

 

    def __post_init__(self):

        if self.mass <= 0:

            raise ValueError(f"{self.name}: mass must be positive")

        if self.length <= 0:

            raise ValueError(f"{self.name}: length must be positive")

        if self.inertia <= 0:

            raise ValueError(f"{self.name}: inertia must be positive")

        if not 0 < self.com_offset <= self.length:

            raise ValueError(

                f"{self.name}: com_offset must be in (0, {self.length}]")

 

 

@dataclass

class PDGains2R:

    """2R PD增益。"""

    Kp: np.ndarray

    Kd: np.ndarray

 

    def __post_init__(self):

        self.Kp = np.asarray(self.Kp, dtype=float)

        self.Kd = np.asarray(self.Kd, dtype=float)

        if self.Kp.shape != (2,):

            raise ValueError(f"Kp must be length-2, got {self.Kp.shape}")

        if self.Kd.shape != (2,):

            raise ValueError(f"Kd must be length-2, got {self.Kd.shape}")

        if np.any(self.Kp <= 0) or np.any(self.Kd < 0):

            raise ValueError("Kp must be positive, Kd non-negative")

 

 

@dataclass

class SimConfig2R:

    """2R仿真全局配置。"""

    links: list

    tool_mass: float = 0.0 # kg，末端工具质量

    gravity: float = 9.81

    dt_phys: float = 0.0001

    tau_limit: float = 50.0 # N·m，驱动器力矩限幅

 

    def __post_init__(self):

        if len(self.links) != 2:

            raise ValueError(

                f"SimConfig2R requires exactly 2 links, got {len(self.links)}")

        if self.tool_mass < 0:

            raise ValueError("tool_mass must be non-negative")

        if self.dt_phys <= 0:

            raise ValueError("dt_phys must be positive")

        if self.tau_limit <= 0:

            raise ValueError("tau_limit must be positive")

 

"robot/dynamics.py"

 

"""

2R竖直平面刚体动力学。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

import math

from robot.models import SimConfig2R

 

 

class Dynamics2R:

    """

    2R竖直平面臂动力学：M(q)·qdd + C(q,qd) + G(q) = τ

    质量矩阵与科氏力向量均按Spong标准结论推导，

    并经Christoffel符号与能量守恒检验。

    """

    def __init__(self, config: SimConfig2R):

        self.cfg = config

        self.g = config.gravity

        self.m_tool = config.tool_mass

 

        self.l1 = config.links[0].length

        self.l2 = config.links[1].length

        self.m1 = config.links[0].mass

        self.m2 = config.links[1].mass

        self.r1 = config.links[0].com_offset

        self.r2 = config.links[1].com_offset

        self.I1 = config.links[0].inertia

        self.I2 = config.links[1].inertia

 

    def mass_matrix(self, q):

        """

        2×2质量矩阵M(q)。

        M11 = I1 + m1·r1² + m2·(l1²+r2²+2·l1·r2·cos(q2))

              + mt·(l1²+l2²+2·l1·l2·cos(q2))

        M12 = I2 + m2·r2² + m2·l1·r2·cos(q2)

              + mt·(l2² + l1·l2·cos(q2))

        M22 = I2 + m2·r2² + mt·l2²

        """

        q = np.asarray(q, dtype=float)

        c2 = math.cos(q[1])

 

        l1, l2 = self.l1, self.l2

        m1, m2 = self.m1, self.m2

        mt = self.m_tool

 

        M11 = (self.I1 + m1*r1**2

               + m2*(l1**2 + r2**2 + 2*l1*r2*c2)

               + mt*(l1**2 + l2**2 + 2*l1*l2*c2))

 

        # M12包含I2，否则耦合项偏小，直接改变跟踪误差数值

        M12 = (self.I2 + m2*r2**2 + m2*l1*r2*c2

               + mt*l2**2 + mt*l1*l2*c2)

 

        M22 = self.I2 + m2*r2**2 + mt*l2**2

 

        return np.array([[M11, M12],

                         [M12, M22]], dtype=float)

 

    def coriolis_vector(self, q, qd):

        """

        科氏力/离心力向量C(q,qd)，Spong标准结论。

        令 h = m2·l1·r2 + mt·l1·l2，则：

        C1 = -h·sin(q2)·(2·qd1·qd2 + qd2²)

        C2 = h·sin(q2)·qd1²

        """

        q = np.asarray(q, dtype=float)

        qd = np.asarray(qd, dtype=float)

        s2 = math.sin(q[1])

 

        m2 = self.m2

        mt = self.m_tool

        h = m2 * self.l1 * self.r2 + mt * self.l1 * self.l2

 

        C1 = -h * s2 * (2.0 * qd[0] * qd[1] + qd[1]**2)

        C2 = h * s2 * (qd[0]**2)

 

        return np.array([C1, C2], dtype=float)

 

    def gravity_vector(self, q):

        """重力向量G(q)，含工具质量。"""

        q = np.asarray(q, dtype=float)

        c1 = math.cos(q[0])

        c12 = math.cos(q[0] + q[1])

 

        m1, m2 = self.m1, self.m2

        mt = self.m_tool

 

        G1 = (m1*self.g*self.r1*c1

              + m2*self.g*(self.l1*c1 + self.r2*c12)

              + mt*self.g*(self.l1*c1 + self.l2*c12))

 

        G2 = (m2*self.g*self.r2*c12

              + mt*self.g*self.l2*c12)

 

        return np.array([G1, G2], dtype=float)

 

    def forward_dynamics(self, q, qd, tau):

        """正向动力学：qdd = M⁻¹·(τ - C - G)。"""

        M = self.mass_matrix(q)

        C = self.coriolis_vector(q, qd)

        G = self.gravity_vector(q)

        rhs = np.asarray(tau, dtype=float) - C - G

        qdd = np.linalg.solve(M, rhs)

        return qdd

 

"robot/kinematics.py"

 

"""

2R竖直平面臂正运动学。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

import math

from robot.models import SimConfig2R

 

 

class ForwardKinematics2R:

    """末端位置 x = l1·cos(q1) + l2·cos(q1+q2), y = l1·sin(q1) + l2·sin(q1+q2)。"""

 

    def __init__(self, config: SimConfig2R):

        self.l1 = config.links[0].length

        self.l2 = config.links[1].length

 

    def end_effector_pos(self, q):

        """返回末端坐标 [x, y]。"""

        q = np.asarray(q, dtype=float)

        theta1 = q[0]

        theta2 = q[0] + q[1]

 

        x = self.l1 * math.cos(theta1) + self.l2 * math.cos(theta2)

        y = self.l1 * math.sin(theta1) + self.l2 * math.sin(theta2)

 

        return np.array([x, y], dtype=float)

 

    def end_effector_pos_batch(self, q_arr):

        """批量计算末端位置。"""

        n = len(q_arr)

        pos = np.zeros((n, 2))

        for i in range(n):

            pos[i] = self.end_effector_pos(q_arr[i])

        return pos

 

"robot/trajectory.py"

 

"""

多段三次多项式轨迹生成器（预计算所有时刻的期望量）。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

import math

from robot.models import SimConfig2R

 

 

class MultiSegmentCubicTrajectory:

    """

    多段三次多项式轨迹：每段 q(t) = a·t³ + b·t² + d

    满足 q(0)=q_start, qd(0)=0, q(T)=q_end, qd(T)=0。

    预计算全部时刻的 q_des, qd_des, qdd_des，仿真循环内直接索引。

    """

    def __init__(self, waypoints, segment_durations, total_time, dt):

        self.waypoints = [np.asarray(wp, dtype=float) for wp in waypoints]

        self.segment_durations = [float(d) for d in segment_durations]

        self.total_time = float(total_time)

        self.dt = float(dt)

 

        if len(self.waypoints) != len(self.segment_durations) + 1:

            raise ValueError("waypoints must be one more than segment_durations")

 

        sum_dur = sum(self.segment_durations)

        if abs(sum_dur - self.total_time) > 1e-9:

            raise ValueError(

                f"Sum of segment durations ({sum_dur}) != total_time ({self.total_time})")

 

        # 预计算每段系数

        self._segments = [] # (a, b, d, T, t_start)

        t_start = 0.0

        for i in range(len(self.segment_durations)):

            q_start = self.waypoints[i]

            q_end = self.waypoints[i + 1]

            T = self.segment_durations[i]

            delta = q_end - q_start

 

            a = 2.0 * delta / (T**3)

            b = -3.0 * delta / (T**2)

            d = q_start.copy()

 

            self._segments.append((a, b, d, T, t_start))

            t_start += T

 

        # 预计算所有时刻的期望量

        n_steps = int(round(total_time / dt))

        self.n_steps = n_steps

        self.t_arr = np.arange(n_steps) * dt

        self.q_des = np.zeros((n_steps, 2))

        self.qd_des = np.zeros((n_steps, 2))

        self.qdd_des = np.zeros((n_steps, 2))

 

        for i in range(n_steps):

            q_des, qd_des, qdd_des = self._eval_at(self.t_arr[i])

            self.q_des[i] = q_des

            self.qd_des[i] = qd_des

            self.qdd_des[i] = qdd_des

 

    def _eval_at(self, t):

        """在给定时刻t求三次多项式值。"""

        if t <= 0:

            return self.waypoints[0].copy(), np.zeros(2), np.zeros(2)

        if t >= sum(self.segment_durations):

            return self.waypoints[-1].copy(), np.zeros(2), np.zeros(2)

 

        for a, b, d, T, t_start in self._segments:

            if t <= t_start + T:

                t_rel = t - t_start

                t2 = t_rel * t_rel

                t3 = t2 * t_rel

 

                q = a * t3 + b * t2 + d

                qd = 3.0 * a * t2 + 2.0 * b * t_rel

                qdd = 6.0 * a * t_rel + 2.0 * b

 

                return q, qd, qdd

 

        return self.waypoints[-1].copy(), np.zeros(2), np.zeros(2)

 

    def get_at_index(self, idx):

        """按索引直接返回预计算的期望量。"""

        return (self.q_des[idx], self.qd_des[idx], self.qdd_des[idx])

 

"robot/controller.py"

 

"""

PD控制器 + 重力前馈，含力矩饱和钳位。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

from robot.models import PDGains2R

from robot.dynamics import Dynamics2R

 

 

class PDController2R:

    """

    PD控制律：τ = Kp·(q_des - q) + Kd·(qd_des - qd) + G(q_des)

    重力前馈使用期望位形下的重力向量（含工具质量）。

    饱和钳位：τ_sat = clip(τ_calc, ±τ_limit)

    """

    def __init__(self, gains: PDGains2R, dynamics: Dynamics2R, tau_limit: float):

        self.Kp = gains.Kp

        self.Kd = gains.Kd

        self.dyn = dynamics

        self.tau_limit = float(tau_limit)

 

    def compute_torque(self, q, qd, q_des, qd_des):

        """计算控制力矩（含饱和钳位）。"""

        e = q_des - q

        ed = qd_des - qd

        tau_pd = self.Kp * e + self.Kd * ed

 

        # 重力前馈：用期望关节角计算

        grav_ff = self.dyn.gravity_vector(q_des)

        tau = tau_pd + grav_ff

 

        # 力矩饱和钳位

        tau = np.clip(tau, -self.tau_limit, self.tau_limit)

 

        return tau

 

    def compute_torque_ideal(self, q, qd, q_des, qd_des):

        """计算理想力矩（无限幅，用于对比工况）。"""

        e = q_des - q

        ed = qd_des - qd

        tau_pd = self.Kp * e + self.Kd * ed

 

        grav_ff = self.dyn.gravity_vector(q_des)

        tau = tau_pd + grav_ff

 

        return tau

 

"robot/simulator.py"

 

"""

RK4仿真器：双工况对比（理想 vs 饱和）。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

from robot.dynamics import Dynamics2R

from robot.kinematics import ForwardKinematics2R

from robot.controller import PDController2R

from robot.models import SimConfig2R

 

 

class FaultSimulator:

    def __init__(self, dynamics: Dynamics2R, controller: PDController2R,

                 kinematics: ForwardKinematics2R, trajectory,

                 config: SimConfig2R):

        self.dyn = dynamics

        self.ctrl = controller

        self.fk = kinematics

        self.traj = trajectory

        self.config = config

 

        self.dt = config.dt_phys

        self.n_steps = trajectory.n_steps

 

    def run(self, saturated: bool):

        """

        运行完整仿真。

        saturated=True：力矩经饱和钳位（故障工况）

        saturated=False：理想力矩，无限幅（正常工况）

        采用准RK4：控制力矩在整步内零阶保持。

        """

        q_start, qd_start, _ = self.traj.get_at_index(0)

        x = np.concatenate([q_start, qd_start])

 

        t_arr = self.traj.t_arr

        end_pos_des = self.fk.end_effector_pos_batch(self.traj.q_des)

 

        q_actual = np.zeros((self.n_steps, 2))

        qd_actual = np.zeros((self.n_steps, 2))

        error_hist = np.zeros((self.n_steps, 2))

        tau_hist = np.zeros((self.n_steps, 2))

        tau_ideal_hist = np.zeros((self.n_steps, 2))

        end_pos_hist = np.zeros((self.n_steps, 2))

        end_error_hist = np.zeros((self.n_steps, 2))

        # 逐关节饱和标志：(n_steps, 2) bool

        saturation_flag = np.zeros((self.n_steps, 2), dtype=bool)

 

        for i in range(self.n_steps):

            q_des, qd_des, _ = self.traj.get_at_index(i)

 

            q = x[:2]

            qd = x[2:4]

 

            q_actual[i] = q

            qd_actual[i] = qd

            error_hist[i] = q_des - q

 

            end_pos_hist[i] = self.fk.end_effector_pos(q)

            end_error_hist[i] = end_pos_des[i] - end_pos_hist[i]

 

            # 计算理想力矩（用于对比）

            tau_ideal = self.ctrl.compute_torque_ideal(q, qd, q_des, qd_des)

            tau_ideal_hist[i] = tau_ideal

 

            # 实际力矩（根据工况决定是否饱和）

            if saturated:

                tau = self.ctrl.compute_torque(q, qd, q_des, qd_des)

                # 逐关节饱和检测：理想力矩绝对值达到限幅即视为饱和

                saturation_flag[i] = (

                    np.abs(tau_ideal) >= self.config.tau_limit - 1e-9

                )

            else:

                tau = tau_ideal

 

            tau_hist[i] = tau

 

            # RK4积分（整步内零阶保持）

            def deriv(xx, tau_fixed=tau):

                qdd = self.dyn.forward_dynamics(xx[:2], xx[2:4], tau_fixed)

                return np.concatenate([xx[2:4], qdd])

 

            k1 = deriv(x)

            k2 = deriv(x + 0.5 * self.dt * k1)

            k3 = deriv(x + 0.5 * self.dt * k2)

            k4 = deriv(x + self.dt * k3)

            x = x + (self.dt / 6.0) * (k1 + 2*k2 + 2*k3 + k4)

 

        return {

            't': t_arr,

            'q_des': self.traj.q_des.copy(),

            'q_actual': q_actual,

            'qd_actual': qd_actual,

            'error': error_hist,

            'tau': tau_hist,

            'tau_ideal': tau_ideal_hist,

            'end_pos': end_pos_hist,

            'end_pos_des': end_pos_des,

            'end_error': end_error_hist,

            'saturation_flag': saturation_flag.copy(),

        }

 

"robot/metrics.py"

 

"""

逐关节饱和段轨迹跟随偏差统计。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

 

 

class SaturationMetrics:

    """

    统计各关节饱和期间的末端位置误差：

    - 每关节饱和持续时间

    - 每关节饱和段最大偏差

    - 每关节饱和段平均偏差

    - 全程最大偏差（用于对比）

    """

    def __init__(self, t, end_error, saturation_flag):

        self.t = np.asarray(t, dtype=float)

        self.end_error = np.asarray(end_error, dtype=float)

        self.sat_flag = np.asarray(saturation_flag, dtype=bool)

 

        if self.end_error.ndim != 2 or self.end_error.shape[1] != 2:

            raise ValueError("end_error must be (n_steps, 2)")

        if self.sat_flag.ndim != 2 or self.sat_flag.shape[1] != 2:

            raise ValueError("saturation_flag must be (n_steps, 2)")

 

        self.norms = np.linalg.norm(self.end_error, axis=1)

 

    def _dt(self):

        return self.t[1] - self.t[0] if len(self.t) > 1 else 0.0

 

    def saturation_duration(self, joint_idx=None):

        """指定关节的饱和持续时间（s）。joint_idx=None时返回两关节总和。"""

        dt = self._dt()

        if joint_idx is not None:

            return float(np.sum(self.sat_flag[:, joint_idx]) * dt)

        return float(np.sum(np.any(self.sat_flag, axis=1)) * dt)

 

    def saturated_max_error(self, joint_idx=None):

        """指定关节饱和段的最大偏差（m）。"""

        if joint_idx is not None:

            if not np.any(self.sat_flag[:, joint_idx]):

                return 0.0

            return float(np.max(self.norms[self.sat_flag[:, joint_idx]]))

        # 任意关节饱和即计入

        any_sat = np.any(self.sat_flag, axis=1)

        if not np.any(any_sat):

            return 0.0

        return float(np.max(self.norms[any_sat]))

 

    def saturated_mean_error(self, joint_idx=None):

        """指定关节饱和段的平均偏差（m）。"""

        if joint_idx is not None:

            if not np.any(self.sat_flag[:, joint_idx]):

                return 0.0

            return float(np.mean(self.norms[self.sat_flag[:, joint_idx]]))

        any_sat = np.any(self.sat_flag, axis=1)

        if not np.any(any_sat):

            return 0.0

        return float(np.mean(self.norms[any_sat]))

 

    def global_max_error(self):

        """全程最大偏差（m）。"""

        return float(np.max(self.norms))

 

    def report(self):

        """返回格式化报告字典。"""

        return {

            'saturation_duration_j1_s': self.saturation_duration(0),

            'saturation_duration_j2_s': self.saturation_duration(1),

            'saturation_duration_total_s': self.saturation_duration(),

            'saturated_max_error_m': self.saturated_max_error(),

            'saturated_mean_error_m': self.saturated_mean_error(),

            'global_max_error_m': self.global_max_error(),

            'saturated_max_error_mm': self.saturated_max_error() * 1000,

            'saturated_mean_error_mm': self.saturated_mean_error() * 1000,

            'global_max_error_mm': self.global_max_error() * 1000,

        }

 

"robot/visualize.py"

 

"""

4张故障仿真可视化图表 + CSV报告。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import os

import csv

import numpy as np

import matplotlib.pyplot as plt

 

plt.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans',

                                    'Noto Sans CJK SC']

plt.rcParams['axes.unicode_minus'] = False

 

 

class Visualizer2R:

    def __init__(self, results_dir="results", tau_limit=50.0):

        self.rd = results_dir

        self.tau_limit = float(tau_limit)

        os.makedirs(results_dir, exist_ok=True)

 

    def plot_torque_comparison(self, result_sat, result_ideal):

        """图1：饱和力矩 vs 理想力矩时程。"""

        t = result_sat['t']

利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！