C++로 SCARA 제어 프로그램 제작하기 Pt. 4 (Inverse Kinematics 선택 제어)

이제 선택으로 제어되도록 해야한다!!

이건 수학적 지식이 전혀 필요없어서 엄청 간단했다..!

 

자 일단 내가 구상하고 있는건 두번째 끝점을 클릭한 상태로 드래그 하면 IK로 움직이는 것이고, IK의 중간 점을 눌르면 방향이 반대로 되서 IK가 다시 계산되도록 하는 것이다..!

 

이걸 구현할려면 버튼이 클릭됬는지 안됬는지를 감지해야한다..

근데 그냥 버튼이 클릭됬는지 안됬는지를 감지하면 안된다. 이게 문제가.. 버튼을 눌렀는지 감지하는 방식이 2가지 정도가 있는데.. 마우스의 위치를 감지하면서 마우스의 위치가 내가 클릭할려는 요소 위에 있는지 감지하는 방법이 있고.. 요소 자체를 클릭했는지 뗐는지를 감지하도록 하는 방법이 있다..

근데 첫번째 방법의 경우는 마우스 아래 그 버튼이 있는지 계산하는게 알고리즘이 필요해서 구현하기는 힘들어서 넘어가고.. 그 요소를 클릭했는지 뗐는지를 감지하도록 해야한다..

 

근데 여기에서 한가지 문제가 생긴다.

끝점을 제어할 것이기 때문에 끝점을 눌르는 것까지는 문제가 안된다..

문제가 되는건 뗄때 인데.. 내가 끝점을 클릭하고 드래그 할땐 IK 계산이 되다가 마우스 클릭을 떼면 IK 계산이 풀리도록 해서 되게 하는 방식인데 요소를 클릭했는지 뗐는지 감지하는건 마우스가 요소 그 위에 있을 때만 감지가 가능하다..

 

그래서 감지를 할려면 항상 어떤 요소가 끝 점을 따라다니다가 마우스로 클릭하면 마우스를 따라오다가 뗐는지 감지하면 다시 끝점으로 돌아가도록 하면서 감지해야한다. (끝점을 따라가야하는 이유는 그 요소에만 마우스 감지 기능을 넣을 것이기 때문이다.. 그래서 끝점을 안따라다니면.. 다시 움직일 때는 그 요소의 위치에 마우스를 두고 클릭해야 움직이게 할 수 있다..)

그러기 위해선 새로운 요소를 만들어야 한다. 끝점과 똑같은걸로

 

일단 joint_2_mouse_point로 만들었는데 이것에 대해 클릭과 클릭이 떼지는걸 감지해서 다른 함수로 넘겨주는 코드를 적어준다. 아래 코드는 InitJoint 함수를 실행하면서 실행되도록 했다.

joint_2_mouse_point.PointerPressed({ this, &MainPage::JointControlJoint2PointerPressed }); // 조인트 2의 포인트를 클릭 했는지
joint_2_mouse_point.PointerReleased({ this, &MainPage::JointControlJoint2PointerReleased }); // 조인트 2의 포인트를 클릭 안했는지

그리고 항상 끝점을 따라가도록 UpdatePos를 실행할 때 마다 팔의 끝점으로 위치를 바꾸도록 했다.

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

    // 파란색 끝 마우스 클릭 감지 포인트의 위치 변경
    ChangePosPoint(joint_2_mouse_point, ControlJoint(), joint_control_mouse_point_pos); // // 마우스 포인터를 따라가게

    // 파란색의 첫 포인트 위치 변경
    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);
}

 

그 후엔 그 요소가 클릭됬는지 클릭에서 뗐는지를 감지할 때 마다 어떤 함수를 실행하니 그 함수를 만들어야 한다.

일단 그 함수에서는 마우스가 클릭되었는지 안되었는지 확인을 한다. 이유는 드래그를 하고 있는지 없는지를 판단해야하기 때문이다. 그리고 마우스를 뗐을 때 UpdatePos 함수를 한번 실행하는데.. 이것의 이유는 이후에 알 수 있다. (주석에 나와있긴 하다)

 

일단 코드는!

void MainPage::JointControlJoint2PointerPressed(IInspectable const& sender, Xaml::Input::PointerRoutedEventArgs const& e) {
    is_joint_2_point_click = true; // 마우스가 클릭 되었다면
}

