描述

假设机械臂末端执行器在pose1位置，x方向正好是它的左侧，想要让它向左侧移动10cm。显然我们直接下发命令，令它的pose1.x = pose1.x + 0.1就好了。

但是，如果我们的需求是机械臂在任何位姿下，都需要向它的左侧移动10cm呢（注意，任意位姿时它的正左侧不可能还是x轴正方向）

原理及解决方案

原理

• 机械臂有pose1和pose2两个位姿，两个6维位姿 {x, y, z, rx, ry, rz }都能得到各自的变换矩阵T1和T2
• pose2位姿在pose1位姿坐标系下的表示，也就是变换矩阵T，可以使用公式T2 = T1 * T来表示
• 机械臂在pose3位姿时的变换矩阵是T3，则新的位姿pose4时的变换矩阵T4为T4 = T3 * T

解决方案

1. 记录机械臂在pose1的位姿，再记录一下正左侧pose2的位姿。
2. 将pose1和pose2之间的变换矩阵T求出。
3. 机械臂在任意pose3位姿时，只要根据已知的变换矩阵T，就能得到在pose3下正左侧的位姿pose4了

python代码

import math
import numpy as np
import scipy.linalg as la

def getPose_fromT(T):
	x = T[0, 3] 
	y = T[1, 3]
	z = T[2, 3]
	rx = math.atan2(T[2, 1], T[2, 2])
	ry = math.asin(-T[2, 0]) 
	rz = math.atan2(T[1, 0], T[0, 0])

	return x, y, z, rx, ry, rz


def getT_fromPose(x, y, z, rx, ry, rz):
	Rx = np.mat([[1, 0, 0], [0, math.cos(rx), -math.sin(rx)], [0, math.sin(rx), math.cos(rx)]])
	Ry = np.mat([[math.cos(ry), 0, math.sin(ry)], [0, 1, 0], [-math.sin(ry), 0, math.cos(ry)]])
	Rz = np.mat([[math.cos(rz), -math.sin(rz), 0], [math.sin(rz), math.cos(rz), 0], [0, 0, 1]])
	t = np.mat([[x], [y], [z]])

	R = Rz * Ry * Rx
	R_ = np.array(R)
	t_ = np.array(t)
	T_1 = np.append(R_, t_, axis = 1)
	
	zero = np.mat([0,0,0,1])
	T_2 = np.array(zero) 
	
	T = np.append(T_1, T_2, axis = 0)
	T = np.mat(T)

	return T


# T2 = T1 * T
def getTransT_Pose2inPose1(T1, T2):
	return T1.I * T2


T1 = getT_fromPose(-0.78399, -0.24807, 0.48833, 2.80385, -1.333807, -2.64379)
# print(T1)
x1, y1, z1, rx1, ry1, rz1 = getPose_fromT(T1)
# print(x1,y1,z1,rx1,ry1,rz1)

T2 = getT_fromPose(-0.78399, -0.24807, 0.53833, 2.80385, -1.333807, -2.64379)
# print(T2)


T = getTransT_Pose2inPose1(T1, T2)
x_T, y_T, z_T, rx_T, ry_T, rz_T = getPose_fromT(T)
# print(T)
# print(x_T,y_T,z_T,rx_T,ry_T,rz_T)



T3 = getT_fromPose(-0.80693, 0.158207, 0.488374, 2.8036, -1.333807, 3.1395)
# print(T3)
T4 = T3*T

x4, y4, z4, rx4, ry4, rz4 = getPose_fromT(T4)
print(x4,y4,z4,rx4,ry4,rz4)

可以看出pose2比pose1z方向高了5cm，pose4比pose3同样高了5cm。注意，这里只是举例子，为了让大家看到代码是没问题的，才使用了z方向。x、y、z本来都应该有变化的，但可以注意到rx、ry、rz在pose1、2、3、4都设置成了同样的数值，因此pose4才只有z方向有变化