《Python机器人基础原理》第二十五篇：离线编程场景——给定末端笛卡尔速度，借助雅可比矩阵映射求解对应关节角速度

 

1. 实际工业应用场景

 

周四下午，某3C电子产线的手机中框螺丝锁附工位。一台4-DOF桌面型SCARA机械臂正在执行Φ2.5 mm微型螺丝的恒速锁附。构型：基座旋转关节θ₁（绕Z轴）、肘部旋转关节θ₂（绕Z轴）、末端旋转关节θ₃（绕Z轴）、Z轴升降关节d₄（行程80 mm）。连杆参数：大臂长350 mm、质量2.1 kg（质心距基座0.16 m），小臂长250 mm、质量1.3 kg（质心距肘部0.11 m），末端电批及套筒0.4 kg（轴向偏置12 mm）。关节限位：θ₁±180°、最大角速度3.5 rad/s、最大角加速度8.0 rad/s²；θ₂±150°、最大角速度3.0 rad/s、最大角加速度6.0 rad/s²；θ₃±180°、最大角速度4.0 rad/s、最大角加速度10.0 rad/s²；d₄行程0~80 mm、最大线速度0.15 m/s、最大线加速度0.8 m/s²。

 

周三晚上小赵在离线编程软件里给了一条末端TCP速度指令：沿X轴以恒定0.08 m/s进给，同时Z轴以0.02 m/s匀速下压。他把这条速度指令通过雅可比逆映射转成关节速度，发现θ₂在轨迹中段飙到了4.3 rad/s——远超硬件限位3.0 rad/s。

 

小李端着咖啡走过来："小赵，你这条离线程序里θ₂角速度超限了，你看到了吗？"

 

"看到了。我用雅可比逆矩阵算的，θ̇ = J⁻¹·ẋ。末端X方向0.08 m/s，Z方向0.02 m/s，算出来θ₂峰值4.3 rad/s。但硬件手册写的是3.0 rad/s。"

 

"问题出在哪你知道吗？"

 

"雅可比矩阵条件数太大。轨迹中段θ₂=-148°，大臂小臂几乎完全折叠共线，J接近奇异。同样的末端速度，在接近奇异的位形下需要关节转得飞快才能维持——这就是雅可比逆映射的经典陷阱。"

 

"对。离线编程阶段必须做速度映射校验。你给定末端TCP速度，用雅可比映射出关节角速度后，必须逐点跟限位比对。超限的点要么降速，要么改路径。"

 

"那我得把整条轨迹的关节角速度都算出来，画个曲线看看哪些段超限。"

 

"对。而且别只算逆映射——同时把条件数也画出来。条件数大于50的区域就是危险区，说明这个位形下微小末端速度扰动会被放大成巨大的关节速度波动。"

 

本篇解决的问题：给定一条末端笛卡尔速度序列（离线编程的TCP速度指令），借助真SCARA构型的4×4几何雅可比矩阵，逐点计算逆映射θ̇ = J⁻¹·ẋ，得到对应关节角速度，校验是否超出关节限位，输出速度曲线与条件数报告。

 

⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于离线编程阶段的速度映射校验。不可直接用于真实机器人控制器、伺服驱动器或产线设备。真实工业现场需结合实时速度规划、关节空间插补、软硬限位保护。代码作者不对直接使用本代码于真实设备造成的任何后果负责。

 

2. 痛点分析

 

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

第3章 连杆参数及运动学求解 → 3.3 雅可比矩阵 离线编程给定末端TCP速度→雅可比逆映射→关节角速度超限→伺服过载/报警

第3章 连杆参数及运动学求解 → 3.4 奇异性分析 轨迹经过近奇异位形→J条件数恶化→微小末端速度扰动被放大为巨大关节速度波动

第6章 轨迹规划与控制 → 6.2 速度规划 笛卡尔空间速度指令未做关节空间校验→离线程序下发给机器人后现场调试频繁超限停机

 

