《Python机器人基础原理》第二十八篇：焊接作业仿真——笛卡尔空间直线轨迹规划，生成末端执行器直线运动位姿序列

 

1. 实际工业应用场景

 

周三下午，某汽车零部件焊接工段。一台3-DOF垂直关节臂（基座偏航θ₁ + 大臂俯仰θ₂ + 小臂俯仰θ₃）搭载焊枪末端执行器，负责在汽车排气管法兰盘上沿直线焊缝进行MAG焊接。机器人构型参数：大臂长450 mm、质量4.2 kg（质心距基座0.22 m），小臂长350 mm、质量2.1 kg（质心距肘部0.18 m），末端焊枪通过腕部法兰刚性连接，质量0.8 kg，TCP（工具中心点）位于法兰正下方120 mm处。关节限位：θ₁±180°、最大角速度2.8 rad/s、最大角加速度7.0 rad/s²；θ₂±130°、最大角速度2.2 rad/s、最大角加速度5.0 rad/s²；θ₃±150°、最大角速度3.0 rad/s、最大角加速度8.0 rad/s²。

 

焊缝是一条空间直线段：起点在基座坐标系中的位置为 (0.35 m, 0.12 m, 0.30 m)，终点为 (0.55 m, 0.12 m, 0.30 m)，长度200 mm，位于水平面内。要求焊枪TCP沿此直线以恒定线速度 0.05 m/s 匀速运动，焊枪姿态保持垂直向下（焊枪轴线与重力方向平行，TCP始终垂直于工件表面）。

 

小赵接到任务：在离线仿真中验证这条直线轨迹是否可行——生成TCP的笛卡尔空间直线位姿序列，通过逆运动学求解各关节角度随时间的变化，输出关节空间的位置/速度/加速度曲线，校验全程是否超出关节限位。

 

他先在纸上画了个草图："TCP从(0.35, 0.12, 0.30)到(0.55, 0.12, 0.30)，沿Y=0.12的水平线走200 mm。速度0.05 m/s，需要4.0秒。"

 

小李路过看了一眼："你打算怎么让TCP走直线？直接给起点终点关节角度做插值？"

 

"对，关节空间五次多项式。"

 

"那TCP走的是直线吗？"

 

小赵愣了一下。

 

"关节空间里做插值，TCP在笛卡尔空间里走的是曲线。你的焊枪火焰轨迹会弯，焊缝直接报废。"

 

"……那怎么办？"

 

"笛卡尔空间直线规划。先在笛卡尔空间把TCP的直线轨迹按时间采样，每个采样点通过逆运动学算出对应的关节角度。这样TCP走的是真直线，但各关节的速度不会均匀——映射回来之后是非线性的。"

 

"逆解失败怎么办？比如某个点够不到？"

 

"离线校验工具的正确行为是直接报出来——哪一段不可达、为什么。不能偷偷用上一个点的角度填上，那样后面的速度差分全是假的。"

 

本篇解决的问题：给定TCP起点和终点的笛卡尔坐标及恒定姿态，在笛卡尔空间生成直线位置序列（姿态恒定，四元数Slerp退化为恒等映射，相关代码为后续姿态渐变场景预留），通过逆运动学求解各关节角度，输出关节空间轨迹曲线，全程校验限位，逆解不可达点直接中断并报告。

 

⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于离线焊接轨迹规划校验。不可直接用于真实机器人控制器、焊接电源或产线设备。真实工业现场需结合焊接工艺参数（电流、电压、送丝速度）、实时逆解、奇异点回避、外部轴同步。代码作者不对直接使用本代码于真实设备造成的任何后果负责。

 

2. 痛点分析

 

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

第2章 位姿描述与坐标变换 → 2.3 旋转矩阵与四元数 姿态表示需从旋转矩阵转换为四元数，为后续球面线性插值（Slerp）打基础

第3章 连杆参数及运动学求解 → 3.2 逆运动学 笛卡尔空间每个采样点需通过逆运动学求解关节角度，解析解存在多解需选最近解

第6章 轨迹规划与控制 → 6.5 笛卡尔空间轨迹规划 关节空间插值无法保证TCP走直线→必须在笛卡尔空间规划→逆解映射

 

本篇代码解决 L2 离线笛卡尔轨迹规划校验层。输入：TCP起点/终点位姿、运动时间、采样频率。在笛卡尔空间线性插值生成位置序列，调用逆运动学求解器逐点计算关节角度，输出关节空间轨迹曲线、限位校验报告、不可达点报告。

 

