Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-15 09:30:47 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Further cleanup of FIR Planner

Browse Source
This commit is contained in:
Unknown
2018-08-11 17:48:19 -04:00
parent 5d9ea7c227
commit 51501d145c
1 changed files with 33 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/Motion Planning/FIR_Planner.py
+33 -27
View File
@@ -13,7 +13,7 @@ import random
# 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 FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax):
def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax):
dX = Xf - Xi # Distance to travel
s = np.sign(dX) # Sign
@@ -21,9 +21,6 @@ def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax):
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
@@ -32,7 +29,7 @@ def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax):
dXmin = Ta*(Vr + Vi)/2.0 + Td*(Vr)/2.0
## Short move handling
if s*dXmin > s*dX:
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
@@ -40,7 +37,6 @@ def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax):
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)
@@ -55,10 +51,10 @@ def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax):
for i in range(len(t_traj)):
t = t_traj[i]
if(t <= 0): # Initial conditions
if(t < 0): # Initial conditions
y[i] = Xi
yd[i] = Vi
ydd[i] = Ai
ydd[i] = Ar
elif(t <= Ta): # Acceleration
y[i] = (Ar * (t*t)/2) + (Vi * t) + Xi
yd[i] = (Ar * t) + Vi
@@ -75,42 +71,52 @@ def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax):
return (y, yd, ydd, t_traj)
#(Y, Yd, Ydd, t) = trapPlan(10, 0, 0, 0, 15.122, 22.022, 22.022)
numRows = 2
numCols = 4
fig, axes = plt.subplots(numRows, numCols, sharey='all')
random.seed()
for x in range(8):
for x in range(numRows*numCols):
Vmax = random.uniform(0.1, 20)
Amax = random.uniform(0.1, 40)
Amax = random.uniform(0.1, 10)
Xi = random.uniform(-100.0, 100.0)
Vi = random.uniform(-Vmax, Vmax)
Xf = random.uniform(-100.0, 100.0)
Xi = random.uniform(-100.0, 100.0)
maxVi = math.sqrt(abs(Xf-Xi)*2*Amax)
Vi = random.uniform(-maxVi, maxVi)
(Y, Yd, Ydd, t) = FIR_trapPlan(Xf, Xi, 0, 0, Vmax, Amax, Amax)
if(abs(Xf-Y[-1]) > 0.0001):
(Y, Yd, Ydd, t) = FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Amax)
if(abs(Xf-float(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()
# plt.figure()
# plt.subplot(2,1,1)
# plt.plot(t, Y)
# plt.plot(t, Yd)
# plt.plot(t[-1], Xf, 'b*')
# plt.plot(t[-1], 0, 'r*')
# plt.subplot(2,1,2)
# 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])))
# plt.figure()
# plt.plot(t, Y)
# plt.plot(t, Yd)
# # plt.plot(t, Ydd)
# plt.show()
axes[int(x/numCols), x%numCols].plot(t, Y)
axes[int(x/numCols), x%numCols].plot(t, Yd)
axes[int(x/numCols), x%numCols].plot(t, Ydd)
axes[int(x/numCols), x%numCols].set_title('Xi: {:.3f} Xf: {:.3f}'.format(Xi, Xf))
axes[int(x/numCols), x%numCols].plot(t[-1], Xf, 'b*')
axes[int(x/numCols), x%numCols].plot(t[-1], 0, 'r*')
dX = abs(Xf - Y[-1])
dV = abs(0 - Yd[-1])
axes[int(x/numCols), x%numCols].set_title('Xi: {:.3f} Xf: {:.3f}\ndX: {:.3f} dV: {:.3f}'.format(Xi, Xf, dX, dV))
plt.show()