用python实现机械臂关节角度三次多项式插值，其中初始位置与目标位置是每个关节的角度值

以下是一个示例代码，可以实现机械臂关节角度三次多项式插值：

import numpy as np
import matplotlib.pyplot as plt

# 初始位置和目标位置，每个关节的角度值
q0 = np.array([0, 0, 0, 0, 0, 0])
qf = np.array([np.pi/4, np.pi/2, np.pi/4, np.pi/4, np.pi/4, np.pi/4])

# 插值时间
t0 = 0
tf = 2

# 插值点数
n = 100

# 生成时间向量
t = np.linspace(t0, tf, n)

# 生成多项式系数矩阵
T = np.array([[1, t0, t0**2, t0**3],
              [0, 1, 2*t0, 3*t0**2],
              [1, tf, tf**2, tf**3],
              [0, 1, 2*tf, 3*tf**2]])

# 生成关节角度矩阵
Q = np.vstack((q0, np.zeros((2, 6)), qf))

# 生成插值多项式系数矩阵
A = np.linalg.inv(T).dot(Q)

# 计算插值点的关节角度值
Qd = np.zeros((n, 6))
for i in range(n):
    Td = np.array([1, t[i], t[i]**2, t[i]**3])
    Qd[i, :] = A.dot(Td)

# 绘制插值曲线
plt.figure()
for i in range(6):
    plt.plot(t, Qd[:, i])
plt.xlabel('Time (s)')
plt.ylabel('Joint Angle (rad)')
plt.legend(['q1', 'q2', 'q3', 'q4', 'q5', 'q6'])
plt.show()

这个代码通过计算初始位置和目标位置的关节角度值，以及插值时间和插值点数，生成了一个多项式系数矩阵，并通过矩阵运算得到了插值多项式系数矩阵。然后，对于每个插值点，计算了对应的关节角度值，并绘制了插值曲线。