同次変換行列を求める

# 同次変換行列の求め方
[UFACTORY Lite6](https://www.ufactory.cc/download-lite6-robot) を使用する機会があり同次変換行列を求めましたので共有します。ROSのMoveIt経由で目標手先座標を入力した後実際のロボットアームの手先が移動しましたが、本当にその座標に到達しているのかを確認するために、関節角度からロボットアームの手先(エンドエフェクタ取り付け位置)の座標を求めることができるC++コードで実際の手先座標を計算しました。
ここでは、Lite6の手先が移動した後の関節角度がrosbagコマンドで取得できたことを前提に話をします。
間違いがありましたらご教示いただけますと幸いです。
※「ロボット工学の基礎(第3版)/川﨑晴久著/森北出版/2020年」を参考にしています。

[理論的な話はこちら](https://qiita.com/ryosuke1212/items/0456ce07bcc933ca4534)

### ロボットアームの座標系を確認する
![2023-02-10_20h07_15.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2657853/27a35d80-16ce-0364-b3fa-96d73ebc645f.png)

### リンク構造を解析する
![2023-02-10_20h04_58.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2657853/c7dec833-19e3-a41e-1c19-051dbc4754eb.png)

### オフセットなしのリンク構造を使って、各関節の座標系の座標変換を考える
![2023-02-10_19h37_48.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2657853/382aae68-c62f-6159-a91c-a0672089c4f9.png)

### 各関節の座標系の同次変換行列を求める
$$
\begin{eqnarray}
^0T_1 =
\begin{bmatrix}
cos\theta_1 & -sin\theta_1 & 0 & 0 \\\\
sin\theta_1 & cos\theta_1 & 0 & 0 \\\\
0 & 0 & 1 & d1 \\\\
0 & 0 & 0 & 1
\end{bmatrix}
\end{eqnarray}
$$
$$
\begin{eqnarray}
^1T_2 =
\begin{bmatrix}
cos\theta_2 & -sin\theta_2 & 0 & 0 \\\\
0 & 0 & 1 & 0 \\\\
-sin\theta_2 & -cos\theta_2 & 0 & 0 \\\\
0 & 0 & 0 & 1
\end{bmatrix}
\end{eqnarray}
$$
$$
\begin{eqnarray}
^2T_3 =
\begin{bmatrix}
cos\theta_3 & -sin\theta_3 & 0 & a2 \\\\
-sin\theta_3 & -cos\theta_3 & 0 & 0 \\\\
0 & 0 & -1 & 0 \\\\
0 & 0 & 0 & 1
\end{bmatrix}
\end{eqnarray}
$$
$$
\begin{eqnarray}
^3T_4 =
\begin{bmatrix}
cos\theta_4 & -sin\theta_4 & 0 & a3 \\\\
0 & 0 & 1 & -d4 \\\\
sin\theta_4 & cos\theta_4 & 0 & 0 \\\\
0 & 0 & 0 & 1
\end{bmatrix}
\end{eqnarray}
$$
$$
\begin{eqnarray}
^4T_5 =
\begin{bmatrix}
cos\theta_5 & -sin\theta_5 & 0 & 0 \\\\
0 & 0 & -1 & 0 \\\\
sin\theta_5 & cos\theta_5 & 0 & 0 \\\\
0 & 0 & 0 & 1
\end{bmatrix}
\end{eqnarray}
$$
$$
\begin{eqnarray}
^5T_6 =
\begin{bmatrix}
cos\theta_6 & -sin\theta_6 & 0 & 0 \\\\
0 & 0 & 1 & d6 \\\\
-sin\theta_6 & -cos\theta_6 & 0 & 0 \\\\
0 & 0 & 0 & 1
\end{bmatrix}
\end{eqnarray}
$$

# 同次変換行列を使ってロボットアームの手先座標を求める
```c++
#include <iostream>
#include <iomanip>
#include <math.h>
using namespace std;

//Display on terminal=================================
void cout_44matrix(double T06[4][4])
{
  int i, j; 
  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++) {
      cout << scientific<< setprecision(5)<< T06[i][j] << " ";
    }
    cout << endl;
  }
}

//Calculation=========================================
void calculator(double d1, double a2, double a3, double d4, double d6, 
                double radian1, double radian2, double radian3, 
                double radian4, double radian5, double radian6)//Load link param(m) and joint angle(radian).
{
    double theta1, theta2, theta3, theta4, theta5, theta6;
    theta1=radian1*180/M_PI;
    theta2=(radian2*180/M_PI) - 90;//Adjust offset.
    theta3=(radian3*180/M_PI) - 90;//Adjust offset.
    theta4=radian4*180/M_PI;
    theta5=radian5*180/M_PI;
    theta6=radian6*180/M_PI;
    cout << "theta1="<< theta1 <<endl;
    cout << "theta2="<< theta2 <<endl;
    cout << "theta3="<< theta3 <<endl;
    cout << "theta4="<< theta4 <<endl;
    cout << "theta5="<< theta5 <<endl;
    cout << "theta6="<< theta6 <<endl;

    double T01[4][4]={
                        { cos(radian1), -sin(radian1), 0,    0},
                        { sin(radian1),  cos(radian1), 0,    0},
                        {            0,             0, 1,   d1},
                        {            0,             0, 0,    1},
    };
    double T12[4][4]={
                        { cos(radian2-1.57079632), -sin(radian2-1.57079632), 0,    0},
                        {                       0,                        0, 1,    0},
                        {-sin(radian2-1.57079632), -cos(radian2-1.57079632), 0,    0},
                        {                       0,                        0, 0,    1},
    };
    double T23[4][4]={
                        { cos(radian3-1.57079632), -sin(radian3-1.57079632), 0,   a2},
                        {-sin(radian3-1.57079632), -cos(radian3-1.57079632), 0,    0},
                        {                       0,                        0,-1,    0},
                        {                       0,                        0, 0,    1},
    };
    double T34[4][4]={
                        { cos(radian4), -sin(radian4), 0,   a3},
                        {            0,             0, 1,  -d4},
                        { sin(radian4),  cos(radian4), 0,    0},
                        {            0,             0, 0,    1},
    };
    double T45[4][4]={
                        { cos(radian5), -sin(radian5), 0,    0},
                        {            0,             0,-1,    0},
                        { sin(radian5),  cos(radian5), 0,    0},
                        {            0,             0, 0,    1},
    };
    double T56[4][4]={
                        { cos(radian6), -sin(radian6), 0,    0},
                        {            0,             0, 1,   d6},
                        {-sin(radian6), -cos(radian6), 0,    0},
                        {            0,             0, 0,    1},
    };

    int i,j,k;

    double T02[4][4];   
    for(i=0;i<4;i++){
      for(j=0;j<4;j++){
        T02[i][j]=0.0;
        for(k=0;k<4;k++){
          T02[i][j]+=T01[i][k]*T12[k][j];
        }
      }
    }

    double T03[4][4];  
    for(i=0;i<4;i++){
      for(j=0;j<4;j++){
        T03[i][j]=0.0;
        for(k=0;k<4;k++){
          T03[i][j]+=T02[i][k]*T23[k][j];
        }
      }
    }

    double T04[4][4];  
    for(i=0;i<4;i++){
      for(j=0;j<4;j++){
        T04[i][j]=0.0;
        for(k=0;k<4;k++){
          T04[i][j]+=T03[i][k]*T34[k][j];
        }
      }
    }

    double T05[4][4]; 
    for(i=0;i<4;i++){
      for(j=0;j<4;j++){
        T05[i][j]=0.0;
        for(k=0;k<4;k++){
          T05[i][j]+=T04[i][k]*T45[k][j];
        }
      }
    }

    double T06[4][4]; 
    for(i=0;i<4;i++){
      for(j=0;j<4;j++){
        T06[i][j]=0.0;
        for(k=0;k<4;k++){
          T06[i][j]+=T05[i][k]*T56[k][j];
        }
      }
    }
    cout << "T06 = "<<endl;
    cout_44matrix(T06);
}

//Input Data=======================================
int main(void)
{   
    //For example
    calculator(0.2433,0.200,0.087,0.2276,0.0615,-0.4637,-0.09667,1.188,9.287e-6,1.284,-0.4636);
    return 0;
}

```
Ubuntu PC上で
C++ファイルを作成し上記コードをコピペする
```
touch cal.cpp && gedit cal.cpp 
```

ビルド
```
g++ -o result cal.cpp
```
実行
```
./result
```
ターミナル表示
```
user@PC:~$ ./result
theta1=-26.5681
theta2=-95.5388
theta3=-21.9326
theta4=0.000532106
theta5=73.5678
theta6=-26.5623
T06 = 
-4.72253e-01 -7.36141e-01 -4.84844e-01 1.70169e-01 
7.36120e-01 -6.31932e-01 2.42464e-01 -8.50949e-02 
4.84876e-01 2.42399e-01 -8.40320e-01 4.09912e-01 
0.00000e+00 0.00000e+00 0.00000e+00 1.00000e+00 
```

したがって、例におけるLite6の手先座標$P_{eef}$は以下のように求まった。

$$
\begin{eqnarray}
P_{eef} =\left(
\begin{array}{ccc}
0.170169 & -0.0850949 & 0.409912
\end{array}
\right)
\end{eqnarray}
m
$$

---
<font color="yellowgreen">**QiitaLink**</font>
<font color="black">**Designed by RENOX**</font>