不覆盖：焊接工艺参数建模（熔池动力学、热变形）、实时迭代逆解、奇异点回避策略、力控焊接（接触力反馈）、姿态渐变焊缝（如船形焊、环缝，需启用Slerp）。

 

3. 核心逻辑讲解

 

物理直觉

 

想象你手里拿着一支笔，要在纸上画一条直线。你不会去想"手腕转多少度、肘关节弯多少度"——你只关心笔尖在纸上的轨迹。这就是笛卡尔空间规划：先确定末端（TCP）要走的路径，再反推每个关节该怎么动。

 

但机器人不是人的手臂，它有严格的关节限位和速度上限。所以你需要把笛卡尔路径"翻译"成关节的运动指令。翻译的过程就是逆运动学。

 

关键难点：笛卡尔空间的匀速直线运动，映射回关节空间后，各关节的角速度不是常数。靠近奇异构型时，某个关节需要快速旋转来补偿TCP的小位移——这就是"奇异点问题"。本篇的3-DOF垂直关节臂在完全伸直或完全折叠时会出现奇异，代码中通过限位校验和不可达检测来暴露这个问题。

 

位置线性插值 + 姿态处理

 

TCP位置从起点到终点做线性插值：p(t) = p₀ + (p_T - p₀) · (t/T_total)。每一项含义：p₀是起点坐标，p_T - p₀是总位移向量，t/T_total是归一化时间（0到1），乘起来就是当前时刻的位置。匀速直线运动的直接结果。

 

关于姿态：本篇焊缝要求焊枪始终垂直向下，起点和终点的姿态四元数完全相同（q₀ = q₁）。因此四元数球面线性插值（Slerp）退化为恒等映射——每个采样点输出相同的四元数。代码中

"transforms.py"实现了完整的Slerp（含最短路径检测和线性退化保护），但本篇不调用它参与逆解。这是有意为之的预留设计：当后续场景需要姿态渐变（如船形焊、角焊缝、法兰环缝起弧段）时，Slerp结果将作为逆解的腕部方向约束输入。本篇重点验证位置→逆解→关节轨迹的核心链路。

 

逆运动学几何法

 

将3-DOF垂直关节臂投影到垂直平面。已知TCP的(x, y, z)，腕部在TCP正上方tcp_offset处（因为焊枪垂直向下），所以腕部位置为(x, y, z + tcp_offset)。

 

- θ₁：基座偏航，由腕部在基座平面的投影决定，θ₁ = atan2(y, x)

- 腕部到基座的水平距离 r = √(x² + y²)，高度 h = z + tcp_offset

- 腕部到基座的直线距离 d = √(r² + h²)

- 余弦定理求θ₃：cos(θ₃) = (d² - L₁² - L₂²) / (2·L₁·L₂)

- θ₂ = atan2(h, r) - atan2(L₂·sin(θ₃), L₁ + L₂·cos(θ₃))

 

可达性检查：d > L₁ + L₂（外不可达）或 d < |L₁ - L₂|（内不可达/奇异）时，该点不可达，直接中断。

 

业务逻辑 → 代码映射

 

输入：TCP起点位姿 (x0,y0,z0, q0)、终点位姿 (xT,yT,zT, qT)、

      运动时间 T_total、采样数 N

  ↓

步骤1：笛卡尔空间位置线性插值

  p(t) = p0 + (pT - p0) · (t/T_total)

  姿态：q(t) = q0（本篇恒定，Slerp预留）

  ↓

步骤2：逆运动学求解（几何法，逐点）

  对每点 (x, y, z)：

    计算腕部位置 → θ₁ = atan2(y, x)

    余弦定理求θ₃ → 两个分支（肘上/肘下）

    求θ₂ → 最近解策略选与上一时刻差值最小的解

    可达性检查：不可达则中断并报告

  ↓

步骤3：有限差分计算速度和加速度

  ω_j = Δθ/Δt（中心差分，端点用前向/后向）

  α_j = Δω/Δt

  ↓

步骤4：全程校验

  角度限位、角速度、角加速度逐点校验

  不可达点报告

  ↓

输出：4张图表 + 校验报告

 

关键公式文字翻译

 

位置线性插值：p(t) = p₀ + (p_T - p₀) × (t/T)。匀速直线运动的直接离散化。

 

余弦定理逆解：cos(θ₃) = (d² - L₁² - L₂²) / (2·L₁·L₂)。d是腕部到基座的距离，L₁和L₂是大小臂长度。这个公式来源于三角形三边关系——大小臂和腕部-基座连线构成一个三角形，已知三边求内角。

 