void MainPage::JointControlJoint2PointerReleased(IInspectable const& sender, Xaml::Input::PointerRoutedEventArgs const& e) {
    is_joint_2_point_click = false; // 마우스가 클릭 되지 않았다면

    UpdatePos(); // 마우스를 떼고 마우스를 움직여야 돌아가기 때문에.. 이걸 방지하기 위해 미리 한번 업데이트 시킴
}

이제 이렇게 한 후.. 마우스가 드래그 되고 있을 때 IK가 계산되도록 해야하기 때문에 캔버스에서 마우스의 움직임을 가져오는 함수에서 수정을 해야한다.

 

여기에서도 간단하게, 마우스가 드래그 되고 있을 때만 IK를 계산하고 업데이트 시켜야 하니 그냥 if문으로 is_joint_2_point_click이 true일 때만 되도록 했다.

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

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

    // 마우스 포인터가 클릭 되었을 때만 계산하고 업데이트 하도록
    if (is_joint_2_point_click) { 
        CalIKPos(joint_control_mouse_point_pos);
        UpdatePos();
    }
}

근데 이걸로 되긴 한다.. 근데 한가지 문제가 발생한다.. 마우스를 드래그 하는 것 까지는 좋은데.. 

아까 위에서 말한 것 처럼 마우스에서 클릭을 뗐을 때 다시 끝점으로 안돌아간다.. 이렇게 되면 그 요소만 클릭할 수 있어서 그 요소를 클릭해야지만 움직이게 할 수 있다.. 그래서 이걸 위해 끝점이 클릭 상태가 아니라면 끝점을 따라가고 있게 바꿨다. 그래서 코드는 이렇게!

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

    // 파란색 끝 마우스 클릭 감지 포인트의 위치 변경
    if(!is_joint_2_point_click) // 끝점이 클릭되지 않았다면
        ChangePosPoint(joint_2_mouse_point, ControlJoint(), joint_2_point_xy); // 끝점을 따라가고 있게
    else // 끝점이 클릭되고 있다면
        ChangePosPoint(joint_2_mouse_point, ControlJoint(), joint_control_mouse_point_pos); // 마우스 포인터를 따라가게

    // 파란색의 첫 포인트 위치 변경
    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);
}

if else로 간단하게 해결할 수 있었다. if에서 처음을 false로 해서 한 이유는 일단 형식상으론 마우스의 끝점이 클릭되고 있는건 특별 경우라서.. 그렇게 했다. 

 

암튼 이걸로 클릭하면 아래처럼 위치가 바뀐다..! (마우스 따라올 때 흰색 점도 같이 따라오는건.. 확인하기 위해 한 것이고 나중엔 투명하게 바꿀 것이다.)

아주 잘 작동된다..!

 

이제 구현할 부분은 중간 점을 눌르면 반대의 각도로 바뀌는 것이다.. 어쨋든 내가 한 방식으로 한다면..

일단 아까처럼 InitJoint 함수에 2가지 클릭하는 것과 떼는 것 2가지를 추가한다.

joint_1_point.PointerPressed({ this, &MainPage::JointControlJoint1PointerPressed }); // 조인트 1의 포인트를 클릭 했는지
joint_1_point.PointerReleased({ this, &MainPage::JointControlJoint1PointerReleased }); // 조인트 1의 포인트를 클릭 안했는지

이런식으로 추가한 후에는 각 함수를 만드는데

어떤 식으로 마우스로 클릭했는지 감지하는 것이 문제이다..

방법이라면 그 자리에서 마우스를 떼는 것을 감지했을 때 바뀌도록 하는 방법이 있지만.. 이것의 문제는 다른 곳에서 마우스를 클릭하고 그곳에서 떼면 이것도 감지가 된다..

그리고 마우스를 그곳에서 클릭했을 때 감지되는건.. 또 너무 빠르다.. 왜냐면 잘 못 눌렀을 때 눌른 상태로 드래그 하면 선택이 안되는걸 원해서..

 

암튼 그래서 내가 원하는건 버튼에서 마우스를 클릭하고 그 버튼에서 마우스를 떼었을 때를 감지해서 일을 처리하도록 하는 것이다.

 

