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

이전글에서는 아쉽게도.. Inverse Kinematics를 구현하기에.. 실패했다..

반은 되고 반은 안되고.. 암튼 그래서.. 유튜브를 찾아봤는데.. 

잘 정리된 영상이 있었고 식도 모두 있었다..!

 

식 찾기

https://youtu.be/IKOGwoJ2HLk

여기에서 식을 찾았는데 

이 식이다.

또한 팔을 반대로 접을 경우에는 q2에 cos 앞에 -만 붙이면 중간의 팔 각도가 반대로 바뀌어서 계산을 시킬 수가 있다.

이걸 이용하면 내 로봇에는 움직일 수 있는 각도가 정해져있는데.. 이 각도가 q2라면 q2가 벗어났을 때 cos값 앞에 -만 붙여서 목표 지점에 도달 할 수 있도록도 할 수 있을 것이다.. 물론.. 어렵겠지만.. 

 

IK를 코드로 구현

아무튼 이 공식으로 하기로 했다..!

근데 이 공식을 안해본건 아니였다..

이전 글에서 했던대로 너무 되지가 않아서 저 식으로 해봤는데.. 아쉽게도 작동을 이상하게 했다.. 근데 부모님에게 물어보다가 이 식을 말했더니.. 내가 했던게 tan -1이 역탄젠트여서 atan으로 했는데.. 부모님이 말하기론.. atan2로 해야한다고 한다.. 무슨 분모가 0이 될 수가 없는데 atan2는 뭐 어떻게 한다 해서.. 그걸로 했었는데 됬었다..!!

 

일단 그 복잡한 식은 전부 버리고.. 단 2개의 식만이 필요해서.. 좀 놀라웠는데.. C++로 변경하면

float theta2 = -acos((pow(x2, 2) + pow(y2, 2) - pow(a, 2) - pow(b, 2)) / (2.0 * a * b));
float theta1 = atan2(y2, x2) - atan2(b * sin(theta2), a + b * cos(theta2));

이렇게 구할 수 있다. 여기에서 q2는 theta2이고 q1은 theta1이다.

굉장히 간단한 식인데.. 그런 놀라운 계산을 할 수 있다는게.. 엄청나다.. 

 

암튼 이걸 이용해서 결국에는 중간점의 x, y 좌표를 구해야하기 때문에 θ1의 값으로 밑변과 높이를 구해야한다..!

이전 글과 마찬가지로 첫번째 팔의 길이가 l이면 l * cos(θ1)으로 밑변을 구할 수 있고 l * sin(θ1)으로 높이를 구할 수 있다..! 그리고 밑변은 x가 되고 높이는 y가 된다..! 

그래서 그걸로 C++에서 구한다면 이렇게!!

float x1 = a * cos(theta1);
float y1 = a * sin(theta1);

 

근데 여기에서 한가지 문제가 발생한다. 이전 글에서도 말했 듯이.. IK를 계산해야하는 위치는 캔버스의 크기인 500 * 500의 중심인 250, 250에서 계산을 해야한다. 근데 이 상태로 계산하면 삼각함수에서 오류가 나서 값을 못 구하게 될 것이다..

그래서 이 식의 계산했던 곳은 센터 점이 0, 0이기 때문에 250, 250에서 0, 0으로 이동시켜야 한다.

근데.. 사실 여기에서 또 문제가 발생하긴 한다.. x값은 왼쪽에서 오른쪽으로 커지니깐.. 이건 상관없는데.. y값은 원래라면 아래쪽에서 위로 커져야 하지만.. 이것의 첫 0, 0이 왼쪽 상단이기 때문에.. y 값을 반전 시켜서 계산 시켜야 하긴 한다..

근데 그러면 계산할 때 식도 어느정도 길어질 수도 있고 하고.. 그냥 y값만 반대로 된거기 때문에.. 정상적으로 작동하는데 팔의 각도가 반대로 되어있다.. θ2에서 acos 앞에 -를 붙이면 같아지긴 한다.. 그래서 그냥 반대로 되고 정상적으로 작동하긴 해서 그냥 이대로 하기로 했다.

 

어쨋든 두번째 문제는 무시하고 첫번째 문제를 해결할려면 중간 점의 x, y좌표를 각각 빼주고 나중에 더해줘야 한다. C++ 코드로 구현한다면 이렇게 된다.

float x2 = point.x - center_point_xy.x;
float y2 = point.y - center_point_xy.y;
            
float theta2 = -acos((pow(x2, 2) + pow(y2, 2) - pow(a, 2) - pow(b, 2)) / (2.0 * a * b));
float theta1 = atan2(y2, x2) - atan2(b * sin(theta2), a + b * cos(theta2));
            
float x1 = a * cos(theta1) + center_point_xy.x;
float y1 = a * sin(theta1) + center_point_xy.y;

여기에서 center_point_xy는 말그대로 중심점의 x, y다 각 값은 그냥 250, 250 이게 있는거다. 그래서 250, 250으로 뺐다가 나중에 더하는 방식으로 되고 이렇게 하는 이유가 캔버스의 크기를 바꾸면 자동으로 되게 하기 위해서이다...!

 

복잡한 코드를 바꾸기!

그리고 이전 글에서 나온 것 처럼 원래 하던 방식은..

UpdatePos에서 각도를 구하고 x, y좌표를 구해서 직접 업데이트 하는 방식인데.. 이 경우 함수화를 해서 나중에 재사용할 때 조금 문제가 생길 수 있다.. 필요한거.. 다시 만들어야 하고.. 그래서 CalIKPos라는 함수를 만들었는데.. 여기에서 각 포인트의 x, y좌표를 구해서 넣어놓고 각 포인트의 x, y좌표는 전역 변수이기 때문에 UpdatePos에서 그 점으로 라인이나 점을 이동시키는 코드로 변경했다.

 

그래서 모든 코드를 아래처럼 고쳤다.

void MainPage::InitJoint() {
    double point_width = 20;
    double point_height = 20;
    double line_thickness = 10;

    // 센터 점 위치 초기화
    center_point_xy = { (float)(ControlJoint().Width() / 2), (float)(ControlJoint().Height() / 2) }; // 기본적으로 Xaml에서 나온 값이 double이므로 float로 변환
    // 초기 상태 IK 계산
    CalIKPos({ center_point_xy.x, center_point_xy.y - joint_1_line_size - joint_2_line_size });

    // 컨트롤러 중심의 고정 센터 포인트를 표시 (그냥 참고용임)
    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 }); // 캔버스의 마우스 포인터 위치를 얻기 위한거
}

void MainPage::CalIKPos(float2 point) {
    float a = joint_1_line_size; // 빨간색 라인의 길이
    float b = joint_2_line_size; // 파란색 라인의 길이

    float c = distance(center_point_xy, point); // 첫점과 끝점을 이은 길이

    // a와 b의 최대 길이는 a + b가 됨, 그리고 c는 빗변이 되고 a + b를 넘으면 안됨.. 넘으면 삼각함수로 계산이 불가함, 그렇기에 미리 맞는지 확인하는 것
    if (c <= a + b) {
        // x2, y2는 끝점 x1, y1은 중간점 x0, y0은 센터점

        // center_point_xy의 x와 y로 빼는 이유는 IK를 구현하는 곳이 250, 250인 중앙이여서 이걸 0, 0으로 옮기기 위해
        float x2 = point.x - center_point_xy.x;
        float y2 = point.y - center_point_xy.y;

        // IK를 구현하기 위한 각도 계산
        float theta2 = -acos((pow(x2, 2) + pow(y2, 2) - pow(a, 2) - pow(b, 2)) / (2.0 * a * b));
        float theta1 = atan2(y2, x2) - atan2(b * sin(theta2), a + b * cos(theta2));

        // 중간 점의 x와 y를 cos, sin으로 구하는 것
        float x1 = a * cos(theta1) + center_point_xy.x; // x, y를 더한 이유, 위에서 x, y에 빼줬었으니 다시 더하는 것
        float y1 = a * sin(theta1) + center_point_xy.y;

        joint_1_point_xy = { x1, y1 };
        joint_2_point_xy = point;

        char debug[512];
        sprintf_s(debug, "theta1 = %.2f, theta2 = %.2f", theta1 * 180.0 / M_PI, theta2 * 180.0 / M_PI);
        debugText2().Text(to_hstring(debug));
    }
    else { // 여기에선 마우스가 a + b보다 넘게 벗어났으므로 마우스와 센터점을 연결해서 삼각형을 그리고 그걸로 각 지점의 위치를 구해야함

    }
}