中心差分速度：ω[i] = (θ[i+1] - θ[i-1]) / (2Δt)。用前后两个点来估计中间点的导数，比前向差分精度更高（O(Δt²)），但端点只能用前向/后向差分。

 

4. OOP代码实现

 

技术栈

 

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

 

项目结构树

 

welding_cartesian_trajectory_sim/

├── run_welding_cartesian_trajectory_sim.py

├── requirements.txt

├── robot/

│ ├── __init__.py

│ ├── models.py

│ ├── transforms.py

│ ├── inverse_kinematics.py

│ ├── trajectory_generator.py

│ └── visualize_trajectory.py

├── tests/

│ ├── __init__.py

│ └── test_welding_cartesian_trajectory_sim.py

└── results/

 

核心源码

 

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

 

"""

机器人数据模型：连杆参数、关节限位、TCP位姿

3-DOF垂直关节臂：基座偏航θ₁ + 大臂俯仰θ₂ + 小臂俯仰θ₃

"""

 

 

from dataclasses import dataclass, field

from typing import List

import numpy as np

 

 

@dataclass

class LinkParams:

    """单连杆几何与质量参数"""

    name: str

    length: float # 连杆长度 (m)

    mass: float # 连杆质量 (kg)

    com_x: float # 质心x坐标（沿连杆方向）(m)

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

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

    inertia_xx: float = 0.0 # 惯性张量Ixx (kg·m²)

    inertia_yy: float = 0.0

    inertia_zz: float = 0.0

 

 

@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:

        """校验关节角度是否全部在限位内"""

        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

 

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

        """校验角速度是否全部在限位内"""

        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

 

    def check_all_accelerations(self, accelerations: List[float]) -> tuple:

        """校验角加速度是否全部在限位内"""

        violations = []

        for i, acc in enumerate(accelerations):

            if i < len(self.limits):

                if abs(acc) > self.limits[i].max_acceleration:

                    violations.append(i)

        return len(violations) == 0, violations

 

 

@dataclass

class TCPPose:

    """TCP位姿：位置(x,y,z) + 姿态四元数(qw,qx,qy,qz)"""

    x: float

    y: float

    z: float

    qw: float = 1.0

    qx: float = 0.0

    qy: float = 0.0

    qz: float = 0.0

 

    def position(self) -> np.ndarray:

        return np.array([self.x, self.y, self.z])

 

    def quaternion(self) -> np.ndarray:

        return np.array([self.qw, self.qx, self.qy, self.qz])

 

"robot/transforms.py" — 坐标变换与四元数运算

 

"""

坐标变换工具：旋转矩阵、四元数、齐次变换矩阵

四元数球面线性插值（Slerp）——本篇姿态恒定时退化为恒等映射，为后续姿态渐变场景预留

"""

 

 

import numpy as np

 

 

def euler_to_rotation_matrix(roll: float, pitch: float, yaw: float) -> np.ndarray:

    """

    将ZYX欧拉角转换为3×3旋转矩阵

    roll=绕x轴, pitch=绕y轴, yaw=绕z轴

    """

    cr, sr = np.cos(roll), np.sin(roll)

    cp, sp = np.cos(pitch), np.sin(pitch)

    cy, sy = np.cos(yaw), np.sin(yaw)

 

    R = np.array([

        [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],

        [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr],

        [-sp, cp*sr, cp*cr]

    ])

    return R

 

 

def rotation_matrix_to_quaternion(R: np.ndarray) -> np.ndarray:

    """

    将3×3旋转矩阵转换为单位四元数 [qw, qx, qy, qz]

    使用Shepperd法避免数值不稳定

    """

    q = np.zeros(4)

    trace = np.trace(R)

 

    if trace > 0:

        s = 0.5 / np.sqrt(trace + 1.0)

        q[0] = 0.25 / s

        q[1] = (R[2, 1] - R[1, 2]) * s

        q[2] = (R[0, 2] - R[2, 0]) * s

        q[3] = (R[1, 0] - R[0, 1]) * s

    else:

        if R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:

            s = 2.0 * np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2])

            q[0] = (R[2, 1] - R[1, 2]) / s

            q[1] = 0.25 * s

            q[2] = (R[0, 1] + R[1, 0]) / s

            q[3] = (R[0, 2] + R[2, 0]) / s

        elif R[1, 1] > R[2, 2]:

            s = 2.0 * np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2])

            q[0] = (R[0, 2] - R[2, 0]) / s

            q[1] = (R[0, 1] + R[1, 0]) / s

            q[2] = 0.25 * s

            q[3] = (R[1, 2] + R[2, 1]) / s

        else:

            s = 2.0 * np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1])

            q[0] = (R[1, 0] - R[0, 1]) / s

            q[1] = (R[0, 2] + R[2, 0]) / s

            q[2] = (R[1, 2] + R[2, 1]) / s

            q[3] = 0.25 * s

 

    return q / np.linalg.norm(q)

 

 

