diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py index 0529a86787..b7ee4eee6a 100644 --- a/src/Robotic_arm/main.py +++ b/src/Robotic_arm/main.py @@ -3,58 +3,56 @@ import numpy as np import os import tempfile -import time # 新增:备用的睡眠函数 +import time +from scipy import interpolate -# ====================== 1. 定义机械臂 XML 模型 ====================== -# 6自由度机械臂的 MuJoCo XML 描述 + +# ====================== 轨迹优化核心(纯原生) ====================== +def quintic_polynomial(start, end, T, t): + """五次多项式插值:保证位置/速度/加速度连续""" + s0, v0, a0 = start, 0, 0 + s1, v1, a1 = end, 0, 0 + + a = s0 + b = v0 + c = a0 / 2 + d = (20 * (s1 - s0) - (8 * v1 + 12 * v0) * T - (3 * a0 - a1) * T ** 2) / (2 * T ** 3) + e = (30 * (s0 - s1) + (14 * v1 + 16 * v0) * T + (3 * a0 - 2 * a1) * T ** 2) / (2 * T ** 4) + f = (12 * (s1 - s0) - (6 * v1 + 6 * v0) * T - (a0 - a1) * T ** 2) / (2 * T ** 5) + + return a + b * t + c * t ** 2 + d * t ** 3 + e * t ** 4 + f * t ** 5 + + +# ====================== 机械臂模型 ====================== arm_xml = """