《Python机器人基础原理》第一百三十八篇：全套工艺仿真——单循环码垛流程，输出力矩、角度、末端误差整套仿真报告数据

 

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

 

1. 实际工业应用场景

 

周三下午，电商仓储分拣线。一台3R竖直平面码垛机械臂执行单循环码垛动作：从待料位出发，到抓取位拾取2.0 kg料箱，提升、水平平移、下降放置到托盘上，最后空载返回待料位。抓具自身质量1.5 kg，全程参与动力学；料箱在抓取瞬间挂接到末端，在放置瞬间脱离，负载在流程中发生两次突变。

 

"第38箱的数据里，关节1力矩在抓取后第一个加速段出现一个明显的台阶，末端到位后横向偏差约3 mm。"小赵放大曲线："抓取前是空载动力学，抓取后一瞬间等效惯量和重力都变了，如果仿真里全程带着2 kg负载跑，返回段的力矩曲线就是假的——它应该在放置后就降下来。"

 

他调出参数：连杆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；连杆3质量1.5 kg、长度0.30 m、转动惯量0.05 kg·m²、质心0.15 m。抓具1.5 kg，料箱2.0 kg，PD增益Kp=[400, 300, 150] N·m/rad，Kd=[40, 30, 15] N·m·s/rad，重力9.81 m/s²。

 

"把单循环跑一遍，负载在抓取段起点切入、放置段终点切出，输出力矩、角度、末端误差整套数据，看看峰值力矩和末端偏差到底在什么水平。"

 

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

 

2. 痛点分析

 

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

第3章 运动学求解 → 正运动学 末端位置由关节角经方向余弦累加得到，角度误差经构型依赖的映射关系放大为末端误差

第4章 机器人动力学建模 → 刚体动力学 3R连杆+抓具+负载形成时变耦合惯性，抓取/放置瞬间负载突变改变系统动力学

第5章 机器人关节驱动与控制 → PD伺服 负载突变后PD参数未自适应调整，过渡段跟踪误差会暂时增大

第6章 轨迹规划与控制 → 多段轨迹拼接 码垛单循环由多段子轨迹组成，每段需独立规划并保证衔接平滑

 

本篇代码解决 L2 离线仿真校核层。输入：3R连杆参数、抓具质量、分段负载配置、多段三次多项式轨迹。处理：RK4积分真实动力学，负载在指定时刻切换。输出：4张分析图表 + CSV仿真报告 + 单元测试。

 

不覆盖：多层多箱循环、奇异位形、碰撞检测、摩擦力矩、传送带同步。

 

3. 核心逻辑讲解

 

物理直觉

 

码垛单循环就像人搬箱子走一趟：弯腰抓箱（空载→负载）、直腰提起、横移、弯腰放下（负载→空载）、直腰退回。两次负载切换是这道工艺的灵魂——抓起箱子的一瞬间，手臂末端突然多了2 kg，惯性和重力都变了，电机需要重新适应；放下箱子后又轻了，动力学回到空载状态。

 

每段轨迹用三次多项式规划，保证起步和到位时速度都为零，不会"撞"到目标。PD控制器像两只手上的"虚拟肌肉"：比例项把偏离目标的关节拉回去，微分项给转得太快的关节踩刹车。重力前馈则像提前估算好的"配重"——按当前构型和当前负载算出一个抵消自重的力矩，让PD只需处理动态跟踪的残差。

 

末端误差来自关节角的跟踪偏差。角度偏差经过正运动学传递：臂伸得越直，同样的关节角度误差被放得越大，末端偏差也越大。

 

业务逻辑 → 代码映射

 

输入：3R连杆参数 + 抓具质量 + 分段负载配置 + 多段航点

  ↓

【步骤1：建立3R动力学模型（含抓具）】

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

  负载作为独立项在仿真循环中按当前时刻动态注入

  ↓

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

  x = Σ l_i·cos(Σ q_j), y = Σ l_i·sin(Σ q_j)

  ↓

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

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

  预计算所有时刻的期望量，避免仿真循环内重复找段

  ↓

【步骤4：RK4闭环仿真（分段负载切换）】

  抓取段起点：load_mass 从 0 切换到 2.0 kg

  放置段终点：load_mass 从 2.0 kg 切换到 0

  每步记录：q_des, q_actual, τ, 末端位置, 末端误差

  ↓

