Rotation Matrices

Inertia.ipynb

{
 "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
}

Rotation.ipynb