针对一款六轴机器臂的DH建模求解

有幸拿到一款喷涂机器人的部分数据资料，着手写它的运动学算法。
第一步：DH建模


根据图纸资料，建立dh参数表格如下：

第二步，写出各关节矩阵

1.每个关节的乘积模板矩阵，该矩阵由每个关节的z轴旋转矩阵*z轴偏移量矩阵 乘以 x轴旋转矩阵 乘以 x轴偏移量矩阵得来。 相乘的顺序不能打乱：

2.各关节对应的矩阵如下：

第三步，
写出正运动学代码：

#BRTIRSE1506A
#authored by Tom Gong  gongal@163.com
import numpy as np
import math
from numpy import *


d1=459.5
a2=680.0   
a3=0   
d4=680
d5=0
d6=284.5

def kinematics(thetas):
    rtheta=np.array(thetas)
    theta1=float(rt