本篇代码解决 L2 离线编程校验层。输入：末端笛卡尔速度序列（N个采样点，含X/Y/Z线速度和绕Z轴角速度）、DH参数（含工具偏置）、连杆长度。逐点计算J⁻¹·ẋ得到关节角速度，与限位比对。输出：关节角速度曲线、条件数曲线、超限统计报告。代码不是设备实时控制器，不做在线速度规划。它做的事情是：校验离线编程生成的TCP速度指令在关节空间是否可行。

 

不覆盖：在线速度规划、阻尼最小二乘法逆解、冗余解析、力矩控制、动力学前馈。

 

3. 核心逻辑讲解

 

物理直觉

 

雅可比矩阵是关节空间和末端空间之间的"翻译器"。正向映射ẋ = J·θ̇就像把一种货币换成另一种——给定关节速度，得到末端速度。逆映射θ̇ = J⁻¹·ẋ就是反过来：给定末端要走的快慢，算出每个关节该转多快。

 

对于SCARA构型，这个翻译器有4种"货币"（末端X/Y/Z线速度+绕Z角速度）和4个"兑换窗口"（θ₁/θ₂/θ₃/d₄）。Z方向位移完全由升降轴d₄独立控制，所以J的第3行是[0, 0, 0, 1]——Z速度和关节角速度无关，直接等于d₄'。

 

问题在于：当大臂小臂接近完全折叠时（θ₁+θ₂接近±180°），XY方向的"汇率"变得极端。同样的末端速度，在近奇异位形下需要关节以数倍甚至数十倍的速度运转——就像杠杆的支点越靠近施力点，需要的力越大。条件数（最大奇异值/最小奇异值）衡量这个"汇率"的极端程度。条件数越大，映射越不稳定。

 

业务逻辑 → 代码映射

 

输入：末端速度序列 v_seq[N×4] (v_x, v_y, v_z, ω_z)、关节状态序列 q_seq[N×4] (θ₁,θ₂,θ₃,d₄)、机器人参数

  ↓

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

  读取当前关节状态 q_i = [θ₁, θ₂, θ₃, d₄]

  读取当前末端速度 cmd_i = [v_x, v_y, v_z, ω_z]

  构建4×4几何雅可比 J：

    行0(X): [-L1·sinθ₁-L2·sin(θ₁+θ₂)-d4·sin(θ₁+θ₂+θ₃), -L2·sin(θ₁+θ₂)-d4·sin(θ₁+θ₂+θ₃), -d4·sin(θ₁+θ₂+θ₃), 0]

    行1(Y): [ L1·cosθ₁+L2·cos(θ₁+θ₂)+d4·cos(θ₁+θ₂+θ₃), L2·cos(θ₁+θ₂)+d4·cos(θ₁+θ₂+θ₃), d4·cos(θ₁+θ₂+θ₃), 0]

    行2(Z): [0, 0, 0, 1]

    行3(ωz):[1, 1, 1, 0]

  计算条件数 cond(J) = σ_max / σ_min

  如果 cond < 1e6: 直接求逆 θ̇_i = J⁻¹ · cmd_i

  否则: 标记该点为奇异不可解

  校验 |θ̇_i| 是否超过各关节限位

  ↓

输出：关节速度序列 qdot_seq[N×4]、条件数序列 cond_seq[N]、超限报告、4张图表

 

关键公式文字翻译

 

4×4雅可比矩阵：SCARA构型下，末端位置为x = L1·cosθ₁ + L2·cos(θ₁+θ₂) + d4·cos(θ₁+θ₂+θ₃)，y = L1·sinθ₁ + L2·sin(θ₁+θ₂) + d4·sin(θ₁+θ₂+θ₃)，z = d₄。对时间求导得到各偏导项。Z行[0,0,0,1]表示Z速度完全由升降轴速度决定，与其他关节无关。ω_z行[1,1,1,0]表示末端绕Z角速度等于三个旋转关节角速度之和。

 

奇异条件：3×3子矩阵（XY映射部分）行列式趋零。当θ₁+θ₂接近±180°时（大臂小臂折叠共线），sin(θ₁+θ₂)趋零，cos(θ₁+θ₂)趋±1但系数耦合导致行列式塌缩。

 

