Marcos Duarte
Laboratory of Biomechanics and Motor Control (http://demotu.org/)
Federal University of ABC, Brazil
Hogan and Flash (1984, 1985), based on observations of voluntary movements in primates, suggested that movements are performed (organized) with the smoothest trajectory possible. In this organizing principle, the endpoint trajectory is such that the mean squared-jerk across time of this movement is minimum.
Jerk is the derivative of acceleration and the observation of the minimum-jerk trajectory is for the endpoint in the extracorporal coordinates (not for joint angles) and according to Flash and Hogan (1985), the minimum-jerk trajectory of a planar movement is such that minimizes the following objective function:
$$ C=\frac{1}{2} \int\limits_{t_{i}}^{t_{f}}\;\left[\left(\frac{d^{3}x}{dt^{3}}\right)^2+\left(\frac{d^{3}y}{dt^{3}}\right)^2\right]\:\mathrm{d}t $$Hogan (1984) found that the solution for this objective function is a fifth-order polynomial trajectory (see Shadmehr and Wise (2004) for a simpler proof):
$$ \begin{array}{l l} x(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5 \\ y(t) = b_0+b_1t+b_2t^2+b_3t^3+b_4t^4+b_5t^5 \end{array} $$With the following boundary conditions for $ x(t) $ and $ y(t) $: initial and final positions are $ (x_i,y_i) $ and $ (x_f,y_f) $ and initial and final velocities and accelerations are zero.
Let's employ Sympy to find the solution for the minimum jerk trajectory using symbolic algebra.
import numpy as np
%matplotlib inline
import matplotlib.pyplot as plt
from IPython.display import display, Math, Latex
from sympy import symbols, Matrix, latex, Eq, collect, solve, diff, simplify
from sympy.utilities.lambdify import lambdify
Using Sympy, the equation for minimum jerk trajectory for x is:
# declare the symbolic variables
x, xi, xf, y, yi, yf, d, t = symbols('x, x_i, x_f, y, y_i, y_f, d, t')
a0, a1, a2, a3, a4, a5 = symbols('a_0:6')
x = a0 + a1*t + a2*t**2 + a3*t**3 + a4*t**4 + a5*t**5
display(Math(latex('x(t)=') + latex(x)))
Without loss of generality, consider $ t_i=0 $ and let's use $ d $ for movement duration ($ d=t_f $). The system of equations with the boundary conditions for $ x $ is:
# define the system of equations
s = Matrix([Eq(x.subs(t,0) , xi),
Eq(diff(x,t,1).subs(t,0), 0),
Eq(diff(x,t,2).subs(t,0), 0),
Eq(x.subs(t,d) , xf),
Eq(diff(x,t,1).subs(t,d), 0),
Eq(diff(x,t,2).subs(t,d), 0)])
display(Math(latex(s, mat_str='matrix', mat_delim='[')))
Which gives the following solution:
# algebraically solve the system of equations
sol = solve(s, [a0, a1, a2, a3, a4, a5])
display(Math(latex(sol)))
Substituting this solution in the fifth order polynomial trajectory equation, we have the actual displacement trajectories:
# substitute the equation parameters by the solution
x2 = x.subs(sol)
x2 = collect(simplify(x2, ratio=1), xf-xi)
display(Math(latex('x(t)=') + latex(x2)))
y2 = x2.subs([(xi, yi), (xf, yf)])
display(Math(latex('y(t)=') + latex(y2)))
And for the velocity, acceleration, and jerk trajectories in x:
# symbolic differentiation
vx = x2.diff(t, 1)
display(Math(latex('v_x(t)=') + latex(vx)))
ax = x2.diff(t, 2)
display(Math(latex('a_x(t)=') + latex(ax)))
jx = x2.diff(t, 3)
display(Math(latex('j_x(t)=') + latex(jx)))
Let's plot the minimum jerk trajectory for x and its velocity, acceleration, and jerk considering $x_i=0,x_f=1,d=1$:
# substitute by the numerical values
x3 = x2.subs([(xi, 0), (xf, 1), (d, 1)])
#create functions for calculation of numerical values
xfu = lambdify(t, diff(x3, t, 0), 'numpy')
vfu = lambdify(t, diff(x3, t, 1), 'numpy')
afu = lambdify(t, diff(x3, t, 2), 'numpy')
jfu = lambdify(t, diff(x3, t, 3), 'numpy')
#plots using matplotlib
ts = np.arange(0, 1.01, .01)
fig, axs = plt.subplots(1, 4, figsize=(12, 5), sharex=True, squeeze=True)
axs[0].plot(ts, xfu(ts), linewidth=3)
axs[0].set_title('Displacement [$\mathrm{m}$]')
axs[1].plot(ts, vfu(ts), linewidth=3)
axs[1].set_title('Velocity [$\mathrm{m/s}$]')
axs[2].plot(ts, afu(ts), linewidth=3)
axs[2].set_title('Acceleration [$\mathrm{m/s^2}$]')
axs[3].plot(ts, jfu(ts), linewidth=3)
axs[3].set_title('Jerk [$\mathrm{m/s^3}$]')
for axi in axs:
axi.set_xlabel('Time [s]', fontsize=14)
axi.grid(True)
fig.suptitle('Minimum jerk trajectory kinematics', fontsize=20, y=1.03)
fig.tight_layout()
plt.show()
Note that for the minimum jerk trajectory, initial and final values of both velocity and acceleration are zero, but not for the jerk.
Read more about the minimum jerk trajectory hypothesis in the Shadmehr and Wise's book companion site and in Paul Gribble's website.
Let's calculate the resulting angular trajectory given a minimum jerk linear trajectory, supposing it is from a circular motion of an elbow flexion. The length of the forearm is 0.5 m, the movement duration is 1 s, the elbow starts flexed at 90$^o$ and the flexes to 180$^o$.
First, the linear trajectories for this circular motion:
# substitute by the numerical values
x3 = x2.subs([(xi, 0.5), (xf, 0), (d, 1)])
y3 = x2.subs([(xi, 0), (xf, 0.5), (d, 1)])
display(Math(latex('y(t)=') + latex(x3)))
display(Math(latex('x(t)=') + latex(y3)))
#create functions for calculation of numerical values
xfux = lambdify(t, diff(x3, t, 0), 'numpy')
vfux = lambdify(t, diff(x3, t, 1), 'numpy')
afux = lambdify(t, diff(x3, t, 2), 'numpy')
jfux = lambdify(t, diff(x3, t, 3), 'numpy')
xfuy = lambdify(t, diff(y3, t, 0), 'numpy')
vfuy = lambdify(t, diff(y3, t, 1), 'numpy')
afuy = lambdify(t, diff(y3, t, 2), 'numpy')
jfuy = lambdify(t, diff(y3, t, 3), 'numpy')
#plots using matplotlib
ts = np.arange(0, 1.01, .01)
fig, axs = plt.subplots(1, 4, figsize=(12, 5), sharex=True, squeeze=True)
axs[0].plot(ts, xfux(ts), 'b', linewidth=3)
axs[0].plot(ts, xfuy(ts), 'r', linewidth=3)
axs[0].set_title('Displacement [$\mathrm{m}$]')
axs[1].plot(ts, vfux(ts), 'b', linewidth=3)
axs[1].plot(ts, vfuy(ts), 'r', linewidth=3)
axs[1].set_title('Velocity [$\mathrm{m/s}$]')
axs[2].plot(ts, afux(ts), 'b', linewidth=3)
axs[2].plot(ts, afuy(ts), 'r', linewidth=3)
axs[2].set_title('Acceleration [$\mathrm{m/s^2}$]')
axs[3].plot(ts, jfux(ts), 'b', linewidth=3)
axs[3].plot(ts, jfuy(ts), 'r', linewidth=3)
axs[3].set_title('Jerk [$\mathrm{m/s^3}$]')
for axi in axs:
axi.set_xlabel('Time [s]', fontsize=14)
axi.grid(True)
fig.suptitle('Minimum jerk trajectory kinematics', fontsize=20, y=1.03)
fig.tight_layout()
plt.show()
Now, the angular trajectories for this circular motion:
from sympy import atan2
ang = atan2(y3, x3)*180/np.pi
display(Math(latex('angle(t)=') + latex(ang)))
xang = lambdify(t, diff(ang, t, 0), 'numpy')
vang = lambdify(t, diff(ang, t, 1), 'numpy')
aang = lambdify(t, diff(ang, t, 2), 'numpy')
jang = lambdify(t, diff(ang, t, 3), 'numpy')
ts = np.arange(0, 1.01, .01)
fig, axs = plt.subplots(1, 4, figsize=(12, 5), sharex=True, squeeze=True)
axs[0].plot(ts, xang(ts), linewidth=3)
axs[0].set_title('Displacement [$\mathrm{m}$]')
axs[1].plot(ts, vang(ts), linewidth=3)
axs[1].set_title('Velocity [$\mathrm{m/s}$]')
axs[2].plot(ts, aang(ts), linewidth=3)
axs[2].set_title('Acceleration [$\mathrm{m/s^2}$]')
axs[3].plot(ts, jang(ts), linewidth=3)
axs[3].set_title('Jerk [$\mathrm{m/s^3}$]')
for axi in axs:
axi.set_xlabel('Time [s]', fontsize=14)
axi.grid(True)
fig.suptitle('Minimum jerk trajectory angular kinematics', fontsize=20, y=1.03)
fig.tight_layout()
plt.show()