이걸 구현하는건 간단하다. 눌렀을 때 A변수를 true로 바꿔주고 떼는 것을 감지하는면 A변수가 true인지 확인하고 true이라면 함수를 실행하고 A변수를 false로 바꿔주면 된다..!

 

그렇기에 바로 구현!

void MainPage::JointControlJoint1PointerPressed(IInspectable const& sender, Xaml::Input::PointerRoutedEventArgs const& e) {
    is_joint_1_point_click = true; // 마우스가 클릭 되었다면
}

void MainPage::JointControlJoint1PointerReleased(IInspectable const& sender, Xaml::Input::PointerRoutedEventArgs const& e) {
    // 중간점을 클릭했을 경우 중간의 각도를 반대로 바꿔서 계산하게 해야함으로 그 버튼만 클릭하고 떼었는지 확인할려고 if문 함
    // 확인이 되면 마지막 포인트로 다시 IK를 계산하고 위치를 업데이트 시키고 다시 클릭을 뗀걸로 인식하도록 바꾼거
    if (is_joint_1_point_click) {
        joint_angle_reverse = !joint_angle_reverse; // 각도를 바꾸면 그 뒤로도 고정되서 계산되게 해야함으로 !로 true false가 서로 토글로 바뀌도록
        CalIKPos(joint_2_point_xy); 
        UpdatePos();
        is_joint_1_point_click = false; // 마우스가 클릭 되지 않았다면
    }
}

근데 여기에서 joint_angle_reverse 라는 것이 보인다. 이것의 경우 현재 중간 점의 각도가 반대로 계산할지 정상으로 계산할지 하는 변수로 했는데.. 

반대로 계산하는건 IK를 계산하는 부분에서 acos 앞에 -만 붙여서 계산하는 것이기 때문에 이후에도 계속 고정으로 계산되게 하기 위해서 필요한 변수이다. 물론 IK를 계산하는 함수에 집어넣는 방식으로 할 수도 있지만.. 내가 생각하는 형식상 맞지도 않고 굳이 또 넣으면 함수를 수정해야하기 때문이다..

 

그래서 저 변수로 각도가 조정되게 하고, 여기에서 처럼 토글을 구현할 경우에는 bool 값에는 !를 붙이면 된다. 

이유는 !는 반대로 만들어주는건데 !true -> false이고 !false -> true로 되기 때문에 앞에 !로 붙이는 것이다.

 

그 후에 CalIKPos와 UpdatePos를 붙여주는 이유는 CalIKPos와 UpdatePos가 구동되는 조건은 마우스가 움직일 때만이다. 근데 이건 클릭된 것을 감지하면 바로 바뀌어야 하는데.. 안되기 때문에 저기에도 추가해서 바뀌도록 하는 것이다.

 

암튼 이렇게 바꾼 후에는 CalIKPos에서도 IK 계산을 바꿔야 하는데 어차피 joint_angle_reverse 변수로 알 수 있기 때문에 이걸로 판단할 수 있도록 했다!

theta2 = acos((pow(mouse_x, 2) + pow(mouse_y, 2) - pow(a, 2) - pow(b, 2)) / (2.0 * a * b));

if (joint_angle_reverse) // 중간 점의 각도를 반대로 돌리는 것
    theta2 = -theta2;

이런식으로

원래 생각은 if와 else로 theta2 구하는 공식을 acos와 -acos 2개로 만들려고 했다. 

근데 그냥 생각해보니 어차피 -만 붙이는거라서 구한 값에 -를 붙일지 말지로 쉽게 구현할 수 있어서 그걸로 바꿨다..!

 

근데.. 해보니깐 잘되긴 한다.. 근데 한가지 문제가 생겼다.. 중간 점을 클릭하면 바뀌긴 하는데.. theta2 값을 확인하니 바뀔 때마다 약간의 오차가 발생한다.. 이 경우는 수학적이라서.. 어차피 계산에는 큰 영향을 안줄 것 같아서 놔두긴 했는데.. theta2값이 완전이 .000000으로 떨어지는 경우가 있다. 그곳에서 중간점을 눌르거나 하면.. 그냥 값이 계산이 안되고 고장나버린다.. 그래서 이유를 못 찾고 있다가..

