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

Add comments to trajplan script

Browse Source
This commit is contained in:
Unknown
2018-08-06 00:49:41 -04:00
parent 26b87bb95b
commit 17966fafde
1 changed files with 16 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/Motion Planning/Planner.py
+16 -10
View File
@@ -17,20 +17,22 @@ def trapPlan(Xf, Xi, Vf, Vi, Af, Ai, Vmax, Amax, Dmax, dT=0.001):
dX = Xf - Xi # Distance to travel
if Vf == 0:
dXmin = Ta*(Vr + Vi)/2 + Td*(Vr)/2
dXmin = Ta*(Vr + Vi)/2 + Td*(Vr)/2 # Basic wedge profile
elif np.sign(Vf) == np.sign(Vr):
dXmin = Ta*(Vr + Vi)/2 + Td*(Vr-Vf)/2 + Td*Vf
dXmin = Ta*(Vr + Vi)/2 + Td*(Vr - Vf)/2 + Td*Vf # Wedge profile with an unfinished end
else:
dXmin = Ta*(Vr + Vi)/2 + Td*(Vr - s*Vf)/2
dXmin = Ta*(Vr + Vi)/2 + Td*(Vr - s*Vf)/2 # Wedge profile that crosses Y axis on decel
if s*dXmin > s*dX:
Vr = s*math.sqrt(-1*Ar*(Vf*Vf-2*Dr*dX))/math.sqrt(Dr-Ar)
if s*dXmin > s*dX: # Short move handling
Vr = s*math.sqrt(-1*Ar*(Vf*Vf-2*Dr*dX))/math.sqrt(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
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)
@@ -38,22 +40,26 @@ def trapPlan(Xf, Xi, Vf, Vi, Af, Ai, Vmax, Amax, Dmax, dT=0.001):
for i in range(len(t_traj)):
t = t_traj[i]
if(t <= 0):
if(t <= 0): # Initial conditions
y[i] = Xi
yd[i] = Vi
ydd[i] = Ai
elif(t <= Ta):
elif(t <= Ta): # Acceleration
y[i] = y[i-1] + yd[i-1] * dT + (0.5*ydd[i-1]*dT*dT)
yd[i] = yd[i-1] + ydd[i-1]*dT
ydd[i] = Ar
elif(t <= Ta+Tv):
elif(t <= Ta+Tv): # Coasting
y[i] = y[i-1] + yd[i-1] * dT
yd[i] = yd[i-1]
ydd[i] = 0
elif(t <= Ta+Tv+Td):
elif(t < Ta+Tv+Td): # Deceleration
y[i] = y[i-1] + yd[i-1] * dT + (0.5*ydd[i-1]*dT*dT)
yd[i] = yd[i-1] + ydd[i-1]*dT
ydd[i] = Dr
else: # Final conditions
y[i] = Xf
yd[i] = Vf
ydd[i] = Af
return (y, yd, ydd)