Derive the homogeneous transformation matrix

In order to obtain the homogeneous transformation matrix of an manufactured robot arm, it is necessary to check how the coordinate system of the robot arm are defined.
Here, UFACTORY Lite6 is used as an example for explanation.

First, check the coordinate system of your robot arm!

Second, analyze the link structure of your robot arm!

Third, plot coordinate transformation of each coordinate system using link structure without theta offset!

Fourth, derive the homogeneous transformation matrix of each coordinate system!

$$
\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}
$$

You were able to derive the homogeneous transformation matrix of each coordinate system with the above.

Derive the end-effector position using the homogeneous transformation matrix

#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;
}

On Ubuntu PC
Make cpp file and copy&past the above code.

touch cal.cpp && gedit cal.cpp

Build the cpp file.

g++ -o result cal.cpp

Run the cpp file.

./result

Terminal

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

Therefore, the end-effector position $P_{eef}$ of Lite6 in the example is

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

QiitaLink
Designed by RENOX