def quaternion_slerp(q0: np.ndarray, q1: np.ndarray, t: float) -> np.ndarray:

    """

    四元数球面线性插值（Slerp）

    本篇焊缝姿态恒定（q0=q1），此函数退化为恒等映射返回q0

    为后续姿态渐变场景（船形焊、环缝）预留

    q0, q1: 单位四元数 [qw, qx, qy, qz]

    t: 插值参数，0≤t≤1

    """

    # 数值保护：确保四元数为单位四元数

    q0 = q0 / np.linalg.norm(q0)

    q1 = q1 / np.linalg.norm(q1)

 

    dot = np.dot(q0, q1)

 

    # 如果点积为负，取q1的反方向以确保最短路径

    if dot < 0.0:

        q1 = -q1

        dot = -dot

 

    # 如果点积接近1，使用线性插值避免除零

    if dot > 0.9995:

        result = q0 + t * (q1 - q0)

        return result / np.linalg.norm(result)

 

    theta_0 = np.arccos(np.clip(dot, -1.0, 1.0))

    theta = theta_0 * t

 

    s0 = np.sin(theta_0 - theta) / np.sin(theta_0)

    s1 = np.sin(theta) / np.sin(theta_0)

 

    return s0 * q0 + s1 * q1

 

 

def quaternion_to_rotation_matrix(q: np.ndarray) -> np.ndarray:

    """将单位四元数转换为3×3旋转矩阵"""

    qw, qx, qy, qz = q

    R = np.array([

        [1 - 2*(qy**2 + qz**2), 2*(qx*qy - qw*qz), 2*(qx*qz + qw*qy)],

        [2*(qx*qy + qw*qz), 1 - 2*(qx**2 + qz**2), 2*(qy*qz - qw*qx)],

        [2*(qx*qz - qw*qy), 2*(qy*qz + qw*qx), 1 - 2*(qx**2 + qy**2)]

    ])

    return R

 

"robot/inverse_kinematics.py" — 逆运动学求解器

 

"""

3-DOF垂直关节臂逆运动学求解器

几何法解析求解：θ₁=atan2(y,x), θ₂和θ₃由余弦定理求得

支持最近解选择策略避免关节跳变

不可达点直接返回失败标志，不静默填充

"""

 

 

import numpy as np

from typing import List, Tuple, Optional

from robot.models import JointLimitSet

 

 