条件数：cond(J) = σ_max/σ_min，无量纲。衡量映射对输入扰动的放大程度。条件数=1表示完美正交；>50映射严重病态；>100数值不可信。注意：条件数是奇异值之比，与单位体系无关（无量纲）。真正依赖单位的是速度限位和雅可比元素的数值大小。

 

4. OOP代码实现

 

技术栈

 

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

 

项目结构树

 

velocity_mapping_sim/

├── run_velocity_mapping_sim.py

├── requirements.txt

├── robot/

│ ├── __init__.py

│ ├── models.py

│ ├── jacobian.py

│ ├── velocity_mapper.py

│ └── visualize_velocity.py

├── tests/

│ ├── __init__.py

│ └── test_velocity_mapping_sim.py

└── results/

 

核心源码

 

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

 

"""

机器人数据模型：DH参数、连杆物理属性、关节限位、末端速度指令

"""

 

 

from dataclasses import dataclass, field

from typing import List

 

 

@dataclass

class JointLimit:

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

    joint_name: str

    min_angle: float # 最小角度 (rad) 或最小位移 (m)

    max_angle: float # 最大角度 (rad) 或最大位移 (m)

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

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

    is_prismatic: bool = False # True表示移动关节(d₄)

 

 

@dataclass

class JointLimitSet:

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

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

 

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

        """

        校验一组关节值是否全部在限位内

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

        """

        violations = []

        for i, val in enumerate(values):

            if i < len(self.limits):

                lim = self.limits[i]

                if val < lim.min_angle or val > lim.max_angle:

                    violations.append(i)

        return len(violations) == 0, violations

 

    def check_all_velocities(self, velocities: List[float]) -> tuple:

        """

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

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

        """

        violations = []

        for i, vel in enumerate(velocities):

            if i < len(self.limits):

                if abs(vel) > self.limits[i].max_velocity:

                    violations.append(i)

        return len(violations) == 0, violations

 

 

@dataclass

class CartesianVelocityCommand:

    """末端笛卡尔速度指令结构体"""

    vx: float # X方向线速度 (m/s)

    vy: float # Y方向线速度 (m/s)

    vz: float # Z方向线速度 (m/s)

    omega_z: float # 绕Z轴角速度 (rad/s)

    timestamp: float = 0.0

 

"robot/jacobian.py" — 真SCARA 4×4雅可比矩阵

 

"""

4-DOF SCARA机械臂几何雅可比矩阵计算

构型：基座旋转θ₁(Z轴) + 肘部旋转θ₂(Z轴) + 末端旋转θ₃(Z轴) + Z轴升降d₄

末端位置：

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

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

  z = d₄

雅可比 J(4×4)：

  行0(X): [-L1·s1-L2·s12-d4·s123, -L2·s12-d4·s123, -d4·s123, 0]

  行1(Y): [ L1·c1+L2·c12+d4·c123, L2·c12+d4·c123, d4·c123, 0]

  行2(Z): [0, 0, 0, 1]

  行3(ωz):[1, 1, 1, 0]

"""

 

 

import numpy as np

from typing import Tuple

 

 

