Added motion control calculations
This commit is contained in:
374
motion-control.ipynb
Normal file
374
motion-control.ipynb
Normal file
@@ -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
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user