[H-mobility Robotics] [Forward Kinematics(정기구학)] 로봇기구학 실습1 (Eigen 라이브러리 사용)

Forward Kinematics(정기구학) : ‘마지막 끝점(end-effector)의 위치를 어떻게 찾아낼 수 있느냐에 대한 것

( θ(세타)가 주어졌을 때, 엑스(X)를 찾는 문제 )

 

Modified DH parameter

Rx : 회전 ,Dx : 이동

즉, T는 X축으로 회전하고 이동한후 Z축으로 회전하고 이동하는 과정을 거친다.

 

 

<코드실습>

- 직접 코드를 작성하여 DH파라미터로 정기구학(FK) 문제를 풀어볼 것임.

- 행렬 계산을 위해 '아이젠 라이브러리' 이용

- '함수 구현'과 '예제 적용'을 분리하여 보여 줄 것임.

 

<Eigen(아이젠) 라이브러리>

: linear algebra를 쉽게 다루기 위한 C++ 라이브러리

 

1) 공식 홈페이지 : https://eigen.tuxfamily.org/

2) 공식 튜토리얼 : https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html

$sudo apt install libeigen3-dev #라이브러리 설치
$whereis eigen3  #설치 확인

 

1. Eigen을 사용하지 않고 C++ 표준 자료형만 사용할 경우의 행렬 곱셈

#include <iostream>
#include <vector>  //vector 포맷 사용

using namespace std;
using matrix = vector<vector<double>>;

matrix multiply(matrix A,matrix B)
{
    matrix C(A.size(), vector<double>(B[0].size(), 0)); 
    // A가 N by M , B가 M by P 라고 하면  N * P 행렬이 도출됨
    for (int i = 0; i<A.size(); i++)
    {
        for (int j = 0; j < B[0].size(); j++)
        {
            for (int k = 0; k < A[0].size(); k++)
            {
                C[i][j] += A[i][k] * B[k][j];
            }
        }
    }
    return C;
}

int main()
{
    matrix matA ={
        {1,2,3},
        {4,5,6},
        {7,8,9}
    };

    matrix matB ={
        {1,2,3},
        {1,2,3},
        {1,2,3}
    };

    matrix matC = multiply(matA, matB);
    
    for (int i = 0; i<matC.size();++i){
        for (int j =0; j<matC[i].size(); ++j){
            cout << " " << matC[i][j];
        }
        cout << "\n";
    }

    return 0;
}

 

$ g++ codename.cpp -o codename.out    // 컴파일

 

 

2. eigen 을 사용 하여 코딩

#include <iostream>
#include <eigen3/Eigen/Dense>

using namespace std;

int main(){
    Eigen::Matrix3d matA;
    matA << 1, 2, 3,
            4, 5, 6,
            7, 8, 9;

    Eigen::Matrix3d matB;
    matB << 1, 2, 3,
            1, 2, 3,
            1, 2, 3;

    Eigen::Matrix3d matC = matA * matB;

    std::cout << matC << "\n";
}