이번 시간에는 3차원 물체의 움짐임을 모델링해보겠습니다. 중력이 작용하는 3차원 state에서 직육면체의 자유 낙하 운동을 모델링해볼 것입니다. (결국 지금까지 반복했던 운동방정식 계산을 3차원에서 세워보는 것입니다.)

• 물체가 갖는 state는 총 6개 (x,y,z / $\phi, \theta, \psi$)가 되며, 각도 체계는 3-2-1 euler angle을 사용하게 됩니다.

운동에너지와 위치에너지를 구해 Euler Lagrange Equation을 계산해야 하는데요. 2차원과 달리 회전에 대해 고려할 것이 추가됩니다.

• 회전 운동 에너지를 구하기 위해선 물체의 질량 관성모멘트가 필요하며, 3차원이 되면서 질량관성모멘트는 3 * 3의 행렬이 됩니다. 그런데 물체가 회전하면 world frame에서의 질량 관성모멘트가 변화하게 되어 계산이 매우 복잡해지기 때문에 회전 운동에너지는 body frame을 기준으로 구하게 됩니다.

• 이제 병진 운동에너지와 회전 운동에너지를 구하고 위치에너지도 구해주어 EL equation을 완성시켜봅시다.

• state가 6개로 늘어나면서 M, G, C Matrix는 모두 6개의 column을 갖게 됩니다. 이를 직접 미분 계산하는 것은 매우 힘들기 때문에, 이번에도 우리는 sympy를 통해 편하게 계산 결과만 사용하도록 하겠습니다.

코드 구현

코드 구현 자체는 간단합니다. sympy와 시뮬레이션 파트로 나뉘어 구현하겠으며 각각의 코드는 다음과 같은 작업을 수행합니다.

코드 1) sympy를 통해 운동방정식 계산

코드 2) odeint를 사용하여 animation 및 plot 시뮬레이션

• sympy 코드부터 분석해봅시다. EL equation을 통해 운동방정식을 계산하기 위해 필요한 요소들을 미리 구해둡니다.

• 3D space에서 주의해야 할 점인 회전 운동에너지 계산을 위해 world frame과 body frame 각속도도 미리 계산합니다.

• 이제 병진 운동에너지와 회전 운동에너지를 계산하며, 이때 사용되는 각속도는 body frame 기준임을, 그리고 사용하는 질량 관성모멘트 또한 body frame 기준임을 명심합니다!

• EL equation을 계산하는 부분입니다. state가 6개로 확장되면서 6번의 for loop가 실행될 것입니다.

• odeint 계산을 위해 state space martix A와 vector B를 추출합니다.

• 마지막으로 world frame, body frame angular velocity 계산을 위한 Matrix 2개를 추출합니다. 이미 앞선 수식에서 body frame angular velocity를 계산한 바 있지만, 시뮬레이션을 통해 world frame과 body frame의 angular velocity 차이를 시각화할 것이기 때문에 미리 sympy를 사용하여 계산해두겠습니다.

이제 코드 실행 결과로 얻어지는 각종 행렬과 수식들을 사용하여 시뮬레이션 코드를 작성해봅시다.

• 자유낙하 운동을 하는 cube를 시뮬레이션할 것이며, cube의 질량 관성모멘트를 구하는 공식에 따라 xyz Inertia를 미리 매개변수화 하였습니다.

image from : https://m.blog.naver.com/saytoloveu/221402425567

• odeint의 input이 되는 dynamics 함수 free_falling_eom를 구현하고, 해당 함수 내에서 앞서 구현한 Matrix A, vector B를 사용하여 다음 state를 도출합니다.

• main 함수에서 구현된 시뮬레이션은 단순히 odeint를 반복 실행하며 state들을 누적하는 것입니다. 코드의 마지막 부분을 살펴보면, 시스템의 운동, 위치 에너지와 body frame, world frame 각속도를 계산하는 부분을 확인할 수 있으며 이는 그래프를 통해 시각화됩니다.

위 결과를 보면 알 수 있듯이

• 전체 시스템의 에너지는 보존되는 상황임을 알 수 있으며,
• world frame / body frame에서의 각속도는 다른 값을 갖습니다.