From 190da53ee9e9ac82ec555eab1475d5842b4f03f6 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 11 Aug 2018 14:31:37 -0400 Subject: [PATCH] Add FIR Planner python script --- tools/Motion Planning/FIR_Planner.py | 113 +++++++++++++++++++++++++++ tools/Motion Planning/Planner.py | 85 -------------------- 2 files changed, 113 insertions(+), 85 deletions(-) create mode 100644 tools/Motion Planning/FIR_Planner.py delete mode 100644 tools/Motion Planning/Planner.py diff --git a/tools/Motion Planning/FIR_Planner.py b/tools/Motion Planning/FIR_Planner.py new file mode 100644 index 00000000..1ec49adc --- /dev/null +++ b/tools/Motion Planning/FIR_Planner.py @@ -0,0 +1,113 @@ +import numpy as np +import math +import matplotlib.pyplot as plt +import random + +# Symbol Description +# Ta, Tv and Td Duration of the stages of the AL profile +# q0f , v0f and a0f Initial conditions of the jerk-limited trajectory +# q0 and v0 Adapted initial conditions for the AL profile +# qe Position set-point +# s Direction (sign) of the trajectory +# vmax, amax, dmax and jmax Kinematic bounds +# vr, ar and dr Reached values of velocity and acceleration +# Tj , Tja, Tjv and Tjd Length of the constant jerk stages (FIR filter time) + +def trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax, dT=0.001): + + dX = Xf - Xi # Distance to travel + s = np.sign(dX) # Sign + + Ar = s*Amax # Maximum Acceleration (signed) + Dr = -s*Dmax # Maximum Deceleration (signed) + Vr = s*Vmax # Maximum Velocity (signed) + + if(s*Vi > s*Vr): + Ar = -s*Amax + + Ta = (Vr - Vi)/Ar # Acceleration Time + Td = (-Vr)/Dr # Deceleration Time + + ## Peak velocity handling + dXmin = Ta*(Vr + Vi)/2.0 + Td*(Vr)/2.0 + + ## Short move handling + if s*dXmin > s*dX: + Vr = s*math.sqrt((-(Vi**2 / Ar)-(2*dX))/(1/Dr - 1/Ar)) + Ta = max(0, (Vr - Vi)/Ar) + Tv = 0 + Td = max(0, (-Vr)/Dr) + else: + Tv = (dX - dXmin)/Vr # non-short move, coast time at constant v + + ## We've computed Ta, Tv, Td, and Vr. Time to produce a trajectory + # Create the time series and preallocate the position, velocity, and acceleration arrays + t_traj = np.linspace(0, Ta+Tv+Td, 10000) + y = [None]*len(t_traj) + yd = [None]*len(t_traj) + ydd = [None]*len(t_traj) + + # We only know acceleration (Ar and Dr), so we integrate to create + # the velocity and position curves + y_Accel = (Ar*Ta*Ta) / 2 + (Vi * Ta) + Xi + Tav = Ta + Tv + + for i in range(len(t_traj)): + t = t_traj[i] + if(t <= 0): # Initial conditions + y[i] = Xi + yd[i] = Vi + ydd[i] = Ai + elif(t <= Ta): # Acceleration + y[i] = (Ar * (t*t)/2) + (Vi * t) + Xi + yd[i] = (Ar * t) + Vi + ydd[i] = Ar + elif(t <= Ta+Tv): # Coasting + y[i] = y_Accel + (Vr * (t - Ta)) + yd[i] = Vr + ydd[i] = 0 + elif(t <= Ta+Tv+Td): # Deceleration + y[i] = y_Accel + (Vr * (t - Ta)) + Dr*((t - Tav)*(t - Tav))/2 + yd[i] = Vr + Dr*(t - Tav) + ydd[i] = Dr + + return (y, yd, ydd, t_traj) + + +#(Y, Yd, Ydd, t) = trapPlan(10, 0, 0, 0, 15.122, 22.022, 22.022) +fig, axes = plt.subplots(2, 4) +random.seed() +for x in range(8): + + Vmax = random.uniform(0.1, 20) + Amax = random.uniform(0.1, 40) + + Xi = random.uniform(-100.0, 100.0) + Vi = random.uniform(-Vmax, Vmax) + Xf = random.uniform(-100.0, 100.0) + + (Y, Yd, Ydd, t) = trapPlan(Xf, Xi, 0, 0, Vmax, Amax, Amax) + + if(abs(Xf-Y[-1]) > 0.0001): + print("Bad final position: ", Xf, Y[-1], abs(Xf-Y[-1])) + plt.plot(t, Y) + plt.plot(t, Yd) + plt.plot(t, Ydd) + plt.show() + + elif(abs(Yd[-1]) > 0.0001): + print("Bad final Velocity: ", Yd[-1]) + plt.plot(t, Y) + plt.plot(t, Yd) + plt.plot(t, Ydd) + plt.show() + + else: + print("Position Error: {:.6f}\tVelocity Error: {:.6f}".format(abs(Xf-Y[-1]),abs(Yd[-1]))) + + axes[int(x/4), x%4].plot(t, Y) + axes[int(x/4), x%4].plot(t, Yd) + axes[int(x/4), x%4].plot(t, Ydd) + axes[int(x/4), x%4].set_title('Xi: {:.3f} Xf: {:.3f}'.format(Xi, Xf)) + +plt.show() diff --git a/tools/Motion Planning/Planner.py b/tools/Motion Planning/Planner.py deleted file mode 100644 index 369d0cf7..00000000 --- a/tools/Motion Planning/Planner.py +++ /dev/null @@ -1,85 +0,0 @@ -import numpy as np -import math -import matplotlib.pyplot as plt -import random - -def trapPlan(Xf, Vf, Xi, Vi, Ai, Vmax, Amax, Dmax, dT=0.001): - - dX = Xf - Xi # Distance to travel - s = np.sign(dX) # Sign - - Ar = s*Amax # Maximum Acceleration (signed) - Dr = -s*Dmax # Maximum Deceleration (signed) - Vr = s*Vmax # Maximum Velocity (signed) - - if(s*Vi > s*Vr): - Ar = -s*Amax - - Ta = (Vr - Vi)/Ar # Acceleration Time - Td = (Vf - Vr)/Dr # Deceleration Time - - ## Peak velocity handling - dXmin = Ta*(Vr + Vi)/2 + Td*(Vr + Vf)/2 - - ## Short move handling - if s*dXmin > s*dX: - Vr = s*math.sqrt(-1*Ar*(Vf*Vf-2*Dr*dX))*math.sqrt(Dr-Ar)/(Dr-Ar) # Modified from paper to handle non-zero Vf - Ta = max(0, (Vr - Vi)/Ar) - Tv = 0 - Td = max(0, (Vf - Vr)/Dr) - else: - Tv = (dX - dXmin)/Vr # non-short move, coast time at constant v - - ## We've computed Ta, Tv, Td, and Vr. Time to produce a trajectory - # Create the time series and preallocate the position, velocity, and acceleration arrays - t_traj = np.arange(0, Ta+Tv+Td, dT) - y = [None]*len(t_traj) - yd = [None]*len(t_traj) - ydd = [None]*len(t_traj) - - # We only know acceleration (Ar and Dr), so we integrate to create - # the velocity and position curves - y_Accel = (Ar*Ta*Ta) / 2 + (Vi * Ta) + Xi - Tav = Ta + Tv - - for i in range(len(t_traj)): - t = t_traj[i] - if(t <= 0): # Initial conditions - y[i] = Xi - yd[i] = Vi - ydd[i] = Ai - elif(t <= Ta): # Acceleration - y[i] = (Ar * (t*t)/2) + (Vi * t) + Xi - yd[i] = (Ar * t) + Vi - ydd[i] = Ar - elif(t <= Ta+Tv): # Coasting - y[i] = y_Accel + (Vr * (t - Ta)) - yd[i] = Vr - ydd[i] = 0 - elif(t <= Ta+Tv+Td): # Deceleration - y[i] = y_Accel + (Vr * (t - Ta)) + Dr*((t - Tav)*(t - Tav))/2 - yd[i] = Vr + Dr*(t - Tav) - ydd[i] = Dr - - return (y, yd, ydd, t_traj) - - -(Y, Yd, Ydd, t) = trapPlan(0.74, 1.797, 0, 0, 0, 15.122, 22.022, 22.022) -# random.seed() -# for x in range(100): - -# Vmax = random.uniform(0.1, 20) -# Amax = random.uniform(0.1, 40) - -# Xf = random.uniform(-100.0, 100.0) -# Vf = random.uniform(-Vmax+0.001, Vmax-0.001) - -# print(round(Xf, 3), round(Vf, 3), round(Vmax, 3), round(Amax, 3)) -# (Y, Yd, Ydd, t) = trapPlan(Xf, Vf, 0, 0, 0, Vmax, Amax, Amax) - - # print(Xf-Y[-1], Vf-Yd[-1]) - - # plt.plot(t, Y) - # plt.plot(t, Yd) - # plt.plot(t, Ydd) - # plt.show() \ No newline at end of file