編碼任務內容：
已知“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