DH坐标系的建立笔记

https://www.cnblogs.com/mightycode/p/9095059.html
如上这篇文章用于学习DH坐标系建立，非常不错，推荐初学者阅读。
在此建立笔记如下：
1.坐标系的建立隐含原则：文章右上方的草图，即为各伺服电机的初始位置，也就是 theta=0的位置。 我经过反复调测，得到这个结论。
2.实际调试中，应该会对伺服电机的正负转向予以定义，用于适应开始建立的这个模型。
3.在编码过程中走了一个弯路： 我曾经试图将每个矩阵都先用round函数，使每个矩阵元素都保留小数点后四位，这样后续计算就比较省计算机时间，然而，这个函数加上之后，矩阵算出来的新矩阵总是很离谱，反复查找原因未果，只好放下这个想法了。至今不明白为何numpy.around()这个函数为何不好用。有一种可能是这个库函数有bug。

上面第一条结论，是通过多次尝试修改六个转角值得出的。
一开始，我想让机械臂沿基础坐标的z轴伸展到最大，也就是末端的z值达到最大，试图发现各角度为0时的起始点。没有发现头绪，但当我把所有角度都置为0时，发现机械臂的位姿和模型草图位姿一样，才发现了这个道理。

下面是我根据文章的模型写的正运动学部分代码：逆运动学部分代码哪位大佬写出来了，请与我分享，谢谢。

import numpy as np
import time
import math

#math.sin(math.radians(90))
from numpy import *;
theta=math.pi/180
d1=89.2
a2=-425.0
a3=-392.25
d4=109.15
d5=94.65
d6=82.3

#6 angles of each joint.
theta1=math.pi
theta2=0
theta3=0
theta4=0
theta5=0
theta6=0

#6 matrix.
T1=np.matrix([[math.cos(theta1),0,math.sin(theta1),0],
       [math.sin(theta1),0,-math.cos(theta1),0],
       [0,1,0,d1],
       [0,0,0,1]])
#T1_=np.round(T1,decimals=4)
T2=np.matrix([[math.cos(theta2),-math.sin(theta2),0,a2*math.cos(theta2)],
       [math.sin(theta2),math.cos(theta2),0,a2*math.sin(theta2)],
       [0,0,1,0],
       [0,0,0,1]])
#T2_=np.round(T2,decimals=4)
T3=np.matrix([[math.cos(theta3),-math.sin(theta3),0,a3*math.cos(theta3)],
           [math.sin(theta3),math.cos(theta3),0,a3*math.sin(theta3)],
           [0,0,1,0],
           [0,0,0,1]])
#T3_=np.round(T3,decimals=4)
T4=np.matrix([[math.cos(theta4),0,math.sin(theta4),0],
    [math.sin(theta4),0,-math.cos(theta4),0],
    [0,1,0,d4],
    [0,0,0,1]])
#T4_=np.round(T4,decimals=4)
T5=[[math.cos(theta5),0,-math.sin(theta5),0],
    [math.sin(theta5),0,math.cos(theta5),0],
    [0,-1,0,d5],
    [0,0,0,1]]
#T5_=np.around(T5,4)
T6=[[math.cos(theta6),-math.sin(theta6),0,0],
    [math.sin(theta6),math.cos(theta6),0,0],
    [0,0,1,d6],
    [0,0,0,1]]
#T6_=np.around(T6,4)


t1t2t3t4t5t6=T1*T2*T3*T4*T5*T6




#output of the matrix

for j in t1t2t3t4t5t6:
	print (j)