Finite Jerk Motion Profile

Question

I try to define a function to plot a motion profile with constant jerk. It seems that my function doesn't work. Would you help me to debug my function ? I guess my issue comes from a bad management of the With function.

Trajectory[q0_, qf_, v0_, vf_, Jmax_, Amax_, Vmax_][t_] :=
  With[{
  Tja = Amax/Jmax,
  Tjd = -Amax/Jmax,
  Ta = (Vmax - v0)/Amax - Amax/Jmax,
  Td = (Vmax - vf)/-Amax - (-Amax)/Jmax,
  \[CapitalDelta]qmin = 
   Vmax^2*(1/(2*Amax) + 1/(2*(-Amax))) + 
    Vmax*(Amax + (-Amax))/(2*Jmax) + (Amax*v0 + (-Amax)*vf)/(2*Jmax) -
     v0^2/(2*Amax) - vf^2/(2*(-Amax)),
  Tv = (qf - q0 - \[CapitalDelta]qmin)/Vmax,
  t1 = Tja,
  t2 = Tja + Ta,
  t3 = Tja + Ta + Tja,
  t4 = Tja + Ta + Tja + Tv,
  t5 = Tja + Ta + Tja + Tv + Tjd,
  t6 = Tja + Ta + Tja + Tv + Tjd + Td,
  t7 = Tja + Ta + Tja + Tv + Tjd + Td + Tjd,
  v1 = v0 + 1/2*Jmax*Tja^2 ,
  v2 = v1 + Amax*Ta,
  v3 = Vmax,
  v4 = v3,
  v5 = v4 - 1/2*Jmax*Tjd^2,
  v6 = v5 - Amax*Td,
  q1 = q0 + v0*Tja + 1/6*Jmax*Tja^3,
  q2 = q1 + v1*Ta + 1/2*Amax*Ta^2,
  q3 = q2 + v2*Tja + 1/2*Amax*Tja^2 - 1/6*Jmax*Tja^3,
  q4 = q3 + v3*Tv,
  q5 = q4 + v4*Tjd - 1/6*Jmax*Tjd^3,
  q6 = q5 + v5*Td - 1/2*Amax*Td^2 
  },
 Piecewise[{
 {q0 + v0*t + 1/6*Jmax*t^3, 0 <= t < t1},
 {q1 + v1*(t - t1) + 1/2*Amax*(t - t1)^2, t1 <= t < t2},
 {q2 + v2*(t - t2) + 1/2* Amax*(t - t2)^2 - 1/6* Jmax*(t - t2)^3, 
 t2 <= t < t3},
 {q3 + v3*(t - t3), t3 <= t < t4},
 {q4 + v4*(t - t4) - 1/6*Jmax*(t - t4)^3, t4 <= t < t5},
 {q5 + v4*(t - t5) - 1/2*Amax*(t - t5)^2, t5 <= t < t6},
 {q6 + v6*(t - t6) - 1/2*Amax*(t - t6)^2 + 1/6*Jmax*(t - t6)^3, t6 <= t < t7}, {qf, t7 <= t   }}]]

by using a Module function, i have corrected my function :

Trajectory[q0_, qf_, v0_, vf_, Jmax_, Amax_, Vmax_][t_] := Module[{


Tja = Amax/Jmax,
   Tjd = Amax/Jmax,
   Ta = (Vmax - v0)/Amax - Amax/Jmax,
   Td = (Vmax - vf)/Amax - Amax/Jmax,
   \[CapitalDelta]qmin = -(v0^2/(2 Amax)) - vf^2/(2 Amax) + (
     Amax v0 + Amax vf)/(2 Jmax) + (Amax Vmax)/Jmax + Vmax^2/Amax, Tv,
    t1, t2, t3, t4, t5, t6, t7, v1, v2, v3, v4, v5, v6, q1, q2, q3, 
   q4, q5, q6},
  Tv = (qf - q0 - \[CapitalDelta]qmin)/Vmax;
  t1 = Tja;
  t2 = Tja + Ta;
  t3 = Tja + Ta + Tja;
  t4 = Tja + Ta + Tja + Tv;
  t5 = Tja + Ta + Tja + Tv + Tjd;
  t6 = Tja + Ta + Tja + Tv + Tjd + Td;
  t7 = Tja + Ta + Tja + Tv + Tjd + Td + Tjd;
  v1 = v0 + 1/2*Jmax*Tja^2;
  v2 = v1 + Amax*Ta;
  v3 = Vmax;
  v4 = v3;
  v5 = v4 - 1/2*Jmax*Tjd^2;
  v6 = v5 - Amax*Td;
  q1 = q0 + v0*Tja + 1/6*Jmax*Tja^3;
  q2 = q1 + v1*Ta + 1/2*Amax*Ta^2;
  q3 = q2 + v2*Tja + 1/2*Amax*Tja^2 - 1/6*Jmax*Tja^3;
  q4 = q3 + v3*Tv;
  q5 = q4 + v4*Tjd - 1/6*Jmax*Tjd^3;
  q6 = q5 + v5*Td - 1/2*Amax*Td^2;
  Piecewise[{
    {q0 + v0*t + 1/6*Jmax*t^3, 0 <= t < t1},
    {q1 + v1*(t - t1) + 1/2*Amax*(t - t1)^2, 
     t1 <= t < t2}, {q2 + v2*(t - t2) + 1/2*Amax*(t - t2)^2 - 
      1/6*Jmax*(t - t2)^3, t2 <= t < t3},
    {q3 + v3*(t - t3), t3 <= t < t4},
    {q4 + v4*(t - t4) - 1/6*Jmax*(t - t4)^3, t4 <= t < t5},
    {q5 + v5*(t - t5) - 1/2*Amax*(t - t5)^2, 
     t5 <= t < t6}, {q6 + v6*(t - t6) - 1/2*Amax*(t - t6)^2 + 
      1/6*Jmax*(t - t6)^3, t6 <= t < t7},
    {qf, t7 <= t}}]]

