python的机器人基础原理工业场景模拟第二十二篇:装配工位，使用五次多项式做关节轨迹规划，保证位置，速度，加速度，全部连续。
《Python机器人基础原理》第二十二篇：机械臂仿真——装配工位，使用五次多项式做关节轨迹规划，保证位置、速度、加速度全部连续
1. 实际工业应用场景
周一早晨，某汽车零部件装配线的拧紧工位。一台3-DOF垂直关节机械臂（基座偏航+肘部俯仰+末端旋转）正在执行螺栓预拧紧任务——从供料槽抓取M8螺栓，放到发动机缸体的螺纹孔上方，再以恒定低速旋入三圈。机器人构型：大臂长600 mm，小臂长450 mm，末端旋转轴位于小臂末端。连杆质量：大臂5.2 kg（质心距基座0.28 m），小臂3.1 kg（质心距肘部0.20 m），末端夹具1.5 kg（质心距肘部0.12 m，质心到旋转轴线横向偏置0.02 m）。关节最大角速度限制：基座偏航2.5 rad/s、肘部俯仰2.0 rad/s、末端旋转3.0 rad/s；最大角加速度限制：基座偏航5.0 rad/s²、肘部俯仰4.0 rad/s²、末端旋转6.0 rad/s²。
上周小李让小赵用三次多项式做了点位搬运仿真。今天小赵把结果拿来了——角度平滑，速度连续，但加速度在起终点有跳变。
小李指着图③起点处加速度的垂直线段："看这儿。三次多项式加速度在t=0瞬间从零跳到非零，jerk无穷大。上周搬运任务节拍慢、负载轻，问题不大。但今天拧紧工位——末端要带着螺栓旋入，起停如果加速度不连续，拧紧枪高频振动，螺栓可能偏斜滑牙。"
小赵翻笔记本："三次只有四个自由度，约束不了加速度。要加速度也连续，需要六个约束——位置×2、速度×2、加速度×2——五次多项式。"
"对。五次多项式六个系数，起终点加速度都设为零，加速度全程连续，jerk有限。"
"参数怎么定？"
"分两段。第一段：快速到位——起点θ₁=0°、θ₂=-30°、θ₃=0°；终点θ₁=60°、θ₂=-80°、θ₃=270°（0.75圈，把螺栓送到孔上方并对准）。T₁=3.0 s。起终点速度全零，起终点加速度全零。第二段：低速旋入——θ₃从270°继续转到1080°（再旋0.75圈，合计1.5圈，实际产线通常分两段规划）。本篇先做第一段快速到位，恒速旋入段留作下一篇多段拼接的素材。"
"那第一段T=3.0 s的峰值是多少？"
"零起终点速度/加速度时，速度峰值=15|Δq|/(8T)，加速度峰值=33√5|Δq|/(8T²)≈5.77|Δq|/T²，jerk端点值=60|Δq|/T³。Joint1的Δq=60°=1.047 rad，速度峰值≈15×1.047/(8×3)=0.654 rad/s，加速度峰值≈5.77×1.047/9≈0.671 rad/s²。Joint3的Δq=270°=4.712 rad，速度峰值≈15×4.712/24≈2.945 rad/s——距离3.0 rad/s的限位只剩1.8%余量，贴着上限在飞。加速度峰值≈5.77×4.712/9≈3.02 rad/s²，余量约50%。"
小赵抬头："Joint3速度余量这么小？"
"对。因为Δq大、T又短。如果真要旋三圈（1080°），单段五次多项式根本吃不消——Δq=18.85 rad，T=3 s时速度峰值≈11.8 rad/s，超限位四倍。这就是为什么实际拧紧要拆成'快速到位+恒速旋入'两段。本篇先把第一段做对。"
本篇解决的问题：给定起点和终点的关节角度，采用五次多项式完成关节空间插值，约束位置、速度、加速度在起终点均为指定值（设为零），生成C²连续（位置、速度、加速度全连续）的轨迹序列，用于装配工位快速到位阶段的离线轨迹验证。恒速旋入段需多段拼接，不在本篇覆盖范围内。
⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于装配工位的关节空间轨迹生成与可视化。不可直接用于真实机器人控制器、伺服驱动器或装配产线设备。真实工业现场需结合运动控制卡、实时插补、软硬限位保护。代码作者不对直接使用本代码于真实设备造成的任何后果负责。
2. 痛点分析
课程模块 本篇对应的工程痛点
第6章 轨迹规划与控制 → 6.2 多项式插值 装配工位要求起停平稳，三次多项式加速度不连续→jerk无穷大→末端振动→螺栓偏斜
第6章 轨迹规划与控制 → 6.3 高阶多项式轨迹 五次多项式提供C²连续（位置、速度、加速度），起终点加速度为零，jerk有限
第5章 机器人关节驱动与控制 → 5.1 伺服驱动基础 伺服电机有jerk限制，有限jerk减少机械磨损和定位超调
本篇代码解决 L2 离线轨迹规划层。输入：起点/终点关节角度、起终点速度（零）、起终点加速度（零）、运动时间T=3.0 s、采样点数。输出：各关节的θ(t)、θ̇(t)、θ̈(t)、θ⃛(t)时间序列。代码不是设备实时控制器，不做伺服闭环。它做的事情是：给定约束，用五次多项式生成C²连续轨迹，校验连续性和物理可行性。
不覆盖：恒速旋入段（需多段拼接，下一篇）、笛卡尔空间插补、梯形速度/S曲线、B样条、实时插补、多段轨迹全局优化。
3. 核心逻辑讲解
物理直觉
三次多项式就像你开车从A到B，缓缓加速再缓缓减速，但一踩油门瞬间车就有个突然的推背感（加速度跳变）。五次多项式相当于你不仅控制油门深浅，还控制油门踩下去的速度——让加速度本身也平滑地从零爬升再平滑地回到零。乘客感觉不到任何顿挫，拧紧枪也不会抖。
关键洞察：五次多项式有六个自由度（六个系数），能同时满足"起点位置、起点速度、起点加速度、终点位置、终点速度、终点加速度"六个约束。起终点加速度设为零时，加速度曲线从零开始、平滑变化、回到零——jerk（加速度的变化率）在起终点取有限值60|Δq|/T³，全程无无穷大脉冲。
业务逻辑 → 代码映射
输入：起点角度q_start、终点角度q_end、运动时间T=3.0 s、采样点数N=150
↓
步骤1：对每个关节，建立五次多项式
θ(t) = a₀ + a₁·t + a₂·t² + a₃·t³ + a₄·t⁴ + a₅·t⁵
↓
步骤2：代入6个约束条件（线性方程组）：
- θ(0) = q_start → a₀ = q_start
- θ'(0) = 0 → a₁ = 0
- θ''(0) = 0 → 2a₂ = 0 → a₂ = 0
- θ(T) = q_end → a₀ + a₃·T³ + a₄·T⁴ + a₅·T⁵ = q_end
- θ'(T) = 0 → 3a₃·T² + 4a₄·T³ + 5a₅·T⁴ = 0
- θ''(T) = 0 → 6a₃·T + 12a₄·T² + 20a₅·T³ = 0
↓
步骤3：化简解3×3线性方程组求a₃,a₄,a₅：
a₃ = 10Δq/T³, a₄ = -15Δq/T⁴, a₅ = 6Δq/T⁵
↓
步骤4：在N个采样点上计算 θ(t), θ'(t), θ''(t), θ'''(t)
↓
步骤5：解析计算峰值（速度=15|Δq|/(8T)，加速度=33√5|Δq|/(8T²)，
jerk端点=60|Δq|/T³）与数值求根结果交叉校验
↓
步骤6：校验三重限位，输出4张图表验证C²连续性
关键公式文字翻译
五次多项式：θ(t) = a₀ + a₁·t + a₂·t² + a₃·t³ + a₄·t⁴ + a₅·t⁵。六个系数由六个边界约束唯一确定。
零起终点速度/加速度特例：a₀=q_start, a₁=0, a₂=0, a₃=10Δq/T³, a₄=-15Δq/T⁴, a₅=6Δq/T⁵。
速度：θ̇(t) = 3a₃t² + 4a₄t³ + 5a₅t⁴。起终点为零，单峰，峰值=15|Δq|/(8T)≈1.875|Δq|/T。
加速度：θ̈(t) = 6a₃t + 12a₄t² + 20a₅t³。起终点为零，先正后负再回零，峰值=33√5|Δq|/(8T²)≈5.77|Δq|/T²。
加加速度（jerk）：θ⃛(t) = 6a₃ + 24a₄t + 60a₅t²。起终点取有限值±60|Δq|/T³，全程无无穷大。
单位：角度 rad，角速度 rad/s，角加速度 rad/s²，jerk rad/s³，时间 s。
4. OOP代码实现
技术栈
严格限定：numpy, scipy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方机器人库。复用第二十一篇的
"trajectory/models.py"（JointWaypoint、JointLimit、JointLimitSet、RobotParams），本篇新增加速度字段并新增五次插值器和可视化。
项目结构树
quintic_trajectory_sim/
├── run_quintic_sim.py
├── requirements.txt
├── trajectory/
│ ├── __init__.py
│ ├── models.py
│ ├── quintic_interpolator.py
│ └── visualize.py
├── tests/
│ ├── __init__.py
│ └── test_quintic_sim.py
└── results/
核心源码
"trajectory/models.py" — dataclass数据模型（修正版，新增加速度字段）
"""
3-DOF垂直关节机械臂数据模型
装配工位仿真：五次多项式关节空间插值
构型说明：基座偏航(θ1, 竖直轴) + 肘部俯仰(θ2, 水平轴) + 末端旋转(θ3)
物理声明：
- 本篇仅做轨迹规划，不涉及动力学计算
- 关节限位用于校验插值结果是否越界
- 速度/加速度限制用于校验轨迹可行性
"""
from dataclasses import dataclass, field
from typing import List, Tuple
import numpy as np
@dataclass
class JointWaypoint:
"""
单个关节的单个路点
angle: 关节角度 (rad)
velocity: 关节角速度 (rad/s)，设为零表示停靠
acceleration: 关节角加速度 (rad/s²)，五次多项式支持非零边界加速度
"""
angle: float
velocity: float = 0.0
acceleration: float = 0.0
joint_name: str = ""
def __post_init__(self):
if not np.isfinite(self.angle):
raise ValueError(f"JointWaypoint: 角度必须为有限数值")
if not np.isfinite(self.velocity):
raise ValueError(f"JointWaypoint: 速度必须为有限数值")
if not np.isfinite(self.acceleration):
raise ValueError(f"JointWaypoint: 加速度必须为有限数值")
@dataclass
class JointLimit:
"""单关节硬件限位"""
min_val: float
max_val: float
joint_name: str = ""
max_velocity: float = 0.0 # 最大角速度 (rad/s)
max_acceleration: float = 0.0 # 最大角加速度 (rad/s²)
max_jerk: float = 0.0 # 最大加加速度 (rad/s³)，可选
def __post_init__(self):
if self.min_val >= self.max_val:
raise ValueError(
f"JointLimit: min_val({self.min_val})必须严格小于"
f"max_val({self.max_val})")
if not np.isfinite(self.min_val) or not np.isfinite(self.max_val):
raise ValueError("JointLimit: 限位值必须为有限数值")
def contains(self, value: float) -> bool:
return (self.min_val - 1e-10) <= value <= (self.max_val + 1e-10)
def velocity_ok(self, vel: float) -> bool:
if self.max_velocity <= 0:
return True
return abs(vel) <= self.max_velocity + 1e-10
def acceleration_ok(self, accel: float) -> bool:
if self.max_acceleration <= 0:
return True
return abs(accel) <= self.max_acceleration + 1e-10
def jerk_ok(self, jerk: float) -> bool:
if self.max_jerk <= 0:
return True
return abs(jerk) <= self.max_jerk + 1e-10
@dataclass
class JointLimitSet:
"""3-DOF机械臂关节限位集合"""
limits: List[JointLimit]
def __post_init__(self):
if len(self.limits) != 3:
raise ValueError(f"需要3个关节限位, 实际{len(self.limits)}个")
@classmethod
def default_3dof_arm(cls):
"""
默认3-DOF机械臂限位
θ1: ±170°, 最大速度2.5 rad/s, 最大加速度5.0 rad/s²
θ2: ±140°, 最大速度2.0 rad/s, 最大加速度4.0 rad/s²
θ3: ±180°(旋转多圈), 最大速度3.0 rad/s, 最大加速度6.0 rad/s²
"""
pi = 3.141592653589793
return cls(limits=[
JointLimit(-170 * pi / 180, 170 * pi / 180, 'joint1_base_yaw',
2.5, 5.0, 0.0),
JointLimit(-140 * pi / 180, 140 * pi / 180, 'joint2_elbow_pitch',
2.0, 4.0, 0.0),
JointLimit(-180 * pi / 180, 180 * pi / 180, 'joint3_end_rotate',
3.0, 6.0, 0.0),
])
def check_all_positions(self, joint_values: List[float]) -> Tuple[bool, List[int]]:
if len(joint_values) != 3:
raise ValueError(f"需要3个关节值, 实际{len(joint_values)}个")
all_pass = True
violation_indices = []
for i, (val, limit) in enumerate(zip(joint_values, self.limits)):
if not limit.contains(val):
all_pass = False
violation_indices.append(i)
return all_pass, violation_indices
def check_all_velocities(self, velocities: List[float]) -> Tuple[bool, List[int]]:
if len(velocities) != 3:
raise ValueError(f"需要3个速度值, 实际{len(velocities)}个")
all_pass = True
violation_indices = []
for i, (vel, limit) in enumerate(zip(velocities, self.limits)):
if not limit.velocity_ok(vel):
all_pass = False
violation_indices.append(i)
return all_pass, violation_indices
def check_all_accelerations(self, accelerations: List[float]) -> Tuple[bool, List[int]]:
if len(accelerations) != 3:
raise ValueError(f"需要3个加速度值, 实际{len(accelerations)}个")
all_pass = True
violation_indices = []
for i, (accel, limit) in enumerate(zip(accelerations, self.limits)):
if not limit.acceleration_ok(accel):
all_pass = False
violation_indices.append(i)
return all_pass, violation_indices
@dataclass
class RobotParams:
"""
3-DOF机械臂参数（本篇仅用于轨迹规划，不涉及动力学）
"""
joint_names: List[str] = field(default_factory=lambda: [
'joint1_base_yaw', 'joint2_elbow_pitch', 'joint3_end_rotate'
])
link_lengths: List[float] = field(default_factory=lambda: [0.600, 0.450, 0.0])
@classmethod
def default_3dof_arm(cls):
return cls()
"trajectory/quintic_interpolator.py" — 五次多项式插值器
"""
五次多项式关节空间轨迹插值器
θ(t) = a₀ + a₁·t + a₂·t² + a₃·t³ + a₄·t⁴ + a₅·t⁵
约束：θ(0)=q_start, θ(T)=q_end, θ'(0)=v_start, θ'(T)=v_end,
θ''(0)=a_start, θ''(T)=a_end
连续性说明：五次多项式保证C²连续（位置、速度、加速度全部连续），
起终点加速度为零时，jerk有限，适合精密装配等低振动场景。
"""
import numpy as np
from trajectory.models import JointWaypoint, JointLimitSet
class QuinticPolynomialSegment:
"""
单个关节的单个五次多项式段
系数: θ(t) = a₀ + a₁·t + a₂·t² + a₃·t³ + a₄·t⁴ + a₅·t⁵
"""
def __init__(self, q_start: float, q_end: float, duration: float,
qd_start: float = 0.0, qd_end: float = 0.0,
qdd_start: float = 0.0, qdd_end: float = 0.0):
if duration <= 0:
raise ValueError(f"运动时间必须大于零, 实际{duration}")
if not np.isfinite(q_start) or not np.isfinite(q_end):
raise ValueError("起点/终点角度必须为有限数值")
if not np.isfinite(qd_start) or not np.isfinite(qd_end):
raise ValueError("起点/终点速度必须为有限数值")
if not np.isfinite(qdd_start) or not np.isfinite(qdd_end):
raise ValueError("起点/终点加速度必须为有限数值")
self.q_start = q_start
self.q_end = q_end
self.duration = duration
self.qd_start = qd_start
self.qd_end = qd_end
self.qdd_start = qdd_start
self.qdd_end = qdd_end
# 求解系数（线性方程组通解，支持任意起终点速度/加速度）
# θ(0) = a₀ = q_start
# θ'(0) = a₁ = qd_start
# θ''(0) = 2a₂ = qdd_start → a₂ = qdd_start / 2
# θ(T) = a₀ + a₁·T + a₂·T² + a₃·T³ + a₄·T⁴ + a₅·T⁵ = q_end
# θ'(T) = a₁ + 2a₂·T + 3a₃·T² + 4a₄·T³ + 5a₅·T⁴ = qd_end
# θ''(T) = 2a₂ + 6a₃·T + 12a₄·T² + 20a₅·T³ = qdd_end
self.a0 = q_start
self.a1 = qd_start
self.a2 = qdd_start / 2.0
T = duration
mat = np.array([
[T**3, T**4, T**5],
[3*T**2, 4*T**3, 5*T**4],
[6*T, 12*T**2, 20*T**3]
], dtype=float)
rhs = np.array([
q_end - self.a0 - self.a1 * T - self.a2 * T**2,
qd_end - self.a1 - 2 * self.a2 * T,
qdd_end - 2 * self.a2
], dtype=float)
try:
coeffs = np.linalg.solve(mat, rhs)
except np.linalg.LinAlgError:
raise ValueError(f"五次多项式系数矩阵奇异, duration={duration}")
self.a3 = coeffs[0]
self.a4 = coeffs[1]
self.a5 = coeffs[2]
def evaluate(self, t: float) -> tuple:
"""计算 t 时刻的角度、角速度、角加速度、加加速度"""
if t < -1e-12 or t > self.duration + 1e-12:
raise ValueError(f"时间t={t}超出范围[0, {self.duration}]")
t = max(0.0, min(t, self.duration))
theta = (self.a0 + self.a1 * t + self.a2 * t**2
+ self.a3 * t**3 + self.a4 * t**4 + self.a5 * t**5)
theta_dot = (self.a1 + 2 * self.a2 * t + 3 * self.a3 * t**2
+ 4 * self.a4 * t**3 + 5 * self.a5 * t**4)
theta_ddot = (2 * self.a2 + 6 * self.a3 * t
+ 12 * self.a4 * t**2 + 20 * self.a5 * t**3)
theta_dddot = (6 * self.a3 + 24 * self.a4 * t
+ 60 * self.a5 * t**2)
return theta, theta_dot, theta_ddot, theta_dddot
def max_velocity(self) -> float:
"""
计算该段的最大绝对速度
速度函数是四次多项式，驻点即加速度零点
用 abs(imag) < 1e-9 容差判定实根，避免漏掉真实驻点
"""
candidates = [0.0, self.duration]
coeffs_accel = [20 * self.a5, 12 * self.a4, 6 * self.a3, 2 * self.a2]
roots = np.roots(coeffs_accel)
for r in roots:
if abs(np.imag(r)) < 1e-9:
r_real = float(np.real(r))
if 0 < r_real < self.duration:
candidates.append(r_real)
max_v = 0.0
for t in candidates:
_, v, _, _ = self.evaluate(t)
max_v = max(max_v, abs(v))
return max_v
def max_acceleration(self) -> float:
"""
计算该段的最大绝对加速度
加速度是三次多项式，驻点即jerk零点
"""
candidates = [0.0, self.duration]
coeffs_jerk = [60 * self.a5, 24 * self.a4, 6 * self.a3]
roots = np.roots(coeffs_jerk)
for r in roots:
if abs(np.imag(r)) < 1e-9:
r_real = float(np.real(r))
if 0 < r_real < self.duration:
candidates.append(r_real)
max_a = 0.0
for t in candidates:
_, _, a, _ = self.evaluate(t)
max_a = max(max_a, abs(a))
return max_a
def peak_velocity_formula(self) -> float:
"""
零起终点速度/加速度时的解析峰值速度：15|Δq|/(8T)
用于与max_velocity()数值结果交叉校验
"""
if (abs(self.qd_start) < 1e-12 and abs(self.qd_end) < 1e-12
and abs(self.qdd_start) < 1e-12 and abs(self.qdd_end) < 1e-12):
delta_q = self.q_end - self.q_start
return 15.0 * abs(delta_q) / (8.0 * self.duration)
return self.max_velocity()
def peak_acceleration_formula(self) -> float:
"""
零起终点速度/加速度时的解析峰值加速度：33√5|Δq|/(8T²) ≈ 5.77|Δq|/T²
"""
if (abs(self.qd_start) < 1e-12 and abs(self.qd_end) < 1e-12
and abs(self.qdd_start) < 1e-12 and abs(self.qdd_end) < 1e-12):
delta_q = self.q_end - self.q_start
return 33.0 * np.sqrt(5.0) * abs(delta_q) / (8.0 * self.duration**2)
return self.max_acceleration()
def peak_jerk_endpoint(self) -> float:
"""
零起终点速度/加速度时的端点jerk绝对值：60|Δq|/T³
五次多项式的jerk在t=0和t=T处取得极值
"""
if (abs(self.qd_start) < 1e-12 and abs(self.qd_end) < 1e-12
and abs(self.qdd_start) < 1e-12 and abs(self.qdd_end) < 1e-12):
delta_q = self.q_end - self.q_start
return 60.0 * abs(delta_q) / (self.duration**3)
# 非零约束时返回全局最大jerk
candidates = [0.0, self.duration]
coeffs_jerk = [60 * self.a5, 24 * self.a4, 6 * self.a3]
roots = np.roots(coeffs_jerk)
for r in roots:
if abs(np.imag(r)) < 1e-9:
r_real = float(np.real(r))
if 0 < r_real < self.duration:
candidates.append(r_real)
max_j = 0.0
for t in candidates:
_, _, _, j = self.evaluate(t)
max_j = max(max_j, abs(j))
return max_j
class QuinticJointTrajectory:
"""
多关节五次多项式轨迹生成器
对每个关节独立生成五次多项式插值序列
构造时校验起终点路点是否在位置限位内（尽早失败）
"""
def __init__(self, start_waypoints: List[JointWaypoint],
end_waypoints: List[JointWaypoint],
duration: float, n_points: int = 150,
limits: JointLimitSet = None):
if len(start_waypoints) != 3 or len(end_waypoints) != 3:
raise ValueError("需要3个关节的起点和终点路点")
self.duration = duration
self.n_points = n_points
self.times = np.linspace(0, duration, n_points)
self.limits = limits
# 构造时尽早校验起终点位置限位
if limits is not None:
start_angles = [wp.angle for wp in start_waypoints]
end_angles = [wp.angle for wp in end_waypoints]
start_ok, start_violations = limits.check_all_positions(start_angles)
if not start_ok:
err_msgs = []
for vi in start_violations:
err_msgs.append(
f"起点关节'{limits.limits[vi].joint_name}' "
f"角度={start_angles[vi]:.3f}超出限位"
f"[{limits.limits[vi].min_val:.3f}, "
f"{limits.limits[vi].max_val:.3f}]"
)
raise ValueError("；".join(err_msgs))
end_ok, end_violations = limits.check_all_positions(end_angles)
if not end_ok:
err_msgs = []
for vi in end_violations:
err_msgs.append(
f"终点关节'{limits.limits[vi].joint_name}' "
f"角度={end_angles[vi]:.3f}超出限位"
f"[{limits.limits[vi].min_val:.3f}, "
f"{limits.limits[vi].max_val:.3f}]"
)
raise ValueError("；".join(err_msgs))
self.segments = []
for i in range(3):
seg = QuinticPolynomialSegment(
q_start=start_waypoints[i].angle,
q_end=end_waypoints[i].angle,
duration=duration,
qd_start=start_waypoints[i].velocity,
qd_end=end_waypoints[i].velocity,
qdd_start=start_waypoints[i].acceleration,
qdd_end=end_waypoints[i].acceleration
)
self.segments.append(seg)
# 批量计算
self.q_array = np.zeros((n_points, 3))
self.q_dot_array = np.zeros((n_points, 3))
self.q_ddot_array = np.zeros((n_points, 3))
self.q_dddot_array = np.zeros((n_points, 3))
for i, t in enumerate(self.times):
for j in range(3):
q, qd, qdd, qddd = self.segments[j].evaluate(t)
self.q_array[i, j] = q
self.q_dot_array[i, j] = qd
self.q_ddot_array[i, j] = qdd
self.q_dddot_array[i, j] = qddd
def validate_limits(self, limits: JointLimitSet) -> tuple:
"""
校验整条轨迹是否满足位置、速度、加速度三重限位
"""
all_pass = True
messages = []
for i in range(self.n_points):
t = self.times[i]
q_vals = self.q_array[i, :].tolist()
qd_vals = self.q_dot_array[i, :].tolist()
qdd_vals = self.q_ddot_array[i, :].tolist()
pos_ok, pos_violations = limits.check_all_positions(q_vals)
if not pos_ok:
all_pass = False
for vi in pos_violations:
messages.append(
f"t={t:.3f}s: 关节'{limits.limits[vi].joint_name}' "
f"角度={q_vals[vi]:.3f}超出限位"
f"[{limits.limits[vi].min_val:.3f}, "
f"{limits.limits[vi].max_val:.3f}]"
)
vel_ok, vel_violations = limits.check_all_velocities(qd_vals)
if not vel_ok:
all_pass = False
for vi in vel_violations:
messages.append(
f"t={t:.3f}s: 关节'{limits.limits[vi].joint_name}' "
f"角速度={qd_vals[vi]:.3f}超出限制"
f"{limits.limits[vi].max_velocity:.3f}"
)
acc_ok, acc_violations = limits.check_all_accelerations(qdd_vals)
if not acc_ok:
all_pass = False
for vi in acc_violations:
messages.append(
f"t={t:.3f}s: 关节'{limits.limits[vi].joint_name}' "
f"角加速度={qdd_vals[vi]:.3f}超出限制"
f"{limits.limits[vi].max_acceleration:.3f}"
)
return all_pass, messages
def get_peak_values(self) -> dict:
"""返回各关节的峰值角度、速度、加速度、jerk"""
return {
'q_peak': np.max(np.abs(self.q_array), axis=0),
'qd_peak': np.max(np.abs(self.q_dot_array), axis=0),
'qdd_peak': np.max(np.abs(self.q_ddot_array), axis=0),
'qddd_peak': np.max(np.abs(self.q_dddot_array), axis=0),
}
@classmethod
def from_angle_pairs(cls, q_start
利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
0
0- 0
已为社区贡献147条内容
所有评论(0)