机械臂正逆运动学-----数值解

机械臂的运动学包括正运动学和逆运动学，其雅可比矩阵代表速度级的正逆运动学，机械臂的逆运动学数值解采用雅可比矩阵在求取速度级的逆运动学，然后采用迭代法求取位置级逆运动学。

建立DH坐标系

源代码：

#DH参数
DH0_armc = np.array([[0, -pi/2, 0, 0.248],
					[0,  pi/2, 0, 0    ],
					[0, -pi/2, 0, 0.305],
					[0,  pi/2, 0, 0    ],
					[0, -pi/2, 0, 0.306],
					[0,  pi/2, 0, 0    ],
					[0,  0,    0, 0.213]])
#关节极限
q_min_armc = np.array([-180, -95, -180, -95, -180, -95, -180])*pi/180
q_max_armc = np.array([180, 95, 180, 95, 180, 95, 180])*pi/180

求正运动学

单关节齐次传递矩阵

源代码：

#单关节传递矩阵
def  trans(theta,alpha,a,d):
	'''
		本函数用于求取n自由度机械臂正运动学
		输入参数为DH参数，角度单位为rad，长度单位为mm
		参数分别为theta,alpha,a,d，为0维常数值
		返回齐次传递函数矩阵
	'''
	T = np.array([[math.cos(theta), -math.sin(theta)*math.cos(alpha), math.sin(theta)*math.sin(alpha),  a*math.cos(theta)],
		[math.sin(theta), math.cos(theta)*math.cos(alpha),  -math.cos(theta)*math.sin(alpha), a*math.sin(theta)],
		[0,               math.sin(alpha),                  math.cos(alpha),                  d                ],
		[0,               0,                                0,                                  1              ]])
	return T

正运动学：返回齐次矩阵

源代码：

#返回齐次矩阵的正运动学
def fkine(theta,alpha,a,d):
	'''
		本函数用于求取n自由度机械臂正运动学
		输入参数为DH参数，角度单位为rad，长度单位为mm
		参数分别为theta,alpha,a,d，为1维常数值
		返回齐次传递函数矩阵 
	'''
	#关节自由度
	n = len(theta)
	#建立4×4的齐次传递矩阵,定义为numpy类型
	An = np.eye(4)
	for i  in range(n):
		T =  trans(theta[i],alpha[i],a[i],d[i])
		An = np.dot(An,T)  #末端到惯性坐标系传递矩阵
	return An

正运动学：返回欧拉角向量

源代码：

#输入初始时刻DH_0和相对转角,输出六维末端位姿
def fkine_euler(DH_0,qr):
	'''
		本函数用于求取n自由度机械臂正运动学
		输入参数为DH参数，角度单位为rad，长度单位为mm
		参数分别为theta,alpha,a,d，为1维常数值
		返回齐次传递函数矩阵
	'''
	#DH参数
	theta = DH_0[:, 0] + qr
	alpha = DH_0[:, 1]
	a = DH_0[:, 2]
	d = DH_0[:, 3]
	#关节自由度
	n = len(theta)
	xe = np.zeros(6)
	#建立4×4的齐次传递矩阵,定义为numpy类型
	An = np.eye(4)
	for i  in range(n):
		T =  trans(theta[i],alpha[i],a[i],d[i]) #需要加入该函数：单关节齐次矩阵
		An = np.dot(An,T)  #末端到惯性坐标系传递矩阵
	xe[0:3] = An[0:3,3]
	xe[3:6] = rot2euler_zyx(An[0:3,0:3]) #需要加入该函数：欧拉角转换函数

	return xe

旋转矩阵与欧拉角转换：

#旋转矩阵转变为ZYX欧拉角
def rot2euler_zyx(Re):
	'''
		ZYX欧拉角速度变为姿态角速度转化矩阵
		input:旋转矩阵
		output:欧拉角[alpha,beta,gamma]
	'''
	euler_zyx = np.zeros(3)
	if(abs(abs(Re[2, 0]) - 1) < math.pow(10, -6)):
		if(Re[2,0] < 0):
			beta = pi/2
			alpha = np.arctan2(-Re[1,2],Re[1,1])
			gamma = 0
		else:
			beta = -pi/2
			alpha = -np.arctan2(-Re[1, 2], Re[1, 1])
			gamma = 0
	else:
		p_beta = math.asin(-Re[2,0])
		cb = np.cos(p_beta)
		alpha = math.atan2(Re[1,0]*cb,Re[0,0]*cb)
		gamma = math.atan2(Re[2,1]*cb,Re[2,2]*cb)
		if((math.sin(gamma)*Re[2,1]) < 0):
			beta = pi - p_beta
		else:
			beta = p_beta
	euler_zyx[0] = alpha
	euler_zyx[1] = beta
	euler_zyx[2] = gamma
	for i in range(3):
		if(euler_zyx[i]>=3.14 or euler_zyx[i]<=-