void MainPage::UpdatePos() {
    // 파란색 끝 포인트의 위치 변경
    ChangePosPoint(joint_2_point, ControlJoint(), joint_2_point_xy);
    ChangePosLine(joint_2_line, joint_2_point_xy);

    // 파란색의 첫 포인트 위치 변경
    ChangePosLine(joint_2_line, joint_1_point_xy, false);

    // 빨간색의 끝 포인트 위치 변경
    ChangePosPoint(joint_1_point, ControlJoint(), joint_1_point_xy);
    ChangePosLine(joint_1_line, joint_1_point_xy);
}

void MainPage::JointControlCanvasPointerMoved(IInspectable const& sender, Xaml::Input::PointerRoutedEventArgs const& e) {
    Input::PointerPoint point = e.GetCurrentPoint(ControlJoint());
    float2 point_pos(point.Position().X, point.Position().Y);

    Point_X().Text(to_hstring(point_pos.x));
    Point_Y().Text(to_hstring(point_pos.y));

    CalIKPos(point_pos);
    UpdatePos();

    char debug[512];
    sprintf_s(debug, "Center X: %.2f, Y: %.2f || Joint1 X: %.2f, Y: %.2f || Joint2 X: %.2f, Y: %.2f", center_point_xy.x, center_point_xy.y, joint_1_point_xy.x, joint_1_point_xy.y, joint_2_point_xy.x, joint_2_point_xy.y);
    debugText1().Text(to_hstring(debug));
}

나머지 ChangePosPoint이나 ChangePosLine이나 DrawLine, DrawPoint에 대한 함수는 이전 글에 있다!

 

암튼 debug는 내가 실시간으로 값을 확인할려고 적어둔 것이고

저기 중간에서 else 에서 비워둔 곳에.. 주석으로 쓰여있긴 하지만..

c 값이 a + b 보다 초과할 경우 삼각함수로 계산이 안되어서.. 이 경우엔 현재 계획중이지만 마우스와 센터점을 연결해서 삼각형을 그리거나 라인을 그려서 그 사이에 점들이 일정하게 들어가도록 할 예정이다.

 

결과

암튼 이렇게 해서 제작된 결과물은 이렇게 된다..!

 

이제 저기 안에서 제작할 부분은.. 끝점을 클릭한 상태로 드래그 할 때만 IK가 계산되도록 하는 것이고 중간 점을 클릭하면 θ2구할 때 acos앞에 - 붙여서 반대로 되게 해서 하는 것과 각도 제한시키는 것과 c값이 벗어날 경우 일자로 뻣어서 마우스 쪽으로 따라오도록 만들 것이다..! 

 

한가지 아쉬운건 IK를 구현하는 다른 방법이 생각나긴 했는데.. 이걸 한다고 해도 제대로 되지 않을 것 같고 그냥 이대로 하는게 가장 나을 것 같아서.. 이건 완성한 뒤에..! 할 수 있으면 해보는걸로 하기로 했다..!

 

암튼 이번건 여기에서 끄읕!!