class InverseKinematicsSolver:

    """

    3-DOF垂直关节臂逆运动学

    构型：基座偏航θ₁ + 大臂俯仰θ₂ + 小臂俯仰θ₃

    焊枪垂直向下，TCP在腕部法兰正下方 tcp_offset 处

    """

 

    def __init__(self, L1: float, L2: float, tcp_offset: float,

                 joint_limits: JointLimitSet):

        self.L1 = L1

        self.L2 = L2

        self.tcp_offset = tcp_offset

        self.limits = joint_limits

 

    def solve(self, tcp_x: float, tcp_y: float, tcp_z: float,

              prev_angles: Optional[List[float]] = None) -> Tuple[np.ndarray, bool]:

        """

        逆运动学求解

        输入：TCP在基座坐标系中的位置 (m)

        输出：(关节角度数组 [θ₁, θ₂, θ₃], 是否成功)

        不可达时返回 (零数组, False)，由调用方决定是否中断

        """

        # θ₁：基座偏航

        theta1 = np.arctan2(tcp_y, tcp_x)

 

        # 腕部位置：焊枪垂直向下，腕部在TCP正上方tcp_offset处

        wrist_x = tcp_x

        wrist_y = tcp_y

        wrist_z = tcp_z + self.tcp_offset

 

        # 腕部到基座的水平距离和高度

        r = np.sqrt(wrist_x**2 + wrist_y**2)

        h = wrist_z

 

        # 腕部到基座的直线距离

        d = np.sqrt(r**2 + h**2)

 

        # 可达性检查：外不可达和内奇异

        if d > self.L1 + self.L2:

            return np.array([0.0, 0.0, 0.0]), False

        if d < abs(self.L1 - self.L2):

            return np.array([0.0, 0.0, 0.0]), False

 

        cos_theta3 = (d**2 - self.L1**2 - self.L2**2) / (2 * self.L1 * self.L2)

        cos_theta3 = np.clip(cos_theta3, -1.0, 1.0)

 

        # 两个解分支：肘部向上(负角)和肘部向下(正角)

        theta3_option1 = -np.arccos(cos_theta3)

        theta3_option2 = np.arccos(cos_theta3)

 

        solutions = []

        for theta3 in [theta3_option1, theta3_option2]:

            alpha = np.arctan2(h, r)

            beta = np.arctan2(self.L2 * np.sin(theta3),

                              self.L1 + self.L2 * np.cos(theta3))

            theta2 = alpha - beta

 

            angles = np.array([theta1, theta2, theta3])

            solutions.append(angles)

 

        # 检查限位并选择最优解

        valid_solutions = []

        for sol in solutions:

            is_valid, _ = self.limits.check_all_positions(sol.tolist())

            if is_valid:

                valid_solutions.append(sol)

 

        if len(valid_solutions) == 0:

            return np.array([0.0, 0.0, 0.0]), False

 

        if prev_angles is None:

            return valid_solutions[0], True

 

        # 最近解策略

        prev = np.array(prev_angles)

        best_sol = valid_solutions[0]

        best_diff = np.sum(np.abs(valid_solutions[0] - prev))

 

        for sol in valid_solutions[1:]:

            diff = np.sum(np.abs(sol - prev))

            if diff < best_diff:

                best_diff = diff

                best_sol = sol

 

        return best_sol, True

 

"robot/trajectory_generator.py" — 笛卡尔空间直线轨迹生成

 

"""

笛卡尔空间直线轨迹规划

位置线性插值 + 逆运动学逐点求解

逆解失败时直接中断并报告不可达点

"""

 

 

import numpy as np

from typing import List, Dict

from robot.models import TCPPose, JointLimitSet

from robot.inverse_kinematics import InverseKinematicsSolver

 

 

class CartesianLinearTrajectoryGenerator:

    """

    笛卡尔空间直线轨迹生成器

    TCP从起点到终点沿直线运动，姿态恒定（垂直向下）

    每个采样点通过逆运动学求解关节角度

    逆解失败直接中断，不静默填充

    """

 

    def __init__(self, ik_solver: InverseKinematicsSolver,

                 joint_limits: JointLimitSet):

        self.ik = ik_solver

        self.limits = joint_limits

 

    def generate(self, start_pose: TCPPose, end_pose: TCPPose,

                 total_time: float, n_samples: int) -> Dict:

 

        if total_time <= 0:

            raise ValueError("运动时间必须大于0")

        if n_samples < 2:

            raise ValueError("采样数至少为2")

 

        t_array = np.linspace(0, total_time, n_samples)

        dt = total_time / (n_samples - 1)

 

        tcp_positions = np.zeros((n_samples, 3))

        joint_angles = np.zeros((n_samples, 3))

        joint_velocities = np.zeros((n_samples, 3))

        joint_accelerations = np.zeros((n_samples, 3))

 

        ik_success_flags = np.zeros(n_samples, dtype=bool)

        unreachable_indices = [] # 记录不可达的采样点索引

        prev_angles = None

 

        start_pos = start_pose.position()

 

        for i, t in enumerate(t_array):

            tau = t / total_time

 

            # 位置线性插值

            tcp_positions[i, :] = start_pos + (end_pose.position() - start_pos) * tau

 

            # 逆运动学求解——失败直接中断

            angles, success = self.ik.solve(

                tcp_positions[i, 0], tcp_positions[i, 1],

                tcp_positions[i, 2], prev_angles

            )

 

            if not success:

                unreachable_indices.append(i)

                # 不可达点不填充假数据，标记失败并停止后续计算

                break

 

            joint_angles[i, :] = angles

            ik_success_flags[i] = True

            prev_angles = angles.tolist()

 

        # 如果有不可达点，截断有效数据到第一个不可达点之前

        if len(unreachable_indices) > 0:

            first_fail = unreachable_indices[0]

            n_valid = first_fail

        else:

            n_valid = n_samples

 

        # 有限差分计算速度和加速度（仅对有效数据）

        if n_valid >= 2:

            for j in range(3):

                # 中心差分（内部点）

                if n_valid > 2:

                    joint_velocities[1:n_valid-1, j] = (

                        joint_angles[2:n_valid, j] - joint_angles[0:n_valid-2, j]

                    ) / (2 * dt)

                # 端点前向/后向差分

                joint_velocities[0, j] = (

                    joint_angles[1, j] - joint_angles[0, j]

                ) / dt

                if n_valid > 1:

                    joint_velocities[n_valid-1, j] = (

                        joint_angles[n_valid-1, j] - joint_angles[n_valid-2, j]

                    ) / dt

 

            for j in range(3):

                if n_valid > 2:

                    joint_accelerations[1:n_valid-1, j] = (

                        joint_velocities[2:n_valid, j] - joint_velocities[0:n_valid-2, j]

                    ) / (2 * dt)

                joint_accelerations[0, j] = (

                    joint_velocities[1, j] - joint_velocities[0, j]

                ) / dt

                if n_valid > 1:

                    joint_accelerations[n_valid-1, j] = (

                        joint_velocities[n_valid-1, j] - joint_velocities[n_valid-2, j]

                    ) / dt

 

        # 全程限位校验（仅对有效数据）

        velocity_violations = []

        acceleration_violations = []

 

        for i in range(n_valid):

            vel_list = joint_velocities[i, :].tolist()

            acc_list = joint_accelerations[i, :].tolist()

 

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

            _, a_viol = self.limits.check_all_accelerations(acc_list)

 

            for j in v_viol:

                velocity_violations.append((i, j, vel_list[j],

                                            self.limits.limits[j].max_velocity))

            for j in a_viol:

                acceleration_violations.append((i, j, acc_list[j],

                                               self.limits.limits[j].max_acceleration))

 

        return {

            'time': t_array[:n_valid],

            'tcp_positions': tcp_positions[:n_valid],

            'joint_angles': joint_angles[:n_valid],

            'joint_velocities': joint_velocities[:n_valid],

            'joint_accelerations': joint_accelerations[:n_valid],

            'ik_success_flags': ik_success_flags[:n_valid],

            'unreachable_indices': unreachable_indices,

            'velocity_violations': velocity_violations,

            'acceleration_violations': acceleration_violations,

            'n_valid_samples': n_valid,

            'n_total_samples': n_samples

        }

 

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

 

