Calculate the equations and values for a seven segment motion profile¶
Define the symbols¶
In [1]:
from sympy import *
from IPython.display import Math
init_printing(use_unicode=True)
x01, v0, a0, t01 = symbols('x01, v0, a0, t01') # distance from x0 to x1, initial velocity, initial acceleration, time from t0 to t1
x12, v1, a1, t12 = symbols('x12, v1, a1, t12')
x23, v2, a2, t23 = symbols('x23, v2, a2, t23')
x34, v3, a3, t34 = symbols('x34, v3, a3, t34')
x45, v4, a4, t45 = symbols('x45, v4, a4, t45')
x56, v5, a5, t56 = symbols('x56, v5, a5, t56')
x67, v6, a6, t67 = symbols('x67, v6, a6, t67')
x07, v7, a7, t07 = symbols('x07, v7, a7, t07')
v, a, d, j = symbols('v, a, d, j') # command position, velocity, acceleration and jerk
Define the system of equations. Segment 0 is the stopped condition¶
In [2]:
# seg 1
eq0 = Eq(v0*t01+(a0/2)*t01**2+(j/6)*t01**3,x01) # substitute 0 for v0 and a0 to simplify below
eq1 = Eq(v0+a0*t01+(j/2)*t01**2,v1)
eq2 = Eq(a0+j*t01,a)
# seg 2
eq3 = Eq(v1*t12+(a/2)*t12**2,x12)
eq4 = Eq(v1+a*t12,v2)
# seg 3 # Segment 3 jerk < 0
eq5 = Eq(v2*t23+(a/2)*t23**2-(j/6)*t23**3,x23)
eq6 = Eq(v2+a*t23-(j/2)*t23**2,v)
eq7 = Eq(a-j*t23,0)
# seg 4
eq8 = Eq(v*t34,x34) # Segment 4. Velocity=v Acceleration=0 Jerk=0
# seg 5
eq9 = Eq(v*t45-(j/6)*t45**3,x45) # Segment 5. Jerk = -j
eq10 = Eq(v-(j/2)*t45**2,v5)
eq11 = Eq(-j*t45,-d)
# seg 6
eq12 = Eq(v5*t56-(d/2)*t56**2,x56) # Segment 6. Acceleration = -d Jerk=0
eq13 = Eq(v5-d*t56,v6)
# seg 7
eq14 = Eq(v6*t67-(d/2)*t67**2+(j/6)*t67**3,x67) #Segment 7. Jerk = +j
eq15 = Eq(v6-d*t67+(j/2)*t67**2,v7)
eq16 = Eq(-d+j*t67,a7)
# Calculate the total time symbolically
eq17 = Eq(t01+t12+t23+t34+t45+t56+t67,t07)
eq18 = Eq(x01+x12+x23+x34+x45+x56+x67,x07)
Pretty print the system of equations using LaTeX¶
In [3]:
lequ = [eq0,eq1,eq2,eq3,eq4,eq5,eq6,eq7,eq8,eq9,eq10,eq11,eq12,eq13,eq14,eq15,eq16,eq17,eq18]
for equ in lequ:
display(Math(latex(equ)))
$\displaystyle \frac{a_{0} t_{01}^{2}}{2} + \frac{j t_{01}^{3}}{6} + t_{01} v_{0} = x_{01}$
$\displaystyle a_{0} t_{01} + \frac{j t_{01}^{2}}{2} + v_{0} = v_{1}$
$\displaystyle a_{0} + j t_{01} = a$
$\displaystyle \frac{a t_{12}^{2}}{2} + t_{12} v_{1} = x_{12}$
$\displaystyle a t_{12} + v_{1} = v_{2}$
$\displaystyle \frac{a t_{23}^{2}}{2} - \frac{j t_{23}^{3}}{6} + t_{23} v_{2} = x_{23}$
$\displaystyle a t_{23} - \frac{j t_{23}^{2}}{2} + v_{2} = v$
$\displaystyle a - j t_{23} = 0$
$\displaystyle t_{34} v = x_{34}$
$\displaystyle - \frac{j t_{45}^{3}}{6} + t_{45} v = x_{45}$
$\displaystyle - \frac{j t_{45}^{2}}{2} + v = v_{5}$
$\displaystyle - j t_{45} = - d$
$\displaystyle - \frac{d t_{56}^{2}}{2} + t_{56} v_{5} = x_{56}$
$\displaystyle - d t_{56} + v_{5} = v_{6}$
$\displaystyle - \frac{d t_{67}^{2}}{2} + \frac{j t_{67}^{3}}{6} + t_{67} v_{6} = x_{67}$
$\displaystyle - d t_{67} + \frac{j t_{67}^{2}}{2} + v_{6} = v_{7}$
$\displaystyle - d + j t_{67} = a_{7}$
$\displaystyle t_{01} + t_{12} + t_{23} + t_{34} + t_{45} + t_{56} + t_{67} = t_{07}$
$\displaystyle x_{01} + x_{12} + x_{23} + x_{34} + x_{45} + x_{56} + x_{67} = x_{07}$
Simplify by making substitutions for the begnning and end points¶
In [4]:
for i in range(len(lequ)): # scan all the equations
lequ[i] = lequ[i].subs([(v0,0),(a0,0), (v7,0),(a7,0)]) # set v0,a0,v7 and a7 to 0 to simpilfy
Assign values to symbols so they are now variables.¶
In [5]:
lsym = [t01,x01,v1,t12,x12,v2,t23,x23,t34,x34,t45,x45,v5,t56,x56,v6,t67,x67,j]
s = solve(lequ,lsym)
leq = simplify(s[0]) # list of equations
for i in range(len(leq)):
print(f"{lsym[i]}={leq[i]}") # print list of symbols and formulas
t01=-(-2*a*d*t07*v + 2*a*d*x07 + a*v**2 + d*v**2)/(d*v*(a + d)) x01=a*(-2*a*d*t07*v + 2*a*d*x07 + a*v**2 + d*v**2)**2/(6*d**2*v**2*(a + d)**2) v1=-a*(-2*a*d*t07*v + 2*a*d*x07 + a*v**2 + d*v**2)/(2*d*v*(a + d)) t12=(-2*a**2*d*t07*v + 2*a**2*d*x07 + a**2*v**2 + 2*a*d*v**2 + d**2*v**2)/(a*d*v*(a + d)) x12=(-2*a**2*d*t07*v + 2*a**2*d*x07 + a**2*v**2 + 2*a*d*v**2 + d**2*v**2)/(2*a*d*(a + d)) v2=(-2*a**2*d*t07*v + 2*a**2*d*x07 + a**2*v**2 + 3*a*d*v**2 + 2*d**2*v**2)/(2*d*v*(a + d)) t23=-(-2*a*d*t07*v + 2*a*d*x07 + a*v**2 + d*v**2)/(d*v*(a + d)) x23=-(-2*a*d*t07*v + 2*a*d*x07 + a*v**2 + d*v**2)*(-2*a**2*d*t07*v + 2*a**2*d*x07 + a**2*v**2 + 7*a*d*v**2 + 6*d**2*v**2)/(6*d**2*v**2*(a + d)**2) t34=(-t07*v + 2*x07)/v x34=-t07*v + 2*x07 t45=-(-2*a*d*t07*v + 2*a*d*x07 + a*v**2 + d*v**2)/(a*v*(a + d)) x45=-(-2*a*d*t07*v + 2*a*d*x07 + a*v**2 + d*v**2)*(6*a**2*v**2 - 2*a*d**2*t07*v + 2*a*d**2*x07 + 7*a*d*v**2 + d**2*v**2)/(6*a**2*v**2*(a + d)**2) v5=(2*a**2*v**2 - 2*a*d**2*t07*v + 2*a*d**2*x07 + 3*a*d*v**2 + d**2*v**2)/(2*a*v*(a + d)) t56=(a**2*v**2 - 2*a*d**2*t07*v + 2*a*d**2*x07 + 2*a*d*v**2 + d**2*v**2)/(a*d*v*(a + d)) x56=(a**2*v**2 - 2*a*d**2*t07*v + 2*a*d**2*x07 + 2*a*d*v**2 + d**2*v**2)/(2*a*d*(a + d)) v6=-d*(-2*a*d*t07*v + 2*a*d*x07 + a*v**2 + d*v**2)/(2*a*v*(a + d)) t67=-(-2*a*d*t07*v + 2*a*d*x07 + a*v**2 + d*v**2)/(a*v*(a + d)) x67=d*(-2*a*d*t07*v + 2*a*d*x07 + a*v**2 + d*v**2)**2/(6*a**2*v**2*(a + d)**2) j=-a*d*v*(a + d)/(-2*a*d*t07*v + 2*a*d*x07 + a*v**2 + d*v**2)
Pretty print the equations for the segment variables¶
In [6]:
for i in range(len(leq)):
# display(Math('\n'+latex(lsym[i])+' = '+latex(leq[i]))
display(Math(latex(lsym[i])+' = '+latex(leq[i])))
$\displaystyle t_{01} = - \frac{- 2 a d t_{07} v + 2 a d x_{07} + a v^{2} + d v^{2}}{d v \left(a + d\right)}$
$\displaystyle x_{01} = \frac{a \left(- 2 a d t_{07} v + 2 a d x_{07} + a v^{2} + d v^{2}\right)^{2}}{6 d^{2} v^{2} \left(a + d\right)^{2}}$
$\displaystyle v_{1} = - \frac{a \left(- 2 a d t_{07} v + 2 a d x_{07} + a v^{2} + d v^{2}\right)}{2 d v \left(a + d\right)}$
$\displaystyle t_{12} = \frac{- 2 a^{2} d t_{07} v + 2 a^{2} d x_{07} + a^{2} v^{2} + 2 a d v^{2} + d^{2} v^{2}}{a d v \left(a + d\right)}$
$\displaystyle x_{12} = \frac{- 2 a^{2} d t_{07} v + 2 a^{2} d x_{07} + a^{2} v^{2} + 2 a d v^{2} + d^{2} v^{2}}{2 a d \left(a + d\right)}$
$\displaystyle v_{2} = \frac{- 2 a^{2} d t_{07} v + 2 a^{2} d x_{07} + a^{2} v^{2} + 3 a d v^{2} + 2 d^{2} v^{2}}{2 d v \left(a + d\right)}$
$\displaystyle t_{23} = - \frac{- 2 a d t_{07} v + 2 a d x_{07} + a v^{2} + d v^{2}}{d v \left(a + d\right)}$
$\displaystyle x_{23} = - \frac{\left(- 2 a d t_{07} v + 2 a d x_{07} + a v^{2} + d v^{2}\right) \left(- 2 a^{2} d t_{07} v + 2 a^{2} d x_{07} + a^{2} v^{2} + 7 a d v^{2} + 6 d^{2} v^{2}\right)}{6 d^{2} v^{2} \left(a + d\right)^{2}}$
$\displaystyle t_{34} = \frac{- t_{07} v + 2 x_{07}}{v}$
$\displaystyle x_{34} = - t_{07} v + 2 x_{07}$
$\displaystyle t_{45} = - \frac{- 2 a d t_{07} v + 2 a d x_{07} + a v^{2} + d v^{2}}{a v \left(a + d\right)}$
$\displaystyle x_{45} = - \frac{\left(- 2 a d t_{07} v + 2 a d x_{07} + a v^{2} + d v^{2}\right) \left(6 a^{2} v^{2} - 2 a d^{2} t_{07} v + 2 a d^{2} x_{07} + 7 a d v^{2} + d^{2} v^{2}\right)}{6 a^{2} v^{2} \left(a + d\right)^{2}}$
$\displaystyle v_{5} = \frac{2 a^{2} v^{2} - 2 a d^{2} t_{07} v + 2 a d^{2} x_{07} + 3 a d v^{2} + d^{2} v^{2}}{2 a v \left(a + d\right)}$
$\displaystyle t_{56} = \frac{a^{2} v^{2} - 2 a d^{2} t_{07} v + 2 a d^{2} x_{07} + 2 a d v^{2} + d^{2} v^{2}}{a d v \left(a + d\right)}$
$\displaystyle x_{56} = \frac{a^{2} v^{2} - 2 a d^{2} t_{07} v + 2 a d^{2} x_{07} + 2 a d v^{2} + d^{2} v^{2}}{2 a d \left(a + d\right)}$
$\displaystyle v_{6} = - \frac{d \left(- 2 a d t_{07} v + 2 a d x_{07} + a v^{2} + d v^{2}\right)}{2 a v \left(a + d\right)}$
$\displaystyle t_{67} = - \frac{- 2 a d t_{07} v + 2 a d x_{07} + a v^{2} + d v^{2}}{a v \left(a + d\right)}$
$\displaystyle x_{67} = \frac{d \left(- 2 a d t_{07} v + 2 a d x_{07} + a v^{2} + d v^{2}\right)^{2}}{6 a^{2} v^{2} \left(a + d\right)^{2}}$
$\displaystyle j = - \frac{a d v \left(a + d\right)}{- 2 a d t_{07} v + 2 a d x_{07} + a v^{2} + d v^{2}}$
Assign values to symbols so they are now variables.¶
In [7]:
dt = 0.001 # seconds, one millisecond time increment
t07 = 1.0 # total move time in seconds
x07 = 4.0 # total move distance in m
v = 1.5*x07/t07 # velocity command, m/s, "from my rules of thumb"
a = 27.*x07/(4.0*t07**2) # acceleration command, m/s^2, from my "rules of thumb"
d = 27.*x07/(4.0*t07**2) # acceleration command, m/s^2, from my "rules of thumb"
x0 = 0.0 # initial position, m
v0 = 0.0 # initial velocity, m/s
a0 = 0.0 # initial acceleration, m/s^2
v7 = 0.0 # not necessary if set to 0 symbolically above
a7 = 0.0 # not necessary if set to 0 symbolically above
Copied and pasted formulas from above. Use code to generate code!¶
In [8]:
t01=-(-a*t07*v + a*x07 + v**2)/(a*v)
x01=(-a*t07*v + a*x07 + v**2)**2/(6*a*v**2)
v1=-(-a*t07*v + a*x07 + v**2)/(2*v)
t12=(-a*t07*v + a*x07 + 2*v**2)/(a*v)
x12=(-a*t07*v + a*x07 + 2*v**2)/(2*a)
v2=(-a*t07*v + a*x07 + 3*v**2)/(2*v)
t23=-(-a*t07*v + a*x07 + v**2)/(a*v)
x23=-(-a*t07*v + a*x07 + v**2)*(-a*t07*v + a*x07 + 7*v**2)/(6*a*v**2)
t34=(-t07*v + 2*x07)/v
x34=-t07*v + 2*x07
t45=-(-a*t07*v + a*x07 + v**2)/(a*v)
x45=-(-a*t07*v + a*x07 + v**2)*(-a*t07*v + a*x07 + 7*v**2)/(6*a*v**2)
v5=(-a*t07*v + a*x07 + 3*v**2)/(2*v)
t56=(-a*t07*v + a*x07 + 2*v**2)/(a*v)
x56=(-a*t07*v + a*x07 + 2*v**2)/(2*a)
v6=-(-a*t07*v + a*x07 + v**2)/(2*v)
t67=-(-a*t07*v + a*x07 + v**2)/(a*v)
x67=(-a*t07*v + a*x07 + v**2)**2/(6*a*v**2)
j=-a**2*v/(-a*t07*v + a*x07 + v**2)
Display the command values¶
In [9]:
print(f"\nComPos={x07:6.3f} m, ComVel={v:6.3f} m/s, ComAcc={a:6.3} m/s^2, ComDec={d:6.3f} m/s^2, ComJrk={j:6.3f} m/s^3")
ComPos= 4.000 m, ComVel= 6.000 m/s, ComAcc= 27.0 m/s^2, ComDec=27.000 m/s^2, ComJrk=243.000 m/s^3
Disp[ay the variable values¶
In [10]:
lvar = [t01,x01,v1,t12,x12,v2,t23,x23,t34,x34,t45,x45,v5,t56,x56,v6,t67,x67,j]
for i in range(len(lvar)):
print(f"{lsym[i]}={lvar[i]}") # symbols = value
t01=0.1111111111111111 x01=0.05555555555555555 v1=1.5 t12=0.1111111111111111 x12=0.3333333333333333 v2=4.5 t23=0.1111111111111111 x23=0.6111111111111112 t34=0.3333333333333333 x34=2.0 t45=0.1111111111111111 x45=0.6111111111111112 v5=4.5 t56=0.1111111111111111 x56=0.3333333333333333 v6=1.5 t67=0.1111111111111111 x67=0.05555555555555555 j=243.0
In [11]:
# intialize some variables.
x1 = x0+x01 # position after the end of segment 1
x2 = x1+x12 # position after the end of segment 2
x3 = x2+x23 # position after the end of segment 3
x4 = x3+x34
x5 = x4+x45
x6 = x5+x56
x7 = x07
t1 = t01 # time at x1
t2 = t1+t12 # time at x2
t3 = t2+t23 # time at x3
t4 = t3+t34
t5 = t4+t45
t6 = t5+t56
t7 = t07
Calculate the motion profile¶
In [ ]:
import numpy as np
N = int(t07*1.1/dt) # N is the number of iterations/milliseconds
aTime = np.empty(N) # array of time increments for plotting
pos = np.empty(N)
vel = np.empty(N)
acc = np.empty(N)
jrk = np.empty(N)
for i in range(N):
t = dt*i
aTime[i] = t
if t < t1: # jerk > 0
pos[i] = x0+v0*t+(a0/2.)*t**2+(j/6.)*t**3
vel[i] = v0+a0*t+(j/2.)*t**2
acc[i] = a0+j*t
jrk[i] = j
elif t1 <= t and t < t2: # jerk = 0
t -= t1
pos[i]= x1+v1*t+(a/2.)*t*t
vel[i] = v1+a*t
acc[i] = a
jrk[i] = 0.0
elif t2 <= t and t < t3: # jerk < 0
t -= t2
pos[i] = x2+v2*t+(a/2)*t**2-(j/6)*t**3
vel[i] = v2+a*t-(j/2)*t**2
acc[i] = a-j*t
jrk[i] = -j
elif t3 <= t and t < t4: # const velocity
t -= t3
pos[i] = x3+v*t
vel[i] = v
acc[i] = 0.0
jrk[i] = 0.0
elif t4 <= t and t < t5: # jerk < 0
t -= t4
pos[i] = x4+v*t-(j/6)*t**3
vel[i] = v - (j/2)*t**2
acc[i] = -j*t
jrk[i] = -j
elif t5 <= t and t < t6: # jerk = 0
t -= t5
pos[i] = x5+v5*t-(a/2)*t**2
vel[i] = v5-a*t
acc[i] = -a
jrk[i] = 0.0
elif t6 <= t and t < t7: # jerk > 0
t -= t6
pos[i] = x6+v6*t-(a/2)*t**2+(j/6)*t**3
vel[i] = v6 - a*t + (j/2)*t**2
acc[i] = -a + j*t
jrk[i] = j
else: # stopped at the comand position
pos[i] = x7
vel[i] = 0.0
acc[i] = 0.0
jrk[i] = 0.0
Plot the motion profile¶
In [13]:
# Plot the motion profile
import matplotlib.pyplot as plt
%matplotlib inline
plot_size = (9,4) # supposedly in inches :(
_fig, _ax_pos = plt.subplots(figsize=plot_size)
_pos_line, = _ax_pos.plot(aTime, pos,'c-', label='position, m')
_ax_pos.set_title('Position and Velocity vs Time')
_ax_pos.set_xlabel('time, s')
_ax_pos.set_ylabel('position, m')
_ax_vel = _ax_pos.twinx()
_vel_line, = _ax_vel.plot(aTime, vel ,'m-',label='velocity, m/s')
_ax_vel.set_ylabel('velocity, m/s')
_lines = [_pos_line, _vel_line]
_ax_pos.legend(_lines, [l.get_label() for l in _lines], loc=0)
_fig.tight_layout()
plt.show(block=False)
plot_size = (9,4) # supposedly in inches :(
_fig, _ax_acc = plt.subplots(figsize=plot_size)
_acc_line, = _ax_acc.plot(aTime, acc,'r-', label='acceleration, m/s^2')
_ax_acc.set_title('Acceleration and Jerk vs Time')
_ax_acc.set_xlabel('time, s')
_ax_acc.set_ylabel('accleration, m/s^2')
_ax_jrk = _ax_acc.twinx()
_jrk_line, = _ax_jrk.plot(aTime, jrk ,'b-',label='jerk, m/s^3')
_ax_jrk.set_ylabel('jerk, m/s^3')
_lines = [_acc_line, _jrk_line]
_ax_acc.legend(_lines, [l.get_label() for l in _lines], loc=3)
_fig.tight_layout()
plt.show(block=False)
