《Python机器人基础原理》第二十四篇：装配工位——遍历连续关节角度序列，逐点计算雅可比行列式，标记奇异危险点位

 

1. 实际工业应用场景

 

周三上午，某汽车零部件产线的涡轮壳螺栓装配工位。一台3-DOF垂直关节机械臂（基座偏航θ₁+肘部俯仰θ₂+末端拧紧轴旋转θ₃）正在执行M8×1.25螺栓的定扭矩装配。机器人构型：大臂长550 mm、质量3.8 kg（质心距基座0.26 m），小臂长400 mm、质量2.2 kg（质心距肘部0.18 m），末端拧紧轴及套筒1.2 kg（质心距肘部0.10 m，工具轴向偏置d₄=18 mm——拧紧轴长度导致末端执行器参考点不在腕关节旋转轴上）。关节限位：θ₁±170°、最大角速度2.5 rad/s、最大角加速度5.0 rad/s²；θ₂±140°、最大角速度2.0 rad/s、最大角加速度4.0 rad/s²；θ₃±180°、最大角速度3.0 rad/s、最大角加速度6.0 rad/s²。

 

周二下午小赵在离线仿真里跑了一条装配轨迹——从取料点θ=[10°, -60°, 0°]经过5个中间路点到达装配点θ=[80°, -100°, 90°]，全程T=4.0 s，采样200个点。周三一早他把雅可比行列式算出来，发现第127个采样点附近行列式值只有-0.0002，几乎为零。

 

小李盯着屏幕："小赵，你这条轨迹第127点θ₂=-100°、θ₃=+95.5°，θ₂+θ₃=-4.5°——大臂小臂几乎完全折叠共线。雅可比行列式接近零意味着什么？"

 

"末端自由度退化。机器人在这个位形下，末端沿某个方向的速度趋近于零——不管关节电机转多快，末端就是动不了。"

 

"对。如果在线运行时走到这个点，伺服会拼命输出转矩试图维持末端速度指令，结果就是过流报警或者末端猛地弹开。M8螺栓的定扭矩装配要求轴向力控制在±5 N范围内，一旦奇异点触发，轴向力直接失控。"

 

"我之前用简化模型算过——忽略工具偏置，行列式应该是L1×L2×sin(θ₂+θ₃)=0.55×0.40×sin(-4.5°)=-0.0173。绝对值0.0173，按咱们的阈值在'危险区'边缘但还没到致命。为什么完整模型算出来是-0.0002？"

 

"因为你忽略了拧紧轴18 mm的轴向偏置。末端位置表达式里多了d₄×sin(θ₂+θ₃)和d₄×cos(θ₂+θ₃)项，雅可比矩阵前两行第三列都变了。行列式展开后多了一项(L1+L2)×d₄×cos(θ₂+θ₃)。在θ₂+θ₃=-4.5°时cos≈0.997，修正项是+0.0171，几乎完全抵消了主项-0.0173。简化模型标记'危险'，完整模型标记'致命'——差了一个数量级。"

 

"那离线阶段怎么防？"

 

"逐点遍历你的关节角度序列，每个采样点都算一次完整雅可比行列式（含工具偏置）。设定阈值——|det(J)|<0.01标记致命区，<0.05标记危险区。轨迹规划阶段直接把这些点踢掉或者绕路。"

 

本篇解决的问题：给定一条连续的3-DOF关节角度序列（离线装配轨迹），逐点计算含工具偏置的完整几何雅可比矩阵行列式值，标记奇异危险点位，输出可视化报告。用于装配工位轨迹的离线奇异校验。

 

⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于装配轨迹的雅可比奇异点检测。不可直接用于真实机器人控制器、伺服驱动器或产线设备。真实工业现场需结合实时奇异回避算法、冗余自由度配置、软硬限位保护。代码作者不对直接使用本代码于真实设备造成的任何后果负责。

 

2. 痛点分析

 

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

