C++로 SCARA 제어 프로그램 제작하기 Pt. 2-1 (Inverse Kinematics 구현하기 - 실패)

이전 글에 이어서 연결됩니다..!

 

이제 프로그램을 제작할려는 이유 중 하나인 제일 어려운 IK(Inverse Kinematics)를 구현해야 된다..!

근데.. 이 것의 경우 수학이기 때문에.. 굉장히 어렵다.. 왜냐면.. 내가 수학을 거의 포기해서 고등학교에서도 수학이 10 ~ 30점 대였으니.. 거기에 지금은 고3이니깐.. (특성화고라서 3학년은 일반 교과목이 없다.)

 

근데.. 이번 기회로 수학을 좀 알아야 됬었다..

일단 수학으로 넘어가기 전에.. 사전 준비를 해야한다.. 그래픽으로 표시를 해야하기 때문에 일단 먼저 라인을 그려줘야 한다.

이전 글에서 일단 그리드를 그렸는데.. 로봇 팔을 그릴 때는 나중에 값을 쉽게 바꿔서 해야하기 때문에 Xaml에는 그리지 않고 C++로 전부 그리게 할 것이다.

 

IK를 적용하기 전 팔 그리기

그래서 일단 그려야 하는데..

한가지 문제는.. 여기에 있는 코드는 아직 확실하지가 않다.. 왜냐면 지금도 글을 쓰는데 IK 문제를 해결하고 있기 때문이다.. 일단 대부분 테스트 코드라서.. 확신하기는 어렵다..

// 필요한 전역 변수 
float joint_1_line_size = 100;
float joint_2_line_size = 100;

float2 center_point_xy = float2(250, 250); // 중앙 센터 포인트
float2 joint_1_point_xy = float2(250, 250 - joint_1_line_size); // 첫번째 조인트
float2 joint_2_point_xy = float2(250, 250 - joint_1_line_size - joint_2_line_size); // 두번째 조인트

Xaml::Shapes::Ellipse center_point = Xaml::Shapes::Ellipse(); // 센터에 표시용 점
Xaml::Shapes::Ellipse joint_1_point = Xaml::Shapes::Ellipse(); // 첫번째 조인트 끝 점 위치 제어
Xaml::Shapes::Ellipse joint_2_point = Xaml::Shapes::Ellipse(); // 두번째 조인트 끝 점 위치 제어
Xaml::Shapes::Line joint_1_line = Xaml::Shapes::Line();
Xaml::Shapes::Line joint_2_line = Xaml::Shapes::Line();
Xaml::Shapes::Line joint_3_line = Xaml::Shapes::Line(); // 마지막 집게 부분 회전시킬 수 있게 하는거

일단 이런식으로 지정했다. 내가 프로그램에 알려줄껀 로봇 팔의 각 길이이고, 최종적으로 움직여야 할 끝점의 좌표를 알려주고 나머지는 다 자동으로 계산해야하기 때문에 저렇게 변수를 만들었다.

일단 임시로 100, 100으로 되고 나중에는 로봇 팔의 길이에 따라서 각각 비율에 맞춰 500 * 500 안에 넣을 생각이다.

 

그 아래는 초기에 그냥 표시하기 용으로 할려고 각각의 점을 미리 위치를 지정한 것이다.

그리고 그 아래는 그냥 GUI에 표시하기 위한 제어할 수 있도록 하는 점이나 선들이다.

 

그리고 C++로 기본 위치를 표시해야한다. 나의 경우엔 아래 처럼 코드를 짰다.

void MainPage::DrawPoint(Xaml::Shapes::Ellipse& point, Xaml::Controls::Canvas& canvas, double point_width, double point_height, float2 pos, Xaml::Media::Brush color) {
    // 중심점 크기나 색 설정
    point.Width(point_width);
    point.Height(point_height);
    point.Fill(color);

    canvas.Children().Append(point);

    // 중심점 위치 설정
    Xaml::Controls::Canvas::SetLeft(point, pos.x - (point_width / 2));
    Xaml::Controls::Canvas::SetTop(point, pos.y - (point_height / 2));

    Xaml::Controls::Canvas::SetZIndex(point, 1);
}