c[t_] := Trajectory[0, 0.5, 0, 0, 37500, 150, 10][t]

Plot[Evaluate[qc[t]], {t, 0, 0.2}]

Plot[Evaluate[D[qc[t], t]], {t, 0, 0.2}]

Plot[Evaluate[D[qc[t], {t, 2}]], {t, 0, 0.2}]

Plot[Evaluate[D[qc[t], {t, 3}]], {t, 0, 0.2}]

It is now possible to plot it. But, it should remains a slight mistake that i didn't find yet since we have some discontinuities. Would you have some ideas ? i might be a slight misake in a formula, i guess.

Thank you for your help

Answer 1

Here a code which works :

Trajectory[q0_, qf_, v0_, vf_, Jmax_, Amax_, Vmax_][t_] := Module[{


 Tja = Amax/Jmax,
   Tjd = Amax/Jmax,
   Ta = (Vmax - v0)/Amax - Amax/Jmax,
   Td = (Vmax - vf)/Amax - Amax/Jmax,
   \[CapitalDelta]qmin = -(v0^2/(2 Amax)) - vf^2/(2 Amax) + (
     Amax v0 + Amax vf)/(2 Jmax) + (Amax Vmax)/Jmax + Vmax^2/Amax, Tv,
    t1, t2, t3, t4, t5, t6, t7, v1, v2, v3, v4, v5, v6, q1, q2, q3, 
   q4, q5, q6},
  Tv = (qf - q0 - \[CapitalDelta]qmin)/Vmax;
  t1 = Tja;
  t2 = Tja + Ta;
  t3 = Tja + Ta + Tja;
  t4 = Tja + Ta + Tja + Tv;
  t5 = Tja + Ta + Tja + Tv + Tjd;
  t6 = Tja + Ta + Tja + Tv + Tjd + Td;
  t7 = Tja + Ta + Tja + Tv + Tjd + Td + Tjd;
  v1 = v0 + 1/2*Jmax*Tja^2;
  v2 = v1 + Amax*Ta;
  v3 = Vmax;
  v4 = v3;
  v5 = v4 - 1/2*Jmax*Tjd^2;
  v6 = v5 - Amax*Td;
  q1 = q0 + v0*Tja + 1/6*Jmax*Tja^3;
  q2 = q1 + v1*Ta + 1/2*Amax*Ta^2;
  q3 = q2 + v2*Tja + 1/2*Amax*Tja^2 - 1/6*Jmax*Tja^3;
  q4 = q3 + v3*Tv;
  q5 = q4 + v4*Tjd - 1/6*Jmax*Tjd^3;
  q6 = q5 + v5*Td - 1/2*Amax*Td^2;
  Piecewise[{
    {q0 + v0*t + 1/6*Jmax*t^3, 0 <= t < t1},
    {q1 + v1*(t - t1) + 1/2*Amax*(t - t1)^2, 
     t1 <= t < t2}, {q2 + v2*(t - t2) + 1/2*Amax*(t - t2)^2 - 
      1/6*Jmax*(t - t2)^3, t2 <= t < t3},
    {q3 + v3*(t - t3), t3 <= t < t4},
    {q4 + v4*(t - t4) - 1/6*Jmax*(t - t4)^3, t4 <= t < t5},
    {q5 + v5*(t - t5) - 1/2*Amax*(t - t5)^2, 
     t5 <= t < t6}, {q6 + v6*(t - t6) - 1/2*Amax*(t - t6)^2 + 
      1/6*Jmax*(t - t6)^3, t6 <= t < t7},
    {qf, t7 <= t}}]]


DonnéesProfil3 := {q0 -> 0, qf -> 0.5, v0 -> 0, vf -> 0, 
Jmax -> N[(150*0.5)/0.004] (*Jerk max*), 
  Amax -> (150*0.5)(*Accélération max*), Vmax -> 1.5(*Vitesse max*)}

qc[t_] := 
 Trajectory[q0, qf, v0, vf, Jmax, Amax, Vmax][t] /. DonnéesProfil3
q[t_] = qc[t];
v[t_] = D[qc[t], t];
a[t_] = D[qc[t], {t, 2}];
j[t_] = D[qc[t], {t, 3}];

ScaledPlot[fcts_, coef_, dom_, options___] :=
 Plot[Evaluate[coef*fcts], dom, 
  Evaluate[PlotLegends -> 
    MapThread[
     Row[{##}] &, {coef, {"\[Times]\[ThinSpace]j[t]", 
       "\[Times]\[ThinSpace]a[t]", "\[Times]\[ThinSpace]v[t]", 
       "\[Times]\[ThinSpace]q[t]"}}]], options]

ScaledPlot[{j[t], a[t], v[t], q[t]}, {1/40000, 1/200, 1/10, 1}, {t, 0,
1}, Filling -> Axis]

Here the plot that i obtain which the jerk, acceleration, velocity and displacement curves: