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

Some tweaks to FIR Planner

Browse Source
This commit is contained in:
Unknown
2018-08-11 14:41:53 -04:00
parent 190da53ee9
commit 332ef149c6
1 changed files with 11 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/Motion Planning/FIR_Planner.py
+11 -8
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 trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax, dT=0.001):
def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax, dT=0.001):
dX = Xf - Xi # Distance to travel
s = np.sign(dX) # Sign
@@ -39,7 +39,8 @@ def trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax, dT=0.001):
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)
@@ -75,7 +76,9 @@ def trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax, dT=0.001):
#(Y, Yd, Ydd, t) = trapPlan(10, 0, 0, 0, 15.122, 22.022, 22.022)
fig, axes = plt.subplots(2, 4)
numRows = 2
numCols = 4
fig, axes = plt.subplots(numRows, numCols, sharey='all')
random.seed()
for x in range(8):
@@ -86,7 +89,7 @@ for x in range(8):
Vi = random.uniform(-Vmax, Vmax)
Xf = random.uniform(-100.0, 100.0)
(Y, Yd, Ydd, t) = trapPlan(Xf, Xi, 0, 0, Vmax, Amax, Amax)
(Y, Yd, Ydd, t) = FIR_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]))
@@ -105,9 +108,9 @@ for x in range(8):
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))
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))
plt.show()