【步骤5：输出报告】

  4张图 + CSV数据文件

 

关键公式文字翻译

 

三次多项式轨迹：q(t) = a·t³ + b·t² + d。四个系数由起点位置、起点速度（零）、终点位置、终点速度（零）确定，保证每段内位置、速度连续，加速度在衔接处有有限跳变但不产生理想冲激。

 

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

 

正运动学：末端坐标由各连杆长度在全局坐标系下的投影累加。每一段连杆的方向取决于前面所有关节角的累加和。

 

末端位置误差：e_end = [x_des - x_actual, y_des - y_actual]，由角度误差经运动学传递得到，反映码垛放置精度。

 

4. OOP代码实现

 

技术栈

 

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

 

项目结构树

 

palletizing_process_sim/

├── run_sim.py # 主入口

├── requirements.txt

├── robot/

│ ├── __init__.py

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

│ ├── dynamics.py # 3R刚体动力学（不含负载，负载由仿真器动态注入）

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

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

│ ├── controller.py # PD+重力前馈

│ ├── simulator.py # RK4分段仿真器（含负载切换）

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

├── tests/

│ ├── __init__.py

│ └── test_integration.py # 3类10个测试（含Coriolis能量一致性检验）

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

└── README.md

 

核心源码

 

"robot/models.py"

 

"""

Data models for 3R planar manipulator palletizing simulation.

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

from dataclass import dataclass

 

 

@dataclass

class Link3R:

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

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

    """3R 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 != (3,):

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

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

            raise ValueError(f"Kd must be length-3, 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 SimConfig3R:

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

    links: list

    end_effector_mass: float = 0.0 # kg，抓具质量（常驻）

    gravity: float = 9.81

    dt_phys: float = 0.0001

    tau_limit: float = 60.0

 

    def __post_init__(self):

        if len(self.links) != 3:

            raise ValueError(

                f"SimConfig3R requires exactly 3 links, got {len(self.links)}")

        if self.end_effector_mass < 0:

            raise ValueError("end_effector_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"

 

"""

3R竖直平面刚体动力学（不含负载，负载由仿真器按时间动态注入）。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

import math

from robot.models import SimConfig3R

 

 

class Dynamics3R:

    """

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

    连杆+抓具参数内置，负载质量由外部在每一步传入。

    """

    def __init__(self, config: SimConfig3R):

        self.cfg = config

        self.g = config.gravity

        self.m_ee = config.end_effector_mass # 抓具质量（常驻）

 

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

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

        self.l3 = config.links[2].length

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

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

        self.m3 = config.links[2].mass

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

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

        self.r3 = config.links[2].com_offset

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

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

        self.I3 = config.links[2].inertia

 

    def mass_matrix(self, q, load_mass=0.0):

        """3×3质量矩阵M(q)。抓具与负载均作为末端质点处理。"""

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

        c2 = math.cos(q[1])

        c3 = math.cos(q[2])

        c23 = math.cos(q[1] + q[2])

 

        l1, l2, l3 = self.l1, self.l2, self.l3

        r1, r2, r3 = self.r1, self.r2, self.r3

        m1, m2, m3 = self.m1, self.m2, self.m3

        me = self.m_ee + load_mass # 末端总质量

 

        # 连杆+抓具（常驻）的惯量项

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

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

                    + m3*(l1**2 + l2**2 + r3**2 + 2*l1*l2*c2 + 2*l1*r3*c23 + 2*l2*r3*c3))

        M12_base = (m2*r2*(r2 + l1*c2)

                    + m3*(r3*l1*c23 + l2*r3*c3 + l2**2 + l1*l2*c2))

        M13_base = m3*r3*(r3 + l1*c23 + l2*c3)

        M22_base = self.I2 + m2*r2**2 + m3*(r3**2 + l2**2 + 2*l2*r3*c3)

        M23_base = m3*r3*(r3 + l2*c3)

        M33_base = self.I3 + m3*r3**2

 

        # 抓具+负载（末端质点）的惯量项

        M11_ee = me*(l1**2 + l2**2 + l3**2 + 2*l1*l2*c2 + 2*l1*l3*c23 + 2*l2*l3*c3)

        M12_ee = me*(l2**2 + l1*l2*c2 + l1*l3*c23 + l2*l3*c3)

        M13_ee = me*(l3**2 + l1*l3*c23 + l2*l3*c3)

        M22_ee = me*(l2**2 + l3**2 + 2*l2*l3*c3)

        M23_ee = me*(l3**2 + l2*l3*c3)

        M33_ee = me*l3**2

 

        M11 = M11_base + M11_ee

        M12 = M12_base + M12_ee

        M13 = M13_base + M13_ee

        M22 = M22_base + M22_ee

        M23 = M23_base + M23_ee

        M33 = M33_base + M33_ee

 

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

                         [M12, M22, M23],

                         [M13, M23, M33]], dtype=float)

 

    def coriolis_vector(self, q, qd, load_mass=0.0):

        """

        科氏力/离心力向量C(q,qd)。

        由Christoffel符号推导：C_ij = Σ_k Γ_ijk·qd_k，其中

        Γ_ijk = 1/2·(∂M_ij/∂q_k + ∂M_ik/∂q_j - ∂M_jk/∂q_i)

        """

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

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

        s2 = math.sin(q[1])

        s3 = math.sin(q[2])

        s23 = math.sin(q[1] + q[2])

 

        l1, l2, l3 = self.l1, self.l2, self.l3

        m2, m3 = self.m2, self.m3

        me = self.m_ee + load_mass

 

        # Christoffel符号系数

        # Γ_111 = 0, Γ_112, Γ_113, Γ_121, Γ_122, Γ_123, ...

        gamma_113 = -me*l1*(l2*s2 + l3*s23) - m3*l1*r3*s23 - m2*l1*r2*s2

        gamma_112 = -gamma_113

        gamma_123 = -me*l2*l3*s3 - m3*l2*r3*s3

        gamma_213 = gamma_123

        gamma_223 = 0

        gamma_312 = me*l1*(l2*s2 + l3*s23) + m3*l1*r3*s23 + m2*l1*r2*s2

        gamma_321 = gamma_312

        gamma_323 = me*l2*l3*s3 + m3*l2*r3*s3

        gamma_332 = gamma_323

 

        C1 = (gamma_112*qd[0]*qd[1] + gamma_113*qd[0]*qd[2]

              + gamma_123*qd[1]*qd[2])

        C2 = (gamma_213*qd[0]*qd[2] + gamma_223*qd[1]*qd[2])

        C3 = (gamma_312*qd[0]**2 + gamma_321*qd[0]*qd[1]

              + gamma_323*qd[0]*qd[2] + gamma_332*qd[1]*qd[2])

 

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

 

    def gravity_vector(self, q, load_mass=0.0):

        """重力向量G(q)，含抓具与负载。"""

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

        c1 = math.cos(q[0])

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

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

 

        l1, l2, l3 = self.l1, self.l2, self.l3

        r1, r2, r3 = self.r1, self.r2, self.r3

        m1, m2, m3 = self.m1, self.m2, self.m3

        me = self.m_ee + load_mass

 

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

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

              + m3*self.g*(l1*c1 + l2*c12 + r3*c123)

              + me*self.g*(l1*c1 + l2*c12 + l3*c123))

 

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

              + m3*self.g*(l2*c12 + r3*c123)

              + me*self.g*(l2*c12 + l3*c123))

 

        G3 = m3*self.g*r3*c123 + me*self.g*l3*c123

 

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

 

    def forward_dynamics(self, q, qd, tau, load_mass=0.0):

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

        M = self.mass_matrix(q, load_mass)

        C = self.coriolis_vector(q, qd, load_mass)

        G = self.gravity_vector(q, load_mass)

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

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

        return qdd

 

"robot/kinematics.py"

 

"""

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

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

import math

from robot.models import SimConfig3R

 

 

class ForwardKinematics3R:

    """末端位置 x = Σ l_i·cos(Σ q_j), y = Σ l_i·sin(Σ q_j)。"""

 

    def __init__(self, config: SimConfig3R):

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

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

        self.l3 = config.links[2].length

 

    def end_effector_pos(self, q):

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

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

        theta1 = q[0]

        theta2 = q[0] + q[1]

        theta3 = q[0] + q[1] + q[2]

 

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

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

 

        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 SimConfig3R

 

 

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 ({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, 3))

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

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

 

        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(3), np.zeros(3)

        if t >= sum(self.segment_durations):

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

 

        t_local = t

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

            if t_local <= t_start + T:

                t_rel = t_local - 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(3), np.zeros(3)

 

    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 PDGains3R

from robot.dynamics import Dynamics3R

 

 

class PDController3R:

    """

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

    重力前馈使用期望位形下的重力向量，负载与当前仿真步一致。

    """

    def __init__(self, gains: PDGains3R, dynamics: Dynamics3R):

        self.Kp = gains.Kp

        self.Kd = gains.Kd

        self.dyn = dynamics

 

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

        """计算控制力矩。"""

        e = q_des - q

        ed = qd_des - qd

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

 

        # 重力前馈：用期望关节角+当前负载计算

        grav_ff = self.dyn.gravity_vector(q_des, load_mass)

        tau = tau_pd + grav_ff

 

        return tau

 

"robot/simulator.py"

 

"""

分段RK4仿真器：含负载切换逻辑。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

from robot.dynamics import Dynamics3R

from robot.kinematics import ForwardKinematics3R

from robot.controller import PDController3R

from robot.models import SimConfig3R

 

 

class LoadSchedule:

    """

    分段负载调度：

    - 初始负载（通常0）

    - 在指定绝对时刻切换负载质量

    """

    def __init__(self, initial_load=0.0, switch_events=None):

        """

        switch_events: list of (time_s, load_mass_kg)

        """

        self.initial_load = float(initial_load)

        self.switch_events = sorted(

            [(float(t), float(m)) for t, m in (switch_events or [])])

 

    def get_load_at(self, t):

        """返回时刻t的负载质量。"""

        load = self.initial_load

        for t_event, m_event in self.switch_events:

            if t >= t_event:

                load = m_event

            else:

                break

        return load

 

 

class PalletizingSimulator:

    def __init__(self, dynamics: Dynamics3R, controller: PDController3R,

                 kinematics: ForwardKinematics3R, trajectory,

                 load_schedule: LoadSchedule, config: SimConfig3R):

        self.dyn = dynamics

        self.ctrl = controller

        self.fk = kinematics

        self.traj = trajectory

        self.load_schedule = load_schedule

        self.config = config

 

        self.dt = config.dt_phys

        self.n_steps = trajectory.n_steps

 

    def run(self):

        """

        运行完整仿真，返回时程数据。

        """

        # 初始状态从轨迹起点对齐

        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, 3))

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

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

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

        load_hist = np.zeros(self.n_steps)

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

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

 

        for i in range(self.n_steps):

            t = t_arr[i]

 

            # 当前负载

            load = self.load_schedule.get_load_at(t)

            load_hist[i] = load

 

            # 期望量（直接索引预计算结果）

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

 

            # 当前状态

            q = x[:3]

            qd = x[3:6]

 

            # 记录

            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]

 

            # 控制力矩（RK4各子步内零阶保持，dt=0.1ms下精度足够）

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

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

            tau_hist[i] = tau

 

            # RK4积分

            def deriv(xx, tau_fixed=tau, load_fixed=load):

                qdd = self.dyn.forward_dynamics(

                    xx[:3], xx[3:6], tau_fixed, load_fixed)

                return np.concatenate([xx[3:6], 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,

            'load': load_hist,

            'end_pos': end_pos_hist,

            'end_pos_des': end_pos_des,

            'end_error': end_error_hist,

        }

 

"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 Visualizer3R:

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

        self.rd = results_dir

        os.makedirs(results_dir, exist_ok=True)

 

    def plot_angle_tracking(self, result):

        """图1：期望角度 vs 实际角度时程。"""

        t = result['t']

        q_des = np.degrees(result['q_des'])

        q_act = np.degrees(result['q_actual'])

        load = result['load']

 

        fig, axes = plt.subplots(3, 1, figsize=(11, 9), sharex=True)

 

        labels = ['关节1', '关节2', '关节3']

        for j in range(3):

            axes[j].plot(t, q_des[:, j], 'k--', linewidth=1.5, label='期望角度')

            axes[j].plot(t, q_act[:, j], 'b-', linewidth=1.0, label='实际角度', alpha=0.8)

            axes[j].set_ylabel(f'{labels[j]}角度 (deg)')

            axes[j].legend(fontsize=7)

            axes[j].grid(True, alpha=0.3)

 

        # 叠加负载曲线

        ax2 = axes[-1].twinx()

        ax2.plot(t, load, 'g-', linewidth=0.8, alpha=0.6)

        ax2.set_ylabel('负载质量 (kg)', color='g')

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