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

FIR Planner working for all conditions

Browse Source
This commit is contained in:
Unknown
2018-08-11 20:59:33 -04:00
parent 51501d145c
commit 2eae19a190
1 changed files with 84 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/Motion Planning/FIR_Planner.py
+84 -30
View File
@@ -15,27 +15,54 @@ import random
def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax):
dX_stop = Vi**2 / (2*Dmax) # Minimum stopping distance
dX = Xf - Xi # Distance to travel
s = np.sign(dX) # Sign
s = np.sign(dX) # Sign of travel direction
Ar = s*Amax # Maximum Acceleration (signed)
Dr = -s*Dmax # Maximum Deceleration (signed)
Vr = s*Vmax # Maximum Velocity (signed)
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)
if abs(dX) <= dX_stop: # Check for an overshoot condition (decelerate only)
Ta = 0
Tv = 0
Td = max(0, (-Vr)/Dr)
Vr = Vi
Dr = -np.sign(Vi)*Dmax
Td = abs(Vi) / Dmax
print("Overshoot Move:")
print("dX: {:.3f}\tdx_Stop: {:.3f}".format(dX, dX_stop))
print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(Xf, Xi, Vi, Vmax, Amax))
print("Ta: {:.3f}\tTv: {:.3f}\tTd: {:.3f}".format(Ta, Tv, Td))
print("Ar: {:.3f}\tDr: {:.3f}\tVr: {:.3f}".format(Ar, Dr, Vr))
print()
else:
Tv = (dX - dXmin)/Vr # non-short move, coast time at constant v
# Correct initial acceleration direction if needed
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 abs(dX) < abs(dXmin):
print("Short Move:")
print("dX: {:.3f}\tdXmin: {:.3f}".format(dX, dXmin))
print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(Xf, Xi, Vi, Vmax, Amax))
print("Ta: {:.3f}\tTd: {:.3f}\tVr: {:.3f}".format(Ta, Td, Vr))
print()
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
@@ -55,11 +82,11 @@ def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax):
y[i] = Xi
yd[i] = Vi
ydd[i] = Ar
elif(t <= Ta): # Acceleration
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
elif(t < Ta+Tv): # Coasting
y[i] = y_Accel + (Vr * (t - Ta))
yd[i] = Vr
ydd[i] = 0
@@ -73,23 +100,43 @@ def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax):
numRows = 2
numCols = 4
fig, axes = plt.subplots(numRows, numCols, sharey='all')
fig, axes = plt.subplots(numRows, numCols)
random.seed()
for x in range(numRows*numCols):
Vmax = random.uniform(0.1, 20)
Amax = random.uniform(0.1, 10)
Amax = random.uniform(0.1, 4)
Dmax = Amax
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)
Vi = random.uniform(-Vmax*2, Vmax*2)
# Vmax = .5
# Amax = .5
# Dmax = Amax
# Xf = 10
# Xi = -2
# Vi = 4
(Y, Yd, Ydd, t) = FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Amax)
(Y, Yd, Ydd, t) = FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax)
if(abs(Xf-float(Y[-1])) > 0.0001):
print("Bad final position: ", Xf, Y[-1], abs(Xf-Y[-1]))
if(abs(Xf-Xi) <= Vi**2 / (2*Dmax)):
print("Overshoot: ",Xf)
print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(Xf, Xi, Vi, Vmax, Amax))
print("Y: {:.3f}\tYd: {:.3f}\tYdd: {:.3f}".format(Y[-1], Yd[-1], Ydd[-1]))
print()
(Y2, Yd2, Ydd2, t2) = FIR_trapPlan(Xf, Y[-1], Yd[-1], Vmax, Amax, Dmax)
Y.extend(Y2)
Yd.extend(Yd2)
Ydd.extend(Ydd2)
t2 = t2 + t[-1]
t = np.append(t, t2, axis=0)
if(abs(Xf-Y[-1]) > 0.0001):
print("Bad Final Position")
print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(Xf, Xi, Vi, Vmax, Amax))
print()
# plt.figure()
# plt.subplot(2,1,1)
# plt.plot(t, Y)
@@ -101,22 +148,29 @@ for x in range(numRows*numCols):
# plt.plot(t, Ydd)
# plt.show()
elif(abs(Yd[-1]) > 0.0001):
print("Bad final Velocity: ", Yd[-1])
print("Bad Final Velocity")
print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(Xf, Xi, Vi, Vmax, Amax))
print()
# 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].plot(t[-1], Xf, 'b*')
axes[int(x/numCols), x%numCols].plot(t[-1], 0, 'r*')
ax1 = axes[int(x/numCols), x%numCols]
ax1.plot(t, Y)
ax1.plot(t, Yd)
ax1.plot(t[-1], Xf, 'b*')
ax1.plot(t[-1], 0, 'r*')
ax1.set_ylabel("Pos and Velocity")
ax2 = ax1.twinx()
ax2.plot(t, Ydd, color='tab:green')
ax2.tick_params(axis='y', labelcolor='tab:green')
ax2.set_ylabel("Acceleration")
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))
axes[int(x/numCols), x%numCols].set_title('Xf: {:.3f} Xi: {:.3f}\ndX: {:.3f} dV: {:.3f}'.format(Xf, Xi, dX, dV))
plt.show()