{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Body and Space Fixed Rotations"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sm\n",
    "import sympy.physics.mechanics as me\n",
    "me.init_vprinting()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "q1, q2, q3 = me.dynamicsymbols('q1:4')\n",
    "u1, u2, u3 = me.dynamicsymbols('u1:4')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "N = me.ReferenceFrame('N')\n",
    "A = me.ReferenceFrame('A')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "\u001b[0;31mSignature:\u001b[0m \u001b[0mA\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0morient\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mparent\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mrot_type\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mamounts\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mrot_order\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;34m''\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
       "\u001b[0;31mDocstring:\u001b[0m\n",
       "Defines the orientation of this frame relative to a parent frame.\n",
       "\n",
       "Parameters\n",
       "==========\n",
       "\n",
       "parent : ReferenceFrame\n",
       "    The frame that this ReferenceFrame will have its orientation matrix\n",
       "    defined in relation to.\n",
       "rot_type : str\n",
       "    The type of orientation matrix that is being created. Supported\n",
       "    types are 'Body', 'Space', 'Quaternion', 'Axis', and 'DCM'.\n",
       "    See examples for correct usage.\n",
       "amounts : list OR value\n",
       "    The quantities that the orientation matrix will be defined by.\n",
       "    In case of rot_type='DCM', value must be a\n",
       "    sympy.matrices.MatrixBase object (or subclasses of it).\n",
       "rot_order : str or int\n",
       "    If applicable, the order of a series of rotations.\n",
       "\n",
       "Examples\n",
       "========\n",
       "\n",
       ">>> from sympy.physics.vector import ReferenceFrame, Vector\n",
       ">>> from sympy import symbols, eye, ImmutableMatrix\n",
       ">>> q0, q1, q2, q3 = symbols('q0 q1 q2 q3')\n",
       ">>> N = ReferenceFrame('N')\n",
       ">>> B = ReferenceFrame('B')\n",
       "\n",
       "Now we have a choice of how to implement the orientation. First is\n",
       "Body. Body orientation takes this reference frame through three\n",
       "successive simple rotations. Acceptable rotation orders are of length\n",
       "3, expressed in XYZ or 123, and cannot have a rotation about about an\n",
       "axis twice in a row.\n",
       "\n",
       ">>> B.orient(N, 'Body', [q1, q2, q3], 123)\n",
       ">>> B.orient(N, 'Body', [q1, q2, 0], 'ZXZ')\n",
       ">>> B.orient(N, 'Body', [0, 0, 0], 'XYX')\n",
       "\n",
       "Next is Space. Space is like Body, but the rotations are applied in the\n",
       "opposite order.\n",
       "\n",
       ">>> B.orient(N, 'Space', [q1, q2, q3], '312')\n",
       "\n",
       "Next is Quaternion. This orients the new ReferenceFrame with\n",
       "Quaternions, defined as a finite rotation about lambda, a unit vector,\n",
       "by some amount theta.\n",
       "This orientation is described by four parameters:\n",
       "q0 = cos(theta/2)\n",
       "q1 = lambda_x sin(theta/2)\n",
       "q2 = lambda_y sin(theta/2)\n",
       "q3 = lambda_z sin(theta/2)\n",
       "Quaternion does not take in a rotation order.\n",
       "\n",
       ">>> B.orient(N, 'Quaternion', [q0, q1, q2, q3])\n",
       "\n",
       "Next is Axis. This is a rotation about an arbitrary, non-time-varying\n",
       "axis by some angle. The axis is supplied as a Vector. This is how\n",
       "simple rotations are defined.\n",
       "\n",
       ">>> B.orient(N, 'Axis', [q1, N.x + 2 * N.y])\n",
       "\n",
       "Last is DCM (Direction Cosine Matrix). This is a rotation matrix\n",
       "given manually.\n",
       "\n",
       ">>> B.orient(N, 'DCM', eye(3))\n",
       ">>> B.orient(N, 'DCM', ImmutableMatrix([[0, 1, 0], [0, 0, -1], [-1, 0, 0]]))\n",
       "\u001b[0;31mFile:\u001b[0m      /opt/conda/lib/python3.6/site-packages/sympy/physics/vector/frame.py\n",
       "\u001b[0;31mType:\u001b[0m      method\n"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "A.orient?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "A.orient(N, 'Body', (q1, q2, q3), 'XYZ')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\operatorname{cos}\\left(q_{2}\\right) \\operatorname{cos}\\left(q_{3}\\right) & \\operatorname{sin}\\left(q_{1}\\right) \\operatorname{sin}\\left(q_{2}\\right) \\operatorname{cos}\\left(q_{3}\\right) + \\operatorname{sin}\\left(q_{3}\\right) \\operatorname{cos}\\left(q_{1}\\right) & \\operatorname{sin}\\left(q_{1}\\right) \\operatorname{sin}\\left(q_{3}\\right) - \\operatorname{sin}\\left(q_{2}\\right) \\operatorname{cos}\\left(q_{1}\\right) \\operatorname{cos}\\left(q_{3}\\right)\\\\- \\operatorname{sin}\\left(q_{3}\\right) \\operatorname{cos}\\left(q_{2}\\right) & - \\operatorname{sin}\\left(q_{1}\\right) \\operatorname{sin}\\left(q_{2}\\right) \\operatorname{sin}\\left(q_{3}\\right) + \\operatorname{cos}\\left(q_{1}\\right) \\operatorname{cos}\\left(q_{3}\\right) & \\operatorname{sin}\\left(q_{1}\\right) \\operatorname{cos}\\left(q_{3}\\right) + \\operatorname{sin}\\left(q_{2}\\right) \\operatorname{sin}\\left(q_{3}\\right) \\operatorname{cos}\\left(q_{1}\\right)\\\\\\operatorname{sin}\\left(q_{2}\\right) & - \\operatorname{sin}\\left(q_{1}\\right) \\operatorname{cos}\\left(q_{2}\\right) & \\operatorname{cos}\\left(q_{1}\\right) \\operatorname{cos}\\left(q_{2}\\right)\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡cos(q₂)⋅cos(q₃)   sin(q₁)⋅sin(q₂)⋅cos(q₃) + sin(q₃)⋅cos(q₁)   sin(q₁)⋅sin(q₃)\n",
       "⎢                                                                             \n",
       "⎢-sin(q₃)⋅cos(q₂)  -sin(q₁)⋅sin(q₂)⋅sin(q₃) + cos(q₁)⋅cos(q₃)  sin(q₁)⋅cos(q₃)\n",
       "⎢                                                                             \n",
       "⎣    sin(q₂)                    -sin(q₁)⋅cos(q₂)                            co\n",
       "\n",
       " - sin(q₂)⋅cos(q₁)⋅cos(q₃)⎤\n",
       "                          ⎥\n",
       " + sin(q₂)⋅sin(q₃)⋅cos(q₁)⎥\n",
       "                          ⎥\n",
       "s(q₁)⋅cos(q₂)             ⎦"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A.dcm(N)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle (\\operatorname{sin}\\left(q_{3}\\right) \\dot{q}_{2} + \\operatorname{cos}\\left(q_{2}\\right) \\operatorname{cos}\\left(q_{3}\\right) \\dot{q}_{1})\\mathbf{\\hat{a}_x} + (- \\operatorname{sin}\\left(q_{3}\\right) \\operatorname{cos}\\left(q_{2}\\right) \\dot{q}_{1} + \\operatorname{cos}\\left(q_{3}\\right) \\dot{q}_{2})\\mathbf{\\hat{a}_y} + (\\operatorname{sin}\\left(q_{2}\\right) \\dot{q}_{1} + \\dot{q}_{3})\\mathbf{\\hat{a}_z}$"
      ],
      "text/plain": [
       "(sin(q₃)⋅q₂̇ + cos(q₂)⋅cos(q₃)⋅q₁̇) a_x + (-sin(q₃)⋅cos(q₂)⋅q₁̇ + cos(q₃)⋅q₂̇)\n",
       " a_y + (sin(q₂)⋅q₁̇ + q₃̇) a_z"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A.ang_vel_in(N)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left[ u_{1} = \\operatorname{sin}\\left(q_{3}\\right) \\dot{q}_{2} + \\operatorname{cos}\\left(q_{2}\\right) \\operatorname{cos}\\left(q_{3}\\right) \\dot{q}_{1}, \\  u_{2} = - \\operatorname{sin}\\left(q_{3}\\right) \\operatorname{cos}\\left(q_{2}\\right) \\dot{q}_{1} + \\operatorname{cos}\\left(q_{3}\\right) \\dot{q}_{2}, \\  u_{3} = \\operatorname{sin}\\left(q_{2}\\right) \\dot{q}_{1} + \\dot{q}_{3}\\right]$"
      ],
      "text/plain": [
       "[u₁ = sin(q₃)⋅q₂̇ + cos(q₂)⋅cos(q₃)⋅q₁̇, u₂ = -sin(q₃)⋅cos(q₂)⋅q₁̇ + cos(q₃)⋅q\n",
       "₂̇, u₃ = sin(q₂)⋅q₁̇ + q₃̇]"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "ueqs = [sm.Eq(u1, A.ang_vel_in(N).dot(A.x)),\n",
    "        sm.Eq(u2, A.ang_vel_in(N).dot(A.y)),\n",
    "        sm.Eq(u3, A.ang_vel_in(N).dot(A.z))]\n",
    "ueqs"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Note that with specific values of some q's there is a divide by zero, i.e. gimbal lock"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left\\{ \\dot{q}_{1} : \\frac{u_{1} \\operatorname{cos}\\left(q_{3}\\right) - u_{2} \\operatorname{sin}\\left(q_{3}\\right)}{\\operatorname{cos}\\left(q_{2}\\right)}, \\  \\dot{q}_{2} : u_{1} \\operatorname{sin}\\left(q_{3}\\right) + u_{2} \\operatorname{cos}\\left(q_{3}\\right), \\  \\dot{q}_{3} : - u_{1} \\operatorname{cos}\\left(q_{3}\\right) \\operatorname{tan}\\left(q_{2}\\right) + u_{2} \\operatorname{sin}\\left(q_{3}\\right) \\operatorname{tan}\\left(q_{2}\\right) + u_{3}\\right\\}$"
      ],
      "text/plain": [
       "⎧     u₁⋅cos(q₃) - u₂⋅sin(q₃)                                                 \n",
       "⎨q₁̇: ───────────────────────, q₂̇: u₁⋅sin(q₃) + u₂⋅cos(q₃), q₃̇: -u₁⋅cos(q₃)⋅\n",
       "⎩             cos(q₂)                                                         \n",
       "\n",
       "                                 ⎫\n",
       "tan(q₂) + u₂⋅sin(q₃)⋅tan(q₂) + u₃⎬\n",
       "                                 ⎭"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sm.solve(ueqs, q1.diff(), q2.diff(), q3.diff())"
   ]
  }
 ],
 "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.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}