Marcos Duarte
Laboratory of Biomechanics and Motor Control
Federal University of ABC, Brazil
# import necessary libraries and configure environment
import numpy as np
%matplotlib inline
import matplotlib.pyplot as plt
import seaborn as sns
from IPython.display import display, Math
from sympy import symbols, Matrix, latex, Eq, collect, solve, diff, simplify, init_printing
from sympy.core import S
from sympy.utilities.lambdify import lambdify
init_printing()
sns.set_context('notebook', rc={"lines.linewidth": 2})
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:
\begin{equation} \begin{array}{rcl} C = \frac{1}{2} \displaystyle\int\limits_{t_{i}}^{t_{f}}\;\left[\left(\dfrac{d^{3}x}{dt^{3}}\right)^2+\left(\dfrac{d^{3}y}{dt^{3}}\right)^2\right]\;\mathrm{d}t \label{} \end{array} \end{equation}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{equation} \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 \label{} \end{array} \end{equation}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.
# 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(Eq(S('x(t)'), 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 = [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(si) for si in s];
Which gives the following solution:
# algebraically solve the system of equations
sol = solve(s, [a0, a1, a2, a3, a4, a5])
display(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(Eq(S('x(t)'), x2))
y2 = x2.subs([(xi, yi), (xf, yf)])
display(Eq(S('y(t)'), y2))
And for the velocity, acceleration, and jerk trajectories in x:
# symbolic differentiation
vx = x2.diff(t, 1)
display(Eq(S('v_x(t)'), vx))
ax = x2.diff(t, 2)
display(Eq(S('a_x(t)'), ax))
jx = x2.diff(t, 3)
display(Eq(S('j_x(t)'), 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=(10, 4), sharex=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=12)
axi.grid(True)
fig.suptitle('Minimum jerk trajectory kinematics', fontsize=16)
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(Eq(S('y(t)'), x3))
display(Eq(S('x(t)'), 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=(10, 4), sharex=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=12)
axi.grid(True)
fig.suptitle('Minimum jerk trajectory kinematics', fontsize=16)
fig.tight_layout()
plt.show()
Now, the angular trajectories for this circular motion:
from sympy import atan2, pi
ang = atan2(y3, x3)*180/pi
display(Eq(S('angle(t)'), 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=(10, 4), sharex=True)
axs[0].plot(ts, xang(ts), linewidth=3)
axs[0].set_title('Displacement [$\mathrm{^o}$]')
axs[1].plot(ts, vang(ts), linewidth=3)
axs[1].set_title('Velocity [$\mathrm{^o/s}$]')
axs[2].plot(ts, aang(ts), linewidth=3)
axs[2].set_title('Acceleration [$\mathrm{^o/s^2}$]')
axs[3].plot(ts, jang(ts), linewidth=3)
axs[3].set_title('Jerk [$\mathrm{^o/s^3}$]')
for axi in axs:
axi.set_xlabel('Time [s]', fontsize=14)
axi.grid(True)
fig.suptitle('Minimum jerk trajectory angular kinematics', fontsize=16)
fig.tight_layout()
plt.show()