파이썬 변수 행렬 연산 ( Symbolic 계산)

종강을 앞두고 그동안 쓴 레포트를 제출하기 전에 점검하면서 생각난 김에 적어봅니다.

뜬금없이 과제로 로봇공학쪽 내용인 자유도6로봇팔의 제어의 Forward Kinetics와 Inverse Kinetics를 계산해야했다.

Forward Kinetics를 계산하려면 회전행렬을 6개를 곱해줘야 하는데 이것을 전부 변수로 계산하려니 머리가 띵해져서 파이썬의 힘을 빌리기 위해 조사했다.

from sympy import *
var('s1 s2 s3 s4 s5 s6 c1 c2 c3 c4 c5 c6 a2 a3 a4 d4')
T1=Matrix([[c1, -s1, 0, 0],[s1, c1, 0, 0],[0, 0, 1, 0],[0, 0, 0, 1]])
T2=Matrix([[s2, c2, 0, a2],[0, 0, 1, 0],[c2, -s2, 0, 0],[0, 0, 0, 1]])
T3=Matrix([[c3, -s3, 0, a3],[s3, c3, 0, 0],[0, 0, 1, 0],[0, 0, 0, 1]])
T4=Matrix([[c4, -s4, 0, a4],[0, 0, 1, d4],[-s4, -c4, 0, 0],[0, 0, 0, 1]])
T5=Matrix([[-s5, -c5, 0, 0],[0, 0, -1, 0],[c5, -s5, 0, 0],[0, 0, 0, 1]])
T6=Matrix([[c6, -s6, 0, 0],[0, 0, 1, 0],[-s6, -c6, 0, 0],[0, 0, 0, 1]])

Tf=T1.multiply(T2).multiply(T3).multiply(T4).multiply(T5).multiply(T6)
print(Tf)

먼저 sympy라는 모듈을 불러온다.
그리고 var 안에 자신이 변수로 쓸 문자들을 미리 선언해준다.
다음 그 변수와 상수를 사용하여 Matrix를 선언하고 곱해준다.

Matrix([[c6*(c5*(c1*c2*c3 - c1*s2*s3) - s5*(c4*(c1*c2*s3 + c1*c3*s2) + s1*s4)) - s6*(-c4*s1 + s4*(c1*c2*s3 + c1*c3*s2)), -c6*(-c4*s1 + s4*(c1*c2*s3 + c1*c3*s2)) - s6*(c5*(c1*c2*c3 - c1*s2*s3) - s5*(c4*(c1*c2*s3 + c1*c3*s2) + s1*s4)), -c5*(c4*(c1*c2*s3 + c1*c3*s2) + s1*s4) - s5*(c1*c2*c3 - c1*s2*s3), a2*c1 + a3*c1*s2 + a4*(c1*c2*s3 + c1*c3*s2) + d4*(c1*c2*c3 - c1*s2*s3)], [c6*(c5*(c2*c3*s1 - s1*s2*s3) - s5*(-c1*s4 + c4*(c2*s1*s3 + c3*s1*s2))) - s6*(c1*c4 + s4*(c2*s1*s3 + c3*s1*s2)), -c6*(c1*c4 + s4*(c2*s1*s3 + c3*s1*s2)) - s6*(c5*(c2*c3*s1 - s1*s2*s3) - s5*(-c1*s4 + c4*(c2*s1*s3 + c3*s1*s2))), -c5*(-c1*s4 + c4*(c2*s1*s3 + c3*s1*s2)) - s5*(c2*c3*s1 - s1*s2*s3), a2*s1 + a3*s1*s2 + a4*(c2*s1*s3 + c3*s1*s2) + d4*(c2*c3*s1 - s1*s2*s3)], [c6*(-c4*s5*(c2*c3 - s2*s3) + c5*(-c2*s3 - c3*s2)) - s4*s6*(c2*c3 - s2*s3), -c6*s4*(c2*c3 - s2*s3) - s6*(-c4*s5*(c2*c3 - s2*s3) + c5*(-c2*s3 - c3*s2)), -c4*c5*(c2*c3 - s2*s3) - s5*(-c2*s3 - c3*s2), a3*c2 + a4*(c2*c3 - s2*s3) + d4*(-c2*s3 - c3*s2)], [0, 0, 0, 1]])

위와 같은 결과를 얻을 수 있다.