第3章 连杆参数及运动学求解 → 3.3 雅可比矩阵 装配轨迹经过奇异位形→雅可比行列式趋零→末端自由度退化→定扭矩装配轴向力失控

第3章 连杆参数及运动学求解 → 3.4 奇异性分析 忽略工具偏置导致奇异判断偏差一个数量级→简化模型标记"危险"、完整模型标记"致命"

第6章 轨迹规划与控制 → 6.1 轨迹可行性校验 离线轨迹未做完整雅可比奇异检测→在线运行时伺服过流/末端弹跳→M8螺栓螺纹损伤

 

本篇代码解决 L2 离线轨迹校验层。输入：连续关节角度序列（N个采样点）、DH参数（含工具偏置d₄）、连杆长度。逐点计算3×3几何雅可比行列式。输出：行列式曲线、危险点标记、统计报告。代码不是设备实时控制器，不做在线奇异回避。它做的事情是：遍历已有轨迹，逐点校验奇异风险，为轨迹重规划提供依据。

 

不覆盖：在线奇异回避、阻尼最小二乘法、冗余自由度解析、力/力矩雅可比、动力学计算。

 

3. 核心逻辑讲解

 

物理直觉

 

雅可比矩阵是关节速度和末端速度之间的"翻译器"：ẋ = J·θ̇。行列式|det(J)|衡量这个翻译器"压缩"空间的倍率。行列式为1意味着关节空间到末端空间是等比例映射；行列式接近0意味着关节空间的一个小位移被压缩成末端空间的几乎零位移——末端在这个方向上"僵死"了。

 

对于带工具偏置的3-DOF垂直关节臂，末端位置为：

 

- x = L₁·cos(θ₁)·cos(θ₂) + L₂·cos(θ₁)·cos(θ₂+θ₃) + d₄·cos(θ₁)·sin(θ₂+θ₃)

- y = L₁·sin(θ₁)·cos(θ₂) + L₂·sin(θ₁)·cos(θ₂+θ₃) + d₄·sin(θ₁)·sin(θ₂+θ₃)

- z = L₁·sin(θ₂) + L₂·sin(θ₂+θ₃) - d₄·cos(θ₂+θ₃)

 

当θ₂+θ₃接近0°时（大臂小臂完全折叠共线），sin项趋零、cos项趋1。无偏置时行列式= L₁·L₂·sin(θ₂+θ₃)趋零；有偏置时行列式= L₁·L₂·sin(θ₂+θ₃) + (L₁+L₂)·d₄·cos(θ₂+θ₃)，偏置修正项恰好在sin趋零时最大——可以几乎完全抵消主项，让奇异比简化模型预测的更早到来。

 

业务逻辑 → 代码映射

 

输入：关节角度序列 q_seq[N×3]、连杆长度 L1=0.55 m, L2=0.40 m、工具偏置 d4=0.018 m

  ↓

遍历每个采样点 i = 0 到 N-1：

  读取 q_i = [θ₁, θ₂, θ₃]

  构建完整几何雅可比矩阵 J(3×3)：

    由末端位置对时间的导数推导各偏导项

    J_11 = -L1·sin(θ₁)cos(θ₂) - L2·sin(θ₁)cos(θ₂+θ₃) + d4·sin(θ₁)sin(θ₂+θ₃)

    J_12 = -L1·cos(θ₁)sin(θ₂) - L2·cos(θ₁)sin(θ₂+θ₃) + d4·cos(θ₁)cos(θ₂+θ₃)

    J_13 = -L2·cos(θ₁)sin(θ₂+θ₃) + d4·cos(θ₁)cos(θ₂+θ₃)

    J_21 = L1·cos(θ₁)cos(θ₂) + L2·cos(θ₁)cos(θ₂+θ₃) - d4·cos(θ₁)sin(θ₂+θ₃)

    J_22 = -L1·sin(θ₁)sin(θ₂) - L2·sin(θ₁)sin(θ₂+θ₃) + d4·sin(θ₁)cos(θ₂+θ₃)

    J_23 = -L2·sin(θ₁)sin(θ₂+θ₃) + d4·sin(θ₁)cos(θ₂+θ₃)

    J_31 = d4·cos(θ₂+θ₃)

    J_32 = 1

    J_33 = 1

  计算 det(J_i) = np.linalg.det(J)

  判断 |det(J_i)| 分级：>0.10安全, 0.05-0.10警告, 0.01-0.05危险, <0.01致命

  ↓

