一.理论需求
最近需要做一个ros机械臂碰撞检测，但是原来的机械臂是windows下控制的，包括双目的视觉也是挂载在windows下的，转移不太方便，就以通讯和仿真的方式做一个ROS碰撞检测，实操时候遇到一些问题，写博客与大家分享一下解决问题的历程和思路，文中代码都是实际写的，如果不能运行可以私信我。
对机械手或agv小车感兴趣的朋友，想学习或者想技术合作的：微信：17805813280
环境是：UBUNTU16.04 ros-kinetic python2.75
二.关系寻找
实际上我们在控制机械臂的时候，无论是古月大神讲的moveit控制，还是我之前讲的moveit控制，它都需要一个实际的终端去走位姿，但是实际的生产中我们可能不一定吧终端的坐标（这里主要指视觉给出的原点坐标）放在机械手的某个关节终端，这时候我们就需要寻找一个转换关系，我以ABB1410为例子
痛点解释：网上寻找的四元数转换矩阵，使用下面公式求解时候位置总是不对，本文解决了该问题，解决方法如下：
中心思想：T0*X=T1
T0=T1*（X的逆）所以：T0=T1*（（（T0的逆）*T1）的逆））
T0这里代表机械臂的6轴终端方向盘位姿 T1指新建，X坐标系为关系
我们需要以矩阵的形式运算整个公式来达到我们的目的
四元数转换矩阵：
这里我们需要先把四元数转换成公式
原理很简单,其实就是四元数转轴角,再用绕任意轴旋转矩阵
里面会涉及比较多的公式
设四元数Q（x, y, z, w）表示向量a（xa,ya, za）经过α角旋转后的结果，则x、y、z和w分别为：
x= sin(α/2)·xa
y= sin(α/2)·ya
z= sin(α/2)·za
w= cos(α/2)
再结合高中所学的半角公式：
sinα = 2 * sin(α/2)·cos(α/2)
cosα = cos(α/2)*cos(α/2) - sin(α/2)*sin(α/2)
cos(α/2)*cos(α/2) = (1 +cosα)/2
sin(α/2)*sin(α/2) = (1 -cosα)/2
然后是绕任意轴旋转矩阵
最后结果
接下来矩阵转换完了，此时矩阵中有四元数但是没有位置，位置加在最后一列 0 0 0 1变成X Y Z 1
博主以为到此为止了，可以直接写代码运算了，最后算出来X总是不对 其他的四元数 Y 和 Z都是对的
后来发现需要吧w变号
也就是这里所有的W（ABB中是q1）全部变号，最终输出的W再变一次号（相当于变回来）
三.代码实现
python代码：
import numpy as np
import math
a_set = np.zeros(shape=(4,4))
b_set = np.zeros(shape=(4,4))
c_set = np.zeros(shape=(4,4))
d_set = np.zeros(shape=(4,4))
b =[0.60146,0.34302,-0.06603,0.71849]
w =-b[0]
x =b[1]
y =b[2]
z =b[3]
c =[0.51196,0.19792,0.49527]
a_set[0][0] = 2*(x*x+w*w)-1
a_set[0][1] = 2*(x*y+z*w)
a_set[0][2] = 2*(x*z-y*w)
a_set[0][3] = c[0]
a_set[1][0] = 2*(x*y-z*w)
a_set[1][1] = 2*(y*y+w*w)-1
a_set[1][2] = 2*(y*z+x*w)
a_set[1][3] = c[1]
a_set[2][0] = 2*(x*z+y*w)
a_set[2][1] = 2*(y*z-x*w)
a_set[2][2] = 2*(z*z+w*w)-1
a_set[2][3] = c[2]
a_set[3][0] = 0
a_set[3][1] = 0
a_set[3][2] = 0
a_set[3][3] = 1
T1 = a_set
q =[0.00647,0.19837,0.97992,-0.01933]
p =[0.72871,0.43406,0.42138]
w =-q[0]
x =q[1]
y =q[2]
z =q[3]
b_set[0][0] = 2*(x*x+w*w)-1
b_set[0][1] = 2*(x*y+z*w)
b_set[0][2] = 2*(x*z-y*w)
b_set[0][3] = p[0]
b_set[1][0] = 2*(x*y-z*w)
b_set[1][1] = 2*(y*y+w*w)-1
b_set[1][2] = 2*(y*z+x*w)
b_set[1][3] = p[1]
b_set[2][0] = 2*(x*z+y*w)
b_set[2][1] = 2*(z*y-x*w)
b_set[2][2] = 2*(z*z+w*w)-1
b_set[2][3] = p[2]
b_set[3][0] = 0
b_set[3][1] = 0
b_set[3][2] = 0
b_set[3][3] = 1
T0 =b_set
#print T0
q =[0.69176,-0.00958,0.06235,0.71937]
p =[0.59565,0.64384,0.02301]
w =-q[0]
x =q[1]
y =q[2]
z =q[3]
c_set[0][0] = 2*(x*x+w*w)-1
c_set[0][1] = 2*(x*y+z*w)
c_set[0][2] = 2*(x*z-y*w)
c_set[0][3] = p[0]
c_set[1][0] = 2*(x*y-z*w)
c_set[1][1] = 2*(y*y+w*w)-1
c_set[1][2] = 2*(y*z+x*w)
c_set[1][3] = p[1]
c_set[2][0] = 2*(x*z+y*w)
c_set[2][1] = 2*(z*y-x*w)
c_set[2][2] = 2*(z*z+w*w)-1
c_set[2][3] = p[2]
c_set[3][0] = 0
c_set[3][1] = 0
c_set[3][2] = 0
c_set[3][3] = 1
T4 =c_set
q =[0.04035,-0.19682,-0.97897,0.0355]
p =[0.62906,0.66595,0.37266]
w =-q[0]
x =q[1]
y =q[2]
z =q[3]
d_set[0][0] = 2*(x*x+w*w)-1
d_set[0][1] = 2*(x*y+z*w)
d_set[0][2] = 2*(x*z-y*w)
d_set[0][3] = p[0]
d_set[1][0] = 2*(x*y-z*w)
d_set[1][1] = 2*(y*y+w*w)-1
d_set[1][2] = 2*(y*z+x*w)
d_set[1][3] = p[1]
d_set[2][0] = 2*(x*z+y*w)
d_set[2][1] = 2*(z*y-x*w)
d_set[2][2] = 2*(z*z+w*w)-1
d_set[2][3] = p[2]
d_set[3][0] = 0
d_set[3][1] = 0
d_set[3][2] = 0
d_set[3][3] = 1
T3 =d_set
#print T3
a_now = np.dot(T1,np.linalg.inv(np.dot(np.linalg.inv(T3),T4)))
#a_now2 = np.dot(T4,np.linalg.inv(np.dot(np.linalg.inv(T0),T1)))
#a_now = np.dot(T1,np.linalg.inv(np.dot(np.linalg.inv(T3),T4)))
a_now2 = np.dot(np.dot(T3,np.linalg.inv(T4)),T1)
print a_now
#print a_now2
i = a_now[0][1]+a_now[1][0] #4xy
j = a_now[0][0]-a_now[1][1] #2(x2-y2)
m = a_now[0][2]+a_now[2][0] #4xz
n = a_now[0][0]-a_now[2][2] #2(x2-z2)
x_not = math.sqrt((i*i-m*m)/(n-j)/8)
j = a_now[1][2]-a_now[2][1]
w_not =j/4/x_not
y_not = i/4/x_not
z_not = (a_now[0][2]+a_now[2][0])/4/x_not
print -w_not
print x_not
print y_not
print z_not
我用了两组一组寻找关系[T3(T0)和T4(T1)]一组用来验证（T0和T1）
大家使用的时候把数据改成自己的然后直接运行就行了