20210914日志——如何用UR相对位姿获得相对的齐次变换矩阵（UR相对位姿是位置加旋转矢量）
编码任务内容：
已知“UR机器人获得，特殊特征坐标系（例如工件，以下简称特征）相对于基座特征坐标系（以下简称基座）的位姿（UR规范位姿）”，
每次给定一个“空间点（例如机械臂末端工具中心点tcp）相对于基座的（UR规范）位姿”，求对应的“该空间点相对于特征的（UR规范）位姿”。
其中tcp_from_Base由函数 rtde_receive.getActualTCPPose()获取。
1首先确定，什么是UR书写规范的位姿：X，Y，Z，RX，RY，RZ
参考古月居教程（UR机器人（一）：坐标系及位姿表示方法）：https://www.guyuehome.com/33774
石锤在UR用户手册里：99419_UR5e_User_Manual_zh_Global.pdf ——第三节polyscope手册——运动选项卡——变量——位姿
2编码：
2.1步 编码思路
2.1.1规律如下
b指代base f指代feature t指代tool或tcp
则：
posef2t (posef2b用不上) poseb2t
f2t_homo_matrix = f2b_homo_matrix * b2t_homo_matrix
2.1.2方法过程如下：
（1）poseb2f ——》b2f_homo_matrix —求逆—》f2b_homo_matrix
（2） poseb2t——》b2t_homo_matrix
（3）f2t_homo_matrix = f2b_homo_matrix * b2t_homo_matrix
（4）f2t_homo_matrix ——》posef2t
2.2步
确认pose与齐次变换矩阵的相互转化公式（每一个位姿pose均对应一个齐次变换方阵，反过来每一个齐次变换方阵对应两个等效位姿。）
（1）资料查阅其计算公式：（该网址上也有：古月居知乎https://www.guyuehome.com/33774 或 https://blog.csdn.net/m0_37251750/article/details/108583687）
（2）以上过程的C++实现代码如下，结果用Eigen矩阵封装：（Eigen库是常见的矩阵运算库，用法有链接：）
1 Eigen::Matrix4d homo_matrix_prepare5(Eigen::VectorXd ur_pose) 2 { 3 //该函数hemo_matrix_prepare调用了函数matix_multiple 4 //定义动系相对于定系的旋转平移,f定系b动系 5 double px, py, pz, rx, ry, rz; 6 px = ur_pose(0); 7 py = ur_pose(1); 8 pz = ur_pose(2); 9 rx = ur_pose(3); 10 ry = ur_pose(4); 11 rz = ur_pose(5); 12 13 double angle = sqrt(rx * rx + ry * ry + rz * rz); 14 15 double kx = rx / angle; 16 double ky = ry / angle; 17 double kz = rz / angle; 18 19 20 21 //4*4齐次变换方阵初始化 22 Eigen::Matrix4d _homo_matrix; 23 24 25 //矩阵各元素赋值为0 26 _homo_matrix.fill(0.0); 27 28 29 //由旋转矢量的位姿pose求齐次变换矩阵 30 _homo_matrix(0, 0) = cos(angle) + kx * kx * (1 - cos(angle)); 31 _homo_matrix(0, 1) = kx * ky * (1 - cos(angle)) - kz * sin(angle); 32 _homo_matrix(0, 2) = ky * sin(angle) + kx * kz * (1 - cos(angle)); 33 _homo_matrix(0, 3) = px; 34 35 _homo_matrix(1, 0) = kz * sin(angle) + kx * ky * (1 - cos(angle)); 36 _homo_matrix(1, 1) = cos(angle) + ky * ky * (1 - cos(angle)); 37 _homo_matrix(1, 2) = -kx * sin(angle) + ky * kz * (1 - cos(angle)); 38 _homo_matrix(1, 3) = py; 39 40 _homo_matrix(2, 0) = -ky * sin(angle) + kx * kz * (1 - cos(angle)); 41 _homo_matrix(2, 1) = kx * sin(angle) + ky * kz * (1 - cos(angle)); 42 _homo_matrix(2, 2) = cos(angle) + kz * kz * (1 - cos(angle)); 43 _homo_matrix(2, 3) = pz; 44 45 _homo_matrix(3, 0) = 0; 46 _homo_matrix(3, 1) = 0; 47 _homo_matrix(3, 2) = 0; 48 _homo_matrix(3, 3) = 1; 49 50 return _homo_matrix; 51 };
（3）若结果矩阵用两层vector容器封装，则可用以下代码：
1 std::vector<std::vector<double>> hemo_matrix_prepare1(std::vector<double> vf_to_b) 2 { 3 //该函数hemo_matrix_prepare调用了函数matix_multiple 4 //定义动系相对于定系的旋转平移,f定系b动系 5 double px, py, pz, rx, ry, rz; 6 px = vf_to_b[0]; 7 py = vf_to_b[1]; 8 pz = vf_to_b[2]; 9 rx = vf_to_b[3]; 10 ry = vf_to_b[4]; 11 rz = vf_to_b[5]; 12 13 //计算旋转角的三角函数值 14 double c1 = cos(rx); 15 double s1 = sin(rx); 16 double c2 = cos(ry); 17 double s2 = sin(ry); 18 double c3 = cos(rz); 19 double s3 = sin(rz); 20 21 int raw = 4; 22 int col = 4; 23 std::vector<std::vector<double>> matri(raw, std::vector<double>(col)); 24 //矩阵各元素初始化为0 25 for (int i = 0; i < raw; i++) 26 { 27 for (int j = 0; j < col; j++) 28 { 29 matri[i][j] = 0.0; 30 } 31 } 32 33 //为矩阵赋值 34 matri[0][0] = c3*c2; 35 matri[0][1] = -s3*c1 + c3*s2*s1; 36 matri[0][2] = s3*s1 + c3*s2*c1; 37 matri[0][3] = px; 38 39 matri[1][0] = s3*c2; 40 matri[1][1] = c3*c1 + s3*s2*s1; 41 matri[1][2] = -c3*s1 + s3*s2*c1; 42 matri[1][3] = py; 43 44 matri[2][0] = -s2; 45 matri[2][1] = c2*s1; 46 matri[2][2] = c2*c1; 47 matri[2][3] = pz; 48 49 matri[3][0] = 0; 50 matri[3][1] = 0; 51 matri[3][2] = 0; 52 matri[3][3] = 1; 53 54 //输出显示矩阵 55 for (int i = 0; i < raw; i++) 56 { 57 for (int j = 0; j < col; j++) 58 { 59 std::cout << matri[i][j] << " "; 60 } 61 std::cout << std::endl; 62 } 63 64 return matri; 65 };
（4）代码实现可以借助eigen库的函数：https://www.cnblogs.com/lovebay/p/11215028.html或https://www.tqwba.com/x_d/jishu/401457.html
1 Eigen::Matrix4d homo_matrix_prepare6(Eigen::VectorXd ur_pose) 2 { 3 //该函数hemo_matrix_prepare调用了函数matix_multiple 4 //定义动系相对于定系的旋转平移,f定系b动系 5 double px, py, pz, rx, ry, rz; 6 px = ur_pose(0); 7 py = ur_pose(1); 8 pz = ur_pose(2); 9 rx = ur_pose(3); 10 ry = ur_pose(4); 11 rz = ur_pose(5); 12 13 double angle = sqrt(rx * rx + ry * ry + rz * rz); 14 15 double kx = rx / angle; 16 double ky = ry / angle; 17 double kz = rz / angle; 18 19 //由UR旋转矢量建立角轴旋转量 20 Eigen::AngleAxisd rotation_vector(angle, Vector3d(kx, ky, kz)); 21 //旋转向量转旋转矩阵 22 Eigen::Matrix3d rotation_matrix; 23 rotation_matrix = rotation_vector.matrix(); 24 25 //4*4齐次变换方阵初始化 26 Eigen::Matrix4d _homo_matrix; 27 //矩阵各元素赋值为0 28 _homo_matrix.fill(0.0); 29 //由旋转矢量的位姿pose求齐次变换矩阵 30 _homo_matrix.block(0,0,3,3) = rotation_matrix; 31 //_homo_matrix(0, 0) = rotation_matrix(0,0); 32 //_homo_matrix(0, 1) = rotation_matrix(0, 1); 33 //_homo_matrix(0, 2) = rotation_matrix(0, 2); 34 _homo_matrix(0, 3) = px; 35 36 //_homo_matrix(1, 0) = rotation_matrix(1, 0); 37 //_homo_matrix(1, 1) = rotation_matrix(1, 1); 38 //_homo_matrix(1, 2) = rotation_matrix(1, 2); 39 _homo_matrix(1, 3) = py; 40 41 //_homo_matrix(2, 0) = rotation_matrix(2, 0); 42 //_homo_matrix(2, 1) = rotation_matrix(2, 1); 43 //_homo_matrix(2, 2) = rotation_matrix(2, 2); 44 _homo_matrix(2, 3) = pz; 45 46 _homo_matrix(3, 0) = 0; 47 _homo_matrix(3, 1) = 0; 48 _homo_matrix(3, 2) = 0; 49 _homo_matrix(3, 3) = 1; 50 51 return _homo_matrix; 52 };
（5）矩阵的快捷获得方法可以借助软件RoboDK或在线工具3D Rotation Converter（https://www.andre-gaschler.com/rotationconverter/）辅助，
但工程中开发不能依赖这两个，还是得代码实现。
RoboDK（三个坐标系指代基座、特征、点，位姿项有专门的UR规范位姿）：
3D Rotation Converter很方便有旋转矩阵和旋转矢量（和轴角量）转换功能：
3代码运算结果
3.1
机器人示教盒获得：特征坐标系相对于基座坐标系的位姿（x,y,z,rx,ry,rz），位置和姿态ur姿态是旋转矢量（弧度）;
旋转矢量换算变换矩阵：特征坐标系相对于基座坐标系的4*4齐次变换矩阵；
变换矩阵求逆：基座坐标系相对于特征坐标系的4*4齐次变换矩阵；
3.2满足所需功能后，UR采集数据的最新结果：
3.2.1错结果是因为b_to_f位姿输入错了：
3.2.2以下为对的结果（姿态有误差）
tcppose[i],X,Y,Z,RX,RY,RZ
tcppose[0],0.457771,-1.16743,-0.839458,-0.422147,-2.97357,-0.291953,
tcppose[1],0.533914,-1.38673,-0.883963,-0.421599,-2.97358,-0.29254,
tcppose[2],0.608705,-1.60282,-0.927664,-0.421725,-2.97339,-0.292593,
tcppose[3],0.473329,-1.64594,-0.946665,-0.42167,-2.97385,-0.29189,
tcppose[4],0.424685,-1.50492,-0.917941,-0.422007,-2.97387,-0.291572,
tcppose[5],0.3631,-1.32744,-0.88223,-0.421892,-2.9736,-0.291954,
tcppose[6],0.220926,-1.37275,-0.902443,-0.421782,-2.97349,-0.292524,
tcppose[7],0.27585,-1.53036,-0.934056,-0.421551,-2.97373,-0.292211,
tcppose[8],0.317723,-1.65118,-0.95874,-0.421726,-2.97375,-0.292612,
参考8号点（第9个点）的ur示教盒数据：
（单位：毫米和rad）
X：317.58~317.66 Y： -1650.99~-1651.07 Z： -958.69~-958.77
RX：0.456~0.457 RY： 3.218 RZ： 0.316
比较得到误差为：
（单位：毫米和rad）
X： 0.14~0.06 Y： -0.18~-0.11 Z： -0.05~0.03
RX：0.034274~0.035274 RY： 0.24425 RZ： 0.023388
用特征相对于基座坐标系的齐次变换方阵，左乘“表示三维位置（工具TCP相对于基座）的四维齐次向量”，
即可得到（表示三维位置的）工具TCP相对于特殊特征坐标系（如工件坐标系）的四维齐次向量。
4 中间遇到的坑和使用的解决方法：
4.1结合仿真环境，检验转换代码
4.2机械臂给的逆位姿是否直接用做feature_to_base？
在仿真空间环境下模拟相对于特征坐标系2逆位姿的坐标系4，并观察坐标系4是否重合回基座坐标系1，发现同样适用：
（1）创建坐标系4，输入相对于坐标系2（Frame2）的UR位姿pose：0.46488, -0.557696, -0.522333, 0.2352, 0.080104, -1.243983
（2）观察坐标系4是否重合回基座坐标系1，确实重合了
4.3Eigen轴角量AngleAxisd的特殊类型
参考：Eigen的使用总结2——geometry
https://blog.csdn.net/reasonyuanrobot/article/details/86614381
4.3.1 ftr_to_tcp_AngleAxisd.axis和ftr_to_tcp_AngleAxisd.axis（）有区别：
前者编译时会报错，且错误不会显示在源代码任何位置，而是显示在库中，难以排查。后者才能返回一个Eigen::vector3&
用以上方法转换位姿向量，得到的是能cout输出的Vector3d类型 。
4.3.2AngleAxisd类型
Eigen中齐次变换矩阵转旋转矢量，离不开AngleAxisd类型，但该类型操作起来繁琐
AngleAxisd类型对象无法cout输出，但可以初始化和调用.angle()或.axis（）
4.3.3以下是排查日志，日志中765.307一项（即featur_from_base位姿的X值）全写错了（正确是-765.307），
导致借助仿真的换算功能完善后在实际机器人上采集数据发现有误。
getActualTCPPose()函数获取UR位姿单位是米和弧度，而不是毫米和弧度
以这个位姿到达工件上方
0.0556585, 0.247949, -0.0146147, -2.51002, -1.68042, -0.0595131
55.6585,247.949, -14.6147, -2.51002, -1.68042, -0.0595131
Framewaypoint3
0.0400149,0.173836,-0.0816024,2.54263,1.70769,-0.0918158
40.0149,173.836,-081.6024,2.54263,1.70769,-0.0918158
Framewaypoint1
0.0367395,0.168845,-0.00321814,-2.58042,-1.73474,-0.0687103
36.7395,168.845,-3.21814,-2.58042,-1.73474,-0.0687103
p[0.0367191, 0.168905, -0.0556391, -2.58054, -1.73458, -0.0686292]
36.7191, 168.905, -55.6391, -2.58054, -1.73458, -0.0686292
初始点2Framewaypoint2
p[0.0212962,-0.205567,-0.0348749,-2.0185,-2.35853,-0.0462862]
21.2962,-205.567,-34.8749,-2.0185,-2.35853,-0.0462862
进针极限点2-2Framewaypoint2-2limit (2-2相对于2是竖直下降，近似于工具坐标z方向的进给)
p[0.0212387,-0.20555,-0.122159,-2.01866,-2.35867,-0.0462557]
21.2387,-205.55,-122.159,-2.01866,-2.35867,-0.0462557
初始点4Framewaypoint4
p[0.0212962,-0.193537,-0.0348749,-2.0185,-2.35853,-0.0462862]
21.2962,-193.537,-34.8749,-2.0185,-2.35853,-0.0462862
进针极限点4-2Framewaypoint4-2limit (4-2相对于4是竖直下降，近似于工具坐标z方向的进给)
p[0.0212962,-0.193537,-0.1166549,-2.0185,-2.35853,-0.0462862]
21.2962,-193.537,-116.6549,-2.0185,-2.35853,-0.0462862