输出：行列式序列 det_seq[N]、危险点索引列表、4张图表

 

关键公式文字翻译

 

完整雅可比行列式：det(J) = L₁·L₂·sin(θ₂+θ₃) + (L₁+L₂)·d₄·cos(θ₂+θ₃)。第一项是经典的主项（无偏置时的全部），第二项是工具偏置引入的修正。当θ₂+θ₃接近0°时sin趋零、cos趋1，修正项最大化——偏置18 mm可使修正量达0.017，与主项同量级。

 

奇异条件：det(J)=0 → tan(θ₂+θ₃) = -(L₁+L₂)·d₄/(L₁·L₂)。本篇参数下tan(φ)≈-0.0777 → φ≈-4.45°或175.55°。也就是说，奇异发生在θ₂+θ₃≈-4.45°附近——大臂小臂接近完全折叠（而非完全伸直）。这与无偏置时的直觉（θ₂+θ₃=0）偏移了4.45°，但后果更严重因为偏置让行列式更快趋零。

 

阈值分级说明：阈值数值（0.10/0.05/0.01）仅在长度单位m、角度单位rad的体系下有效。如果读者改用mm或deg，阈值需等比缩放。这是混合量纲（m²·rad）的固有特性。

 

单位：角度 rad，长度 m，雅可比行列式量纲 m²·rad（位置映射×角度映射的混合量）。

 

4. OOP代码实现

 

技术栈

 

严格限定：numpy, scipy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方机器人库。复用前篇的 

"trajectory/models.py"。

 

项目结构树

 

singularity_detection_sim/

├── run_singularity_sim.py

├── requirements.txt

├── robot/

│ ├── __init__.py

│ ├── models.py

│ ├── jacobian.py

│ └── visualize_singularity.py

├── tests/

│ ├── __init__.py

│ └── test_singularity_sim.py

└── results/

 

核心源码

 

"robot/models.py" — 数据模型

 

"""

机器人数据模型：DH参数、连杆物理属性、关节限位、轨迹路点

"""

 

 

from dataclasses import dataclass, field

from typing import List

 

 

@dataclass

class DHLink:

    """单连杆DH参数结构体（修正DH约定）"""

    link_length: float # a_i, 连杆长度 (m)

    link_twist: float # alpha_i, 连杆扭角 (rad)

    link_offset: float # d_i, 连杆偏距 (m)

    joint_angle: float # theta_i, 关节角 (rad)

    mass: float # 连杆质量 (kg)

    com_x: float = 0.0 # 质心x坐标 (m)

    com_y: float = 0.0 # 质心y坐标 (m)

    com_z: float = 0.0 # 质心z坐标 (m)

    joint_name: str = ""

 

 

@dataclass

class JointLimit:

    """单关节限位结构体"""

    joint_name: str

    min_angle: float # 最小角度 (rad)

    max_angle: float # 最大角度 (rad)

    max_velocity: float # 最大角速度 (rad/s)

    max_acceleration: float # 最大角加速度 (rad/s²)

 

 

@dataclass

class JointLimitSet:

    """关节限位集合"""

    limits: List[JointLimit] = field(default_factory=list)

 

    def check_all_positions(self, angles: List[float]) -> tuple:

        """

        校验一组关节角度是否全部在位置限位内

        返回 (bool通过标志, list超限索引列表)

        """

        violations = []

        for i, angle in enumerate(angles):

            if i < len(self.limits):

                if angle < self.limits[i].min_angle or angle > self.limits[i].max_angle:

                    violations.append(i)

        return len(violations) == 0, violations

 

 