"""

焊接作业笛卡尔轨迹规划可视化

4张核心图表：TCP三维直线轨迹、关节角度曲线、关节角速度曲线、关节角加速度曲线

"""

 

 

import numpy as np

import matplotlib.pyplot as plt

 

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

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

 

 

class TrajectoryVisualizer:

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

        self.results_dir = results_dir

        import os

        os.makedirs(results_dir, exist_ok=True)

 

    def plot_tcp_3d_trajectory(self, tcp_positions, start_pos, end_pos):

        """

        ① TCP三维直线轨迹：验证笛卡尔空间直线运动

        """

        fig = plt.figure(figsize=(10, 8))

        ax = fig.add_subplot(111, projection='3d')

 

        ax.plot(tcp_positions[:, 0], tcp_positions[:, 1], tcp_positions[:, 2],

                'b-', linewidth=1.5, label='TCP Trajectory')

        ax.scatter([start_pos[0]], [start_pos[1]], [start_pos[2]],

                   c='green', marker='o', s=80, label='Start', zorder=5)

        ax.scatter([end_pos[0]], [end_pos[1]], [end_pos[2]],

                   c='red', marker='s', s=80, label='End', zorder=5)

 

        ax.set_xlabel('X (m)')

        ax.set_ylabel('Y (m)')

        ax.set_zlabel('Z (m)')

        ax.set_title('TCP 3D Linear Trajectory in Cartesian Space')

        ax.legend(fontsize=8)

        ax.grid(True, alpha=0.3)

 

        plt.tight_layout()

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

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_joint_angles(self, timestamps, joint_angles):

        """

        ② 关节角度曲线：验证逆运动学求解的平滑性

        """

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

 

        joint_names = ['θ₁ (Base Yaw)', 'θ₂ (Elbow Pitch)', 'θ₃ (Wrist Pitch)']

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

 

        for j in range(3):

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

                    color=colors[j], linewidth=1.2, label=joint_names[j])

 

        ax.set_xlabel('Time (s)')

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

        ax.set_title('Joint Angle Profiles from Cartesian Linear Trajectory')

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

        ax.grid(True, alpha=0.3)

 

        plt.tight_layout()

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

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

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

        """

        ③ 关节角速度曲线+限位线：验证速度不超限

        """

        fig, axes = plt

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