github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 04:28:04 -05:00
Code Issues Packages Projects Releases Wiki Activity

Fix the start state to use the current accel in frent planner(#755) (#756)

Browse Source 
The robot didn't reach the target velocity when running frenet_optimal_trajectory.py.

To fix this, I changed the constraints for determining polynomials that generate longitudinal trajectories.

Expressly, I set the initial acceleration to the current acceleration.
This commit is contained in:
KohMat
2022-11-20 13:19:42 +09:00
committed by GitHub
parent 8f14488ec2
commit bcb2c863eb
1 changed files with 7 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py
View File

@@ -116,7 +116,7 @@ class FrenetPath:
self.c = []
def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
def calc_frenet_paths(c_speed, c_accel, c_d, c_d_d, c_d_dd, s0):
frenet_paths = []
# generate path to each offset goal
@@ -139,7 +139,7 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE,
TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
tfp = copy.deepcopy(fp)
lon_qp = QuarticPolynomial(s0, c_speed, 0.0, tv, 0.0, Ti)
lon_qp = QuarticPolynomial(s0, c_speed, c_accel, tv, 0.0, Ti)
tfp.s = [lon_qp.calc_point(t) for t in fp.t]
tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
@@ -225,8 +225,8 @@ def check_paths(fplist, ob):
return [fplist[i] for i in ok_ind]
def frenet_optimal_planning(csp, s0, c_speed, c_d, c_d_d, c_d_dd, ob):
fplist = calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0)
def frenet_optimal_planning(csp, s0, c_speed, c_accel, c_d, c_d_d, c_d_dd, ob):
fplist = calc_frenet_paths(c_speed, c_accel, c_d, c_d_d, c_d_dd, s0)
fplist = calc_global_paths(fplist, csp)
fplist = check_paths(fplist, ob)
@@ -274,6 +274,7 @@ def main():
# initial state
c_speed = 10.0 / 3.6 # current speed [m/s]
c_accel = 0.0 # current acceleration [m/ss]
c_d = 2.0 # current lateral position [m]
c_d_d = 0.0 # current lateral speed [m/s]
c_d_dd = 0.0 # current lateral acceleration [m/s]
@@ -283,13 +284,14 @@ def main():
for i in range(SIM_LOOP):
path = frenet_optimal_planning(
csp, s0, c_speed, c_d, c_d_d, c_d_dd, ob)
csp, s0, c_speed, c_accel, c_d, c_d_d, c_d_dd, ob)
s0 = path.s[1]
c_d = path.d[1]
c_d_d = path.d_d[1]
c_d_dd = path.d_dd[1]
c_speed = path.s_d[1]
c_accel = path.s_dd[1]
if np.hypot(path.x[1] - tx[-1], path.y[1] - ty[-1]) <= 1.0:
print("Goal")