@dataclass

class TrajectoryWaypoint:

    """轨迹路点结构体"""

    joint1: float

    joint2: float

    joint3: float

    timestamp: float = 0.0

 

"robot/jacobian.py" — 完整雅可比矩阵与奇异检测

 

"""

3-DOF垂直关节机械臂完整几何雅可比矩阵计算与奇异检测

构型：基座偏航θ₁ + 肘部俯仰θ₂ + 末端旋转θ₃ + 工具轴向偏置d₄

末端位置（含偏置）：

  x = L1·cos(θ₁)cos(θ₂) + L2·cos(θ₁)cos(θ₂+θ₃) + d4·cos(θ₁)sin(θ₂+θ₃)

  y = L1·sin(θ₁)cos(θ₂) + L2·sin(θ₁)cos(θ₂+θ₃) + d4·sin(θ₁)sin(θ₂+θ₃)

  z = L1·sin(θ₂) + L2·sin(θ₂+θ₃) - d4·cos(θ₂+θ₃)

行列式解析展开：det(J) = L1·L2·sin(θ₂+θ₃) + (L1+L2)·d4·cos(θ₂+θ₃)

"""

 

 

import numpy as np

from typing import List, Tuple, Optional

 

 

class GeometricJacobian3DOF:

    """

    3-DOF垂直关节臂完整几何雅可比计算器（含工具轴向偏置）

    """

 

    def __init__(self, upper_arm_length: float, forearm_length: float,

                 tool_offset: float = 0.0):

        if upper_arm_length <= 0 or forearm_length <= 0:

            raise ValueError(

                f"连杆长度必须为正数: upper_arm={upper_arm_length}, forearm={forearm_length}"

            )

        if tool_offset < 0:

            raise ValueError(f"工具偏置不能为负: {tool_offset}")

        self.L1 = upper_arm_length # 大臂长度 (m)

        self.L2 = forearm_length # 小臂长度 (m)

        self.d4 = tool_offset # 工具轴向偏置 (m)

 

    def compute_jacobian(self, theta1: float, theta2: float, theta3: float) -> np.ndarray:

        """

        计算给定关节角度下的3×3完整几何雅可比矩阵

        由末端位置对时间求导得到各偏导项

        """

        if not all(np.isfinite([theta1, theta2, theta3])):

            raise ValueError("关节角度必须为有限数值")

 

        J = np.zeros((3, 3))

 

        sin_t1 = np.sin(theta1)

        cos_t1 = np.cos(theta1)

        sin_t2 = np.sin(theta2)

        cos_t2 = np.cos(theta2)

        sin_t23 = np.sin(theta2 + theta3)

        cos_t23 = np.cos(theta2 + theta3)

 

        # 第一行：末端X方向速度对关节速度的映射

        J[0, 0] = -self.L1 * sin_t1 * cos_t2 - self.L2 * sin_t1 * cos_t23 + self.d4 * sin_t1 * sin_t23

        J[0, 1] = -self.L1 * cos_t1 * sin_t2 - self.L2 * cos_t1 * sin_t23 + self.d4 * cos_t1 * cos_t23

        J[0, 2] = -self.L2 * cos_t1 * sin_t23 + self.d4 * cos_t1 * cos_t23

 

        # 第二行：末端Y方向速度对关节速度的映射

        J[1, 0] = self.L1 * cos_t1 * cos_t2 + self.L2 * cos_t1 * cos_t23 - self.d4 * cos_t1 * sin_t23

        J[1, 1] = -self.L1 * sin_t1 * sin_t2 - self.L2 * sin_t1 * sin_t23 + self.d4 * sin_t1 * cos_t23

        J[1, 2] = -self.L2 * sin_t1 * sin_t23 + self.d4 * sin_t1 * cos_t23

 

        # 第三行：末端Z方向速度对关节速度的映射（含偏置耦合）

        J[2, 0] = self.d4 * cos_t23

        J[2, 1] = 1.0

        J[2, 2] = 1.0

 

        return J

 

    def compute_determinant(self, theta1: float, theta2: float, theta3: float) -> float:

        """

        计算完整雅可比行列式（数值法，通过np.linalg.det）

        含工具偏置，结果与解析公式一致

        """

        J = self.compute_jacobian(theta1, theta2, theta3)

        det_val = np.linalg.det(J)

 

        # 浮点精度保护：绝对值小于1e-12视为零

        if abs(det_val) < 1e-12:

            det_val = 0.0

 

        return float(det_val)

 

    def compute_determinant_analytic(self, theta2: float, theta3: float) -> float:

        """

        解析行列式（含工具偏置）

        det(J) = L1*L2*sin(θ₂+θ₃) + (L1+L2)*d4*cos(θ₂+θ₃)

        用于批量计算时的性能优化和交叉验证

        """

        phi = theta2 + theta3

        return self.L1 * self.L2 * np.sin(phi) + (self.L1 + self.L2) * self.d4 * np.cos(phi)

 

    def evaluate_trajectory(self, waypoints: List[Tuple[float, float, float]],

                            timestamps: Optional[List[float]] = None,

                            danger_threshold: float = 0.05) -> dict:

        """

        遍历连续关节角度序列，逐点计算雅可比行列式

        输入：waypoints = [(θ₁, θ₂, θ₃), ...]

        输出：字典包含 determinants, timestamps, singular_points(分级)

        """

        n_points = len(waypoints)

        if n_points == 0:

            raise ValueError("路点序列不能为空")

 

        if timestamps is None:

            timestamps = list(range(n_points))

 

        det_values = np.zeros(n_points)

        singular_points = [] # (索引, 行列式值, 分级标签)

 

        for i, (t1, t2, t3) in enumerate(waypoints):

            det_val = self.compute_determinant(t1, t2, t3)

            det_values[i] = det_val

 

            abs_det = abs(det_val)

            if abs_det <= 0.01:

                level = "FATAL"

            elif abs_det <= 0.05:

                level = "DANGER"

            elif abs_det <= 0.10:

                level = "WARNING"

            else:

                level = "SAFE"

 

            if level != "SAFE":

                singular_points.append((i, det_val, level))

 

        return {

            'determinants': det_values,

            'timestamps': np.array(timestamps),

            'singular_points': singular_points,

            'n_points': n_points,

        }

 

    def check_singularity_at_config(self, theta1: float, theta2: float, theta3: float,

                                    danger_threshold: float = 0.05) -> Tuple[bool, float, str]:

        """

        单点奇异检测

        返回 (是否危险, 行列式值, 分级标签)

        """

        det_val = self.compute_determinant(theta1, theta2, theta3)

        abs_det = abs(det_val)

 

        if abs_det <= 0.01:

            level = "FATAL"

        elif abs_det <= 0.05:

            level = "DANGER"

        elif abs_det <= 0.10:

            level = "WARNING"

        else:

            level = "SAFE"

 

        is_dangerous = abs_det <= danger_threshold

        return is_dangerous, det_val, level

 

