content/homework-notebooks/mae223-ps04.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# PS4 (Secs 2.9-2.15): 2,3,7,8,11,13,14"
   ]
  },
  {
   "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": "markdown",
   "metadata": {},
   "source": [
    "# P4.2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "A = me.ReferenceFrame('A')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "x1, x2 = me.dynamicsymbols('x1:3')\n",
    "y1, y2 = me.dynamicsymbols('y1:3')\n",
    "z1, z2 = me.dynamicsymbols('z1:3')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "L1, L2 = sm.symbols(\"L1:3\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "f1, f2, f3, f4 = me.dynamicsymbols('f1:5')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "p1 = x1*A.x + y1*A.y + z1*A.z\n",
    "p2 = x2*A.x + y2*A.y + z2*A.z"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle f_{1} = - L_{1} + \\sqrt{x^{2}_{1} + y^{2}_{1}}$"
      ],
      "text/plain": [
       "              ___________\n",
       "             ╱   2     2 \n",
       "f₁ = -L₁ + ╲╱  x₁  + y₁  "
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sm.Eq(f1, (p1 - p1.dot(A.z)*A.z).magnitude() - L1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle f_{2} = - L_{2} + \\sqrt{\\left(- x_{1} + x_{2}\\right)^{2} + \\left(- y_{1} + y_{2}\\right)^{2}}$"
      ],
      "text/plain": [
       "              ___________________________\n",
       "             ╱           2             2 \n",
       "f₂ = -L₂ + ╲╱  (-x₁ + x₂)  + (-y₁ + y₂)  "
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p2minp1 = p2 - p1\n",
    "sm.Eq(f2, (p2minp1 - p2minp1.dot(A.z)*A.z).magnitude() - L2) "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAD4AAAAXCAYAAABTYvy6AAAABHNCSVQICAgIfAhkiAAAAi5JREFUWIXt1k+ITWEYx/HP/FFTg2FlJ0mZzUhYUZqkFHbDggXJyp9kMcKOhdBkIyllMSw0iiiRMkiSsjBIaWIx/kQsRmFD01yL90zdOc655547Z+am7rfezr3P+7zP87v3PM/7vjSYxFycxQj+oITD9RRUNE0p9lvYjDsYwhiu4s0M6aoLncIbvltvIdNJc4JtXfS8PpNC6kmP8KaTRmcddcV5Il1nCY+qCdJa9vkLjmMv5uNEZC/hbSGSi+EG7iXYd2EhHlYTJL65teAn3mHZVNRFHMS8HP4vcLOGPH3oRT92YzxvgC7hDffXkDyJEZXLMj7y5m3C+WjtOemnVCY7oyAHYvZ9eIUf0XiKTbUmKYgWXBL0no7NrRWO5M/R/Jb44viuviJ6DsXsn3AEK7EKD4SSLKIdamEWBrADx/x7uWrHS+xPCxAvjcdYgw6h1ysxiqO4UMFnOnq8DdeEiuvFmQz/ErZGaxJpEsp4OCNQC7YJV9muDN8RxfZ4O+4Lm9eeDN8JEku9nKWR00DKfBd+CdfX79hYZeKi6BDO8DGhxKsl8YeXn+MT/f08JcAwlgul24PL6MbrHCKmwhWsxjMsFno7zkn8zhu4T/h31lfpP4iLeZPUSLOw51Rqk68pazN39UNCnw/mENNWpe9UGcccQV/aWJAnYGu2CziF2/gYCdgulHm9z/I0ZmNJ2fdFQpuO4kOeQP14L/TPN6EqNhShcJroVszNsEGDBv8pfwHG7JlPmysVAwAAAABJRU5ErkJggg==\n",
      "text/latex": [
       "$\\displaystyle f_{3} = z_{1}$"
      ],
      "text/plain": [
       "f₃ = z₁"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sm.Eq(f3, p1.dot(A.z))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAD4AAAAXCAYAAABTYvy6AAAABHNCSVQICAgIfAhkiAAAAiNJREFUWIXt1j1oFEEUB/BfPvwAxUQrbSxsEuS0SCUGLYJgoZ21X4iFFmIhiBYaCxUJKQySOhaKQSWCIIqIZ2FjUGMlomA6FSu1MpyexUzwsuydt7nLrcX9Ydjd92b+7z+892aWNhZgDcYwizmUcTpPQc1GRxX7fezFA7xGCZN42yJduaBfyPDDvIUsJTpTbEPxebeVQvLEPiHTaaM/R11JPFddZxnP6iHprnj/hAs4jrW4GO1lvG+K5OZgCo9T7IexEU/rIUkebl34gQ/Y2oi6iJPozTB/BvcWEWcEpzCBI/idlWCLkOGJRQRPw6zaZZkcWeN2YDyuvab6LfVPHIwkJ2rMOVsRKE904XrUciXhO4NpfMdX4Xou1CK7Gol2VPFvw0e8ke/Gl+G2oPV8iv+R0PMFoYqn8Bnr5id0JxYMRLKZFLIe3BB66FydApeix1fiDvYIfT2aMmd34ns/vmFQyP4CdAil8a5KwEl/S6qovozPam6Pr8IT4fA6Vkf8eWyI/INpzr7ovJXiO4qXWB6/i1pf6j3CHV7CgYxrJ4Vf7655Q2WpD8Tnq8SiPlwS+n4uY8Bm4ia24wU2YThlzmX8TNhGsFPQ/yuNeETI+K6E/VC0lypGWSi3ElZk3kJ2dAr/F7Xa5EvKutFo37yYoL3C6Vg5poUMFDRwdy4xxjSw6Wooyv8er4Vx4aAewvqKsbpR4qL/e+PV2mE4R01ttNFGC/EHSR+epICK3nAAAAAASUVORK5CYII=\n",
      "text/latex": [
       "$\\displaystyle f_{4} = z_{2}$"
      ],
      "text/plain": [
       "f₄ = z₂"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sm.Eq(f4, p2.dot(A.z))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# P4.3"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It has the same number as P4.2 plus one additional one."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "L3, L4 = sm.symbols('L3, L4')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle - L_{3} + \\sqrt{\\left(- L_{4} + x_{2}\\right)^{2} + y^{2}_{2}}$"
      ],
      "text/plain": [
       "         ___________________\n",
       "        ╱           2     2 \n",
       "-L₃ + ╲╱  (-L₄ + x₂)  + y₂  "
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p2_Q = p2 - L4*A.x\n",
    "(p2_Q - p2_Q.dot(A.z)*A.z).magnitude() - L3"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# P4.7"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "(a) Each body has 6 coordinates but a ball and socket removes the need for translation coordinates for one body, so the total is 12-3=9.\n",
    "\n",
    "(b) B has 6 and the rotor has 1 coordinate, so 7.\n",
    "\n",
    "(c) B has 6 and the rotor's angular position is prescribed so this does not add a generalized coordinate. Total is 6.\n",
    "\n",
    "(d) In 4.2 only 2 coordinate angles are needed.\n",
    "\n",
    "(e) In 4.3 only 1 independent coordinate is needed, but you can get it down to 3 before introducing the two holonomic constraints."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# P4.8"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "R = sm.symbols('R')\n",
    "q1, q2, q3, q4, q5 = me.dynamicsymbols('q1:6')\n",
    "u1, u2, u3, u4, u5 = me.dynamicsymbols('u1:6')\n",
    "A = me.ReferenceFrame('A')\n",
    "Y = me.ReferenceFrame('Y')  # intermediate frame so that simple rotations can be used\n",
    "B = me.ReferenceFrame('B')\n",
    "C = me.ReferenceFrame('C')\n",
    "Y.orient(A, 'Axis', (q1, A.z))\n",
    "B.orient(Y, 'Axis', (sm.pi/2 - q2, Y.x))\n",
    "C.orient(B, 'Axis', (q3, B.z))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "O, P, Cs, Q = sm.symbols('O, P, Cs, Q', cls=me.Point)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "P.set_pos(O, q4*A.x + q5*A.y)\n",
    "Cs.set_pos(P, R*B.y)\n",
    "O.set_vel(A, 0)\n",
    "P.set_vel(A, P.pos_from(O).dt(A))\n",
    "Cs.set_vel(A, Cs.pos_from(O).dt(A))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "u1_eq = sm.Eq(u1, C.ang_vel_in(A).dot(B.x))\n",
    "u2_eq = sm.Eq(u2, C.ang_vel_in(A).dot(B.y))\n",
    "u3_eq = sm.Eq(u3, C.ang_vel_in(A).dot(B.z))\n",
    "u4_eq = sm.Eq(u4, P.vel(A).dot(A.x))\n",
    "u5_eq = sm.Eq(u5, P.vel(A).dot(A.y))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To be valid GS's these equations must be linear in the time derivatives of the q's and we must be able to uniquely solve for the time derivatives of the q's. Below I show that they can be solved:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left\\{ \\dot{q}_{1} : \\frac{u_{2}}{\\operatorname{cos}\\left(q_{2}\\right)}, \\  \\dot{q}_{2} : - u_{1}, \\  \\dot{q}_{3} : - u_{2} \\operatorname{tan}\\left(q_{2}\\right) + u_{3}, \\  \\dot{q}_{4} : u_{4}, \\  \\dot{q}_{5} : u_{5}\\right\\}$"
      ],
      "text/plain": [
       "⎧        u₂                                                     ⎫\n",
       "⎨q₁̇: ───────, q₂̇: -u₁, q₃̇: -u₂⋅tan(q₂) + u₃, q₄̇: u₄, q₅̇: u₅⎬\n",
       "⎩     cos(q₂)                                                   ⎭"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sm.solve([u1_eq, u2_eq, u3_eq, u4_eq, u5_eq], q1.diff(), q2.diff(), q3.diff(), q4.diff(), q5.diff())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# P4.11"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "R, L = sm.symbols(\"R, L\")\n",
    "q1, q2, q3, q4, q5, q6 = me.dynamicsymbols('q1:7')\n",
    "u1, u2, u3, u4, u5, u6 = me.dynamicsymbols('u1:7')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "A, N, C1, S, C2 = sm.symbols('A, N, C_1, S, C_2', cls=me.ReferenceFrame)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Each of the bodies rotates about the axle. All rotations are relative to the yaw frame N."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "N.orient(A, 'Axis', (q3, A.y))\n",
    "C1.orient(N, 'Axis', (q4, N.z))\n",
    "S.orient(N, 'Axis', (q5, N.z))\n",
    "C2.orient(N, 'Axis', (q6, N.z))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "O, Ss, C1s, C2s, C1p, C2p = sm.symbols('O, S*, C_1^*, C_2^*, C_1^p, C_2^p', cls=me.Point)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "Ss.set_pos(O, q1*A.x + q2*A.z + R*N.y + L*S.z)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "O.set_vel(A, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "Ss.set_vel(A, Ss.pos_from(O).dt(A))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\dot{q}_{1}\\mathbf{\\hat{a}_x} + \\dot{q}_{2}\\mathbf{\\hat{a}_z} + L \\operatorname{cos}\\left(q_{5}\\right) \\dot{q}_{3}\\mathbf{\\hat{s}_x} -  L \\operatorname{sin}\\left(q_{5}\\right) \\dot{q}_{3}\\mathbf{\\hat{s}_y}$"
      ],
      "text/plain": [
       "q₁̇ a_x + q₂̇ a_z + L⋅cos(q₅)⋅q₃̇ s_x + -L⋅sin(q₅)⋅q₃̇ s_y"
      ]
     },
     "execution_count": 25,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Ss.vel(A).simplify()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAHUAAAAdCAYAAACUoyOLAAAABHNCSVQICAgIfAhkiAAABKhJREFUaIHtmV2IVVUUx3/qoOYHqIUNWTRZL4lC0ZcQ0S2KqOgDerCeOkGoGPVgSj1NQz2kaRBkBVEyE9bEROGDRZZ9GGRmpGapSIkn0JrpQ0lEG3O8Pay1u/vuOWfOOXv2nTtD5w+bc/ZaZ6299n/vvfa++0I2LtVSoh6jlpdxOb6J9dkWuO0rgXv0vQfYF9h/oxHrsy2gzxHjJKbWgVCYDhwEzgJVYC9wTuA2Go2YsLyMeU7eQAJ/DHhC319qakTNx5jm5H4k4Kcs2XMqu6spETUfJSclSgBEyKyvNDeMkcH4BFkLkte/A/4GeoF1wBTgL4qfyCKEUFPmAkuB3cAp4A/gLeCCwtGPLELzMgtYDXwMHFIf/wB/AtuAlQQ6KE0ENiPkf4Pk9fXASeADlb9Z0GdE/aB+4dRN2Q9MGm4HMmKoeNo3gpf5JPNgl2+BqZ4x/4dX1dkKR36j1dDjBX1GDA52C/A0sMeRL/KMO28MFU/7RvAyD5nIXciKfRJoB7qRFWv8um0WwjXqZHOK/qDqby7oN6J+4N6jdukxCzhj6Z4v6LtoDBUP20bxYjAHuBdYhkyMFcD31Dj5xNMvIDOmClyfot+h+pla72DwCuxNsIucb2519L2Wbr1v8BbihLiGKp0Z/ory8giSgY5r+Qq4M8FuJrCR2mVDWjmQEd8gtFjvtyGb9Jcp385BNvRjluwA9bN/IEebsVPvt96TDm5F8QIww5FdgVy/dSW0vzvDX1FeDiOp9EckIz2IDN5VyGAbvE7tSnAoeJ8zJiOzYmeK3mzq71qyDuCHHL4j6mdem6OPGbxq2kifuR052kyLoVLQzoeXJBwFllj1KdRvO58ClwETVN9j6WLLrltl2x3/21TeDbWVMaDl3JSg2vXpdm4ucASZqd2Eu9zuB762yiFLdypQG3ngy4vBBOABYBpCvMEMagMIsAn4SduaDdyU4u9lfV6HHLRAMsVCfe9yDfYjo207HKeBm1lzh6W7HbgPWADcgmzovyKHHxsRxVeqjfORDleR/cnnt5uJoeJhW5QXEE5OIKvxWIK+ReXG/ihyAl6FLBKbr9ixNb8Y1mr9Ua0foX6iAPCQKk8i+X4NsAshdK/qWofo/FSgD1juyCP8B3U6sgrMgSFtxWTBxFDxsPXhZSKSTq8GnkUuWOY736wkeXs5DHxE+qAuVXkfMjm2an11WgeWI6muX59rkFTxO/BLRucBPgNecWQRfoM6CdlrqkgGcO2KwMRQ8bQfLi9bgNcS5IuRm6jTwG/ABiSddpI+qNOQ26eq2g/o++X5uwMXqdH7Gd9NRshvz/guD8YD72i7x5E/j0cb8vICMjk3BGx7nbZ9Qp87ijq4Ww2fceRrkRuVS5CNexMyABcPI1iDRdRmah9y2jPl4QD+QyCNl1XADUhmWYCk37PIGSQU5lGf/ZbZypYkCwdmlexy5BciJ97zkDS0HTmF/TyMYA3sw9BsLQYfBvAfAmm8tCKrshVJk3uQAU27kfLBPuBzZDvpB94u6mAjyXvh/x3N5uVFbb+nSe2XCIjFyIQ6jaT1a5sbTokQ6KT202e0nC9KlChRokSJEiXGBv4F4UTB4NZ1h9wAAAAASUVORK5CYII=\n",
      "text/latex": [
       "$\\displaystyle \\dot{q}_{5}\\mathbf{\\hat{n}_z} + \\dot{q}_{3}\\mathbf{\\hat{a}_y}$"
      ],
      "text/plain": [
       "q₅̇ n_z + q₃̇ a_y"
      ]
     },
     "execution_count": 26,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "S.ang_vel_in(A)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAHUAAAAdCAYAAACUoyOLAAAABHNCSVQICAgIfAhkiAAABJJJREFUaIHtmV2IVVUUx3/qMJYpjQo2JMFQQSUjFH0JEd2EiIo+3qynbpAmRj2UUvRgUQ9pFgRNPUjJTFQT0wc+VGSZTEGWivmFSpR4HtQ0S0lEGxu9Pay1O/vsOeeeu4/7zp2h84fNOWetvdZe+7/3XnvffSEfV2gpkcSY5WVCA3UifXYFbvs64H59HwD2BPbfbET67Aroc9Q4iYg7EArTgH3AOaAG7AYuDNxGsxERlpdxz8m7SOBPAs/o+5stjaj1GNecPIgE/Lwle0Vl97Ykotaj5KRECYAqMusrrQ1jdDAxRdaG5PUdwN/AYaAHmAL8hf+JrIoQasrlwGJgO3Aa+AP4ALjUO/rRRWheZgArga+B/erjH+BPYCOwjEAHpXZgHUL+FiSvrwFOAV+o/H1Pn1WSg/qd823KXmDy+XYgJ4ZKQftm8NJNOg922QpcVDDm/7BanS115LdZDT3t6bPKyGDXAy8COx35goJxNxpDpaB9M3iZg0zkPmTFPgssB/qRFWv8um164UZ1si5Dv0/18z39VkkO3KfElx4zgGFL95qnb98YKgVsm8WLwWzgAWAJMjGWAruIOfmmoF9AZkwNuCVDv1n101N0z6muJ0VXJTmodzj6w5ZujW/QKYic9vJKb44/X14eRzLQCS0/APek2E0H1hJfNmSVn3PiG4E26/1OZJP+PqPubGRDP+7I5wELtSONIHK+h6z3tIObL14HOhzZtcj1W19K+9tz/PnycgBJpb8gGelhZPCuJ8nRO8RXgvVQ+JxxATIrfsrQm039E0d+MZJ+5gODNLZSuxx9xMhV00X2zH0hrzN1Yqh42hXlxcUx4DHrewrJbWcDcCUwSfUDli6y7PpV9qPjf6PK+yFeGWe1zMwIark+3c6tBj7WoEJiCNhklf2W7nTgtuqhKC8Gk4CHgKkI8QYdxAMI8Bnwq7Y1C7g9w99b+rwZOWiBZIp5+t7nGuxFRtt2OEEDN7Pmbku3EDlyt+v3IOFWqo1LkA7XkP2pyG83E0OlgK0vLwBzgZPIajyeom9TubE/hpyAVwAHSfIVObbmF8Or+v2Efh8kOVEAeESVp5B8vwrYhhC6W3WdWvcq4ChwtWU/SPhBnYasAnNgyFoxeTAxVArY+vBi0I6k0xuAl5ELlm6nzjLSt5cDwFdkD+pilR9BJse3+r0yqwNPIaluSJ+rkFRxFDhk1auqo2Gr1JCT3DDJzd3U9R3UyUharwG/pdj5wMRQKWjfKC9ZWA+8nSJfhNxEnQF+B95D0mkv2YM6Fbl9qqn9WX2/pvHuwGVq9Lkl60Bmnl22INd93TT253s9TAQ+0nZPIH8ejzWk8ZKFDciAhUKPtn1Sn5t9Hdynhi/l1BskPf0WwQLimXoEOe2Z8migNs4XWbysAG5FMstcJP2eA+4K2PYcktlvia1sS7NwYFbJtoBB5cE+DM3SYvDlKMZRD1m8dCKrshNJkzuRAc26kSqCPcgiqiBbwoe+DtaSvhf+39FqXt7Q9gda1H6JgFiETKgzSFq/qbXhlAiBXuKfPmPlfFGiRIkSJUqUKDE+8C/6tMEPCisv6gAAAABJRU5ErkJggg==\n",
      "text/latex": [
       "$\\displaystyle \\dot{q}_{4}\\mathbf{\\hat{n}_z} + \\dot{q}_{3}\\mathbf{\\hat{a}_y}$"
      ],
      "text/plain": [
       "q₄̇ n_z + q₃̇ a_y"
      ]
     },
     "execution_count": 27,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "C1.ang_vel_in(A)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAHUAAAAdCAYAAACUoyOLAAAABHNCSVQICAgIfAhkiAAABKdJREFUaIHtmW2IFlUUx3/qkmkKalCWBEv5JUkoygokegoiSioJwvoQTSAman0opSDQSMi3giDrQ6TsRu3CRuGHCqzN6M3MqLbEpBdxAi11zUgsdzf18cM509y5z4zPc2/32WeX5g+XmTlnzrln/vfec18G6uMyLSWyGLG8jGngnViv7YHrvgq4S+97gO8D+282Yr22B/Q5bJzEpB8QCpOBvcBpoArsBiYErqPZiAnLy6jn5FUk8EeAx/X+xZZG1HqMak7uRQJeZcjWq+yOlkTUepSclCgBECG9vtLaMIYHY3NkbUhe/xYYAA4CG4GJwJ+4r8gihNCkXAosBvqAE8ARoAu42Dn64UVoXqYB64D3gX3q4x/gd2A7sIJAC6VzgK0I+V8ieX0z8Dfwrspfd/QZkW3Uj63npOwBxv/XD6gTQ8XTvhm8XEE+D2b5CjjPM+Z/8bI6W27JbzQqeszRZ0RtsL3A08B3lnyBZ9yNxlDxtG8GL7OQjtyJjNgngJVANzJiE792nU6Yo062Fuj3qv5mR78R2YZ7i/TQYxpw0tA95+jbNYaKh22zeEkwA5gPLEE6xnJgFyknH3j6BaTHVIG5Bfqdqp9qyC5Su35kftyF9F4TEdlGvcXSHzR0m72jTxFTmxnOVjrq+HPlZSmSgY5p+RyYl2M3FdhCethQVH6oE18N2oz7W5FJ+rOCd2cgE/of+jxF3/1Ug+5HFkGH69QZW8+Dxn3ews0Vz2tsJq5Ejt86c+rvq+PPlZf9SCr9CclIDyCNdzXS2Ak2kR4Jng3e64xzkV7xdYE+mdTfNGTPUPyhJiKyPa/d0sfUjpp2invuUw3UWRRDxdHOh5c8HAUeMp4nkp12tgEzgXGq7zF0sWHXrbIdlv/tKu+GdGSc0nJ+QVAr9Wp+3Hx13oWMzj5gGY39JKiHQeALo+wzdCcC+G8UPryYGAfcB0xCiE8whbQBAd4Gfta6LgBuKvD3kl6vQxZaIJnier3vtA32IK1tOhyjgSe95nZDN6BlDfJ34UHgODKnmIhwH6kmLkQ+uIrMTz57tySGioetKy8AsxEuTiJp2da3qTyxP4qsgNcCB8jyFVu2yY7hWX1+WJ8PkO0ogDRKFdl7bQI2AN8ghO5W3XTj/SGEZBNrqN2ER/g36mRkFCQLhqIRUw9JDBUPW1deQPa1M4FrEE6OIKnaxAryp5f9wHsUN+pilR9COsdH+ryu6AMeRVLdoF43IKmiH/jVevcX4BVLdj/wlyWL8GvU8chcUwV+y7FzQRJDxdPehZc89FLLFcAiZBAMIVPYa0g67aC4USchp09VtT+l95c3/jlwiRq9Y8m7gE8s2WrC/NgdC7yh9R5D0vtIQxEvediGNFgobNS6j+t1p6uDO9VwtSWfg5x8PImkmnuQHmTPqT5YQNpTDyELsqQsDOA/BIp4WQvcgGSW2Uj6PQ3cFrDuWWSz3xJXB6vU8O4c3TzSA+4fkQPvEKvfiLBbmmagiJcOZGoaRFJqL7LXDY0Ptf4B5GTOCVvInwv/72g1Ly9o/T0tqr9EQCxCOtQQktavbW04JUKgg3TrM1LWFyVKlChRokSJEqMDZwBHqNFDvQibEgAAAABJRU5ErkJggg==\n",
      "text/latex": [
       "$\\displaystyle \\dot{q}_{6}\\mathbf{\\hat{n}_z} + \\dot{q}_{3}\\mathbf{\\hat{a}_y}$"
      ],
      "text/plain": [
       "q₆̇ n_z + q₃̇ a_y"
      ]
     },
     "execution_count": 28,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "C2.ang_vel_in(A)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Make equations for each u:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [],
   "source": [
    "u_eqs = [sm.Eq(u1, Ss.vel(A).dot(N.x)),\n",
    "         sm.Eq(u2, S.ang_vel_in(A).dot(N.y)),\n",
    "         sm.Eq(u3, S.ang_vel_in(A).dot(N.z)),\n",
    "         sm.Eq(u4, C1.ang_vel_in(A).dot(N.z)),\n",
    "         sm.Eq(u5, C2.ang_vel_in(A).dot(N.z)),\n",
    "         sm.Eq(u6, Ss.vel(A).dot(N.z))]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left[ u_{1} = L \\operatorname{sin}^{2}\\left(q_{5}\\right) \\dot{q}_{3} + L \\operatorname{cos}^{2}\\left(q_{5}\\right) \\dot{q}_{3} - \\operatorname{sin}\\left(q_{3}\\right) \\dot{q}_{2} + \\operatorname{cos}\\left(q_{3}\\right) \\dot{q}_{1}, \\  u_{2} = \\dot{q}_{3}, \\  u_{3} = \\dot{q}_{5}, \\  u_{4} = \\dot{q}_{4}, \\  u_{5} = \\dot{q}_{6}, \\  u_{6} = \\operatorname{sin}\\left(q_{3}\\right) \\dot{q}_{1} + \\operatorname{cos}\\left(q_{3}\\right) \\dot{q}_{2}\\right]$"
      ],
      "text/plain": [
       "⎡          2                2                                                 \n",
       "⎣u₁ = L⋅sin (q₅)⋅q₃̇ + L⋅cos (q₅)⋅q₃̇ - sin(q₃)⋅q₂̇ + cos(q₃)⋅q₁̇, u₂ = q₃̇, u\n",
       "\n",
       "                                                           ⎤\n",
       "₃ = q₅̇, u₄ = q₄̇, u₅ = q₆̇, u₆ = sin(q₃)⋅q₁̇ + cos(q₃)⋅q₂̇⎦"
      ]
     },
     "execution_count": 30,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "u_eqs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAABFEAAAAaCAYAAAB/ycMbAAAABHNCSVQICAgIfAhkiAAAEFdJREFUeJztnXusHVUVh7+2PFpabAWsoAEqYEFEoRREVOBSQN6KLwQVuUgIJjxE5A+BBKsQSkBJIQSVYDg85JXyCIjyEooIYlF5FCGopKcGqdBSEJGWWnr9Y+3xzp27Z2bvPc9z7vqSk9vOc531W7PWPnv27IFy2NZ8FEVRFKUOtO4oiqKMXbQGuKO+8kP95Yf6K8EE4BjzyaNrPoqiKIpSB1207iiKooxVumgNcKWL+sqHLuovH7r0n7+mA2cDO6dtMD5l+frA7cBy4Nry7VIURVEURVEURVEURWkVrwDzgVOBo3x2PAG4vAqLlL7hGiTAJtdwrtnAEHB8DedS6tNWdR3mdMQXR5d0PL0+FRtlxpnGWDOohkoeqquiKEo5TAJexCOf3gWcVpk5vckDSKHYq2lDWsBuwDqkMVcXtwHLgCk1nnMsUre2qqtwPZJfZpZwLL0+66PX6kJZcaYx1hyqoZKF6qooilIuXeAg142fQTtR4owDXgfeQYsEwL2IPybVeM6PIQ3Hs2o851ikbm1VV2ErYAck1xRFr8966MW6UFacaYw1h2qoZKG6KoqilEsXONFnY+1EGWYmUiCebdqQFjATuctxRQPnfg5Yikx6HMIgouNASfb0G01pq7qWRy9fn73GWK0LGmO9Ty9rOIjm+zR6WVdFUZS20gXOSC5Mm1gWpEilsR4y0cpTwGrgn8BlwEbAv+i/RuVs8/ePjtufhfjvc5Z1W5t1txawZwC4CXlG621kGOU9wBGJ7cYhDY6HgFeBVcATwLEpx93L2PUCousrwCLg/Ng23zDHvSnDvqri40bkLtz+gfsXpZ91hXxtVVd/XHy/rznHhYl9DzDL5wE7IcP4XwbeBB4F9rCcrykNoXod21Z3fOpC0zEG9jirIsagP3OFaqga5jFANTUctH0G7aoBVcdSUdrkK1B/+aL+8qNqf1n7RNI6UcYjCdrGBsicKZcAa4BLgV8iyXsB8C6kGBSlgxg9WMKxirKb+fsHx+13NX9tjevoWKE+mg88CMwB7gcuNv+fDXwitt0k4G7gKmAacLX59+aIb5PDLs8CfmOO82tz3DuBDRn5HNj+yPD1x1LsqzI+HjF/Dwjcvyj9rCtka6u6+uPq++j8yXPMMn9nAo8jj4xcjTS89wTuADZO7NOUhlCtjm2sOz51oekYi9sQP0/ZMQb9mytUQ9Uwi6pqOGj7DNpXA6qMpaLU5StQf/nQwb19of5ql79WkD3w5P9sitzB2Dxl/RXIl0oOa9nHLB8CvhNm4wg6tKcT5SHElk86br8EeT20jXnmWIcF2HG+2XcBo2cJnoL0tkXchv0Z1fcgdz5WA+82y94LrAUeRi6OJJuZv5PNdoszbKwyPqaa/RcF7j9IsWHA/aor5Guruvrh4/ufm3Nsn9jmRrP8FYYLRMQtjJ7QtEkNobiOWbSx7vjUhaZjDOxxVnaMQbtzRRFUw3ZoOEh4vu+1Gg5jp32WR9tqQFWxVAZ1+QrUXz50cG9fqL/a5a/vIW89y2QC0iv+lZT1uxtD7klZ/4JZPyfAwCRbIJOnTS3hWEUYD7yBFCeX1xttgvjg7pT195n17/O0YxZyh+Fx7IU0zmHmHDenrL/arN/P/D8K/J/lHDeaA+DelPV1xMcqZNhYCIOEN776WVfI1lZ19dfVx/fPAf9mdC/38+YYh1r2Oc+si9/1a1pDKKZjGm2sOz51oQ0xBvY4KzPGoP25IhTVcJimNRwkLN/3Yg2HsdM+y6JtNaCqWCqDOn0F6i8fXNsX6i+hTf7aCBn9t2/aBtEQw6znGaMEn3bnbZFZH+9F39uc+CWz7ovOJofTZbg3zOVzXcaxdjDbPON47v3N9uelrH8VeV454kyk+L6B9KLdiTzXnORaRhfXNO4w285OWX+xWf9p8//NkNnch8y+X2akhhF7mm3SnrcNiY+TgKeR7/8G8DvsjcGIfyA/XPLo4hcDnZzj+erqGvdt0BWytVVdR7MF4pflSMNxMdLYjXD1/WSkAf5wYvkUs3wp9rdwRK853Sa2rGkNwV1HH0Jsd82rofjUhZAYc7Hf5/q2xVnZMQb+WlUZY13KaweEaOhSA/pBQ+iNfF9Vrq+yhkN/ts98qat+uVJVTi+DEF/FieaWuKxEm0KvvSptigjx11xG554yOw9D/FVlvMcJja+8PFoEX3/NJUy/8Uhn9vHRgvViK1cBhwBXAtOR4pvkQGPMI5Z1AO9HhtS8Fls2GZl45ipkaGkdRBNvufJSxjrf+VCyJhvcBukxi/eWDQCXI8l1HPAD5HnaHYGVse0ORPz6oIMN+yAXUdqEh1uYv383f1cAn0KGKx0CHI40zO4Dzgb+ZLZbZf5OTDluSHy8CHwX+Cvy/Y8Fbkf8+LTlGJNidmQxH3neOM4uwGeRC7mbWPdkzvF8dXWN+zboCtnaqq4jmYb44rdIkVputo3PI+Xq+52RxBzXIvpO45G7ikMW23ZFJu9aElvWtIbgrqMPIbYP4JZXQ/GpCyExNkC+/T7Xty3Oyo4x8Neqyhgrsx0QoqFLDegHDaE38n1Vub7KGg792T7zpa765UpVOb0MQnwV8XHgBIr7J0mIv6q2KSLUX88zcjTcOyXaFOKvKuM9Toi/XPJoEUL85avfJKSD+mJgYdaGmyDDRZNzokxEinSysR+xk1mf9YMx7W5Mm5mP2H2K4/bR88lbW9adbNYlZ9yPE91ZOjy2LPK9y6Q4G5ttn0pZPwFpLL6M/c7VBkiv3s3mOCuQyctAhkINIRdCkjLiI2Il9vdxj0de3/eCwzFsDBobBgL2LaJrWty3RVdI11Z1Ha3r+aQXEBtZvo/OMZjY51Sz/JuW422M5Ihko71JDaG4jjbKst2WV4vgUxeK1gTItz/v+rbFWZkxBuVpVXeMuVBUQ5e2Tz9pCO3L91Xk+jprOPR3+yyNuuqXD3Xk9BCK+Goqot0c5EdimaM+Qv1VpU0Q7q+5uD+dEEIZ8QXlxXtEqL9828y++PprLv76nYOMOhyBbabZlUgPUvJ5pnfMZ9OME0C6c3uVqIfLdSTKDsB/keG1cTZkOJizfLQxoku8ZzoqqNMdzv82UsjShuoNInc7rsJ+52oN0jt+JFKMN0UmNgN5Xd9yRk+ACeXExwTgaKS4PGpZvz3ii7zRBVVQVFcbbdEV0rVVXUd/tyOQtx9cj/SkP4kk6rSGb5bvo4kgk+eI3rhhyzuzkByR7HVvUkOoRsey6o4trxbBpy6UkTvy7M+7vm1xVmaMQXGtmooxF6rI/0n6QUNob76vItfXWcNhbLbP6qpfPtSR00Mo4qsrkImRHyjRnohQf1VpExTz1zbI42tLgBuAGSXaVTS+yo73iFB/+baZfQnxl69+h5E9afcIusC3LMufQ5J7fGKVcYjjhsznkIzj+oxEacPEsuORNxWtRYbyuBA9DzYztmwyw7PqDwHbZux/E3JXY0Ji+WKz75GWfWYmtn/MbJuc32Y/5PssYdivs1Ls2Q7pTFvKyM62BebY21n2CY2PjzDs59dStgE4zhzj5JT1eQwSPmKhiK5Zcd8WXSFdW9V1JKvNZx7i5+OQ73mSWe/j+yeR4c/rJbZ9Cml02yYq/LaxyzYBeFMaQrqOHeyjbVwpWncgPa8mcak7vnWhaE2A0fb7Xt+2OCs7xiBMqzJirGqKamirAf2kIbQ/31eR66G6Gg790z7r0EwN8MktES41oIqcHqdDuL9CfHUC0tka5ZGFuI/6qMpfrjZ1qD+2Dga+gMTX/shrx5chT3Jk4fq7NjS+XOK9Q/3+csmjNqryV4h+Sxj9NqJUusBpluVRknwLmVzlIiQJ/A34s1mX9mpk8OtE6VBM6DL4sLHhTWSuGNvngsQ+55p9liHPP16D9Hb9Chmq+TrpvW8Xmf1sBfBQpAdwHfJ+7guRXtpFZp84+yEX0Wpz/gvM+dchgRA//lVm+WOIpvOQoVFvmU9ysrSjzfezBX9ofGxgbNrNnH8F9gm3bjDfa0vLOhcGCf+xXUTXrLhvi66Qrq3qOpI1yKRdceYBz5p/u/p+Q3Os5Csho+Vpz81fZ2y23XFsSkNI1/Eac/yvpeyXR9G6k5VXk3TIrzu+daFI7kiz3+f6tsVZFTEGYVqVEWNVU1RDWw3oJw2h/fm+ilwP1dVw6J/2WVM1wCe3RHTIrwFV5PQ4Rfzl66vtkRFMO8SWLcS9E6VD+f7ysanp9gXID/aXgdNztuvg9rs2NL5c4r0Jf7nkURsdqvVXhIt+XTw6UZ7B3omCOckS5O7HEsSB05CAz5qYDXqvE+XrDPdipX2Sk9VMBC5BfPEWMsz2RMRH60ifkOZHiIg7ZtizF1KkVyJDl5YhE8p91bLtPsgQuDfNZzHwfWQIYZwjkOe8nkcmPFuDBMuVwActx90AmcX49yk2FomPiPvN+eNMRe7C3e54DBuDhP/YDtUV8uO+DbpCtraq6zBLGf09jgH+Y/7t6vvZxu6fJI4VLf9pis1pr0SGZjSEbB2fQPyQNozdhVDbXfJqnA75dce3LhTJHWn2+1zftjirKsageJyFxFjVFNEQ7DWgnzWE9uX7KnJ9RBU1HPqnfdZkDYiTllvidMivAVXk9DhF/eXjq0Hk+66NfYaQ77GWkfMy2ehQvr98bGpLbD0I/Dhnmw5uv2uL1psIW7w34S/XPJqkQ33+ytOvi8f8MneR3oliY0vki96Vs51PJ8pY4lL8GvpNcyai5ay8DQ2u8RHxAKNfN3mKOcZejsdoE70U9z7ajlVdr2f0K4nPJb9XvS7q1hDSdZyG3KW90PH4PuTZ3mt5NUmb7a+yBvjGWC/QxhqgdTyftud6G23TtckakCQtt9SFS06vyl9pvpqGjFaIfx5HYn8nypu3wgdXm9oSWxORjtNz8jasmWS8N+WvtudRF/26wEGuBzweudhd+QzivHMt66Ygr6TbxWxzhvn3Vh7H72cuR3oF5yBDoKLPlCaNymEi0rN4p+P2WfFxAVJ4ZyDPp81Deg4Pjm0zCelhXBBmbiP0atz7aDsWdQXYHbnbeDYydPJLyGtE857vrIs6NYRsHQ9Hhq7nDYcNIcv2Xsyrcdpuf1k1oIwYayttrwFax/Npe6630TZdm6oBrrmlLlxzelX+yvJVkoWU/yacoixktE1NxdYPkRFlHwD2AH6BaLt1BXa44hLvTfmrbXnUV78pyONBk11PsD5wK26TMIG8v34I+Lxl3QD24c4dV2P6nLTh4HMbtMmFvRHdXYIqKz46SMF/G5m1+X7kPeRxPoT4Y0aQpc0wQO/Gvau2Y1HXiEORSR1XA39BXjXaxB2bNOrSEJrTMcv2Xs2rEb1gfxk1oEO7Y6wIA7S/Bmgdz6ftud7GWNG1jPpVF03n9CxfJVlIb3SiVEmWv25EOg7XID+ub6H5EaMdmo33vPhqUx710W8iMsfKUb4nmYA8s3Scw7a3I86b4XsSZUyg8dGfqK69Ty9r2Mu2jzVUq95HNexPelnXXra9btRXfqi//OhHf01HHo/8aNOGKIqiKIqiKIqiKIqiKIqiKIqiKIqiKIqiKIqiKIqiKIqiKIqiKIqiKIqiKIqiKIqiKIqiuPA/rs3vplJn1IMAAAAASUVORK5CYII=\n",
      "text/latex": [
       "$\\displaystyle \\left\\{ \\dot{q}_{1} : - L u_{2} \\operatorname{cos}\\left(q_{3}\\right) + u_{1} \\operatorname{cos}\\left(q_{3}\\right) + u_{6} \\operatorname{sin}\\left(q_{3}\\right), \\  \\dot{q}_{2} : L u_{2} \\operatorname{sin}\\left(q_{3}\\right) - u_{1} \\operatorname{sin}\\left(q_{3}\\right) + u_{6} \\operatorname{cos}\\left(q_{3}\\right), \\  \\dot{q}_{3} : u_{2}, \\  \\dot{q}_{4} : u_{4}, \\  \\dot{q}_{5} : u_{3}, \\  \\dot{q}_{6} : u_{5}\\right\\}$"
      ],
      "text/plain": [
       "{q₁̇: -L⋅u₂⋅cos(q₃) + u₁⋅cos(q₃) + u₆⋅sin(q₃), q₂̇: L⋅u₂⋅sin(q₃) - u₁⋅sin(q₃) \n",
       "+ u₆⋅cos(q₃), q₃̇: u₂, q₄̇: u₄, q₅̇: u₃, q₆̇: u₅}"
      ]
     },
     "execution_count": 31,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "qdots = sm.solve(u_eqs, *[q.diff() for q in (q1, q2, q3, q4, q5, q6)])\n",
    "qdots"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Redefine all of the velocities in terms of the u's."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [],
   "source": [
    "Ss.set_vel(A, Ss.vel(A).simplify().subs(qdots))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [],
   "source": [
    "C1.set_ang_vel(A, C1.ang_vel_in(A).subs(qdots))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {},
   "outputs": [],
   "source": [
    "C2.set_ang_vel(A, C2.ang_vel_in(A).subs(qdots))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Calculate the contact point velocities so motion constraints can be formed."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "metadata": {},
   "outputs": [],
   "source": [
    "C1p.set_pos(Ss, -L*N.z - R*N.y)\n",
    "C2p.set_pos(Ss, L*N.z - R*N.y)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle (- L u_{2} \\operatorname{cos}\\left(q_{3}\\right) + u_{1} \\operatorname{cos}\\left(q_{3}\\right) + u_{6} \\operatorname{sin}\\left(q_{3}\\right))\\mathbf{\\hat{a}_x} + (L u_{2} \\operatorname{sin}\\left(q_{3}\\right) - u_{1} \\operatorname{sin}\\left(q_{3}\\right) + u_{6} \\operatorname{cos}\\left(q_{3}\\right))\\mathbf{\\hat{a}_z} + L u_{2} \\operatorname{cos}\\left(q_{5}\\right)\\mathbf{\\hat{s}_x} -  L u_{2} \\operatorname{sin}\\left(q_{5}\\right)\\mathbf{\\hat{s}_y} + (- L u_{2} + R u_{4})\\mathbf{\\hat{n}_x}$"
      ],
      "text/plain": [
       "(-L⋅u₂⋅cos(q₃) + u₁⋅cos(q₃) + u₆⋅sin(q₃)) a_x + (L⋅u₂⋅sin(q₃) - u₁⋅sin(q₃) + u\n",
       "₆⋅cos(q₃)) a_z + L⋅u₂⋅cos(q₅) s_x + -L⋅u₂⋅sin(q₅) s_y + (-L⋅u₂ + R⋅u₄) n_x"
      ]
     },
     "execution_count": 36,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A_v_C1p = C1p.v2pt_theory(Ss, A, C1).simplify()\n",
    "A_v_C1p"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 37,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAABNoAAAAcCAYAAAC3ddLwAAAABHNCSVQICAgIfAhkiAAAE4dJREFUeJztnXvUHkV9xz8JkSQkVLkUuRwhIg2XaiEEtbQNRIhSLxTbeqM9rWvpEertHFOsBXrktXAIAiJaq9TDJVDKrUE5IIrIpRSkFFoIBcVUad54ghhAQgOFJJq8/eM3e5599p29zOzus7P7/D7nPOd5npnZ2dn5fXd+s7OzOzNQFEVR+sgi4ATz+3rgBy2WRVEURQkP9RNKU6i2lDpQHSltoLpTFEVRrOwMPAFsB6aA7wNzWy2RoiiKEhLqJ5SmUG0pdaA6UtpAdacoiqJkciXiHD4BfNr8/vtWS6QoiqKEhPoJpSlUW0odqI6UNmhcd1cCTwPz6sxUsTLKul6MiOWkEexr3FG7tsNypC5OrCGvkGzoYuMPmLRnJsLOM2HHVyjjqOiL/+mrFscJtaFSREh2HSc/oQgh6S9GtTUa+u6fuqyjvvRj+4Brf6hx3R2BTJVbXkdmFu5ECrukofy7RNN1beMbwFPA/BHuc9xQu7bH1Uj7srBiPiHacBxsnFfvXfMdfdbiuKA2VPII0a59t3vX/ECThKi/LtM1bal/CpOi+uyaznz5LnKcyc/TwD3Ae0dclqD0dhvwPM08izrD5L2NQA62ZZqs6yzehIj99BHuc9xQu7bHvsBBSFtThRBtOA42zqr3LvqOPmtxXFAbKnmEaNc+272LfqBJQtRfV+mittQ/hUlefXZRZ778HBlw/CwwAZwNrAJ+idj+kyMsSzB6W4hUytcazH8KXbkBmq/rPB4H1gE7eG4fIXZcWlN5+oTatfuEbMOqNm6SiGr6yav3cfUdIWtRKUeXbRihPiGLkO0a6rkbUd1HjKMfsBGy/rrIuGpLdTRMRHP92Di+CzqLqFYPrzPb/9ASd4qJm/TM25dW9DYz9f/PkNHW6xra32Lz/Z8l0p6OGOL3LXH7mbivVyjLUuQ41wNbkCmF3wHenUo3AxHc3cjo7MvAw8AHc/JeYsr2BLAZmSr5AHBOIk2Zup6FvIjvEZPPz4AvAzsB/4v/iXotcidkmef2VVC7ql19KFP3bzH7OS+17VtN+Arg9ch0+w3Ai8B9wJtT6UO2YVb8rsDnkKnaa00ZfoFo+z7gU4S/Yk5evbv4DmhWj6rF0aA2VBsWsZRm/H3XfX1f/cQ4XEOU0R6Eq78PIDN6NiDaegG5oL4VOAvY03OfTROKtsra3+afXH0TqI7qpqg+Q9FZ0xxhvm3Heav53iMV3vTx2vQWMfxo6/7IQOBqpK1+FjmP9vbdaXqgbRkynfF+3wwLiCv+P0qkPdx824wU5/OwZzkuAu4CjgFuBy40/xcDv5VINxcRxOXAq4ArzO89gZXYpyCeDvyryesOk/fNwGzgdxPpiup6R+AW4IvAVuBLwLeQk3gV8Cv4H//3zPdbPbevgtpV7epK2bqPy5DezyLzvRB4EJmufQXS8T0SuAlZyjkmZBtmxe8N/BVS9gWmDLOQC6sjkc7YvYT9Yta8enfxHdCcHlWLo0NtqDbMoyl/3wdf31c/0fdriLLagzD1dzpwjQnfA9HWfOQi+Tjgb5BHHUMkBG252N/mn1x9E6iO6qaoPkPQ2SjIO84DzPfjqfCmj7dMf2gl8FXgUGAOsBuy2MgdyHlYiXnIc7OPVs0oh7uREcPfLpF2LfBMRtwKk8+7PMpwjtl2FdM7E/GJHPMN7M/0/ipyZ2ozsEsi/NVIHd6DNE5pdjffZer6a2bfp6bCj2Yw8vqXOdvn8Uqz/QOe20f4TylVu6pdXShb9wD/ZPZzYCrNtQxewnl4Ku4Ghl9KGroNs+IPQZzWFciMhb8GPoN0Vn6RKFe6zHUS4a+fonp38R3QjB5Vi6NFbdi+DSOqPT7SNX/fF18fqp+IqKanPl9DuLRNoervZ4m8H0RW7TsTuMSk3Ua4j6G3rS0X+4PdP7n4JlAd2Yhorh8L7eusLBHV2uq7zPZHpcJ3Q2aLTTF9QYSmj9emt4jhGW1TyM2TvwX+KxX+/gr7BgbPDd9WNaMMZgKbEBEW3S3b1ZTl1oz4eCUL16l8i5AT9EHsDVmSd5l9XJ8Rf4WJPzYRFjc8lxbkXVTXbzTx38mIf8LEH1OwnzxeRhozHyL8TkC1q9rVlbJ1D3IB8QLTZ+quMXm807LN2SYuvsPRBRvmxe+DPLryEaTzcyri9GNHcUeFchUR4e+Y8+rdxXdAc3pULY4OtaHQtg0j/M/pLvr7Pvn6EP1EhL+e+n4N4dI2haq/5xlo6Dct2+yOXOyGRgjacrE/2P2Ti28C1ZGNiGb6sRCGzsoS4V8P8YIPU8AFyEIIZwFXmvBNwF+kthnV8ab1FjE8mPZ1BouL7Mpg4YYp4PMV982RJqOs54onU4Up+lyV2v4gE/5YibIsM2nPzoj/OfLcdpLTEOe3CRkRvRl5Rj3JPzJ9ECWLm0zaxRnxF5r4tyXCdmcgrpuQ0c9dpm9aWNexA84a8X7AxMd5fxQZed1kPv+GvaFN8iQioCImcbP7ypy8fOx6FGLLn5pt32PZrq92BXfbdsWuAHshdfIM0vg9inQ2kpSt+3lIB/ieVPh8E74O+8pM8fLo+5v/XbChLX4X4EbkJax5dlyTk68LkwX7cdEP5Ne7i+8Adz2W8R2gWsxjkmr9gzSuNizjJ0BtmMck9Z7TPn6hjE9o0t/3yde37ScmC/bhqqe+X0OU1R7Ur78JptujaHDepq/4mKeQQaDbgYuB5cgjtembFr5MWspbxd+EoC0X+9v8k6tvAtXRpKW8TfVjIQyd2Zik3no4MGfbF5n+GDT4He+EJX9XvUWp7dOPlSZnV15WkLeVWYnfL5vvORlp4xczluWnqf8uzyXnvSxwf2SUMT3quRT4CiKyGci0v9uRqfLPmTTHARuRKY1FHI2INeuFhXuZ758kwp4FfgeZ4voO4Hik4fsucAbwkElXVNfHIaL6Xkb8PsgUy43m/3rkEYAfIcf+QaQztRjpuNmYmyhHHhch75ZIchhwAtIIT6biVufk5WPXecgLOC9Hpj7b6Ktdwd22XbHrq5B6uBe5mHjGpH06la5s3R+KON+HUtsfZsJvQxrKNIcjL3Nda/53wYa2+EsR2xVR+R0Dhjr1A/n17vp+Nlc9LqXYd4BqMY+q/YM0rjYs4ydAbZhH3ee0qw3L+oQm/X2ffH3bfqJuPfX9GqKs9qAZ/a1hePbKtoxtY2z6Ohl5Z9fRyMDPsQwPSv4YmelXdSC3bn8TgrZc7G/zT66+CVRHo+zHQhg6s9FUW30ZcJL5vSvi0y5EXlXwWmRgOcbneKEevSWZTP3fkvhdeYB3b+TkvLdqRhlcZPL/eIm08XPm+1niPmbi0iuwpIlH9483/+eY7cq8SG9nk/aRjPgdkIZ7A/Y7ByDTypch08ankEY07rzk1XVcznQHP+b1Jj7vYgLkpDo5I24mckfziYI8sohMGZY6blfVrlNMn6kwbnaFbNt2ya7nkO28s8ir+3g/UWqbT5jwUyz57Yy0EclOc+g2tMXvxPD05juRl43GS1hfn4ibNGG/lwiLV+A5hsFMB9uqP0VE+OkH8uvdxXdA9XYm7TtsqBabpYoNbX7ChtqwmAj/c9rVhmV8wij9fZd9fV1+AmTRhKmMz0SJY0gS4a+ncbqGyNMe1K+/CcrPGIdi/e2LtMGnIbNfNjKsO2imD+JL29pKU2R/m39y9U3QDx1dY/6nFx+4z4Rf41AeaK4fC+HpLI8I/3r4gtn2I5a4WNPpOvA53gmq6y1i2KctSG0zmYhbmQj3ar9mIHcOs15EV5V7zM6PLJF2NbK6SZrZDN4jUdSR3suki6e8zjX/nyyx/x0Rcf4kI/4kk9e5JfKCwbHva/7n1fUrkM7Quoy8YpGekRG/A7JCxlbgDRlpDqZ8R89GhN8JWNWutvhxsSsU27ZLdv0B8rz71UidrUYa06wBzjTpur/M/P+NVLrLTfgRTOcoE3dBIix0G9riY+cef5Yn4vZAjsV2AXWxCVuLrIK21vy/JKf8eUT4O+a8enfxHVC9nUn7jiLGVYtNUsWGZQfakqgN7UT4n9OuNizjE9ry913z9XX6ib2Qi9j48z+JdJ/OOQYbEf56GtdriLT2oH79TQAvIce2FhmcWJBTpiz9Hcpg4DbJcgaa2ZQIr7sP4kvb2ipTtqT9bf7J1TdBP3S0JBF2iAnbh8GAh+0xxTwimunHQtg6SxNRva1+syXubSbuvlS4z/FOUF1vEX4DbeDZfq0yiQ7IS+TBTOS53F8izqqI+LnvhYmweQxWWZkCXleQx3XInafkyRob632W9AtTae9neKQy5ljkWNYy/DLGRRllOgAZhV/H8LTDvLp+3MS9JRE2A1kdKj7+d6S2eQODOt5oiU/yIZPHx3LS5BHhdwJWtWtWw9Jnu0J523bJrpvNZwVSxx9CjvGjiTQudb8amQ48K5X2EWTqr+3FxZ80ZfujVHjINrTFz2L4Tt9zyIpy5yIOKOlEJhPb7QT80ITH7yH4EXIXzIcIf8cM9np39R1QvZ2x+Q7V4mipYsMsP6E2dCfC/5x2tWEZnwDN+fs++fo6/USSVyOPbU0h75Ar2ybHRPjpqe/XEK7ag3r193bgDxH9LUMWw3gKeVzLRpb+HkMGGa5DBs1PQx4VSw7iJgdu6u6D+BCCtlztb/NPPr4J+qGjeHXIeCDx4+b/k9gH7PKIqL8fC2HozIUI/7b6BWQVa9sjtK9ANL2d4YUNfI63Dr1FifyncBto82q/TjQJ0x2bqvy6yfdFZKTP9kne2TnLpH8Ked74SuSE+TYy3fp58me+nG+2TQv9nchdpu3ALcB5yNLFD5j0SY5FTojNZv/nmv1vRxxkOu/LTdz9yHswViBTIV8yn/TLU/PqOhbDSyav85GT5cfA903cnqltdjRlOsLs+1myX4J4jTm212TEFxHhdwJWtWvWBVSf7Qrlbdslu25FOupJViCzGmLK1v1sk196ifA4POsdKVeZch+YCg/Zhlnxn2LYWcSf9Qze2WG7gFrM4HGibdjvQJUloloHxVbvrr4DqrUzWb5DtThaqtgwy0+oDd2J8D+nXW1YxidAc/6+T76+bj8B8vjZQyZ+DbBbxr7ziPDTU9+vIVy1B83oL2Ye8ljr8oz4LH09hl1fyU/60cY6+yA+hKAtF/vb/JOvb4J+6OgUE7YBGXy82/z/XE7Zsoiovx8LYejMhYhqbXXee9ziwbNkHVU9XvDTW4T/QBt4tF87IqNy/16U0JE/pfikSb7kbg7wRaRyX0JeHHgy8rK+7cC/5Ozr80hFH5IRvwRxkM8hI65PIR2MP7akPRp5DvxF83kU+CzS4UjzbmRFojXIlNatiIEuAX7Nkr6orpcjzniL+T4fOf5nKH6xJ8gLEG3TF1+J3Am5sUQeWUT4nYBV7ArZF1AwPnYFu227Ztd1TD+GPwH+L/G/bN0vNuW+OJVfHP4PGeW2LY8O4dqwKP7DyEXpVmT6+lXIFPqVZF9AncBwO3xiyfLbiKjWQbHVu6vvAP92Js93qBZHSxVfkeUn1IbuRPif0642LOMTYprw933x9U34idlIHcYXQgtKlj9NhJ+e+n4N4ao9aF5/dwFftYTn6esE4MumTOuRQcYtyCO0NzBYYTW9TV19EB9C0JaL/W3+ydc3QT90NB9Z6GEKad+2md8Hlyhfmoj6+7EQhs5ciKjWVl+ak+Y9Js2dibCq4wMxrnqLqDbQ5tV+nWYSLyqTODC+RHVxjRLXun6NSX9LibR3Yl/SOp5Su6TkPkMib6AtJJq0K9ht2zW7Xs3w0uQgdzTSsxfaIkQb1m3jPRlMxX/YfG+knRlMMW35n5B9R4haDJkQ/YTasJjQfYKNEO1at91nAv9s8txEN68NyhKyH7DRlP7mIAOIn7HE1amvEPsgTRGytrquI5CBuSlkQHuK6TPSR0mb4ygh66xpRqk3qNB+zUHuLN5cY2FGwVeQTsAxyMHHn1E+6++Ka13Hq1yclQo/FxHPAuRZ5RXICPDbU+nmIqPFq/yK2wrzkWWGD0OO/VTze9+8jVqmLrtCOdt20a5vRO4Gn4FMbX4vckeq7sfWfQnNhnXbeAYyJTvukMxG7sBNIXeEKi9j7Ukb/id03xGaFkMkdD+hNiwmdJ9gIzS7NmH39zO4W7+B4YUR/rzG/bRN6H7ARl36uwCZffda5NGnbyJ1sV8qXZ36CrUP0gSha6vLOoo5hOGZRbYVL0dFW+MooeusbtrUW+X26yjgTOR5166QNRVzosUylcGlrs9EjukPUuErkZN6C/IowO3AcZbtD0bqY4FXSdthKXa7rmyvSKWow65QzrZdtCvI+04eQaaF/zeyPHnZVUdHQUg2rNvG8VLwmxlMrz8ImVo9hbzLpy1G7X+64DtC0mKILCV8P6E2LCZ0n2AjJLs2YfeIbrSRVenqMdahv2uRC9GtyHuRbsA+I6ZOfYXcB6mbLmirqzpKchcDTWW9EH9UtDGO0gWd1Umbehun9musuBEx4IKWy6HUi9q1+6gNlVBQLXYftWE/UbsqbaL6U+ogVB39HVKu69suiKIoiqIoiqIoiqIoiqJ0kQ8jg39bkcft39RucRRFURRFURRFURRFURSlm6xEZrKtp1/vjVQURVEURVEURVEURVEURVEURVEURVEURVEURVEURVEURVEURVEURVEURVEURVEURVEURVEURVEURVGUXvH/Lc2lyz1RY8MAAAAASUVORK5CYII=\n",
      "text/latex": [
       "$\\displaystyle (- L u_{2} \\operatorname{cos}\\left(q_{3}\\right) + u_{1} \\operatorname{cos}\\left(q_{3}\\right) + u_{6} \\operatorname{sin}\\left(q_{3}\\right))\\mathbf{\\hat{a}_x} + (L u_{2} \\operatorname{sin}\\left(q_{3}\\right) - u_{1} \\operatorname{sin}\\left(q_{3}\\right) + u_{6} \\operatorname{cos}\\left(q_{3}\\right))\\mathbf{\\hat{a}_z} + L u_{2} \\operatorname{cos}\\left(q_{5}\\right)\\mathbf{\\hat{s}_x} -  L u_{2} \\operatorname{sin}\\left(q_{5}\\right)\\mathbf{\\hat{s}_y} + (L u_{2} + R u_{5})\\mathbf{\\hat{n}_x}$"
      ],
      "text/plain": [
       "(-L⋅u₂⋅cos(q₃) + u₁⋅cos(q₃) + u₆⋅sin(q₃)) a_x + (L⋅u₂⋅sin(q₃) - u₁⋅sin(q₃) + u\n",
       "₆⋅cos(q₃)) a_z + L⋅u₂⋅cos(q₅) s_x + -L⋅u₂⋅sin(q₅) s_y + (L⋅u₂ + R⋅u₅) n_x"
      ]
     },
     "execution_count": 37,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A_v_C2p = C2p.v2pt_theory(Ss, A, C2).simplify()\n",
    "A_v_C2p"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Form the expressions that should equate to zero:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 38,
   "metadata": {},
   "outputs": [],
   "source": [
    "motion_con = [A_v_C1p.dot(N.x).simplify(),\n",
    "              A_v_C1p.dot(N.z).simplify(),\n",
    "              A_v_C2p.dot(N.x).simplify(),\n",
    "              A_v_C2p.dot(N.z).simplify()]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that two constraints are redundant, so we can only solve for three dependent u's."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 39,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left[ - L u_{2} + R u_{4} + u_{1}, \\  u_{6}, \\  L u_{2} + R u_{5} + u_{1}, \\  u_{6}\\right]$"
      ],
      "text/plain": [
       "[-L⋅u₂ + R⋅u₄ + u₁, u₆, L⋅u₂ + R⋅u₅ + u₁, u₆]"
      ]
     },
     "execution_count": 39,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "motion_con"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 40,
   "metadata": {},
   "outputs": [],
   "source": [
    "dep_us = sm.solve(motion_con, u4, u5, u6)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 41,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left\\{ u_{4} : \\frac{L u_{2} - u_{1}}{R}, \\  u_{5} : - \\frac{L u_{2} + u_{1}}{R}, \\  u_{6} : 0\\right\\}$"
      ],
      "text/plain": [
       "⎧    L⋅u₂ - u₁      -(L⋅u₂ + u₁)        ⎫\n",
       "⎨u₄: ─────────, u₅: ─────────────, u₆: 0⎬\n",
       "⎩        R                R             ⎭"
      ]
     },
     "execution_count": 41,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "dep_us"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "I use a list comprehension to calculate the A terms for each dependent u."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 42,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAJAAAAAVCAYAAACpO2fTAAAABHNCSVQICAgIfAhkiAAAA+hJREFUaIHtmluITlEUx3/fDHK/F4lchpFHIST6UFIiijRCniTlwSUpL6MkSe7Kg1shD4QX9+KTyAPhgRkk44Vo3M24Xx7WOubMnrO/c85858xs9f3rtM+39tpr/fd31t577X0OFFFEQsgCf3xXdauyKcIl9KZxbPzxKkoClK8BG4A9KRCZC+wGrgMflcjRiG37AL+AXYb8Mk0791p9zCucshVX1NfEiPqu8QfoDxwEXgDfgBpgB9DD0KtHYmID8NxmLIuQr0ycZgPuqY9PQBXxAmip6k825G+A30jnKoGNwEngp+qvLJR0ADLAeyQgOkds4xJ/gDLglfo4A2ymYVBUA70s7XL4ZiA/sqQfQJOBYcgD8PxFDaDzQC1Q6pOVYV9ul2ldTfOo5kW52n4Yo41L/AEuqv0VhnybyvdZ2uVoxQAK8hclgLohU+whQz5fbRwLaDNI6+qbzdCOCrV9JKK+a/yHqO1nNE1jugCfgTqgU0DbHCE5kIuYAbQDThny0VreDmgzVMuqFPjk8xsE1/hP0fISsnz68Qm4AXQExoUZ+l8CaA4yIi4bcu8B3DHkvYCter85BT5xA8g1/sO1fGypf6JleZihNonQSRftgelIDvHVJ88AI/V+FjKqSoGB+rsEWA6cSJhPifr9hWwKwuAaf5AlFeCDpd6Tdw8zFCeAapDORcUxYGEMfRumITud04a8nIY/YrVRV4ccGVxIwL+JciRPeKB+wuAa/yjIaBmYLPsRZwl7CjyKcb2IYTsf5gDfgbOG3Jv+DyIdziBT/yok+TtOhBHUDDRn+XKJPzTMMN0s9V0NPSvizEBTY+gmhVJgJnI+YXYmKH94C2wHxiOHcIuQg8skYctbguAif5ABDvYcZ5iWthzpH1xPoicho9Kc/iH/g9yvZUUKnEZpGWUGcpE/wFUtpxG8jZ8AfAFuxTGaxb1zoN1IstrHkJcg280fSJJqoi3wDtmi9jPqDqvfJTH5en4/I6fEHSLop8EfCuuDh0QOElt6FzZbL4C+Wo5H/hCQk9o1hv5N5MjdjxFIYnqfxjsbDz+Ac8ACJAfZ66vzRtzP2OzFbyckybUtLbXAOr1Pgz8U1gcPy5XbLiQ9qQLGIm8LHgPr4xrMkv4MVEnTF4f+q8anO0ZlqwLsLNa6A3l8zVWdK4b8LvIi13xhGAWe33yXt3NKiz8U1gc/BiCn4y+RRP85sBPomadNDkdeZYRhE8JncII2uyNLypYEbdqQBn9o2T4EIUdIALnyPVAV0Q7q4mAmsmT0DVNMAGnwh5btgwfr90AZn9IgGidltaTzTVAR/x86AmsNWWUr8CiiiCKK8OEvjoJPCmCClVwAAAAASUVORK5CYII=\n",
      "text/latex": [
       "$\\displaystyle \\left[ - \\frac{1}{R}, \\  \\frac{L}{R}, \\  0\\right]$"
      ],
      "text/plain": [
       "⎡-1   L   ⎤\n",
       "⎢───, ─, 0⎥\n",
       "⎣ R   R   ⎦"
      ]
     },
     "execution_count": 42,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A4 = [dep_us[u4].diff(u) for u in (u1, u2, u3)]\n",
    "A4"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 43,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAKkAAAAVCAYAAAA93c4hAAAABHNCSVQICAgIfAhkiAAAA/VJREFUaIHtmluoTVsYgL+9NnLfbkUiHGx5FDokWigp0VF0IrdSkvLgktR52eokSe7Kg1shD4QXjkuxJPJAeGC7JMsL0XY7bNft8vD/M3ONPedcc+4951pjb/Or2ZjrH/8Y8x//+ucc/xhzQkpKCyIL/HAd98pqTcrvRi8K4++HU5HxUL4ErAN2JmDILGAHcBn4Xw05FLJtb+AbsN2Qn6fx4F7oNWY33+SycgEZz/iQ+rb5qB+wD3gKfAbywFagu4fuByTu1gFP/DrMIsbXxGpmIbf0Gu+AWqIF6RLVn2jIXwLfkcHVAP8Cx4AG1V/RXKPLRAXwBgm6ziHb2OSjwcBz7f8ksIFfN909oGdA2xyuJ6mbLMkH6URgKPIHONcLG6T/AXVApUs2GP/UZKnW5ZtmatmpRuy/G6GNTT46q30vN+SbVb47oG2OMgap1/XCBGkVMl3sN+R/ax+HPdoM1LoPTbawvMxB7D8YUt8mH/2h/T6mcUrZBXgP1AOdfNrnKJKT2sg0oB1w3JCP0vK6R5shWtYmZVTCBI3NC5t8NEnLc0ia4eYdcAXoCIwJ01lLCdKZyJ133pA7f8ANQ94T2KTnGxK0K0miBqlNPhqm5QOf+odaVofprE2zzUme9sBUJN/65JJXACP0fAZy91YCA/R3BlgGHC2ZpfGRQcb2DVlsFsM2H1Vp+dan3pF3C9NZlCDNI4MLy2FgXgR9P6Ygq9sThryaX85YZdTVI9tdZ2K4fhjyxOubaiR3u4OMpRgtwUduKrT0XByZRAnSRxTepcV4GkE3iJnAF+CUIXemsX3AYj3vASxEVpBHgEHINk7SxO2bpkz1NvnIeVJW+dR3NfQCiRKkkyPoxkUlMB3ZXzMH5JVrvQK2AGORTer5yMuDpInbN355pBc2+ui+ln4551At/XLWAmxfOE1AEnxzGoPgP3KPlnOSMKoEjNQyzJPURh9d1HIK3ltQ44CPwLWoHWexb590B7J46G3IM8hWxldk0WDSFniNbH/0NeoO6HUXRbS3VGSQfcQGoEMIfVt9FNtmfqlX93/pAdBHy7GIU0Delqw29K8ir9fcDEcWCrfxzgW/AqeBuUi+tstV59zZDZGtLw3DkU3uevyn4TpgrZ7b6qNlatd2JB2qBf5E3jo+AP5pSqdZkn+S1tD4Qwf3kXfpjlbZSo9+Fmjd3oBrzVKdC4b8JvJxi9dHDjbgjC3ocFbktvuoP/IG7BmysHsCbEMWb0HksOS1aDHWI/YMirHPbsjUuDHGPstJa/VRjiJBasv3pLWE28iOwnRk6utTTLGF0Jp85Ps9aYVLaSCFiXIdyXxTmpLiRUdgjSGrKYMdKSkpKa2Qnx9mT1ENR4SKAAAAAElFTkSuQmCC\n",
      "text/latex": [
       "$\\displaystyle \\left[ - \\frac{1}{R}, \\  - \\frac{L}{R}, \\  0\\right]$"
      ],
      "text/plain": [
       "⎡-1   -L    ⎤\n",
       "⎢───, ───, 0⎥\n",
       "⎣ R    R    ⎦"
      ]
     },
     "execution_count": 43,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A5 = [dep_us[u5].diff(u) for u in (u1, u2, u3)]\n",
    "A5"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 44,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAFgAAAAVCAYAAADCUymGAAAABHNCSVQICAgIfAhkiAAAAdZJREFUWIXtmD0vBEEchx+iEMKdl0JB44KvoNzofQOFL+A7uIoohEShQnwCiYpCRqHWOgqnIRKNtwsSTrGzubV2dmd3xs2d3JNcJtn/5P/M/e4y2Rno0DQ8oB76XDhdTXsxys/s6kGhO2byKVAGtmJq48AOcAu8A1VgAxiyutzW8GZx1vAzKwM3qoYefvLLinoJuJdzDoBV4ITGv30k6zfQxIXXxCkI/YPDeCQHfCTrS5Hn6/L5tsbC8+DCa+IU5Ah4Utau+b2tDAAvwCvQn7TqHLjwmjoFKXtwHHNyPAa+IrVn4AzoA2Y1++niwmvVqRvwjBwvFfUrOU5r9tPFhdeqUzfgghwfFfXgeVGzny4uvFadugGn0SXH2M39D3HhzeTUDTj41QqK+mBkni1ceK06dQOuyFG170zJUbVv5cWF98+cHurXtBLpry417L+mufCaOgVNPGjsydqioqcOWb0unGEEOQOOHh9XaBwfK8QfH/dlfSFhQWlk9bpwhhHkDBhgAtgF7oAP/IuNTWBYMf8ceML8UiaL14UzjMAg4CwUgU9gzVK/VnVGEaQEbOs+eB54A8YM+7S6ExLug3tCk6r495kBD4bSQ6DXsEc7OKFxH9yhwz/jG8323J07M5FdAAAAAElFTkSuQmCC\n",
      "text/latex": [
       "$\\displaystyle \\left[ 0, \\  0, \\  0\\right]$"
      ],
      "text/plain": [
       "[0, 0, 0]"
      ]
     },
     "execution_count": 44,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A6 = [dep_us[u6].diff(u) for u in (u1, u2, u3)]\n",
    "A6"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "By inspection it is clear all the B terms would be zero."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# P4.13"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Repeat from 4.8:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 45,
   "metadata": {},
   "outputs": [],
   "source": [
    "R = sm.symbols('R')\n",
    "q1, q2, q3, q4, q5 = me.dynamicsymbols('q1:6')\n",
    "u1, u2, u3, u4, u5 = me.dynamicsymbols('u1:6')\n",
    "A = me.ReferenceFrame('A')\n",
    "Y = me.ReferenceFrame('Y')  # intermediate frame so that simple rotations can be used\n",
    "B = me.ReferenceFrame('B')\n",
    "C = me.ReferenceFrame('C')\n",
    "Y.orient(A, 'Axis', (q1, A.z))\n",
    "B.orient(Y, 'Axis', (sm.pi/2 - q2, Y.x))\n",
    "C.orient(B, 'Axis', (q3, B.z))\n",
    "O, P, Cs, Ch = sm.symbols('O, P, Cs, Ch', cls=me.Point)\n",
    "P.set_pos(O, q4*A.x + q5*A.y)\n",
    "Cs.set_pos(P, R*B.y)\n",
    "O.set_vel(A, 0)\n",
    "P.set_vel(A, P.pos_from(O).dt(A))\n",
    "Cs.set_vel(A, Cs.pos_from(O).dt(A))\n",
    "Ch.set_pos(Cs, -R*B.y)\n",
    "Ch.v2pt_theory(Cs, A, C)\n",
    "u1_eq = sm.Eq(u1, C.ang_vel_in(A).dot(B.x))\n",
    "u2_eq = sm.Eq(u2, C.ang_vel_in(A).dot(B.y))\n",
    "u3_eq = sm.Eq(u3, C.ang_vel_in(A).dot(B.z))\n",
    "u4_eq = sm.Eq(u4, P.vel(A).dot(A.x))\n",
    "u5_eq = sm.Eq(u5, P.vel(A).dot(A.y))\n",
    "qdots = sm.solve([u1_eq, u2_eq, u3_eq, u4_eq, u5_eq], q1.diff(), q2.diff(), q3.diff(), q4.diff(), q5.diff())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 46,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAALwAAAAgCAYAAACo7RKTAAAABHNCSVQICAgIfAhkiAAABSVJREFUeJztnHuIFVUcxz93jdZyhbI/ksIUi7BFwVzJ+iMZLIjSkiAo/wgvEW70+qvo8U9S0DsoLSFq8WqFZVRSQUGLFoGmlZVRFBY9yGwreiyS2eNOf/zO4Z47zbn3nnnszLDnA8PMeX/ne8+cc+Y4a43iOBNYqa63Ap8WqKVseG/sVNKb6cCXQBMIgU+AYwpVVB68N3Yq681mRPCNwC3q+rFCFZUH742dSnpzBSL0DiPufhV3cSGKyoP3xo73xuPxeDwej2fiqCPrLH3MKVJMiajjfbFRJydv+rKqyOOpAr7DeyYVk6nD15HpMShWRumoM4l8iXb425GbvzQm72yV9mLKNmvAMPARcBj4EdgInJSy3jzxvtiplDdHRcKL1Pn9mLyL1fkD10YirANWGOGpyChzHnAOcCBl/XngfbFTKW+iI/wQ8DPwbUzerMSvAF4F7gTeNuJnAetT1p0X3hc7lfVmBjL9vG5Jf0Olu04jddq3mEaMtD5gh5HWBGY61u+qI3As532xUzlvzBFeT03vWfIuQtZO36vwUuAVFQ6By3psc5Nx3QSeMsI1Q0cavqbdsBBZ80G7WfpodKjL1ReA24B3gXHgJ8Sn+V00V80XSObNdcA+xJtxYBewvEs7mXljruGH1DluLTYXeZrNJ3ka8hKxEXih1waBsS7h4x3qsvEwcFwkbiHyHfUm5Ic3+bBDXa6+gIyWG5BOX0Om4lFgEPjF0k7VfIFk3nwH3ArsR7xZDWxTde2ztJOLN88iT/XsmLTrVdrdlrKdRvg67aPG0kj6VZH05cAlRvh8lW8Zre+g43YEuqF1BI7l0viiGQD+pf2rPq3HxReALSr8TiT/ThW/pYuWKFpH4FgOsvEGZBAYjtHk6s2cSLx5rIX2Jc084G/gm0jl/YaYvT2I78Zq47oPuNIIh8ho8TLwuIp7AlmjjSAjwgjwUgY6eiULX6Yj92ob3aE3X0BmDoAlyIwBcDJwtro2p/+8SevNFGAVMiDs7JCvV2+OALuN4ysj3+FopXtU4dONuGnAM7SeklMtglxG+BBZ064F3orEbzPKHQt8puJ/UOf9iDlJ0DoCx3JpfNE8h+xUTInR4+oLyNQfAg+q8A0qfCDSRi9oHYFjOUjuzQLgEPAP8CtwkUVTEm80JwJfqDy7iPnLqLtU4kFkFNmMGPga8tLxGzLCxuHS4bfH3EyIrO1mRcoOIaaEyJJgiaWNXtA6AsdyaXwBeECVPc2iJ4kv16i0MeQ9THeC+xzvzdQRJCib1JujET8WA/cg25rmS73WlMQbkBl1r8rzOXBCnPipwCNK6B/Im/cw8pLTBN7scOMuHX4uMiJ9DPyJ7GI0kGk5yspI2VUdNHRD6wgcy6Xx5SGkUw7GpGk9SXwZAH5X5dYgg0EInNH7bf1PR5CgbBpvTEaBJ2M0JfGmn9YDcpCcvkLt1OGTMhO5sRBZDoTI9Bf3RJeRddg7exY8inhySJ335NTORLAdeDqDevqA5xE/xpH/4SAzBpAtrYWqgZvU9SkZ1F1DpkX9Q/YjLyEhsl9c9o/eNiCGL0MeXH0kff+IY5D2UfDaDOvOk3uBc5GRdwGypGkCF2ZQ9+W0/BhDdrL0cXXaygPaDddHI23FyF+lh8j0pafpecibdgjcnEEbedJxayxDdtDyaUbGdedFA9nVOYL8o9QocEFGddeZOO89BbAe+TG3Fi3E48mTNch23F/IcuCsYuV4PPnSoLUtl3pt6vF4PB6Px+PxeDxl5T9GEEa64aWbHAAAAABJRU5ErkJggg==\n",
      "text/latex": [
       "$\\displaystyle u_{1}\\mathbf{\\hat{b}_x} + u_{2}\\mathbf{\\hat{b}_y} + u_{3}\\mathbf{\\hat{b}_z}$"
      ],
      "text/plain": [
       "u₁ b_x + u₂ b_y + u₃ b_z"
      ]
     },
     "execution_count": 46,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A_w_C = C.ang_vel_in(A).subs(qdots).express(B).simplify()\n",
    "A_w_C"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 47,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAALMAAAAgCAYAAABZ5kkeAAAABHNCSVQICAgIfAhkiAAABDZJREFUeJzt202oFWUcx/HPtUAqTStIy4RJ6c1dBbWIIKqFRGZBUUGLsxATQVuFS6VNJBGFRUZlVgvBNkVBBpG9gL0sioois3cqu0VUYmVRnhbPczhz7z3TPefM3Dtzr88XhmeelzP/35znP888839mRpTnQqyO+7vxcQXHrJqk8RhlCT7DPuzBo//Tdj4+x1G08RFOmGqBA5I0zj7WC775suCrNxU1zIQ/dE0fB30qtt2ITXH/oZJCqyZpnL1kJvHVSRtEbontNufKtsayVWUUVkjSOLvJVOTMiUTdZHK+OqdWKYlEhSRnTswaBnHmljCkd7ZsCvSUpSVpPGZJI3OiiLOwA9/jL3yF+3FKU20eP2WyEjOZ5cI6w+l4Dp/gEtyBlbgMP88Em5ne0YyW5t8eW5LGKnhJ0LZhXPl9sXx7Q2xmhgzNtYzthLNxO97Hn/gRT+DMQc6gYlqSxrIsi7q+NHEaOh+H8TtOaoDNTEXO/Py4fGf7RlgSr4OWpLEsa6L9RwrqOyPoVQ2wmakoznwtXsBdeCNXvhTbShy3SpLGwTkvpp8W1B+I6blNs1nGmXcIy62bcQVezdVdj8Uljl0VSePgLIjpbwX1nfKFTbNZxpmfzO0fxdO5/AguKnHsqkgaq2ckpu2m2SzjzKOT5KcyHtkvSePgdEbBBQX1J49r1xibZZx50ST5X3Gd7gPN1bH8St33dm8oYb8f+tG4K2p5a1zdvli+a2qkFWrqpTHT+yGxjS0V69kf06L56TkxLZrfNspmpr9oxuO5ujnYm6s7qjvX264bdlkc0zYeG1RYHwyj8fJc2YrYdonuBbeyARrPEC62zvZFrs2mivUtN3mY7A/VhuaGtZmpKDTXCSttwWvjyp/N/e5EYTWnjR9iegDzetjeGetbReImYViNH8Sye2N+Q8x/h+MaorHDIuHrijbeNPGLlLL6GHwBow6bVOjMr5jYKW18K4SV8lyMf2L9v7i0wHbni4vbisRNwrAa18XyUWGJv+NU9zRII2GUejfW78dpU6CPMFKO6l5Qd+d09rJbh00qdOZlwlX0IY7gJ+EK7RXoXz3ut7cW2H4Phwz/0DOsxnnCw0Uba4ULro0LGqRxrm7nHlS8DF5WX4elwkrkQfyNr/EATm2ITWr40mSx0EFt4aTb+MXEUWeh4ERbK7I7KA8K2g7H9J0eberSOAfPCLoOCV9y96IOfXX2W2YanXkEL+o6x1y8HfN7jZ3srxJGproWMlYYO2Ku79GmLo036+oaNfZhMN9Pdeirs98y0+jMG+Oxjujess8XXqhp484KbFRJJ5JwRPGtrQ5aes+r26oPzc0kMtM8zZhJbBPOfXfdQhJ9kcn5ano5P7AW18QtH6JLzHCWCHHh14W3uR6uV860sFM3HHYs3pFmGusE39wj+OqN9cpJJBKJRCKRSCRmBv8BeYYqdDKJMb8AAAAASUVORK5CYII=\n",
      "text/latex": [
       "$\\displaystyle \\left[ \\mathbf{\\hat{b}_x}, \\  \\mathbf{\\hat{b}_y}, \\  \\mathbf{\\hat{b}_z}, \\  0, \\  0\\right]$"
      ],
      "text/plain": [
       "[b_x, b_y, b_z, 0, 0]"
      ]
     },
     "execution_count": 47,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "[A_w_C.diff(u, A) for u in (u1, u2, u3, u4, u5)]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 48,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle -  R u_{2} \\operatorname{tan}\\left(q_{2}\\right)\\mathbf{\\hat{b}_x} + R u_{1}\\mathbf{\\hat{b}_z} + u_{4}\\mathbf{\\hat{a}_x} + u_{5}\\mathbf{\\hat{a}_y}$"
      ],
      "text/plain": [
       "-R⋅u₂⋅tan(q₂) b_x + R⋅u₁ b_z + u₄ a_x + u₅ a_y"
      ]
     },
     "execution_count": 48,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A_v_Cs = Cs.vel(A).subs(qdots).simplify()\n",
    "A_v_Cs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 49,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left[ R\\mathbf{\\hat{b}_z}, \\  -  R \\operatorname{tan}\\left(q_{2}\\right)\\mathbf{\\hat{b}_x}, \\  0, \\  \\mathbf{\\hat{a}_x}, \\  \\mathbf{\\hat{a}_y}\\right]$"
      ],
      "text/plain": [
       "[R b_z, -R⋅tan(q₂) b_x, 0, a_x, a_y]"
      ]
     },
     "execution_count": 49,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "[A_v_Cs.diff(u, A) for u in (u1, u2, u3, u4, u5)]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 50,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle R \\left(- u_{2} \\operatorname{tan}\\left(q_{2}\\right) + u_{3}\\right)\\mathbf{\\hat{b}_x} + u_{4}\\mathbf{\\hat{a}_x} + u_{5}\\mathbf{\\hat{a}_y}$"
      ],
      "text/plain": [
       "R⋅(-u₂⋅tan(q₂) + u₃) b_x + u₄ a_x + u₅ a_y"
      ]
     },
     "execution_count": 50,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A_v_Ch = Ch.vel(A).subs(qdots).simplify()\n",
    "A_v_Ch"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 51,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left[ 0, \\  -  R \\operatorname{tan}\\left(q_{2}\\right)\\mathbf{\\hat{b}_x}, \\  R\\mathbf{\\hat{b}_x}, \\  \\mathbf{\\hat{a}_x}, \\  \\mathbf{\\hat{a}_y}\\right]$"
      ],
      "text/plain": [
       "[0, -R⋅tan(q₂) b_x, R b_x, a_x, a_y]"
      ]
     },
     "execution_count": 51,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "[A_v_Ch.diff(u, A) for u in (u1, u2, u3, u4, u5)]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# P4.14"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 52,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left[ R \\left(- u_{2} \\operatorname{tan}\\left(q_{2}\\right) + u_{3}\\right) + u_{4} \\operatorname{cos}\\left(q_{1}\\right) + u_{5} \\operatorname{sin}\\left(q_{1}\\right), \\  - u_{4} \\operatorname{sin}\\left(q_{1}\\right) + u_{5} \\operatorname{cos}\\left(q_{1}\\right)\\right]$"
      ],
      "text/plain": [
       "[R⋅(-u₂⋅tan(q₂) + u₃) + u₄⋅cos(q₁) + u₅⋅sin(q₁), -u₄⋅sin(q₁) + u₅⋅cos(q₁)]"
      ]
     },
     "execution_count": 52,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "motion_con = [A_v_Ch.dot(Y.x), A_v_Ch.dot(Y.y)]\n",
    "motion_con"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 53,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left\\{ u_{4} : R \\left(u_{2} \\operatorname{tan}\\left(q_{2}\\right) - u_{3}\\right) \\operatorname{cos}\\left(q_{1}\\right), \\  u_{5} : R \\left(u_{2} \\operatorname{tan}\\left(q_{2}\\right) - u_{3}\\right) \\operatorname{sin}\\left(q_{1}\\right)\\right\\}$"
      ],
      "text/plain": [
       "{u₄: R⋅(u₂⋅tan(q₂) - u₃)⋅cos(q₁), u₅: R⋅(u₂⋅tan(q₂) - u₃)⋅sin(q₁)}"
      ]
     },
     "execution_count": 53,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "dep_us = sm.solve(motion_con, u4, u5)\n",
    "dep_us"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 54,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAALwAAAAgCAYAAACo7RKTAAAABHNCSVQICAgIfAhkiAAABSVJREFUeJztnHuIFVUcxz93jdZyhbI/ksIUi7BFwVzJ+iMZLIjSkiAo/wgvEW70+qvo8U9S0DsoLSFq8WqFZVRSQUGLFoGmlZVRFBY9yGwreiyS2eNOf/zO4Z47zbn3nnnszLDnA8PMeX/ne8+cc+Y4a43iOBNYqa63Ap8WqKVseG/sVNKb6cCXQBMIgU+AYwpVVB68N3Yq681mRPCNwC3q+rFCFZUH742dSnpzBSL0DiPufhV3cSGKyoP3xo73xuPxeDwej2fiqCPrLH3MKVJMiajjfbFRJydv+rKqyOOpAr7DeyYVk6nD15HpMShWRumoM4l8iXb425GbvzQm72yV9mLKNmvAMPARcBj4EdgInJSy3jzxvtiplDdHRcKL1Pn9mLyL1fkD10YirANWGOGpyChzHnAOcCBl/XngfbFTKW+iI/wQ8DPwbUzerMSvAF4F7gTeNuJnAetT1p0X3hc7lfVmBjL9vG5Jf0Olu04jddq3mEaMtD5gh5HWBGY61u+qI3As532xUzlvzBFeT03vWfIuQtZO36vwUuAVFQ6By3psc5Nx3QSeMsI1Q0cavqbdsBBZ80G7WfpodKjL1ReA24B3gXHgJ8Sn+V00V80XSObNdcA+xJtxYBewvEs7mXljruGH1DluLTYXeZrNJ3ka8hKxEXih1waBsS7h4x3qsvEwcFwkbiHyHfUm5Ic3+bBDXa6+gIyWG5BOX0Om4lFgEPjF0k7VfIFk3nwH3ArsR7xZDWxTde2ztJOLN88iT/XsmLTrVdrdlrKdRvg67aPG0kj6VZH05cAlRvh8lW8Zre+g43YEuqF1BI7l0viiGQD+pf2rPq3HxReALSr8TiT/ThW/pYuWKFpH4FgOsvEGZBAYjtHk6s2cSLx5rIX2Jc084G/gm0jl/YaYvT2I78Zq47oPuNIIh8ho8TLwuIp7AlmjjSAjwgjwUgY6eiULX6Yj92ob3aE3X0BmDoAlyIwBcDJwtro2p/+8SevNFGAVMiDs7JCvV2+OALuN4ysj3+FopXtU4dONuGnAM7SeklMtglxG+BBZ064F3orEbzPKHQt8puJ/UOf9iDlJ0DoCx3JpfNE8h+xUTInR4+oLyNQfAg+q8A0qfCDSRi9oHYFjOUjuzQLgEPAP8CtwkUVTEm80JwJfqDy7iPnLqLtU4kFkFNmMGPga8tLxGzLCxuHS4bfH3EyIrO1mRcoOIaaEyJJgiaWNXtA6AsdyaXwBeECVPc2iJ4kv16i0MeQ9THeC+xzvzdQRJCib1JujET8WA/cg25rmS73WlMQbkBl1r8rzOXBCnPipwCNK6B/Im/cw8pLTBN7scOMuHX4uMiJ9DPyJ7GI0kGk5yspI2VUdNHRD6wgcy6Xx5SGkUw7GpGk9SXwZAH5X5dYgg0EInNH7bf1PR5CgbBpvTEaBJ2M0JfGmn9YDcpCcvkLt1OGTMhO5sRBZDoTI9Bf3RJeRddg7exY8inhySJ335NTORLAdeDqDevqA5xE/xpH/4SAzBpAtrYWqgZvU9SkZ1F1DpkX9Q/YjLyEhsl9c9o/eNiCGL0MeXH0kff+IY5D2UfDaDOvOk3uBc5GRdwGypGkCF2ZQ9+W0/BhDdrL0cXXaygPaDddHI23FyF+lh8j0pafpecibdgjcnEEbedJxayxDdtDyaUbGdedFA9nVOYL8o9QocEFGddeZOO89BbAe+TG3Fi3E48mTNch23F/IcuCsYuV4PPnSoLUtl3pt6vF4PB6Px+PxeDxl5T9GEEa64aWbHAAAAABJRU5ErkJggg==\n",
      "text/latex": [
       "$\\displaystyle u_{1}\\mathbf{\\hat{b}_x} + u_{2}\\mathbf{\\hat{b}_y} + u_{3}\\mathbf{\\hat{b}_z}$"
      ],
      "text/plain": [
       "u₁ b_x + u₂ b_y + u₃ b_z"
      ]
     },
     "execution_count": 54,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A_w_C.subs(dep_us)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 55,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAHgAAAAgCAYAAADZubxIAAAABHNCSVQICAgIfAhkiAAAA0VJREFUaIHtmkto1EAYgL+thaJU8HFosQiDJ/WmHvQsnsRaBEUFxT1ILQV7E48WbxZv9YGUYtVDoR4UFexBFBXqA1RUFEVFEEWrB7UIraCNh39Cpmm2m01mN+vufBA288jM1/zJdCZJjvSsATr0/gjw0kKbtnGOBm3AW2AMGAUG5qi7EHgHTAMe8AKYXy6xhNSLYzcSrxtI/HYUqqh0J/tjNHpe1+0BDuv9kyWKlZt6c1QUiV/RCppdut4RI69P57UnlLNNPToqLAXYUZ0ojPg1ZKriKDsuwDVOKQHOI7e+v6ky+KQlj3OcgbuDaxwX4BrHBbjGSRPgHHAAeApMAl+Bs8AyC162cI4RKKLXwXlmTg6uhtL+9gF53JkFeZyjwtI6eAtwDTgK3DXylwP9Kdq1iXOMQBHvDh40yhqAW0bZNNBaDrki5HGOCkt38Dljfxq4YKRzwNoUbdui7h3TBHi8SHpxirZtUfeOaQLcUiT9A9hKMNxs0vkbCd57bkvRfxziOA5rl/uhsjGdP1wetYJOUY6K6ImYB/TO1XiaAO8LtbPXSHvAI+AKcEbnDSD/TwaRoWcQuJSif1uOp3R6PbBa77cBG/S+OYSWgziOv4EHxvbeqDNZaoeKeJMsf4rfC9wO5V82jlsAvNL5X/TvG6A5ou8hXZ4vVTql4zOdd1ynD+r0J2BelTj6tCBfbHjAPWZ/+aFI+D44LHYzQtQDPiJTfJN1wB9d/he5W6Lwv2zYU0iuCEkdu3T+ONBIcKKPVZEjyKc9j3X5a2BpRPsKSwFegVzpz4Ep4BtydUctzjtCx+4u0PcTYILkE4ukjs3AT31MJ3IResCqKnJsIrgYPlP4LZQiYYCT0opIe8jJ8YDvzL46FyEnts9Sv6VyAnH7pX8fRtTJyrEBuIh4TSBfYBZCUcEA54DrBCesCZkkeMiC3pzktSNXcBYPH0AmWOad1R1RJyvHnQRe48iM39/CcVJUMMA9uq0pguFuJTLz84BDFvqwif8UaQpYkrGLSZ74yyRFhYfo/4l+5G8fyVokBQojfo2ZqlQPncBmvZnLpZqkDVm33kHecpzOVqciDBEsTf7XkasLidcoEr/t2eo4HA6Hw1Hf/AOcdJfRUCW6GQAAAABJRU5ErkJggg==\n",
      "text/latex": [
       "$\\displaystyle \\left[ \\mathbf{\\hat{b}_x}, \\  \\mathbf{\\hat{b}_y}, \\  \\mathbf{\\hat{b}_z}\\right]$"
      ],
      "text/plain": [
       "[b_x, b_y, b_z]"
      ]
     },
     "execution_count": 55,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "[A_w_C.subs(dep_us).diff(u, A) for u in (u1, u2, u3)]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 56,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle (R \\left(u_{2} \\operatorname{tan}\\left(q_{2}\\right) - u_{3}\\right) \\operatorname{sin}^{2}\\left(q_{1}\\right) + R \\left(u_{2} \\operatorname{tan}\\left(q_{2}\\right) - u_{3}\\right) \\operatorname{cos}^{2}\\left(q_{1}\\right) - R u_{2} \\operatorname{tan}\\left(q_{2}\\right))\\mathbf{\\hat{b}_x} + R u_{1}\\mathbf{\\hat{b}_z}$"
      ],
      "text/plain": [
       "⎛                       2                              2                   ⎞\n",
       "⎝R⋅(u₂⋅tan(q₂) - u₃)⋅sin (q₁) + R⋅(u₂⋅tan(q₂) - u₃)⋅cos (q₁) - R⋅u₂⋅tan(q₂)⎠ b\n",
       "\n",
       "\n",
       "_x + R⋅u₁ b_z"
      ]
     },
     "execution_count": 56,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A_v_Cs = A_v_Cs.subs(dep_us).express(B)\n",
    "A_v_Cs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 57,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAKAAAAAdCAYAAAAtm2eGAAAABHNCSVQICAgIfAhkiAAABKJJREFUeJztm1uoFVUYgL+9NUrrpBHq6UbRSTOiQI0s6iEsrIc0w6IIhQGFpMhASR/d1YMiUWEEPdTpdJEgH7rZnZSQNEO7QJanOnWgkOzm/YZ2pof/H/ac2bP3mtlnzcyeWB8Ma2atddb/r/P/s2atf61doRimAXfo/evAdwXp4UhGrvZaAmwH+vQaa7n9LmAAGAJ8YBcwxrIMhz1s2es+xJ/eBna0qlgDfmpDQFJeRjqyFFip989mKM8xMmzbywNOtapQIzsHvBfpwKpQ3lrNm5ORTEf7ZGEvjwId0OHwCDlgtTg9HA7ngI6CydoBPWS+EFyXZCzPMTI8crZXWgf8mOEK+sAfwBbgbruq5cqFQC+wBzgBDAJPA+cUqJMNOt5eo1PWn4504nFNRwNTgXnAjcAy4CmbCuZAD7AVmAi8BewGrgUeBm4DbgD+Lky7kVFKe9WIXwX3IJ3YHVO2RMsGI/kenf8J/hDR7aFI/pOa/1zuGtmhU+3l0eYq+BpNd8aUfaDpREMbFeB+4BvgGPI5eBE4P4UeNrkUmI0YIhpcXQUcARYCZ+arlhVKYa92HDBuG+UyTb83tLEOGVGuBs4AJiBvxOfABSl0scUsTT9CtprCHAI+Q7Yir8tTKUuUwl42RsBzgSf0fo2hjduBjcBjyEQ44CLgmRS62OJyTX9oUv6jplNy0MU2pbBX0kVIBTkRATAXGTlGARfrcxV4ANhgaKcXWKT3jwKfADfp8zygG/g9oU42GKfpgSblQf74HHSxSWnsldQBp1A31vJI2RHgLurzila8FLofAl6h3qEKsmp7L6FOeVDR1M9YziDiHElZDyxoUV4aeyX9BAfDea8KriBD+TJkgv4ayUaJvYbnvONuwQg3rkn52ZF6WTEA9Ke49hjaK4290jpgeD7xDxJD2oB0ZmGCdiYZnvcjS/9o8DS4agn1TUq/ps3meJM1bTZHtMXNSHwu6bXC0F6e9ppL3T63aP4s6ucH70wgZxg1GuOAW7SxmTH1Z2vZ1pgyj+EO9EKorApsDpUNIXOK85BVVnD9HKqzMm1nDASxsl9ofBm7gMPAUcoXhsnTXiAr5eD/2K2pDzzfREaq41hVJCRxElmKRzkN2KcKReND0Q75wDsq49NI/psxbU9SXXxgG40ncfu0zGvVIQPtBKJtyM2KIuw1Fgl4+8iixEciCGfFyPdI6YBXaoNft/ib9VrnQUOHNtHYQR/4DVnah+kCvtTyfmQOEyU4rdtqQm6iB5nbBP/U1SE9s5SbFUXZawbiWD7wL/GjbyAj1U7IDE3jIuoBb2g639DWYuRo97fIpv9fyEprJvBrqN7pyL7sNOSNupX4/dirkLf9XYPcVgwgc6Y+1WM54pTrgOszlJsVRdgL5EDHKL2vIrtMbVGj2BPRVWSi7AMHqcezooxH3rS1OelVtNxOphv4E7HZV5ruo3GUhBIcyb+H+lC/l+ELksWhenOA49QnwnlRlNxOpQK8j9jrC+TrtV2fN9P4lfUwOGDws8xX9bL9s0wTHvmFYRwjZylim+PAFZo3FTm84AOPaN4CxJ82YvhZpsPhcDgcDofj/81/9ZGo1NWEEsAAAAAASUVORK5CYII=\n",
      "text/latex": [
       "$\\displaystyle \\left[ R\\mathbf{\\hat{b}_z}, \\  0, \\  -  R\\mathbf{\\hat{b}_x}\\right]$"
      ],
      "text/plain": [
       "[R b_z, 0, -R b_x]"
      ]
     },
     "execution_count": 57,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "[A_v_Cs.diff(u, A).simplify() for u in (u1, u2, u3)]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 58,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle (R \\left(- u_{2} \\operatorname{tan}\\left(q_{2}\\right) + u_{3}\\right) + R \\left(u_{2} \\operatorname{tan}\\left(q_{2}\\right) - u_{3}\\right) \\operatorname{sin}^{2}\\left(q_{1}\\right) + R \\left(u_{2} \\operatorname{tan}\\left(q_{2}\\right) - u_{3}\\right) \\operatorname{cos}^{2}\\left(q_{1}\\right))\\mathbf{\\hat{b}_x}$"
      ],
      "text/plain": [
       "⎛                                              2\n",
       "⎝R⋅(-u₂⋅tan(q₂) + u₃) + R⋅(u₂⋅tan(q₂) - u₃)⋅sin (q₁) + R⋅(u₂⋅tan(q₂) - u₃)⋅cos\n",
       "\n",
       "2    ⎞\n",
       " (q₁)⎠ b_x"
      ]
     },
     "execution_count": 58,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A_v_Ch = A_v_Ch.subs(dep_us).express(B)\n",
    "A_v_Ch"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 59,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAFgAAAAVCAYAAADCUymGAAAABHNCSVQICAgIfAhkiAAAAdZJREFUWIXtmD0vBEEchx+iEMKdl0JB44KvoNzofQOFL+A7uIoohEShQnwCiYpCRqHWOgqnIRKNtwsSTrGzubV2dmd3xs2d3JNcJtn/5P/M/e4y2Rno0DQ8oB76XDhdTXsxys/s6kGhO2byKVAGtmJq48AOcAu8A1VgAxiyutzW8GZx1vAzKwM3qoYefvLLinoJuJdzDoBV4ITGv30k6zfQxIXXxCkI/YPDeCQHfCTrS5Hn6/L5tsbC8+DCa+IU5Ah4Utau+b2tDAAvwCvQn7TqHLjwmjoFKXtwHHNyPAa+IrVn4AzoA2Y1++niwmvVqRvwjBwvFfUrOU5r9tPFhdeqUzfgghwfFfXgeVGzny4uvFadugGn0SXH2M39D3HhzeTUDTj41QqK+mBkni1ceK06dQOuyFG170zJUbVv5cWF98+cHurXtBLpry417L+mufCaOgVNPGjsydqioqcOWb0unGEEOQOOHh9XaBwfK8QfH/dlfSFhQWlk9bpwhhHkDBhgAtgF7oAP/IuNTWBYMf8ceML8UiaL14UzjMAg4CwUgU9gzVK/VnVGEaQEbOs+eB54A8YM+7S6ExLug3tCk6r495kBD4bSQ6DXsEc7OKFxH9yhwz/jG8323J07M5FdAAAAAElFTkSuQmCC\n",
      "text/latex": [
       "$\\displaystyle \\left[ 0, \\  0, \\  0\\right]$"
      ],
      "text/plain": [
       "[0, 0, 0]"
      ]
     },
     "execution_count": 59,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "[A_v_Ch.diff(u, A).simplify() for u in (u1, u2, u3)]"
   ]
  }
 ],
 "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
}