void MainPage::DrawLine(Xaml::Shapes::Line& line, Xaml::Controls::Canvas& canvas, double thickness, float2 pos_1, float2 pos_2, Xaml::Media::Brush color) {
    // 위치나 색 설정
    line.X1(pos_1.x);
    line.Y1(pos_1.y);
    line.X2(pos_2.x);
    line.Y2(pos_2.y);

    line.StrokeThickness(thickness);
    line.Stroke(color);

    canvas.Children().Append(line);

    Xaml::Controls::Canvas::SetZIndex(line, 0);
}

void MainPage::InitJoint() {
    double canvas_width = ControlJoint().ActualWidth();
    double canvas_height = ControlJoint().ActualHeight();

    double point_width = 20;
    double point_height = 20;
    double line_thickness = 10;

    // 컨트롤러 중심의 고정 센터 포인트를 표시 (그냥 참고용임)
    DrawPoint(center_point, ControlJoint(), point_width, point_height, center_point_xy);

    // 첫번째 조인트 선
    DrawLine(joint_1_line, ControlJoint(), line_thickness, center_point_xy, joint_1_point_xy, Xaml::Media::SolidColorBrush(Colors::Red()));

    // 컨트롤러 조인트 1 끝의 점
    DrawPoint(joint_1_point, ControlJoint(), point_width, point_height, joint_1_point_xy);

    // 두번째 조인트 선
    DrawLine(joint_2_line, ControlJoint(), line_thickness, joint_1_point_xy, joint_2_point_xy, Xaml::Media::SolidColorBrush(Colors::Blue()));

    // 컨트롤러 조인트 2 끝의 점
    DrawPoint(joint_2_point, ControlJoint(), point_width, point_height, joint_2_point_xy);

    joint_2_point.PointerPressed({ this, &MainPage::JointControlJoint2PointerPressed }); // 조인트 2의 포인트를 클릭 했는지
    joint_2_point.PointerReleased({ this, &MainPage::JointControlJoint2PointerReleased }); // 조인트 2의 포인트를 클릭 안했는지
    ControlJoint().PointerMoved({ this, &MainPage::JointControlCanvasPointerMoved }); // 캔버스의 마우스 포인터 위치를 얻기 위한거
}

