이전 시간, DH table과 joint angle, distance들을 통해 3D manipulator를 시각화하는 예시들을 실행했습니다. 이번에는 end effector가 특정 position / orientation을 만족하도록 하는 joint angle을 계산하는, inverse kinematics에 대해 배워봅시다.

• 3차원에서 end effector의 위치와 각도를 모두 제어하는 것은 6dof 문제입니다. 이런 이유로 보통 로봇팔들은 적어도 6개 이상의 모터를 사용합니다.

• homogeneous matrix에서, 특정 position을 만족시키는 것은 쉽지만, 특정 Orientation을 만족시키는 것은 어떻게 할 수 있을까요? Rotation Matrix는 9개의 값을 갖는데, orientation은 3개인 상황입니다. 😕

• 3D Rotation Matrix의 계산식과 약간의 트릭을 사용하여 손쉽게 9개의 원소를 갖는 Rotation Matrix를 도출할 수 있습니다. $r_{31}$을 통해 $\theta$를 얻을 수 있고, 계산된 $\theta$와 $r_{32}$를 통해 $\phi$를, $r_{21}$를 통해 $\psi$를 얻을 수 있지요.

실제 코드 구현시에는 위 식들을 사용하여 End Effector가 특정 위치를 만족하도록 fslove를 통해 수치적 계산을 진행합니다. 이렇게 구한 joint angle들로 다시 Forward Kinematics를 적용해주는 것이지요.

코드 구현

• 이번 예시를 위해 사용할 6DOF manipulator와 DH table입니다. 5개의 revolute joint와 1개의 prismatic joint로 구성된 manipulator가 되며, 이러한 형태를 “Stanford manipulator”라고 부릅니다.

이번 예시인 3D inverse kinematics를 구현하는 절차는 다음과 같습니다.

1. 지난 강의와 동일하게 DH Table에 기반한 Forward Kinematics 구현
2. Forward Kinematics 결과인 Homogeneous matrix에서 x y z phi theta psi를 추출
3. 2번의 x y z phi theta psi가 원하는 값이 되도록 fsolve를 통해 새로운 joint 값들을 계산
4. 3번의 값들로 다시 Forward Kinematics 하여 모든 joint 위치를 계산, 최종 시각화

• DH table과 Parameter Class는 다음과 같습니다.

• Forward Kinematics는 fslove의 input이 되는 forward_kinematics 함수 안에 구현되어 있습니다. 모든 homogeneous matrix를 구하고, end effector의 position과 rotation을 추출한 다음, reference point와의 차이를 return 합니다.

• main 함수의 fslove에서는 initial guess를 비롯한 여러 매개변수를 전달하여 inverse kinematics를 계산하며, 이렇게 계산된 결과인 result는 6개 joint value를 담고 있습니다.

시각화를 담당하는 animate 함수는 간단하게 설명으로 짚고 넘어가겠습니다.

1. 주어진 joint value들을 통해 forward kinematics 계산
2. 모든 joint들의 좌표를 추출하여 plot
3. reference end effector에 대해선 굵은 점으로 별도 시각화

코드의 실행 결과입니다. 왼쪽에는 설정한 initial state를, 오른쪽에는 fslove를 통해 계산된 결과를 시각화하여 보여줍니다.