class SCARAJacobian:

    """

    4-DOF SCARA几何雅可比计算器

    输出 J(4×4)：X/Y/Z线速度 + 绕Z角速度 → θ₁/θ₂/θ₃/d₄

    """

 

    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, d4: float = 0.0) -> np.ndarray:

        """

        计算4×4几何雅可比矩阵

        """

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

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

 

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

 

        sin_t1 = np.sin(theta1)

        cos_t1 = np.cos(theta1)

        sin_t12 = np.sin(theta1 + theta2)

        cos_t12 = np.cos(theta1 + theta2)

        sin_t123 = np.sin(theta1 + theta2 + theta3)

        cos_t123 = np.cos(theta1 + theta2 + theta3)

 

        # 行0: X线速度

        J[0, 0] = -self.L1 * sin_t1 - self.L2 * sin_t12 - self.d4 * sin_t123

        J[0, 1] = -self.L2 * sin_t12 - self.d4 * sin_t123

        J[0, 2] = -self.d4 * sin_t123

        J[0, 3] = 0.0

 

        # 行1: Y线速度

        J[1, 0] = self.L1 * cos_t1 + self.L2 * cos_t12 + self.d4 * cos_t123

        J[1, 1] = self.L2 * cos_t12 + self.d4 * cos_t123

        J[1, 2] = self.d4 * cos_t123

        J[1, 3] = 0.0

 

        # 行2: Z线速度（完全由升降轴d₄独立控制）

        J[2, 0] = 0.0

        J[2, 1] = 0.0

        J[2, 2] = 0.0

        J[2, 3] = 1.0

 

        # 行3: 绕Z轴角速度（三个旋转关节之和）

        J[3, 0] = 1.0

        J[3, 1] = 1.0

        J[3, 2] = 1.0

        J[3, 3] = 0.0

 

        return J

 

    def compute_condition_number(self, theta1: float, theta2: float,

                                  theta3: float, d4: float = 0.0) -> float:

        """

        计算雅可比矩阵的条件数（2-范数，无量纲）

        cond = σ_max / σ_min

        """

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

        singular_values = np.linalg.svd(J, compute_uv=False)

        if singular_values[-1] < 1e-12:

            return float('inf')

        return float(singular_values[0] / singular_values[-1])

 

    def compute_xy_submatrix_determinant(self, theta1: float, theta2: float,

                                          theta3: float) -> float:

        """

        计算XY映射子矩阵(3×3，取J前2行前3列)的某种标量指标

        这里用2×2主子式（θ₁和θ₂对X/Y的映射）作为奇异判断

        det_2x2 = (J[0,0]*J[1,1] - J[0,1]*J[1,0]) 的简化

        """

        sin_t12 = np.sin(theta1 + theta2)

        cos_t12 = np.cos(theta1 + theta2)

        sin_t123 = np.sin(theta1 + theta2 + theta3)

        cos_t123 = np.cos(theta1 + theta2 + theta3)

 

        # 2×2子矩阵（θ₁,θ₂对X,Y）

        a = -self.L1 * np.sin(theta1) - self.L2 * sin_t12 - self.d4 * sin_t123

        b = -self.L2 * sin_t12 - self.d4 * sin_t123

        c = self.L1 * np.cos(theta1) + self.L2 * cos_t12 + self.d4 * cos_t123

        d = self.L2 * cos_t12 + self.d4 * cos_t123

 

        det_2x2 = a * d - b * c

        if abs(det_2x2) < 1e-12:

            det_2x2 = 0.0

        return float(det_2x2)

 

"robot/velocity_mapper.py" — 速度映射核心算法

 

"""

末端笛卡尔速度 → 关节速度映射（SCARA 4×4雅可比直接求逆）

"""

 

 

import numpy as np

from typing import List, Tuple, Optional, Dict

from robot.jacobian import SCARAJacobian

from robot.models import JointLimitSet

 

 