한가지 아쉬운점은.. 라인의 위치를 지정하는건 쉬운데.. Ellipse의 위치를 지정할 땐.. 캔버스 기준으로 지정해야하는게 조금 아쉬웠다.. 다른 방법도 있긴 했지만.. 저게 가장 쉽고 기본적인 방법인 것 같다.. 그리고 이전글에서 마우스 정보를 함수로 넘기는 것이 저 아래쪽에 추가를 했다. (초기 팔 위치가 잡혀야지 마우스 포인트 위치를 얻어서 제어를 할 수 있기 때문에.. 그리고 맨 끝점의 마우스를 클릭하는 것과 떼는 것도 넘겨야 한다. 지금은 구현을 안했지만.. 나중에 각 점을 클릭해서 어떤 작업을 수행하도록 해야하기 때문이다.

 

그렇게 해서 위 코드의 결과물은 이렇다..!

 

짜증나는 수학..

그러면 이제 준비를 완료 했으므로 수학으로 넘어가야 한다..

처음엔 내가.. 이걸 이해하기 위해 3일을 썼다.. 그래서 나만의 방식을 찾았고 그걸 구현했지만.. 결과로는 실패다..

반은 되는데 반은 안되었기 때문에..

그래서 만약 이 글을 참고한다면.. Y값이 양수나 음수 한쪽만 필요하다면 이 글의 방법을 참고하면 되지만.. 그게 아니라면 그냥 이후 글을 참고하던지.. 유튜브 영상을 보면 된다..

 

일단 내가 최대한 이해해서 정리한건 아래 사진이다.

여기에서 구해야할 값은 C로 표시되어있는 부분의 점의 좌표이다. b와 a가 만나는 지점의 좌표

 

일단 여기에서 고정되어있거나 기본적으로 계산 없이 알 수 있는 값은

코드의 결과물 사진 처럼 빨간색은 첫번째 팔(a)이고 파란색이 두번째 팔(b)과 초록색 선으로 표시된 x와 y이다.. 

각 a와 b는 팔의 길이를 가지고 있고.. x와 y는 파란색의 끝점의 좌표이다.

 

일단 여기에서 필요 없는 변수는 C와 θ2 이다.. 지금 내 방법으론 필요없다는거지.. 이후 글에서 나올 식에는 필요하다..

 

암튼 x, y좌표를 알고 있다면 c의 값을 쉽게 구할 수 있다. 센터 점에서 x와 y 값의 거리를 구하면 되기 때문이다.. 이때! 유클리드 거리 공식을 이용하면 된다..! 근데 WinRT에서 float2에 쪽에 기본적으로 distance 함수가 있어서 이걸 이용했다. distance 함수도 유클리드 거리 공식을 이용해서 float 값으로 나온다..!

 

그렇게 해서 c를 구하게 되고

위 사진처럼 대충 칠해놓은 것을 보면 삼각형인 것을 알 수가 있다..!

 

여기에서 초록색 삼각형에서 오른쪽 상단에 I 로 되어있는 값을 구해야한다..!

I 값의 공식은 물론 직각 삼각형이기 때문에 역코사인으로 I 값을 찾을 수 있다. 하지만 x, y, c 값을 이미 알고 있고 이 뒤에 계산해야할 것은 3개의 변의 길이로 구하는 것이기 때문에 함수화를 할려고 3개 변으로 각도를 구하는 방식으로 했다.

 

https://www.mathsisfun.com/algebra/trig-solving-sss-triangles.html

Solving SSS Triangles

3개의 변으로 구할 땐 위 링크에서 처럼 SSS 방식을 이용해서 풀면 된다..! (분명 저거 중학교 때 열심히 외웠던 것 같은데..고1 수학 선행 때문에..)

 

그렇게 하면 I 값이 나오게 되고 아래 X 선과 위의 초록색 X선은 평행하기 때문에 왼쪽 아래에 B + θ1의 값은 I와 동일하다는 것이 된다..! 내가 이걸 기억하고 있어서 다른 영상에서는 안나오는 방법이지만.. 찾아내서 좋았는데.. 결국 이것 때문에.. 결과물이 이상해지긴 했다..

 

암튼 이제 I값과 동일하기 때문에 θ1을 구할려면 I 값에서 B 값을 빼면 θ1 값이 나오는 것이다..!

그리고 B 값도 아까 I 값을 구한 것 처럼 주황색 삼각형의 세 변을 알고 있고 내부 각을 알고 싶기 때문에 위 사이트에 나온 걸로 구하면.. B 값도 쉽게 구할 수 있다..!

 

그렇게 구하면 B를 알게 되고 I - B를 해서 θ1을 알게 된다..

 

그 후에

보라색으로 대충 칠한 부분을 보면 이것도 삼각형인걸 알 수 있다..!

그리고 여기에서는 알고 싶은건 a와 b가 만나는 점의 좌표를 구하고 싶은 것이기 때문에 θ1의 값이 필요했던 것이다.

암튼 그걸로 직각삼각형에서는 θ1 부분의 각도를 알면 밑변과 높이를 구할 수 있는데

밑변은 a * cos(θ1) 로 구할 수 있고 높이는 a * sin(θ1)으로 구할 수 있다.

그래서 밑변은 구할려는 값의 x가 되고 높이는 y값이 됨으로.. 이걸로 구할 수가 있다..!

 

그래서 이 모든 과정을 코드로 짜보았다..!

// SSS로 각을 구하는 것, 입력 값은 △ 이런 삼각형에서 오른쪽변, 왼쪽변, 밑변 순서로 입력하고 필요한 각도의 번호를 입력하는 것, a, b, c를 입력하고 b c사이의 각도를 구하고 싶다면 1
// 입력값 a, b, c / 구하고 싶은 각 bc: 1, ca : 2, ab : 3 으로
float MainPage::CalSSSRadian(float a, float b, float c, int angleNum) {
    float angle = 0;

    switch (angleNum) {
        case 1: // A
            angle = (pow(b, 2) + pow(c, 2) - pow(a, 2)) / (2.0 * b * c);
            break;
        case 2: // B
            angle = (pow(c, 2) + pow(a, 2) - pow(b, 2)) / (2.0 * c * a);
            break;
        case 3: // C
            angle = (pow(a, 2) + pow(b, 2) - pow(c, 2)) / (2.0 * a * b);
            break;
    }

    return acos(angle);
}

void MainPage::UpdatePos() {
    float x = joint_2_point_xy.x;
    float y = joint_2_point_xy.y;

    ChangePosPoint(joint_2_point, ControlJoint(), float2(x, y));
    ChangePosLine(joint_2_line, float2(x, y));

    // 각 변수 이름은 사진에 있는 이름을 참고
    // 그래픽을 표시할 땐 theta 2의 값을 구할 필요가 없음, 다만 나중에 로봇을 제어할 땐 필요함, C도 구할 필요는 없을 듯
    float a = joint_1_line_size; // 빨간색 라인의 길이
    float b = joint_2_line_size; // 파란색 라인의 길이
    float c = distance(center_point_xy, joint_2_point_xy); // 가상 라인의 길이, 유클리드 거리 사용, 첫번째와 마지막의 점의 길이를 구하는 것

    if (c <= joint_1_line_size + joint_2_line_size) { // 임시로 하는 것, c의 구하는 방식 때문에 제한없이 joint_2_point가 따라오고 그것과 센터 포인트의 길이를 구하면서 200을 넘어가게 되서.. 일단 임시로
        float C = CalSSSRadian(a, b, c, 3); // C의 각도를 구하는 것
        float B = CalSSSRadian(a, b, c, 2); // B의 각도를 구하는 것

        float I = CalSSSRadian(x - 250, y - 250, c, 2); // 250을 빼주는 이유는 계산했던 기준의 센터점이 0, 0 이였기 때문에 그쪽으로 옮기기 위해

        float theta1 = I - B;

        float j1x = a * cos(theta1) + 250; // 250을 더한 이유, 위에서 x, y에 빼줬었으니 다시 더하는 것
        float j1y = a * sin(theta1) + 250;

        ChangePosLine(joint_2_line, float2(j1x, j1y), false);
        ChangePosPoint(joint_1_point, ControlJoint(), float2(j1x, j1y));
        ChangePosLine(joint_1_line, float2(j1x, j1y));

        char debug1[512];
        sprintf_s(debug1, "a = %.2f, b = %.2f, c = %.2f || C = %.2f, B = %.2f || I = %.2f, theta1 = %.2f", distance(center_point_xy, joint_1_point_xy), distance(joint_1_point_xy, joint_2_point_xy), c, C, B, I, theta1);
        debugText1().Text(to_hstring(debug1));

        char debug2[512];
        sprintf_s(debug2, "BX = %.2f, BY = %.2f || CX = %.2f, CY = %.2f || AX = %.2f, AY = %.2f", center_point_xy.x, center_point_xy.y, j1x, j1y, x, y);
        debugText2().Text(to_hstring(debug2));
    }
}

여기에서 UpdatePos 함수는 마우스의 위치 값을 가져오는 JointControlCanvasPointerMoved 함수에다가 넣었다. 

 

현재는 저기에서 모든 것을 구현하지만.. 나중에는 먼저 값을 계산해서 각 좌표 변수에다가 값을 넣고 UpdatePos를 실행해서 업데이트 할 예정이다.

 

여기에서 CalSSSRadian은 각도를 구하기 위한 함수이다. 여기에서 Radian은.. SSS로 계산을 하면.. Radian 값으로 나와서이다. 물론 이걸 각도로 다시 바꿀려면 radian * 180.0 / M_PI 이런식으로 하면 되긴 하는데.. 이후에 cos와 sin으로 밑변와 높이를 구할 때 각도를 넣는게 아닌 radian 값을 넣어서 구해야하기 때문이다.

 

그리고 if (c <= joint_1_line_size + joint_2_line_size) 이 if문이 필요한 이유는 c가 첫점과 끝점의 길이인데.. 각 팔의 길이가 100이고 더하면 200이다. 그래서 첫점과 끝점의 길이가 최대 200으로 밖에 될 수 밖에 없다.. 만약 여기에서 200 이상인걸 넣으면 cos나 sin 값이 계산이 안된다. 그렇기 때문에 미리 제한을 두는 것이다.

 

또한 아래에 debug라고 되어있는건 그냥 내가 실시간으로 각 값을 확인할려고 해둔 것이다.

그리고 250을 빼고 더하고 하는 부분이 있는데 이유는 내가 계산한 좌표는 첫점이 0, 0일 경우를 두고 한건데.. 지금의 경우 캔버스의 가로 세로가 500이기 때문에 가운데 좌표가 250, 250이다.. 그래서 이것 때문에 250, 250을 뺀 후에 계산을 하고 나중에 250, 250을 더하는 것이다. 물론 나중에 이것도 250, 250도 자동으로 구해서 자동으로 넣어지도록 할 예정이다. 암튼 이걸로 만든 프로그램을 실행해보았다..!!

 

결과

영상을 보면 알겠지만.. 망했다.. 중앙보다 아래에서 마우스를 움직일 경우에는 정상적으로 작동하지만.. 위에선 비정상적으로 작동하게 된다.. 물론 이 경우에 if문을 사용하면 처리가 가능하긴 하다. 각 4분면 마다 x, y 값을 경우데 따라서 반전 시키면 되긴 한다. 근데.. 굉장히 어렵기도 하고.. y값이 250 보다 작을 때는 다른 계산을 해서 해결하는 방법도 가능할 것이다.. 근데 해보긴 했다.. 근데 안된다.. 만약 이 방법이 되었다 해도 아마 쓰진 않았을 것 같다. 분명히 이건 나중에 로봇을 제어할 때 각도를 알아야 하기 때문이다.. 근데 4분면에서 필요에 따라 반전을 시키면 각도 값을 정확하게 알기는 어려울 것이다.. 그래서 이 방법은 버렸다..

 

위에서 말한대로 하면.. 되긴 한다.. 근데 이것도.. 저렇게 되는거라면.. 문제가 있는 것이기 때문에.. 마찬가지로 버리기로 했다..

 

실패지만.. 실패는 아닌 듯..

사실 여기까지 구하기 위해.. 거의 잊어버린 수학을 다시 꺼내기 위해.. 삼각함수에 대해서 다시 공부해야하기도 했고.. 학교 수학 선생님이나.. 부모님이나.. IK와 로봇을 잘 아시는 분에게 물어보았다.. 결국에는 이 방법은 내가 찾았긴 했지만.. 이 것을 찾기 위해 도와주신 분들에게 뭔가.. 죄송한 마음이 들었다.. 물론 실패도 있어보는 것이.. 좋다고 할 순 있지만.. 그래도 도와주신 분들이 있는데.. 이렇게 실패를 해버리니.. 아쉬웠다..

 

어쨋든 이 방법은 실패를 했지만.. 그동안 내가 수학 수업을 받으면서.. 지루한 식 외우기나 문제집에서 내준 예시 문제를 푸는 것이 아닌.. 내가 직접 문제 상황을 해결하기 위해 수학을 쓸려는 예시를 보게 되어서.. 예전까지만 해도.. 코딩이나 로봇에는 + - * / 만 알면 되는줄 알았는데.. 그것이 아니란 것을 이제야 알 수 있었다.. 그래서 나는 이 과정을 통해서 엄청난 큰 도움을 얻기도 해서 실패라고 할 순 없는 것 같긴 하다..!

 

암튼 그래서 이번 파트는 여기까지 쓰지만.. 다음 파트에선 진짜로 IK를 구현한 것을 쓸 것이다..! 왜냐면.. 유튜브 영상에서 식을 가져왔기 때문.. 암튼.. 여기서 끝..! 내.. 3일..!! 어디갔어..!