옆에 있는 나보다 수학 잘하는 친구(이 문제도 그 친구가 테스트로 가지고 돌려보다가 발견한거..)가 무슨 이유인지 찾아볼려다가 내가 삼각함수에서 계산이 안되면 이런 문제가 일어난다고 하고 atan2 부분에서 문제가 일어나는 것 같다고 하니깐.. atan2에서 분모가 0이 될 수 없는데.. 0이 되버리면.. 무한의 값.. 뭐라면서 알려줬다.. 그래서 바로 나도 생각났다.. 이전에 a + b가 c보다 같거나 작을 때 계산한다고 했었는데.. 이걸 하기전에 본건.. 같거나 일 경우가 없었긴 했다.. 그래서 원래는 c <= a + b가 조건이고 이게 참일 때마다 IK를 계산하도록 했는데 이걸 c < a + b로 바꾸니.. 성공적으로 해결이 되었다..!  

 

그렇게 되어서 CalIKPos의 전체코드는 이렇다..!

void MainPage::CalIKPos(float2 point) {
    float a = joint_1_line_size; // 빨간색 라인의 길이
    float b = joint_2_line_size; // 파란색 라인의 길이
    float c = distance(center_point_xy, point); // 첫점과 끝점을 이은 길이

    // https://youtu.be/IKOGwoJ2HLk
    // x2, y2는 끝점 x1, y1은 중간점 x0, y0은 센터점

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

    float theta1 = 0, theta2 = 0;
    float x2 = 0, y2 = 0;
    // a와 b의 최대 길이는 a + b가 됨, 그리고 c는 빗변이 되고 a + b를 넘으면 안됨.. 넘으면 삼각함수로 계산이 불가함, 그렇기에 미리 맞는지 확인하는 것
    if (c < a + b) { // 원래는 c <= a + b로 했는데.. 이렇게 할 경우 tan으로 구할 때 각도가 둘다 90도일 경우 터짐.. 그래서 그걸 막기 위해 완전히 같은 것도 막음
        // IK를 구현하기 위한 각도 계산
        theta2 = acos((pow(mouse_x, 2) + pow(mouse_y, 2) - pow(a, 2) - pow(b, 2)) / (2.0 * a * b));

        if (joint_angle_reverse) // 중간 점의 각도를 반대로 돌리는 것
            theta2 = -theta2;

        theta1 = atan2(mouse_y, mouse_x) - atan2(b * sin(theta2), a + b * cos(theta2));

        // 위에서 theta1구할 때 theta1 + B에서 B를 뺐을 때 그 B 값을 theta1으로 사용해서 밑변과 높이를 구할 수 있긴 하지만.. 
        // 굳이 필요 없는 계산을 해야하기도 하고.. 굳이 할 필요가 없기 때문에 그냥 마우스 포인트로 되게 함
        x2 = mouse_x + center_point_xy.x;
        y2 = mouse_y + center_point_xy.y;
    }
    else {
        // 마우스 포인트와 선을 이어서 삼각형을 만들고 theta1의 각도를 얻어서 
        // 마우스 포인트와 선을 이은거에서 중간점은 구하는 방식이 같기 때문에 넘기고, 끝점을 구하는게 목적이기 때문에
        // 끝점을 구하기 위해 쫙 폈을 때 a + b이기 때문에 그걸로 밑변과 높이를 구하는거
        theta1 = atan2(mouse_y, mouse_x);

        x2 = (a + b) * cos(theta1) + center_point_xy.x;
        y2 = (a + b) * sin(theta1) + center_point_xy.y;
    }

    // 중간 점의 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 = { x2, y2 };

    char debug[512];
    sprintf_s(debug, "theta1 = %f, theta2 = %f", theta1 * 180.0 / M_PI, theta2 * 180.0 / M_PI);
    debugText2().Text(to_hstring(debug));
}

 

그래서 이렇게 해서 IK 시뮬레이션 부분에선 각도 제한하는거 빼곤 전부 완성시켰다..!

그래서 결과물은 이렇다..!

 

암튼 끄읕..!

내 옆에 있던 친구는.. 내가 만드는거 자랑할 때마다.. 긍정적으로 봐주는 친구인데..! 매번 테스트할 때마다 짜잔하면 신기한건지.. 몇분을 움직여보기도 한다.. (오류를 찾아낼려고 하는 것일 수도 있지만..) 암튼.. IK를 고민할 때도 계속 도와줄려고 했었기도 했다..! 내년엔.. 다른 대학교로 간다는데.. 좀 아쉽긴 하다..