class VelocityMapper:

    """

    离线编程速度映射器

    给定末端速度序列和关节状态序列，逐点计算关节速度

    """

 

    def __init__(self, jacobian: SCARAJacobian, joint_limits: JointLimitSet):

        self.jac = jacobian

        self.limits = joint_limits

 

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

                              d4: float, vx: float, vy: float, vz: float,

                              omega_z: float = 0.0) -> Tuple[np.ndarray, float, bool]:

        """

        单点速度映射

        输入：关节状态(θ₁,θ₂,θ₃,d₄)、末端速度(vx,vy,vz,ωz)

        输出：(关节速度数组[θ₁',θ₂',θ₃',d₄'], 条件数, 是否超限)

        """

        if not all(np.isfinite([theta1, theta2, theta3, d4, vx, vy, vz, omega_z])):

            raise ValueError("所有输入必须为有限数值")

 

        cart_vel = np.array([vx, vy, vz, omega_z])

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

        cond_num = self.jac.compute_condition_number(theta1, theta2, theta3, d4)

 

        # 直接求逆（4×4方阵）

        if np.isfinite(cond_num) and cond_num < 1e6:

            try:

                J_inv = np.linalg.inv(J)

                joint_vel = J_inv @ cart_vel

            except np.linalg.LinAlgError:

                joint_vel = np.zeros(4)

                cond_num = float('inf')

        else:

            joint_vel = np.zeros(4)

 

        # 校验速度限位

        vel_list = joint_vel.tolist()

        is_valid, _ = self.limits.check_all_velocities(vel_list)

        if not np.all(np.isfinite(joint_vel)):

            is_valid = False

 

        return joint_vel, cond_num, not is_valid

 

    def evaluate_trajectory(self,

                            joint_states_seq: List[Tuple[float, float, float, float]],

                            cart_velocity_seq: List[Tuple[float, float, float, float]],

                            timestamps: Optional[List[float]] = None) -> dict:

        """

        遍历整条轨迹，逐点映射速度

        输入：关节状态序列 [(θ₁,θ₂,θ₃,d₄),...]、末端速度序列 [(vx,vy,vz,ωz),...]

        """

        n_points = len(joint_states_seq)

        if n_points == 0:

            raise ValueError("关节状态序列不能为空")

        if len(cart_velocity_seq) != n_points:

            raise ValueError("关节状态序列与末端速度序列长度必须一致")

 

        if timestamps is None:

            timestamps = np.linspace(0, n_points / 50.0, n_points).tolist()

 

        joint_velocities = np.zeros((n_points, 4)) # θ̇₁, θ̇₂, θ̇₃, ḋ₄

        condition_numbers = np.zeros(n_points)

        velocity_violations = [] # (索引, 关节索引, 实际速度, 限位)

        condition_warnings = [] # (索引, 条件数)

 

        for i in range(n_points):

            t1, t2, t3, d = joint_states_seq[i]

            vx, vy, vz, wz = cart_velocity_seq[i]

 

            jvel, cond, violated = self.map_velocity_at_point(t1, t2, t3, d, vx, vy, vz, wz)

            joint_velocities[i, :] = jvel

            condition_numbers[i] = cond

 

            if violated:

                vel_list = jvel.tolist()

                for j in range(4):

                    if abs(vel_list[j]) > self.limits.limits[j].max_velocity:

                        velocity_violations.append((

                            i, j, vel_list[j],

                            self.limits.limits[j].max_velocity

                        ))

 

            if np.isfinite(cond) and cond > 50.0:

                condition_warnings.append((i, cond))

 

        return {

            'joint_velocities': joint_velocities,

            'condition_numbers': condition_numbers,

            'timestamps': np.array(timestamps),

            'velocity_violations': velocity_violations,

            'condition_warnings': condition_warnings,

            'n_points': n_points,

        }

 

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

 

"""

离线编程速度映射可视化

4张核心图表：关节速度曲线、条件数曲线、超限散点标记、末端vs关节速度对比

"""

 

 

import numpy as np

import matplotlib.pyplot as plt

 

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

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

 

 