"robot/visualize_singularity.py" — 可视化模块

 

"""

装配工位奇异检测可视化

4张核心图表：行列式曲线、危险点散点标记、工作空间奇异分布热力图、轨迹关节角变化

"""

 

 

import numpy as np

import matplotlib.pyplot as plt

 

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

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

 

 

class SingularityVisualizer:

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

        self.results_dir = results_dir

        import os

        os.makedirs(results_dir, exist_ok=True)

 

    def plot_determinant_curve(self, timestamps, det_values, singular_points=None):

        """

        ① 雅可比行列式随轨迹点演变曲线：核心输出，直接读出奇异危险区间

        """

        fig, ax = plt.subplots(figsize=(12, 5))

 

        # 绘制原始行列式值（可正可负）

        ax.plot(timestamps, det_values, '-', color='steelblue',

                linewidth=1.2, label='det(J)')

 

        # 绘制绝对值曲线

        abs_det = np.abs(det_values)

        ax.plot(timestamps, abs_det, '--', color='darkblue',

                linewidth=0.8, alpha=0.6, label='|det(J)|')

 

        # 阈值线

        ax.axhline(y=0.10, color='orange', linestyle='--', linewidth=1.0, label='Warning (0.10)')

        ax.axhline(y=0.05, color='red', linestyle='--', linewidth=1.0, label='Danger (0.05)')

        ax.axhline(y=0.01, color='darkred', linestyle='-', linewidth=1.0, label='Fatal (0.01)')

        ax.axhline(y=0, color='black', linestyle='-', linewidth=0.5, alpha=0.5)

 

        # 标记危险点竖线

        if singular_points is not None:

            for idx, _, level in singular_points:

                if idx < len(timestamps):

                    color = 'red' if level in ('DANGER', 'FATAL') else 'orange'

                    ax.axvline(x=timestamps[idx], color=color,

                               linestyle=':', linewidth=0.6, alpha=0.4)

 

        ax.set_xlabel('Trajectory Point Index')

        ax.set_ylabel('Jacobian Determinant (m²·rad)')

        ax.set_title('Jacobian Determinant Along Assembly Trajectory — Singularity Check')

        ax.legend(loc='upper right', fontsize=8)

        ax.grid(True, alpha=0.3)

 

        plt.tight_layout()

        path = f"{self.results_dir}/01_determinant_curve.png"

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_singular_point_scatter(self, timestamps, det_values, singular_points):

        """

        ② 奇异危险点散点标记图：用颜色区分安全/警告/危险/致命

        """

        fig, ax = plt.subplots(figsize=(12, 5))

 

        colors = {'WARNING': 'yellow', 'DANGER': 'orange', 'FATAL': 'red'}

        abs_det = np.abs(det_values)

 

        # 安全点底色

        safe_mask = abs_det > 0.10

        ax.scatter(timestamps[safe_mask], det_values[safe_mask],

                   c='green', s=8, alpha=0.4, label='Safe (>0.10)')

 

        # 分级标记

        for idx, det_val, level in singular_points:

            if idx < len(timestamps):

                ax.scatter(timestamps[idx], det_val,

                           c=colors.get(level, 'gray'), s=30,

                           marker='x' if level in ('DANGER', 'FATAL') else 'o',

                           label=f'{level} (idx={idx})' if idx < 3 else "")

 

        # 去重图例

        handles, labels = ax.get_legend_handles_labels()

        by_label = dict(zip(labels, handles))

        ax.legend(by_label.values(), by_label.keys(), loc='upper right', fontsize=8)

 

        ax.set_xlabel('Trajectory Point Index')

        ax.set_ylabel('Jacobian Determinant (m²·rad)')

        ax.set_title('Singular Point Detection — Color-Coded by Risk Level')

        ax.grid(True, alpha=0.3)

 

        plt.tight_layout()

        path = f"{self.results_dir}/02_singular_point_scatter.png"

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_workspace_singularity_heatmap(self, theta2_range, theta3_range, L1, L2, d4):

        """

        ③ 工作空间奇异分布热力图：θ₂ vs θ₃平面上|det(J)|的分布

        含偏置：det(J) = L1*L2*sin(θ₂+θ₃) + (L1+L2)*d4*cos(θ₂+θ₃)

        """

        T2, T3 = np.meshgrid(theta2_range, theta3_range)

        Det = L1 * L2 * np.sin(T2 + T3) + (L1 + L2) * d4 * np.cos(T2 + T3)

 

        fig, ax = plt.subplots(figsize=(8, 6))

        c = ax.contourf(T2 * 180 / np.pi, T3 * 180 / np.pi, np.abs(Det),

                        levels=20, cmap='viridis')

        plt.colorbar(c, ax=ax, label='|det(J)| (m²·rad)')

 

        # 标记奇异线 det(J)=0 → tan(θ₂+θ₃) = -(L1+L2)*d4/(L1*L2)

        singular_angle = np.arctan(-(L1 + L2) * d4 / (L1 * L2))

        singular_line = []

        for t2 in theta2_range:

            t3_val = singular_angle - t2

            if theta3_range[0] <= t3_val <= theta3_range[-1]:

                singular_line.append((t2 * 180 / np.pi, t3_val * 180 / np.pi))

 

        if singular_line:

            sl_x, sl_y = zip(*singular_line)

            ax.plot(sl_x, sl_y, 'r--', linewidth=1.5,

                    label=f'Singular Line (θ₂+θ₃={singular_angle*180/np.pi:.1f}°)')

 

        # 标记第二条奇异线（加π）

        singular_angle2 = singular_angle + np.pi

        singular_line2 = []

        for t2 in theta2_range:

            t3_val = singular_angle2 - t2

            if theta3_range[0] <= t3_val <= theta3_range[-1]:

                singular_line2.append((t2 * 180 / np.pi, t3_val * 180 / np.pi))

 

        if singular_line2:

            sl2_x, sl2_y = zip(*singular_line2)

            ax.plot(sl2_x, sl2_y, 'r-.', linewidth=1.0,

                    label=f'Singular Line (θ₂+θ₃={singular_angle2*180/np.pi:.1f}°)')

 

        ax.set_xlabel('θ₂ (Elbow Pitch) [deg]')

        ax.set_ylabel('θ₃ (End Rotate) [deg]')

        ax.set_title('Workspace Singularity Heatmap — |det(J)| vs (θ₂, θ₃)')

        ax.legend(loc='upper right', fontsize=8)

        ax.grid(True, alpha=0.3)

 

        plt.tight_layout()

        path = f"{self.results_dir}/03_workspace_singularity_heatmap.png"

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_joint_angles_trajectory(self, timestamps, waypoints):

        """

        ④ 关节角度变化曲线：辅助判断奇异点对应的关节位形

        """

        fig, ax = plt.subplots(figsize=(12, 5))

 

        waypoints = np.array(waypoints)

        ax.plot(timestamps, waypoints[:, 0] * 180 / np.pi, '-',

                color='steelblue', linewidth=1.2, label='θ₁ (Base Yaw)')

        ax.plot(timestamps, waypoints[:, 1] * 180 / np.pi, '-',

                color='orange', linewidth=1.2, label='θ₂ (Elbow Pitch)')

        ax.plot(timestamps, waypoints[:, 2] * 180 / np.pi, '-',

                color='green', linewidth=1.2, label='θ₃ (End Rotate)')

 

        ax.set_xlabel('Trajectory Point Index')

        ax.set_ylabel('Joint Angle (deg)')

        ax.set_title('Joint Angles Along Assembly Trajectory — Context for Singular Points')

        ax.legend(loc='upper right', fontsize=8)

        ax.grid(True, alpha=0.3)

 

        plt.tight_layout()

        path = f"{self.results_dir}/04_joint_angles_trajectory.png"

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

5. 可视化要求

 

序号 图表文件 服务结论

① 

"01_determinant_curve.png" 行列式沿轨迹演变曲线，阈值线标记安全/警告/危险/致命区，直接读出奇异危险区间

② 

"02_singular_point_scatter.png" 逐点散点标记，颜色分级（绿/黄/橙/红），精确定位危险点索引

③ 

"03_workspace_singularity_heatmap.png" θ₂-θ₃工作空间奇异分布热力图，红色虚线标记含偏置的奇异线，指导轨迹避障

④ 

"04_joint_angles_trajectory.png" 关节角度变化曲线，辅助判断奇异点对应的具体关节位形

 

6. README使用说明 + 核心知识点卡片

 

快速开始

 

pip install -r requirements.txt

python -m pytest tests/test_singularity_sim.py -v # 3/3 测试通过

python run_singularity_sim.py # 

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