이전 inverse kinematics 시간, 원을 그리는 double linked pendulum을 구현한 바 있습니다. Jacobian 시간에도 cartesian 좌표계에서 joint space로의 변환을 통해 비슷한 구현을 진행한 바 있었지요.

하지만 이렇게 작성한 trajectory는 우리가 원하는 시점에, 로봇이 완벽하게 특정 joint angle을 가진다는 가정을 포함합니다. 더불어 실제 로봇은 물리적인 한계로 인해 최대 속도, 최대 가속도의 제한을 가지지요.

따라서 이번 시간에는 보다 현실적인 trajectory, joint angle, velocity, effort의 시간에 대한 경로를 계산해내는 방법에 대해 함께 살펴보고자 합니다.

• 로봇팔이 움직이는 경로 $q(t)$는 곧 end effector의 좌표 변화, 혹은 joint angle의 시간에 따른 변화라고 말할 수 있습니다. 이 $q(t)$는 $x(t), y(t), z(t)$로 표현될 수 있고 $\theta_1(t), \theta_2(t), \theta_3(t)$로도 표현될 수 있을 것입니다.

• 초기 상태 $q(0)$에서 목적 지점 $q(f)$에 도달하기 위한 최적 경로를 생각해보면, 간단하게 두 지점을 직선으로 연결하는 방식을 생각해볼 수 있습니다. 그리고 이는 곧 1차 함수의 계수를 구하는 것과 동일하지요.

• 1차 함수의 계수 $a0, a1$ 정도는 손으로 풀 수도 있지만 sympy를 통해 프로그래밍으로 도출해보겠습니다.

• $t_0 = 0,\,q(t_0) = 10,\,t(t_f) = 1,\, q(t_f) = 20$ 을 가정 하여 $q, \dot{q}$ 의 그래프를 그려보았습니다.

• $\dot{q}$ 의 그래프를 보면, 갑자기 속도를 0에서 10으로 높이고, 또 10에서 0으로 낮춰야 하는 지점이 발생합니다. 이는 transient라고 하며, 이러한 움직임은 모터에 큰 무리를 줍니다.

• 이를 해결하기 위해서 1계 미분 조건인 $\dot{q}(t_0) = 0, \, \dot{q}(t_f) = 0$이라는 조건을 추가해보았습니다. 종합해보면 총 4개의 조건이 만족되어야 하고 이는 곧 계수가 4개인 3차 함수를 구해내야 한다는 뜻이 됩니다.

• 행렬 형태로 조건식을 정리할 수 있으므로, 이번에도 sympy를 통해 해를 구해보겠습니다. 일전 코드에서 확장만 시키면 되기 때문에 구현 자체는 간단합니다.

• $q, \dot{q}, \ddot{q}$ 의 그래프를 살펴봅시다. 문제가 되었던 transient는 해결되었지만 여전히 $\ddot{q}$ 에서 transient가 존재합니다.

• 이를 해결하기 위해 다시 $\ddot{q}$ 에 대한 조건을 추가해보겠습니다.

• 이렇게 총 6가지 조건을 모두 만족하는 $q(t)$ 를 찾기 위해선, 5차 방정식을 도입해야 합니다. 5차 방정식 6개의 조건식이 사용되며, 이는 6 * 6 사이즈의 행렬을 푸는 문제가 되기 때문에 sympy를 사용하더라도 오랜 시간이 걸리게 됩니다.

💡 위 작업을 좀 더 편하게 공식화할 수는 없을까요? sympy의 diff, subs를 통해 구현하는 방법을 생각해보세요!

• 최종 결과의 그래프를 살펴볼까요? $q$에서의 transient, $\dot{q}$에서의 transient, $\ddot{q}$ 에서의 transient가 모두 해결된 모습이 보입니다.

😒 그런데… 또다시 3계 미분에서 transient가 보이는군요… 이를 해결하기 위해서는 7차 방정식 형태의 $q(t)$를 구해야 합니다. 하지만 계산식이 복잡해질 뿐, 지금까지 강의를 잘 따라오셨다면 해결 방법에 대해서는 모두 이해하셨으리라 생각합니다. 따라서 5계, 7계, 9계 미분을 지칭하는 용어만 정리하고 넘어가 보겠습니다.

Trajectory Generation Example

지금까지 배운 내용들을 바탕으로 실질적인 예시를 살펴보겠습니다.

• 전체 경로는 0 ~ 1초, 1 ~ 3초로 나뉘어 있으며 각 경로마다 4개씩의 조건을 갖는 상황입니다. 따라서, 3차 함수 두쌍의 계수 8개를 구해야 하는 문제로 정의할 수 있지요.

• 각 조건에 대한 수식을 정리한 뒤 8 * 8 크기를 갖는 행렬로 이를 정리해봅시다.

• 다음으로, sympy를 통해 이 수식을 구현하고, Ax = B 문제의 해를 구해냅니다.

• Numpy를 통해 0 ~ 1초, 1 ~ 3초에 대한 각각의 $q, \dot{q}, \ddot{q}$ 그래프를 구해냅니다.

• numpy의 결과를 그래프로 그려보았습니다. $\dot{q}$와 $\ddot{q}$를 살펴보면, t=1에서 transient가 발생한 것을 확인할 수 있지요!

• t=1 에서의 transient를 해결하기 위해 조건식을 아래와 같이 변경해봅시다.

• t=1에서의 속도와 가속도가 두 경로에서 같다는 조건을 추가해준 것입니다. 이에 따라 기존 $\theta_1(t=1), \theta_2(t=1)$ 였던 조건 대신, 아래의 새로운 조건을 적용합니다.

• 조건식의 변경에 따라 새롭게 정의된 Ax = B는 다음과 같습니다.

• numpy를 사용하여 다시 그래프를 그려보면, $\dot{q}$에서의 transient가 사라진 모습을 볼 수 있습니다! 만약 $\ddot{q}$에서의 transient를 제거하고 싶다면, 경로식의 차원을 높인 다음 일전 수행했던 작업을 반복 적용하면 될 것입니다. 이는 과제로 진행해 보겠습니다.

과제 - jerk를 사용하여 $\ddot{q}$ 을 최적화하기

앞선 예시의 $\ddot{q}(1)$을 보면 가해지는 토크가 갑자기 변화하는 것을 알 수 있습니다. 이는 곧 jerk의 transient를 의미하지요. 따라서 3계 미분에 대한 조건을 추가하여 아래와 같이 스무스한 q_dotdot을 구현해보는 과제를 제시드립니다.

jeck 조건이 추가되므로 q(t)는 4차 방정식이 될 것이며, 총 9개의 조건식을 사용하게 될 것입니다.

과제 2 - Velocity Upper Bound

• 예시 trajectory의 velocity를 살펴보면 0.5를 넘어 피크를 찍은 뒤 다시 감소하는 모습을 볼 수 있습니다. 하지만 실제 모터는 최대, 최소 속도를 갖기 때문에 이러한 동작이 불가할 수 있습니다.

• 이를 해결하기 위해 지난 시간 학습한 opt.minimize trajectory를 사용하여 최대 속도 제한을 만족하는 최적 경로를 구해봅시다!