3d_no_variable_v.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy.physics.vector import (\n",
    "    ReferenceFrame,\n",
    "    dynamicsymbols,\n",
    "    get_motion_params\n",
    ")\n",
    "from sympy import symbols, Piecewise, Eq, solveset, Min, Max, latex, acos, Matrix, eye\n",
    "from sympy.utilities.lambdify import lambdify\n",
    "\n",
    "from IPython.display import Markdown, display\n",
    "import ipywidgets as widgets\n",
    "from ipywidgets import interactive_output\n",
    "\n",
    "import numpy as np\n",
    "import plotly.graph_objects as go"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Reference frame"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "N = ReferenceFrame('N')\n",
    "A = ReferenceFrame('A')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "jupyter": {
     "source_hidden": true
    }
   },
   "outputs": [
    {
     "data": {
      "text/markdown": [
       "\n",
       "$$N = \\{ \\mathbf{\\hat{n}_x}, \\mathbf{\\hat{n}_y}, \\mathbf{\\hat{n}_z} \\}$$\n",
       "$$A = \\{ \\mathbf{\\hat{a}_x}, \\mathbf{\\hat{a}_y}, \\mathbf{\\hat{a}_z} \\}$$\n"
      ],
      "text/plain": [
       "<IPython.core.display.Markdown object>"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Markdown(rf\"\"\"\n",
    "$$N = \\{{ {latex(N.x)}, {latex(N.y)}, {latex(N.z)} \\}}$$\n",
    "$$A = \\{{ {latex(A.x)}, {latex(A.y)}, {latex(A.z)} \\}}$$\n",
    "\"\"\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Reference variables"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "T = symbols('T', nonnegative=True)\n",
    "R = symbols('R', nonnegative=True)\n",
    "t = symbols('t', nonnegative=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "a_magnitude = symbols('|a|', nonnegative=True)\n",
    "a_vector = A.x * Piecewise((-a_magnitude, T * R - dynamicsymbols._t < 0), (a_magnitude, True))\n",
    "# a_vector = A.x * a_magnitude * (T * R - dynamicsymbols._t) / Abs(T * R - dynamicsymbols._t)\n",
    "\n",
    " # Since we have no initial velocity, acceleration and velocity will be on the same axis, so velocity can be defined in A Frame\n",
    "v0_magnitude = symbols('|v_{0}|', nonnegative=True)\n",
    "vT_magnitude = symbols('|v_{T}|')\n",
    "v0_vector = A.x * v0_magnitude\n",
    "vT_vector = A.x * vT_magnitude\n",
    "\n",
    "p0_x, p0_y, p0_z = symbols('p_{0x} p_{0y} p_{0z}')\n",
    "pT_x, pT_y, pT_z = symbols('p_{Tx} p_{Ty} p_{Tz}')\n",
    "p0_vector = N.x * p0_x + N.y * p0_y + N.z * p0_z\n",
    "pT_vector = N.x * pT_x + N.y * pT_y + N.z * pT_z\n",
    "\n",
    "direction_vector = pT_vector - p0_vector"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "Nx = N.x.to_matrix(N)\n",
    "NA1 = direction_vector.normalize().to_matrix(N)\n",
    "\n",
    "v = Nx.cross(NA1)\n",
    "c = Nx.dot(NA1) # cosine of angle\n",
    "K = Matrix(3, 3, [\n",
    "    0, - v[2], v[1],\n",
    "    v[2], 0, - v[0],\n",
    "    - v[1], v[0], 0\n",
    "])\n",
    "rotation_matrix = eye(3) + K + (K * K) * (1 - c) / (1 - c ** 2)\n",
    "\n",
    "NA2 = (rotation_matrix * N.y.to_matrix(N)).simplify()\n",
    "\n",
    "dcm = Matrix(3, 3, [*NA1, *NA2, *NA1.cross(NA2)])\n",
    "\n",
    "A.orient(N, 'DCM', dcm.transpose())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/markdown": [
       "\n",
       "Direction vector in N: $\\left[\\begin{matrix}- p_{0x} + p_{Tx}\\\\- p_{0y} + p_{Ty}\\\\- p_{0z} + p_{Tz}\\end{matrix}\\right]$\n",
       "\n",
       "Direction vector in A: $\\left[\\begin{matrix}\\sqrt{\\left(p_{0x} - p_{Tx}\\right)^{2} + \\left(p_{0y} - p_{Ty}\\right)^{2} + \\left(p_{0z} - p_{Tz}\\right)^{2}}\\\\0\\\\0\\end{matrix}\\right]$\n"
      ],
      "text/plain": [
       "<IPython.core.display.Markdown object>"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Markdown(rf\"\"\"\n",
    "Direction vector in N: ${latex(direction_vector.to_matrix(N).simplify())}$\n",
    "\n",
    "Direction vector in A: ${latex(direction_vector.to_matrix(A).simplify())}$\n",
    "\"\"\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/markdown": [
       "\n",
       "$$\n",
       "\\begin{align*}\n",
       "|a| & \\quad & m.s^{-2} \\quad & \\text{ Acceleration magnitude} \\\\\n",
       "v_0 &= |v_{0}|\\mathbf{\\hat{a}_x} \\quad & m.s^{-1} \\quad & \\text{ Velocity at } t_0 \\\\\n",
       "v_T &= |v_{T}|\\mathbf{\\hat{a}_x} \\quad & m.s^{-1} \\quad & \\text{ Velocity at destination} \\\\\n",
       "p_0 &= p_{0x}\\mathbf{\\hat{n}_x} + p_{0y}\\mathbf{\\hat{n}_y} + p_{0z}\\mathbf{\\hat{n}_z} \\quad & m \\quad & \\text{ Position at } t_0 \\\\\n",
       "p_T &= p_{Tx}\\mathbf{\\hat{n}_x} + p_{Ty}\\mathbf{\\hat{n}_y} + p_{Tz}\\mathbf{\\hat{n}_z} \\quad & m \\quad & \\text{ Position at destination} \\\\\n",
       "direction &= (- p_{0x} + p_{Tx})\\mathbf{\\hat{n}_x} + (- p_{0y} + p_{Ty})\\mathbf{\\hat{n}_y} + (- p_{0z} + p_{Tz})\\mathbf{\\hat{n}_z} \\quad & m \\quad & \\text{ Direction vector }\n",
       "\\end{align*}\n",
       "$$\n"
      ],
      "text/plain": [
       "<IPython.core.display.Markdown object>"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Markdown(rf\"\"\"\n",
    "$$\n",
    "\\begin{{align*}}\n",
    "|a| & \\quad & m.s^{{-2}} \\quad & \\text{{ Acceleration magnitude}} \\\\\n",
    "v_0 &= {latex(v0_vector)} \\quad & m.s^{{-1}} \\quad & \\text{{ Velocity at }} t_0 \\\\\n",
    "v_T &= {latex(vT_vector)} \\quad & m.s^{{-1}} \\quad & \\text{{ Velocity at destination}} \\\\\n",
    "p_0 &= {latex(p0_vector)} \\quad & m \\quad & \\text{{ Position at }} t_0 \\\\\n",
    "p_T &= {latex(pT_vector)} \\quad & m \\quad & \\text{{ Position at destination}} \\\\\n",
    "direction &= {latex(direction_vector)} \\quad & m \\quad & \\text{{ Direction vector }}\n",
    "\\end{{align*}}\n",
    "$$\n",
    "\"\"\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Motion parameters, with constant acceleration  𝑎"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "def replace_max_0_RT(f):\n",
    "    # Given that T >= 0 and R in [0, 1]:\n",
    "    #  We can say that max(0, R * T) = R * T\n",
    "    #  However sympy is not able to infer this, so we replace manually\n",
    "    return f.replace(Max, lambda a, b: (b if a == 0 else a_))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "original_a_fn, original_v_fn, original_d_fn = get_motion_params(\n",
    "    A,\n",
    "    acceleration=a_vector, \n",
    "    velocity=v0_vector,\n",
    "    position=p0_vector\n",
    ")\n",
    "\n",
    "a_fn = original_a_fn.subs(dynamicsymbols._t, t)\n",
    "v_fn = original_v_fn.subs(dynamicsymbols._t, t)\n",
    "d_fn = original_d_fn.subs(dynamicsymbols._t, t).applyfunc(replace_max_0_RT)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "jupyter": {
     "source_hidden": true
    }
   },
   "outputs": [
    {
     "data": {
      "text/markdown": [
       "\n",
       "These formulae depend on:\n",
       "* $T$ (in seconds): The total time to arrival\n",
       "* $R \\in [0, 1]$: The ratio at which we start decelerating\n",
       "\n",
       "$$\n",
       "\\begin{align}\n",
       "a(t) &= \\begin{cases} - |a| & \\text{for}\\: R T - t < 0 \\\\|a| & \\text{otherwise} \\end{cases}\\mathbf{\\hat{a}_x} \\\\\n",
       "v(t) &= (- t |a| + 2 |a| \\min\\left(t, R T\\right) + |v_{0}|)\\mathbf{\\hat{a}_x} \\\\\n",
       "d(t) &= (- \\frac{t^{2} |a|}{2} + t \\left(2 R T |a| + |v_{0}|\\right) + |a| \\min\\left(t, R T\\right)^{2} + |v_{0}| \\min\\left(t, R T\\right) - \\left(2 R T |a| + |v_{0}|\\right) \\min\\left(t, R T\\right))\\mathbf{\\hat{a}_x} + p_{0x}\\mathbf{\\hat{n}_x} + p_{0y}\\mathbf{\\hat{n}_y} + p_{0z}\\mathbf{\\hat{n}_z}\n",
       "\\end{align}\n",
       "$$\n",
       "\n",
       "**Remark**: $d(t)$ is rewritten here as a combination of A and N for brevity\n"
      ],
      "text/plain": [
       "<IPython.core.display.Markdown object>"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Markdown(rf\"\"\"\n",
    "These formulae depend on:\n",
    "* $T$ (in seconds): The total time to arrival\n",
    "* $R \\in [0, 1]$: The ratio at which we start decelerating\n",
    "\n",
    "$$\n",
    "\\begin{{align}}\n",
    "a(t) &= {latex(a_fn)} \\\\\n",
    "v(t) &= {latex(v_fn)} \\\\\n",
    "d(t) &= {latex(d_fn.subs({p0_x: 0, p0_y: 0, p0_z: 0}) + p0_vector)}\n",
    "\\end{{align}}\n",
    "$$\n",
    "\n",
    "**Remark**: $d(t)$ is rewritten here as a combination of A and N for brevity\n",
    "\"\"\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Finding R"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "def replace_min_T(f):\n",
    "    # Given that T >= 0 and R in [0, 1]:\n",
    "    #  We can say that min(t, R * T) = R * T\n",
    "    #  However sympy is not able to infer this, so we replace manually\n",
    "    return f.replace(Min(T, R*T), R*T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "eq_vT_x = Eq(\n",
    "    vT_vector.to_matrix(A)[0],\n",
    "    v_fn.subs(t, T).applyfunc(replace_min_T).to_matrix(A)[0]\n",
    ")\n",
    "\n",
    "solved_R = list(solveset(eq_vT_x, R))[0] # The solveset returns a singleton, which is also a list of size 1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {
    "jupyter": {
     "source_hidden": true
    }
   },
   "outputs": [
    {
     "data": {
      "text/markdown": [
       "\n",
       "$R \\in [0, 1]$: The ratio at which we start decelerating\n",
       "\n",
       "$$\n",
       "\\begin{align}\n",
       "v_T &= v(T) \\\\\n",
       "|v_{T}| &= - T |a| + 2 |a| \\min\\left(T, R T\\right) + |v_{0}| \\\\\n",
       "\\end{align}\n",
       "$$\n",
       "\n",
       "This solves into:\n",
       "\n",
       "$$R = \\frac{T |a| - |v_{0}| + |v_{T}|}{2 T |a|}$$\n"
      ],
      "text/plain": [
       "<IPython.core.display.Markdown object>"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Markdown(rf\"\"\"\n",
    "$R \\in [0, 1]$: The ratio at which we start decelerating\n",
    "\n",
    "$$\n",
    "\\begin{{align}}\n",
    "v_T &= v(T) \\\\\n",
    "{latex(vT_vector.to_matrix(A)[0])} &= {latex(v_fn.subs(t, T).to_matrix(A)[0])} \\\\\n",
    "\\end{{align}}\n",
    "$$\n",
    "\n",
    "This solves into:\n",
    "\n",
    "$$R = {latex(solved_R)}$$\n",
    "\"\"\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Finding T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "eq_dT_x = Eq(\n",
    "    pT_vector.to_matrix(A).simplify()[0],\n",
    "    d_fn.subs({t: T}).applyfunc(replace_min_T).subs({R: solved_R}).to_matrix(A)[0]\n",
    ")\n",
    "\n",
    "# eq_dT_x solves as a quadratic expression, thus returns a finite set of two values.\n",
    "solved_T1, solved_T2 = solveset(eq_dT_x, T)\n",
    "solved_T = Max(solved_T1, solved_T2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "jupyter": {
     "source_hidden": true
    }
   },
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "319597a1995848c6ac0a86f9626c7730",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Checkbox(value=False, description='Display T', indent=False)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "1f7bdba837d04ff39cf2de2720a15f9d",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Output()"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "show_toggle = widgets.Checkbox(\n",
    "    value=False,\n",
    "    description='Display T',\n",
    "    disabled=False,\n",
    "    indent=False\n",
    ")\n",
    "\n",
    "out = widgets.Output(layout=widgets.Layout())\n",
    "\n",
    "md_content = Markdown(rf\"\"\"\n",
    "$T$ (in seconds): The total time to arrival\n",
    "\n",
    "$$\n",
    "\\begin{{align}}\n",
    "p_T &= p(T) \\\\\n",
    "{latex(pT_vector.to_matrix(A)[0])} &= {latex(d_fn.subs({t: T}).to_matrix(A)[0])} \\\\\n",
    "\\\\\n",
    "\\text{{Given that }} R &= {latex(solved_R)} \\\\\n",
    "{latex(pT_vector.to_matrix(A)[0])} &= {latex(d_fn.subs({t: T, R: solved_R}).to_matrix(A)[0])} \\\\\n",
    "\\end{{align}}\n",
    "$$\n",
    "\n",
    "This solves into:\n",
    "\n",
    "$$T = {latex(solved_T)}$$\n",
    "\"\"\")\n",
    "\n",
    "def display_T(displayed):\n",
    "    with out:\n",
    "        if displayed['new']:\n",
    "            display(md_content)\n",
    "        else:\n",
    "            out.clear_output()\n",
    "        \n",
    "show_toggle.observe(display_T, 'value')\n",
    "\n",
    "display(show_toggle)\n",
    "display(out)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Motion parameters, expand equations"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "expanded_a_fn = a_fn.subs({T: solved_T, R: solved_R})\n",
    "expanded_v_fn_2 = v_fn.subs({T: solved_T, R: solved_R})\n",
    "expanded_d_fn_2 = d_fn.subs({T: solved_T, R: solved_R})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {
    "jupyter": {
     "source_hidden": true
    }
   },
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "1b1ce3acfef44e1f83850b5502ff05e2",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Checkbox(value=False, description='Display expanded motion parameters', indent=False)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "a1dd4f69face4c9ab82c639e4d500896",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Output()"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "show_toggle = widgets.Checkbox(\n",
    "    value=False,\n",
    "    description='Display expanded motion parameters',\n",
    "    disabled=False,\n",
    "    indent=False\n",
    ")\n",
    "\n",
    "out = widgets.Output(layout=widgets.Layout())\n",
    "\n",
    "md_content = Markdown(rf\"\"\"\n",
    "After injecting $T$ and $R$, we obtain:\n",
    "\n",
    "$$\n",
    "\\begin{{align}}\n",
    "a(t) &= {latex(expanded_a_fn)} \\\\\n",
    "v(t) &= {latex(expanded_v_fn_2)} \\\\\n",
    "d(t) &= {latex(expanded_d_fn_2)}\n",
    "\\end{{align}}\n",
    "$$\n",
    "\"\"\")\n",
    "\n",
    "def display_motion_parameters(displayed):\n",
    "    with out:\n",
    "        if displayed['new']:\n",
    "            display(md_content)\n",
    "        else:\n",
    "            out.clear_output()\n",
    "        \n",
    "show_toggle.observe(display_motion_parameters, 'value')\n",
    "\n",
    "display(show_toggle)\n",
    "display(out)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Plotting"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "73ac66102ae843cb9a6756a04a437c39",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "VBox(children=(HBox(children=(BoundedFloatText(value=1.0, description='Acceleration (m.s-2)', max=15.0, min=0.…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "a_knob = widgets.BoundedFloatText(value=1, min=0.1, max=15.0, step=0.1, description='Acceleration (m.s-2)')\n",
    "v0_knob = widgets.IntText(value=0, step=10, description='v0 (m.s-1)')\n",
    "vT_knob = widgets.IntText(value=0, step=10, description='vT (m.s-1)')\n",
    "\n",
    "p0x_knob = widgets.IntText(value=0, step=10, description='p0.x (m)')\n",
    "p0y_knob = widgets.IntText(value=0, step=10, description='p0.y (m)')\n",
    "p0z_knob = widgets.IntText(value=0, step=10, description='p0.z (m)')\n",
    "\n",
    "pTx_knob = widgets.IntText(value=100000, step=10, description='pT.x (m)')\n",
    "pTy_knob = widgets.IntText(value=0, step=10, description='pT.y (m)')\n",
    "pTz_knob = widgets.IntText(value=0, step=10, description='pT.z (m)')\n",
    "\n",
    "ui = widgets.VBox([\n",
    "    widgets.HBox([a_knob, v0_knob, vT_knob]),\n",
    "    widgets.HBox([p0x_knob, p0y_knob, p0z_knob]),\n",
    "    widgets.HBox([pTx_knob, pTy_knob, pTz_knob])\n",
    "])\n",
    "display(ui)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "def make_plot(title, short_name, fn, args, line_shape=\"spline\"):\n",
    "    v_args = {a_magnitude: args[\"a_knob\"], v0_magnitude: args[\"v0_knob\"], vT_magnitude: args[\"vT_knob\"]}\n",
    "    R_val = solved_R.subs(v_args)\n",
    "    \n",
    "    p_args = {\n",
    "        **v_args,\n",
    "        p0_x: args[\"p0x_knob\"],\n",
    "        p0_y: args[\"p0y_knob\"],\n",
    "        p0_z: args[\"p0z_knob\"],\n",
    "        pT_x: args[\"pTx_knob\"],\n",
    "        pT_y: args[\"pTy_knob\"],\n",
    "        pT_z: args[\"pTz_knob\"],\n",
    "    }\n",
    "    T_val = solved_T.subs(p_args)\n",
    "\n",
    "    \n",
    "    t_sample = np.linspace(0, float(T_val), 99)\n",
    "    image_sample = lambdify(t, fn.to_matrix(N).subs({**p_args, T: T_val, R: R_val}))\n",
    "    \n",
    "    fig = go.Figure()\n",
    "    fig.add_trace(go.Scatter(x=list(t_sample), y=[image_sample(t_val)[0][0] for t_val in t_sample], line_shape=line_shape, name=f\"{short_name} - x\"))\n",
    "    fig.add_trace(go.Scatter(x=list(t_sample), y=[image_sample(t_val)[1][0] for t_val in t_sample], line_shape=line_shape, name=f\"{short_name} - y\"))\n",
    "    fig.add_trace(go.Scatter(x=list(t_sample), y=[image_sample(t_val)[2][0] for t_val in t_sample], line_shape=line_shape, name=f\"{short_name} - z\"))\n",
    "\n",
    "    fig.update_layout(title=title, xaxis_title=\"t\", yaxis_title=short_name)\n",
    "    return fig\n",
    "\n",
    "def plot_position(**knob_args):\n",
    "    plot = make_plot(\"Position\", \"p(t)\", d_fn, knob_args)\n",
    "    plot.show()\n",
    "    \n",
    "def plot_velocity(**knob_args):\n",
    "    plot = make_plot(\"Velocity\", \"v(t)\", v_fn, knob_args)\n",
    "    plot.show()\n",
    "    \n",
    "def plot_acceleration(**knob_args):\n",
    "    plot = make_plot(\"Acceleration\", \"a(t)\", a_fn, knob_args, line_shape=\"hv\")\n",
    "    plot.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "a90025ca843b43719b5b2711f557a2ac",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Output()"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "f2cee76d9b054c30b882f93f05207a84",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Output()"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "ffd22dacf27b47ef94fa6fb8bd67b868",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Output()"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "knob_args = {\n",
    "    \"a_knob\": a_knob,\n",
    "    \"v0_knob\": v0_knob, \"vT_knob\": vT_knob,\n",
    "    \"p0x_knob\": p0x_knob, \"p0y_knob\": p0y_knob, \"p0z_knob\": p0z_knob,\n",
    "    \"pTx_knob\": pTx_knob, \"pTy_knob\": pTy_knob, \"pTz_knob\": pTz_knob\n",
    "}\n",
    "\n",
    "position_plot = interactive_output(plot_position, knob_args)\n",
    "velocity_plot = interactive_output(plot_velocity, knob_args)\n",
    "acceleration_plot = interactive_output(plot_acceleration, knob_args)\n",
    "display(position_plot, velocity_plot, acceleration_plot)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {
    "jupyter": {
     "source_hidden": true
    }
   },
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "61ad0300642949bbad6c4d89b7605e4f",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Output()"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "knob_args = {\n",
    "    \"a_knob\": a_knob,\n",
    "    \"v0_knob\": v0_knob, \"vT_knob\": vT_knob,\n",
    "    \"p0x_knob\": p0x_knob, \"p0y_knob\": p0y_knob, \"p0z_knob\": p0z_knob,\n",
    "    \"pTx_knob\": pTx_knob, \"pTy_knob\": pTy_knob, \"pTz_knob\": pTz_knob\n",
    "}\n",
    "\n",
    "def plot_3d_position(**args):\n",
    "    v_args = {a_magnitude: args[\"a_knob\"], v0_magnitude: args[\"v0_knob\"], vT_magnitude: args[\"vT_knob\"]}\n",
    "    R_val = solved_R.subs(v_args)\n",
    "    \n",
    "    p_args = {\n",
    "        **v_args,\n",
    "        p0_x: args[\"p0x_knob\"],\n",
    "        p0_y: args[\"p0y_knob\"],\n",
    "        p0_z: args[\"p0z_knob\"],\n",
    "        pT_x: args[\"pTx_knob\"],\n",
    "        pT_y: args[\"pTy_knob\"],\n",
    "        pT_z: args[\"pTz_knob\"],\n",
    "    }\n",
    "    T_val = solved_T.subs(p_args)\n",
    "\n",
    "    t_sample = np.linspace(0, float(T_val), 99)\n",
    "    d = lambdify(t, d_fn.to_matrix(N).subs({**p_args, T: T_val, R: R_val}))\n",
    "    p_mag = lambdify(t, d_fn.to_matrix(A).subs({**p_args, T: T_val, R: R_val}))\n",
    "    v = lambdify(t, v_fn.to_matrix(N).subs({**p_args, T: T_val, R: R_val}))\n",
    "    v_mag = lambdify(t, v_fn.to_matrix(A).subs({**p_args, T: T_val, R: R_val}))\n",
    "    a = lambdify(t, a_fn.to_matrix(N).subs({**p_args, T: T_val, R: R_val}))\n",
    "    a_mag = lambdify(t, a_fn.to_matrix(A).subs({**p_args, T: T_val, R: R_val}))\n",
    "\n",
    "    fig = go.Figure()\n",
    "    fig.add_trace(go.Scatter3d(\n",
    "        x=[d(t_val)[0][0] for t_val in t_sample],\n",
    "        y=[d(t_val)[1][0] for t_val in t_sample],\n",
    "        z=[d(t_val)[2][0] for t_val in t_sample],\n",
    "        customdata=np.dstack((\n",
    "            [p_mag(t_val)[0][0] for t_val in t_sample],\n",
    "            [v_mag(t_val)[0][0] for t_val in t_sample],\n",
    "            [v(t_val)[0][0] for t_val in t_sample],\n",
    "            [v(t_val)[1][0] for t_val in t_sample],\n",
    "            [v(t_val)[2][0] for t_val in t_sample],\n",
    "            [a_mag(t_val)[0][0] for t_val in t_sample],\n",
    "            [a(t_val)[0][0] for t_val in t_sample],\n",
    "            [a(t_val)[1][0] for t_val in t_sample],\n",
    "            [a(t_val)[2][0] for t_val in t_sample],\n",
    "        ))[0],\n",
    "        hovertemplate='''\n",
    "            p:%{customdata[0]:.1f} m - (%{x:.1f} %{y:.1f} %{z:.1f})<br>\n",
    "            v:%{customdata[1]:.1f} m/s - (x:%{customdata[2]:.1f}, y:%{customdata[3]:.1f}, z:%{customdata[4]:.1f})<br>\n",
    "            a:%{customdata[5]:.1f} m/s2 - (x:%{customdata[6]:.1f}, y:%{customdata[7]:.1f}, z:%{customdata[8]:.1f})\n",
    "        ''',\n",
    "        line={\n",
    "            \"color\": list(t_sample),\n",
    "            \"colorscale\": \"Viridis\",\n",
    "            \"colorbar\": { \"title\": \"t\" }\n",
    "        },\n",
    "        marker={ \"size\": 2, \"color\": list(t_sample), \"colorscale\": \"Viridis\" },\n",
    "        name=f\"p(t)\",\n",
    "\n",
    "    ))\n",
    "\n",
    "    fig.update_layout(title=\"3D Position\", xaxis_title=\"p(x)\", yaxis_title=\"p(y)\", autosize=True, height=700)\n",
    "    fig.show(config={'scrollZoom': False})\n",
    "\n",
    "position_3d_plot = interactive_output(plot_3d_position, knob_args)\n",
    "display(position_3d_plot)"
   ]
  }
 ],
 "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.8.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}