From cb975938876abbf7e457b028b5a3b4e47575fae8 Mon Sep 17 00:00:00 2001 From: TQ Hirsch Date: Sat, 2 Apr 2022 14:45:38 +0200 Subject: [PATCH] Added motion control calculations --- motion-control.ipynb | 374 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 374 insertions(+) create mode 100644 motion-control.ipynb diff --git a/motion-control.ipynb b/motion-control.ipynb new file mode 100644 index 0000000..be7e9e9 --- /dev/null +++ b/motion-control.ipynb @@ -0,0 +1,374 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 1, + "id": "10e2e893-05e2-4d0d-8a31-454372ef4c65", + "metadata": {}, + "outputs": [], + "source": [ + "import sympy" + ] + }, + { + "cell_type": "markdown", + "id": "97f366f8-d3c5-487d-b38b-5db31804a604", + "metadata": {}, + "source": [ + "We attempt to model motion as a series of piecewise-constant jerk segments (where jerk is $d^4p \\over dp^4$)\n", + "\n", + "For an individual segment, we have:" + ] + }, + { + "cell_type": "code", + "execution_count": 66, + "id": "76a230d3-e50b-450e-bd40-47d7a9632043", + "metadata": {}, + "outputs": [], + "source": [ + "sympy.var(\"p,v,a,j,t,v0,a0, a_max, v_max, j_max\", real=True)\n", + "g_a0 = a0\n", + "g_v0 = v0\n", + "g_t = t" + ] + }, + { + "cell_type": "code", + "execution_count": 67, + "id": "475d7b0c-7961-4554-b1ef-acea504d2010", + "metadata": {}, + "outputs": [], + "source": [ + "def seg_d(j=j, a0=a0, v0=v0, t=t, p0=0):\n", + " res = dict(\n", + " dt = t,\n", + " da = t * j,\n", + " dv = j * t**2 / 2 + a0*t,\n", + " dp = j*t**3/6 + a0 * t**2 / 2 + v0 * t\n", + " )\n", + " res[\"ae\"] = res[\"da\"] + a0\n", + " res[\"ve\"] = res[\"dv\"] + v0\n", + " res[\"pe\"] = p0 + res[\"dp\"]\n", + " return dict((k, v.simplify()) for k,v in res.items())\n", + "seg_p = seg_d()[\"dp\"]" + ] + }, + { + "cell_type": "code", + "execution_count": 68, + "id": "378b1d65-afdb-496a-8217-f17ce9e92783", + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle p = \\frac{t \\left(3 a_{0} t + j t^{2} + 6 v_{0}\\right)}{6}$" + ], + "text/plain": [ + "Eq(p, t*(3*a0*t + j*t**2 + 6*v0)/6)" + ] + }, + "execution_count": 68, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "sympy.Eq(p, seg_p)" + ] + }, + { + "cell_type": "markdown", + "id": "6874bd52-7f23-494b-bf5a-5d01f141cae6", + "metadata": {}, + "source": [ + "We have three machine limits to consider at this point: we have a maximum jerk (derived from the shock limits of the system), maximum acceleration (derived from the torque limits of the servos), and maximum velocity (from the frequency at which we update the steppers). We can ignore the position bounds at this point; the commands to this stage of the motion planner are simply \"move by ΔX,ΔY\", and so we can assume that the source of those commands will never ask us to move outside the range of motion.\n", + "\n", + "This leaves us with the following general motion profile for a single linear move:\n", + "1. $j>0$; Increase acceleration to $a_{max}$\n", + "2. $j=0$; continue accelerating at $a_{max}$ to somewhat before $v_{max}$\n", + "3. $j<0$; reduce acceleration to 0 to reach constant velocity $v_{max}$\n", + "4. $j=0$; proceed at constant velocity to decelleration point\n", + "5. $j<0$; begin decelleration to $-a_{max}$\n", + "6. $j=0$; decelerate at max rate to just before 0 velocity\n", + "7. $j>0$; decrease decelleration until $v=0$\n", + "\n", + "Let's call the time taken for segment $n$ \"$t_n$\". Note that, through a simple coordinate transform, $t_1=t_3=t_5=t_7$. Further, $t_2=t_6$. We'll rename these to $t_j$ and $t_a$ (for constant jerk and constant accelleration), respectively, and introduce $t_v = t_4$ for consistency.\n", + "\n", + "\n", + "There are several possible degenerate cases:\n", + "\n", + "* The move is too short to reach $v_{max}$. In this case, we lower $v_max$ accordingly and set $t_v=0$.\n", + "* $a_{max}$ is unreachable given the maximum velocity. This results in $t_a$ both being 0 for all moves and the calculation proceeding with lowered $a_{max}$\n", + "\n", + "Note that the first situation can give rise to the second.\n", + "\n", + "We can handle these degenerate situations using the following algorithm:\n", + "\n", + "1. Assume the most general form of the profile. In this case, all but stage 4 is constant-time, so solve for $t_v$\n", + "2. If the calculated $t_v < 0$, calculate the actual $v_{max}$ and recalculate $t_a$ holding $t_j$ constant and $t_v=0$.\n", + "3. If $t_a<0$, recalculate $t_j$ holding $t_v=t_a=0$\n", + "\n", + "This means that to solve the motion equations, we need a couple of closed-form solutions:" + ] + }, + { + "cell_type": "code", + "execution_count": 96, + "id": "a1722a31-0b31-4cfd-8179-4222fdb2fb56", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'dt': a_max/j_max,\n", + " 'da': a_max,\n", + " 'dv': a_max**2/(2*j_max),\n", + " 'dp': a_max**3/(6*j_max**2),\n", + " 'ae': a_max,\n", + " 've': a_max**2/(2*j_max),\n", + " 'pe': a_max**3/(6*j_max**2)}" + ] + }, + "execution_count": 96, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "# Time and position calculations for general form\n", + "t_a = a_max / j_max\n", + "p1s1 = seg_d(j=j_max, a0=0, v0=0, t=t_a)\n", + "p1s1" + ] + }, + { + "cell_type": "code", + "execution_count": 97, + "id": "56e480b7-0f00-4791-a443-b4891bb522b1", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'dt': a_max/j_max,\n", + " 'da': -a_max,\n", + " 'dv': a_max**2/(2*j_max),\n", + " 'dp': -a_max**3/(6*j_max**2) + a_max*v_max/j_max,\n", + " 'ae': 0,\n", + " 've': v_max,\n", + " 'pe': -a_max**3/(6*j_max**2) + a_max*v_max/j_max}" + ] + }, + "execution_count": 97, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "p1s3 = seg_d(j=-j_max, a0=p1s1[\"ae\"], t=t_a)\n", + "p1s3_v0 = sympy.solve(p1s3[\"ve\"] - v_max, v0)[0]\n", + "p1s3 = seg_d(j=-j_max, a0=p1s1[\"ae\"], t=t_a, v0 = p1s3_v0)\n", + "p1s3" + ] + }, + { + "cell_type": "code", + "execution_count": 98, + "id": "9554e8a3-f968-4ae2-b0b0-52e0e6489631", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'dt': -a_max/j_max + v_max/a_max,\n", + " 'da': 0,\n", + " 'dv': -a_max**2/j_max + v_max,\n", + " 'dp': v_max*(-a_max**2 + j_max*v_max)/(2*a_max*j_max),\n", + " 'ae': a_max,\n", + " 've': -a_max**2/(2*j_max) + v_max,\n", + " 'pe': a_max**3/(6*j_max**2) - a_max*v_max/(2*j_max) + v_max**2/(2*a_max)}" + ] + }, + "execution_count": 98, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "t_v = (p1s3_v0 - p1s1[\"ve\"]) / p1s1[\"ae\"]\n", + "p1s2 = seg_d(j = 0, a0=p1s1[\"ae\"], v0 = p1s1[\"ve\"], t = t_v, p0=p1s1[\"pe\"])\n", + "p1s3[\"pe\"] = (p1s3[\"dp\"] + p1s2[\"pe\"]).simplify()\n", + "p1s2" + ] + }, + { + "cell_type": "code", + "execution_count": 99, + "id": "61ae6edb-d153-4f71-aa0f-fe1b6b882e2b", + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\frac{a_{max}^{3}}{6 j_{max}^{2}}$" + ], + "text/plain": [ + "a_max**3/(6*j_max**2)" + ] + }, + "execution_count": 99, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "p1s1_dp.subs(a_max, None)" + ] + }, + { + "cell_type": "code", + "execution_count": 100, + "id": "6494ee63-9e1f-4482-864a-4f14683f3fb0", + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\frac{v_{max} \\left(a_{max}^{2} + j_{max} v_{max}\\right)}{2 a_{max} j_{max}}$" + ], + "text/plain": [ + "v_max*(a_max**2 + j_max*v_max)/(2*a_max*j_max)" + ] + }, + "execution_count": 100, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "p1s3[\"pe\"]" + ] + }, + { + "cell_type": "code", + "execution_count": 108, + "id": "f2ae9840-53ff-4295-8555-fc5ae26affcf", + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle 0$" + ], + "text/plain": [ + "0" + ] + }, + "execution_count": 108, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "(p1s3[\"pe\"] - sum(x[\"dp\"] for x in [p1s5, p1s6, p1s7])).simplify()" + ] + }, + { + "cell_type": "code", + "execution_count": 106, + "id": "b81e62d7-62ec-4d11-8ba4-c9c8d460ebfb", + "metadata": {}, + "outputs": [], + "source": [ + "p1s5 = seg_d(j=-j_max, a0=0, v0=v_max, t=t_a, p0=p1s3[\"pe\"])\n", + "p1s6 = seg_d(j=0, a0=p1s5[\"ae\"], v0=p1s5[\"ve\"], t=t_v, p0=p1s5[\"pe\"])\n", + "p1s7 = seg_d(j=j_max, a0=p1s6[\"ae\"], v0=p1s6[\"ve\"], t=t_a, p0=p1s6[\"pe\"])" + ] + }, + { + "cell_type": "code", + "execution_count": 107, + "id": "ceeac2db-ca02-4a7f-a1cd-5b48c9b944ca", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'dt': a_max/j_max,\n", + " 'da': a_max,\n", + " 'dv': -a_max**2/(2*j_max),\n", + " 'dp': a_max**3/(6*j_max**2),\n", + " 'ae': 0,\n", + " 've': 0,\n", + " 'pe': a_max*v_max/j_max + v_max**2/a_max}" + ] + }, + "execution_count": 107, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "p1s7" + ] + }, + { + "cell_type": "code", + "execution_count": 109, + "id": "3b0efbae-6e97-406c-849f-e627fe388335", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'dt': a_max/j_max,\n", + " 'da': -a_max,\n", + " 'dv': a_max**2/(2*j_max),\n", + " 'dp': -a_max**3/(6*j_max**2) + a_max*v_max/j_max,\n", + " 'ae': 0,\n", + " 've': v_max,\n", + " 'pe': v_max*(a_max**2 + j_max*v_max)/(2*a_max*j_max)}" + ] + }, + "execution_count": 109, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "p1s3" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "739d944e-19ae-4b8c-866b-f430c11b1576", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "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.11" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +}