Last active
September 25, 2018 16:24
-
-
Save iamareebjamal/6f06c1afe797d3fac6bef3471eb69f3e to your computer and use it in GitHub Desktop.
Rotation Matrices
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
{ | |
"cells": [ | |
{ | |
"cell_type": "code", | |
"execution_count": 1, | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"from sympy import *\n", | |
"init_printing()" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 2, | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"x, y, z, t = symbols('x y z t')" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 3, | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"theta1 = Function('O1')(t)\n", | |
"theta2 = Function('O2')(t)\n", | |
"theta3 = Function('O3')(t)" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 4, | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"S1, S2, S3 = sin(theta1), sin(theta2), sin(theta3)\n", | |
"C1, C2, C3 = cos(theta1), cos(theta2), cos(theta3)" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 5, | |
"metadata": {}, | |
"outputs": [ | |
{ | |
"data": { | |
"text/latex": [ | |
"$$\\left ( \\left[\\begin{matrix}\\cos{\\left (\\operatorname{O_{1}}{\\left (t \\right )} \\right )} & - \\sin{\\left (\\operatorname{O_{1}}{\\left (t \\right )} \\right )} & 0\\\\\\sin{\\left (\\operatorname{O_{1}}{\\left (t \\right )} \\right )} & \\cos{\\left (\\operatorname{O_{1}}{\\left (t \\right )} \\right )} & 0\\\\0 & 0 & 1\\end{matrix}\\right], \\quad \\left[\\begin{matrix}1 & 0 & 0\\\\0 & \\cos{\\left (\\operatorname{O_{2}}{\\left (t \\right )} \\right )} & - \\sin{\\left (\\operatorname{O_{2}}{\\left (t \\right )} \\right )}\\\\0 & \\sin{\\left (\\operatorname{O_{2}}{\\left (t \\right )} \\right )} & \\cos{\\left (\\operatorname{O_{2}}{\\left (t \\right )} \\right )}\\end{matrix}\\right], \\quad \\left[\\begin{matrix}\\cos{\\left (\\operatorname{O_{3}}{\\left (t \\right )} \\right )} & 0 & \\sin{\\left (\\operatorname{O_{3}}{\\left (t \\right )} \\right )}\\\\0 & 1 & 0\\\\- \\sin{\\left (\\operatorname{O_{3}}{\\left (t \\right )} \\right )} & 0 & \\cos{\\left (\\operatorname{O_{3}}{\\left (t \\right )} \\right )}\\end{matrix}\\right]\\right )$$" | |
], | |
"text/plain": [ | |
"⎛⎡cos(O₁(t)) -sin(O₁(t)) 0⎤ ⎡1 0 0 ⎤ ⎡cos(O₃(t)) 0 \n", | |
"⎜⎢ ⎥ ⎢ ⎥ ⎢ \n", | |
"⎜⎢sin(O₁(t)) cos(O₁(t)) 0⎥, ⎢0 cos(O₂(t)) -sin(O₂(t))⎥, ⎢ 0 1 \n", | |
"⎜⎢ ⎥ ⎢ ⎥ ⎢ \n", | |
"⎝⎣ 0 0 1⎦ ⎣0 sin(O₂(t)) cos(O₂(t)) ⎦ ⎣-sin(O₃(t)) 0 \n", | |
"\n", | |
"sin(O₃(t))⎤⎞\n", | |
" ⎥⎟\n", | |
" 0 ⎥⎟\n", | |
" ⎥⎟\n", | |
"cos(O₃(t))⎦⎠" | |
] | |
}, | |
"execution_count": 5, | |
"metadata": {}, | |
"output_type": "execute_result" | |
} | |
], | |
"source": [ | |
"M1 = Matrix([[C1, -S1, 0], [S1, C1, 0], [0, 0, 1]])\n", | |
"M2 = Matrix([[1, 0, 0], [0, C2, -S2], [0, S2, C2]])\n", | |
"M3 = Matrix([[C3, 0, S3], [0, 1, 0], [-S3, 0, C3]])\n", | |
"M1, M2, M3" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 6, | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"R = M1*M2*M3" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 7, | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"x = Function('x')(t)\n", | |
"y = Function('y')(t)\n", | |
"z = Function('z')(t)" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 8, | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"q = Matrix([[theta1, theta2, theta3, x, y, z]]).T" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 9, | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"m, r = symbols('m r')\n", | |
"Idash = 1/4*m*r**2*(Matrix([[1, 0, 0], [0, 2, 0], [0, 0, 1]]))" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 10, | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"I = diag(m, m, m)" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 11, | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"I1 = simplify(R*Idash*R.T)" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 12, | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"Z = zeros(3, 3)\n", | |
"M = I1.row_join(Z).col_join(Z.row_join(I))" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 13, | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"C = zeros(6, 6)\n", | |
"\n", | |
"for i in range(6):\n", | |
" for j in range(6):\n", | |
" result = 0\n", | |
" for k in range(6):\n", | |
" result += (diff(M[i, j], q[k]) + diff(M[i, k], q[j]) + diff(M[k, j], q[k])) * diff(q[k], t)\n", | |
" C[i, j] = result/2\n", | |
"\n", | |
"C = simplify(C)" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 14, | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"g = symbols('g')\n", | |
"PE = m*g*z\n", | |
"G = zeros(6, 1)\n", | |
"\n", | |
"for i in range(6):\n", | |
" G[i] = diff(PE, q[i])" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 15, | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"psi = Matrix([\n", | |
" [r*C1*S2, r*C2*S1, r*C1, -1, 0, 0],\n", | |
" [r*S1*S2, -r*C1*C2, r*S1, 0, -1, 0]\n", | |
"])" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 16, | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"#lambdaa = -(psi*M.inv()*psi.T)*(diff(psi, t)*diff(q, t) + psi*M.inv()*(-C * diff(q, t) - G))" | |
] | |
} | |
], | |
"metadata": { | |
"kernelspec": { | |
"display_name": "Python 3", | |
"language": "python", | |
"name": "python3" | |
}, | |
"language_info": { | |
"codemirror_mode": { | |
"name": "ipython", | |
"version": 3 | |
}, | |
"file_extension": ".py", | |
"mimetype": "text/x-python", | |
"name": "python", | |
"nbconvert_exporter": "python", | |
"pygments_lexer": "ipython3", | |
"version": "3.6.4" | |
} | |
}, | |
"nbformat": 4, | |
"nbformat_minor": 2 | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment