描述

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

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

原理及解决方案

原理

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

解决方案

记录机械臂在pose1的位姿，再记录一下正左侧pose2的位姿。将pose1和pose2之间的变换矩阵T求出。机械臂在任意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方向有变化