{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "thirty-surrey",
   "metadata": {
    "lines_to_next_cell": 0
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "plt.style.use('fivethirtyeight')"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "color-individual",
   "metadata": {},
   "source": [
    "# Building Quiz 07"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "still-party",
   "metadata": {},
   "source": [
    "![coordinates and path of bead](../images/bead-path.svg)\n",
    "\n",
    "A bead moves along a planar sinusoidal track. It does not rotate as it moves along the path, so the generalized coordinates are its x-position y-position, $$\\mathbf{q} = [x,~y]$$. There are two constraints on the motion of the bead, \n",
    "\n",
    "- $y = \\sin x$\n",
    "- $x = 10 t$, where $t$ is time in seconds. \n",
    "\n",
    "1. write the constraint equations, $\\mathbf{C}(\\mathbf{q},~t)$. \n",
    "\n",
    "2. write out the Jacobian of the constraints, $\\mathbf{C_q}$\n",
    "\n",
    "3. Invert the Jacobian to solve for $\\dot{x}~and~\\dot{y}$, using $\\mathbf{C_q \\dot{q}} = -\\mathbf{C}_t$\n",
    "\n",
    "## Numerical approach"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "positive-terror",
   "metadata": {},
   "outputs": [],
   "source": [
    "def C(q, t):\n",
    "    C = np.zeros(2)\n",
    "    C[0] = np.array([q[1] - np.sin(q[0])])\n",
    "    C[1] = np.array(q[0] - 10*t)\n",
    "    return C"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "durable-planet",
   "metadata": {},
   "outputs": [],
   "source": [
    "def Cq(q, t):\n",
    "    Cq = np.zeros((2, 2))\n",
    "    Cq[0, 0] = -np.cos(q[0])\n",
    "    Cq[0, 1] = 1\n",
    "    Cq[1, 0] = 1\n",
    "    return Cq"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "fiscal-disclaimer",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[-1.,  1.],\n",
       "       [ 1.,  0.]])"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Cq(np.array([0,0]), 1)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "crude-contents",
   "metadata": {},
   "source": [
    "## SymPy approach"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "coastal-penalty",
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "curious-horizontal",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}x\\\\y\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[x],\n",
       "[y]])"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sympy.var('x, y, t')\n",
    "q = sympy.Matrix([x, y])\n",
    "q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "id": "remarkable-disclosure",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}y - \\sin{\\left(x \\right)}\\\\- 10 t + x\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[y - sin(x)],\n",
       "[ -10*t + x]])"
      ]
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "C = sympy.Matrix([y - sympy.sin(x), x - 10*t])\n",
    "C"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "id": "dental-crisis",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}- \\cos{\\left(x \\right)} & 1\\\\1 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[-cos(x), 1],\n",
       "[      1, 0]])"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "C.jacobian(q)"
   ]
  }
 ],
 "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.9.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}