class VelocityMappingVisualizer:

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

        self.results_dir = results_dir

        import os

        os.makedirs(results_dir, exist_ok=True)

 

    def plot_joint_velocity_curves(self, timestamps, joint_velocities, joint_limits):

        """

        ① 关节速度沿轨迹演变曲线：核心输出，直接读出超限区间

        """

        fig, axes = plt.subplots(4, 1, figsize=(12, 11), sharex=True)

 

        joint_names = ['θ₁ (Base)', 'θ₂ (Elbow)', 'θ₃ (End)', 'd₄ (Z Lift)']

        colors = ['steelblue', 'orange', 'green', 'purple']

        units = ['rad/s', 'rad/s', 'rad/s', 'm/s']

 

        for j in range(4):

            ax = axes[j]

            ax.plot(timestamps, joint_velocities[:, j], '-',

                    color=colors[j], linewidth=1.2, label=f'Actual {joint_names[j]}')

 

            limit = joint_limits.limits[j].max_velocity

            ax.axhline(y=limit, color='red', linestyle='--', linewidth=1.0,

                       label=f'Max (+{limit} {units[j]})')

            ax.axhline(y=-limit, color='red', linestyle='--', linewidth=1.0,

                       label=f'Min (-{limit} {units[j]})')

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

 

            ax.set_ylabel(f'Velocity ({units[j]})')

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

            ax.grid(True, alpha=0.3)

            ax.set_title(f'Joint {joint_names[j]} Velocity Profile', fontsize=10)

 

        axes[3].set_xlabel('Time (s)')

        plt.tight_layout()

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

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_condition_number_curve(self, timestamps, cond_numbers):

        """

        ② 雅可比条件数曲线：反映映射病态程度（无量纲）

        """

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

 

        plot_cond = np.copy(cond_numbers)

        inf_mask = ~np.isfinite(plot_cond)

        if np.any(inf_mask):

            finite_max = np.nanmax(plot_cond[~inf_mask]) if np.any(~inf_mask) else 1000

            plot_cond[inf_mask] = finite_max * 2

 

        ax.plot(timestamps, plot_cond, '-', color='purple',

                linewidth=1.0, label='Condition Number cond(J)')

 

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

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

 

        if np.any(inf_mask):

            inf_times = timestamps[inf_mask]

            for t in inf_times[:5]:

                ax.axvline(x=t, color='darkred', linestyle=':', linewidth=0.8, alpha=0.5)

 

        ax.set_xlabel('Time (s)')

        ax.set_ylabel('Condition Number (dimensionless)')

        ax.set_title('Jacobian Condition Number Along Trajectory — Mapping Stability')

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

        ax.grid(True, alpha=0.3)

        ax.set_yscale('log')

 

        plt.tight_layout()

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

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_velocity_violation_scatter(self, timestamps, joint_velocities, violations):

        """

        ③ 速度超限点散点标记：精确定位哪些点、哪个关节超限

        """

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

 

        colors_j = ['steelblue', 'orange', 'green', 'purple']

        joint_names = ['θ₁', 'θ₂', 'θ₃', 'd₄']

 

        for j in range(4):

            ax.scatter(timestamps, joint_velocities[:, j],

                       c=colors_j[j], s=5, alpha=0.3,

                       label=f'{joint_names[j]} (all points)')

 

        for idx, j, actual_vel, limit in violations:

            if idx < len(timestamps):

                ax.scatter(timestamps[idx], actual_vel,

                           c='red', s=50, marker='x', linewidths=1.5,

                           label=f'VIOLATION {joint_names[j]} 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=7)

 

        ax.set_xlabel('Time (s)')

        ax.set_ylabel('Joint Velocity')

        ax.set_title('Velocity Violation Detection — Red X Marks Exceed Limit')

        ax.grid(True, alpha=0.3)

 

        plt.tight_layout()

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

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_cartesian_vs_joint_velocity(self, timestamps, cart_speeds, joint_velocities):

        """

        ④ 末端合成速度与关节角速度对比：直观展示速度放大效应

        """

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

 

        cart_mag = np.sqrt(cart_speeds[:, 0]**2 + cart_speeds[:, 1]**2 +

                          cart_speeds[:, 2]**2)

        joint_ang_mag = np.sqrt(joint_velocities[:, 0]**2 + joint_velocities[:, 1]**2 +

                                joint_velocities[:, 2]**2)

 

        ax.plot(timestamps, cart_mag, '-', color='darkblue', linewidth=1.5,

                label='TCP Speed Magnitude (m/s)')

        ax.plot(timestamps, joint_ang_mag, '-', color='darkred', linewidth=1.5,

                label='Joint Angular Velocity Magnitude (rad/s)')

 

        ratio = np.zeros_like(cart_mag)

        mask = cart_mag > 1e-6

        if np.any(mask):

            ratio[mask] = joint_ang_mag[mask] / cart_mag[mask]

            ax2 = ax.twinx()

            ax2.plot(timestamps[mask], ratio[mask], '--', color='gray', linewidth=0.8,

                     alpha=0.6, label='Amplification Ratio (joint/TCP)')

            a

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