From 19630bc0ce9419a70c3ae301fed0f70553a2e5be Mon Sep 17 00:00:00 2001 From: Chachay Date: Fri, 20 Mar 2020 10:13:41 +0100 Subject: [PATCH 1/7] Integrate update function into state class --- .../rear_wheel_feedback.py | 32 ++++--------------- 1 file changed, 6 insertions(+), 26 deletions(-) diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py index 758e9ad9..af9fb068 100644 --- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py +++ b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py @@ -16,7 +16,6 @@ try: except: raise - Kp = 1.0 # speed propotional gain # steering control parameter KTH = 1.0 @@ -26,34 +25,24 @@ dt = 0.1 # [s] L = 2.9 # [m] show_animation = True -# show_animation = False - class State: - def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): self.x = x self.y = y self.yaw = yaw self.v = v - -def update(state, a, delta): - - state.x = state.x + state.v * math.cos(state.yaw) * dt - state.y = state.y + state.v * math.sin(state.yaw) * dt - state.yaw = state.yaw + state.v / L * math.tan(delta) * dt - state.v = state.v + a * dt - - return state - + def update(self, a, delta, dt): + self.x = self.x + self.v * math.cos(self.yaw) * dt + self.y = self.y + self.v * math.sin(self.yaw) * dt + self.yaw = self.yaw + self.v / L * math.tan(delta) * dt + self.v = self.v + a * dt def PIDControl(target, current): a = Kp * (target - current) - return a - def pi_2_pi(angle): while(angle > math.pi): angle = angle - 2.0 * math.pi @@ -63,7 +52,6 @@ def pi_2_pi(angle): return angle - def rear_wheel_feedback_control(state, cx, cy, cyaw, ck, preind): ind, e = calc_nearest_index(state, cx, cy, cyaw) @@ -78,7 +66,6 @@ def rear_wheel_feedback_control(state, cx, cy, cyaw, ck, preind): return 0.0, ind delta = math.atan2(L * omega / v, 1.0) - # print(k, v, e, th_e, omega, delta) return delta, ind @@ -104,9 +91,7 @@ def calc_nearest_index(state, cx, cy, cyaw): return ind, mind - def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal): - T = 500.0 # max simulation time goal_dis = 0.3 stop_speed = 0.05 @@ -126,7 +111,7 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal): di, target_ind = rear_wheel_feedback_control( state, cx, cy, cyaw, ck, target_ind) ai = PIDControl(speed_profile[target_ind], state.v) - state = update(state, ai, di) + state.update(ai, di, dt) if abs(state.v) <= stop_speed: target_ind += 1 @@ -163,11 +148,8 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal): return t, x, y, yaw, v, goal_flag - def calc_speed_profile(cx, cy, cyaw, target_speed): - speed_profile = [target_speed] * len(cx) - direction = 1.0 # Set stop point @@ -190,7 +172,6 @@ def calc_speed_profile(cx, cy, cyaw, target_speed): return speed_profile - def main(): print("rear wheel feedback tracking start!!") ax = [0.0, 6.0, 12.5, 5.0, 7.5, 3.0, -1.0] @@ -237,6 +218,5 @@ def main(): plt.show() - if __name__ == '__main__': main() From 74739078fdcc18cd12a2347efec0dac4ccca6dc2 Mon Sep 17 00:00:00 2001 From: Chachay Date: Fri, 20 Mar 2020 12:08:19 +0100 Subject: [PATCH 2/7] Replace Spline module to Scipy interpolate --- .../rear_wheel_feedback.py | 139 +++++++++++------- 1 file changed, 84 insertions(+), 55 deletions(-) diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py index af9fb068..5e6cedb3 100644 --- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py +++ b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py @@ -9,12 +9,9 @@ import matplotlib.pyplot as plt import math import numpy as np import sys -sys.path.append("../../PathPlanning/CubicSpline/") -try: - import cubic_spline_planner -except: - raise +from scipy import interpolate +from scipy import optimize Kp = 1.0 # speed propotional gain # steering control parameter @@ -39,6 +36,61 @@ class State: self.yaw = self.yaw + self.v / L * math.tan(delta) * dt self.v = self.v + a * dt +class TrackSpline: + def __init__(self, x, y): + x, y = map(np.asarray, (x, y)) + s = np.append([0],(np.cumsum(np.diff(x)**2) + np.cumsum(np.diff(y)**2))**0.5) + + self.X = interpolate.CubicSpline(s, x) + self.Y = interpolate.CubicSpline(s, y) + + self.dX = self.X.derivative(1) + self.ddX = self.X.derivative(2) + + self.dY = self.Y.derivative(1) + self.ddY = self.Y.derivative(2) + + self.length = s[-1] + + def yaw(self, s): + dx, dy = self.dX(s), self.dY(s) + return np.arctan2(dy, dx) + + def curvature(self, s): + dx, dy = self.dX(s), self.dY(s) + ddx, ddy = self.ddX(s), self.ddY(s) + return (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) + + def __findClosestPoint(self, s0, x, y): + def f(_s, *args): + _x, _y= self.X(_s), self.Y(_s) + return (_x - args[0])**2 + (_y - args[1])**2 + + def jac(_s, *args): + _x, _y = self.X(_s), self.Y(_s) + _dx, _dy = self.dX(_s), self.dY(_s) + return 2*_dx*(_x - args[0])+2*_dy*(_y-args[1]) + + minimum = optimize.fmin_cg(f, s0, jac, args=(x, y), full_output=True, disp=False) + return minimum + + def TrackError(self, x, y, s0): + ret = self.__findClosestPoint(s0, x, y) + + s = ret[0][0] + e = ret[1] + + k = self.curvature(s) + yaw = self.yaw(s) + + dxl = self.X(s) - x + dyl = self.Y(s) - y + angle = pi_2_pi(yaw - math.atan2(dyl, dxl)) + if angle < 0: + e*= -1 + + return e, k, yaw, s + def PIDControl(target, current): a = Kp * (target - current) return a @@ -52,46 +104,22 @@ def pi_2_pi(angle): return angle -def rear_wheel_feedback_control(state, cx, cy, cyaw, ck, preind): - ind, e = calc_nearest_index(state, cx, cy, cyaw) - - k = ck[ind] +def rear_wheel_feedback_control(state, e, k, yaw_r): v = state.v - th_e = pi_2_pi(state.yaw - cyaw[ind]) + th_e = pi_2_pi(state.yaw - yaw_r) omega = v * k * math.cos(th_e) / (1.0 - k * e) - \ KTH * abs(v) * th_e - KE * v * math.sin(th_e) * e / th_e if th_e == 0.0 or omega == 0.0: - return 0.0, ind + return 0.0 delta = math.atan2(L * omega / v, 1.0) - return delta, ind + return delta -def calc_nearest_index(state, cx, cy, cyaw): - dx = [state.x - icx for icx in cx] - dy = [state.y - icy for icy in cy] - - d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)] - - mind = min(d) - - ind = d.index(mind) - - mind = math.sqrt(mind) - - dxl = cx[ind] - state.x - dyl = cy[ind] - state.y - - angle = pi_2_pi(cyaw[ind] - math.atan2(dyl, dxl)) - if angle < 0: - mind *= -1 - - return ind, mind - -def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal): +def closed_loop_prediction(track, speed_profile, goal): T = 500.0 # max simulation time goal_dis = 0.3 stop_speed = 0.05 @@ -105,17 +133,17 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal): v = [state.v] t = [0.0] goal_flag = False - target_ind = calc_nearest_index(state, cx, cy, cyaw) + + s = np.arange(0, track.length, 0.1) + e, k, yaw_r, s0 = track.TrackError(state.x, state.y, 0.0) while T >= time: - di, target_ind = rear_wheel_feedback_control( - state, cx, cy, cyaw, ck, target_ind) - ai = PIDControl(speed_profile[target_ind], state.v) + e, k, yaw_r, s0 = track.TrackError(state.x, state.y, s0) + di = rear_wheel_feedback_control(state, e, k, yaw_r) + #ai = PIDControl(speed_profile[target_ind], state.v) + ai = PIDControl(speed_profile, state.v) state.update(ai, di, dt) - if abs(state.v) <= stop_speed: - target_ind += 1 - time = time + dt # check goal @@ -132,23 +160,22 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal): v.append(state.v) t.append(time) - if target_ind % 1 == 0 and show_animation: + if show_animation: plt.cla() # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) - plt.plot(cx, cy, "-r", label="course") + plt.plot(track.X(s), track.Y(s), "-r", label="course") plt.plot(x, y, "ob", label="trajectory") - plt.plot(cx[target_ind], cy[target_ind], "xg", label="target") + plt.plot(track.X(s0), track.Y(s0), "xg", label="target") plt.axis("equal") plt.grid(True) - plt.title("speed[km/h]:" + str(round(state.v * 3.6, 2)) + - ",target index:" + str(target_ind)) + plt.title("speed[km/h]:{:.2f}, target s-param:{:.2f}".format(round(state.v * 3.6, 2), s0)) plt.pause(0.0001) return t, x, y, yaw, v, goal_flag -def calc_speed_profile(cx, cy, cyaw, target_speed): +def calc_speed_profile(track, target_speed, s): speed_profile = [target_speed] * len(cx) direction = 1.0 @@ -178,14 +205,16 @@ def main(): ay = [0.0, 0.0, 5.0, 6.5, 3.0, 5.0, -2.0] goal = [ax[-1], ay[-1]] - cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course( - ax, ay, ds=0.1) + track = TrackSpline(ax, ay) + s = np.arange(0, track.length, 0.1) + target_speed = 10.0 / 3.6 - sp = calc_speed_profile(cx, cy, cyaw, target_speed) + # Note: disable backward direction temporary + #sp = calc_speed_profile(track, target_speed, s) + sp = target_speed - t, x, y, yaw, v, goal_flag = closed_loop_prediction( - cx, cy, cyaw, ck, sp, goal) + t, x, y, yaw, v, goal_flag = closed_loop_prediction(track, sp, goal) # Test assert goal_flag, "Cannot goal" @@ -194,7 +223,7 @@ def main(): plt.close() plt.subplots(1) plt.plot(ax, ay, "xb", label="input") - plt.plot(cx, cy, "-r", label="spline") + plt.plot(track.X(s), track.Y(s), "-r", label="spline") plt.plot(x, y, "-g", label="tracking") plt.grid(True) plt.axis("equal") @@ -203,14 +232,14 @@ def main(): plt.legend() plt.subplots(1) - plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw") + plt.plot(s, np.rad2deg(track.yaw(s)), "-r", label="yaw") plt.grid(True) plt.legend() plt.xlabel("line length[m]") plt.ylabel("yaw angle[deg]") plt.subplots(1) - plt.plot(s, ck, "-r", label="curvature") + plt.plot(s, track.curvature(s), "-r", label="curvature") plt.grid(True) plt.legend() plt.xlabel("line length[m]") From a7c82ccb9e5cc18e6aaca3b3a8ff4de6fdcf1c21 Mon Sep 17 00:00:00 2001 From: Chachay Date: Mon, 23 Mar 2020 16:00:19 +0100 Subject: [PATCH 3/7] - remove unsued import and variable - rename functions - add target speed controller (as the substitute of calc speed profile) --- .../rear_wheel_feedback.py | 83 ++++++++----------- 1 file changed, 34 insertions(+), 49 deletions(-) diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py index 5e6cedb3..9b69f8ea 100644 --- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py +++ b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py @@ -8,7 +8,6 @@ author: Atsushi Sakai(@Atsushi_twi) import matplotlib.pyplot as plt import math import numpy as np -import sys from scipy import interpolate from scipy import optimize @@ -24,11 +23,12 @@ L = 2.9 # [m] show_animation = True class State: - def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): + def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0, direction=1): self.x = x self.y = y self.yaw = yaw self.v = v + self.direction = direction def update(self, a, delta, dt): self.x = self.x + self.v * math.cos(self.yaw) * dt @@ -52,36 +52,36 @@ class TrackSpline: self.length = s[-1] - def yaw(self, s): + def calc_yaw(self, s): dx, dy = self.dX(s), self.dY(s) return np.arctan2(dy, dx) - def curvature(self, s): + def calc_curvature(self, s): dx, dy = self.dX(s), self.dY(s) ddx, ddy = self.ddX(s), self.ddY(s) return (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) - def __findClosestPoint(self, s0, x, y): - def f(_s, *args): + def __find_nearest_point(self, s0, x, y): + def calc_distance(_s, *args): _x, _y= self.X(_s), self.Y(_s) return (_x - args[0])**2 + (_y - args[1])**2 - def jac(_s, *args): + def calc_distance_jacobian(_s, *args): _x, _y = self.X(_s), self.Y(_s) _dx, _dy = self.dX(_s), self.dY(_s) return 2*_dx*(_x - args[0])+2*_dy*(_y-args[1]) - minimum = optimize.fmin_cg(f, s0, jac, args=(x, y), full_output=True, disp=False) + minimum = optimize.fmin_cg(calc_distance, s0, calc_distance_jacobian, args=(x, y), full_output=True, disp=False) return minimum - def TrackError(self, x, y, s0): - ret = self.__findClosestPoint(s0, x, y) + def calc_track_error(self, x, y, s0): + ret = self.__find_nearest_point(s0, x, y) s = ret[0][0] e = ret[1] - k = self.curvature(s) - yaw = self.yaw(s) + k = self.calc_curvature(s) + yaw = self.calc_yaw(s) dxl = self.X(s) - x dyl = self.Y(s) - y @@ -91,7 +91,7 @@ class TrackSpline: return e, k, yaw, s -def PIDControl(target, current): +def pid_control(target, current): a = Kp * (target - current) return a @@ -119,10 +119,9 @@ def rear_wheel_feedback_control(state, e, k, yaw_r): return delta -def closed_loop_prediction(track, speed_profile, goal): +def simulate(track, goal): T = 500.0 # max simulation time goal_dis = 0.3 - stop_speed = 0.05 state = State(x=-0.0, y=-0.0, yaw=0.0, v=0.0) @@ -135,13 +134,14 @@ def closed_loop_prediction(track, speed_profile, goal): goal_flag = False s = np.arange(0, track.length, 0.1) - e, k, yaw_r, s0 = track.TrackError(state.x, state.y, 0.0) + e, k, yaw_r, s0 = track.calc_track_error(state.x, state.y, 0.0) while T >= time: - e, k, yaw_r, s0 = track.TrackError(state.x, state.y, s0) + e, k, yaw_r, s0 = track.calc_track_error(state.x, state.y, s0) di = rear_wheel_feedback_control(state, e, k, yaw_r) - #ai = PIDControl(speed_profile[target_ind], state.v) - ai = PIDControl(speed_profile, state.v) + + speed_r = calc_target_speed(state, yaw_r) + ai = pid_control(speed_r, state.v) state.update(ai, di, dt) time = time + dt @@ -175,29 +175,20 @@ def closed_loop_prediction(track, speed_profile, goal): return t, x, y, yaw, v, goal_flag -def calc_speed_profile(track, target_speed, s): - speed_profile = [target_speed] * len(cx) - direction = 1.0 +def calc_target_speed(state, yaw_r): + target_speed = 10.0 / 3.6 - # Set stop point - for i in range(len(cx) - 1): - dyaw = cyaw[i + 1] - cyaw[i] - switch = math.pi / 4.0 <= dyaw < math.pi / 2.0 + dyaw = yaw_r - state.yaw + switch = math.pi / 4.0 <= dyaw < math.pi / 2.0 - if switch: - direction *= -1 - - if direction != 1.0: - speed_profile[i] = - target_speed - else: - speed_profile[i] = target_speed - - if switch: - speed_profile[i] = 0.0 - - speed_profile[-1] = 0.0 - - return speed_profile + if switch: + state.direction *= -1 + return 0.0 + + if state.direction != 1: + return -target_speed + else: + return target_speed def main(): print("rear wheel feedback tracking start!!") @@ -208,13 +199,7 @@ def main(): track = TrackSpline(ax, ay) s = np.arange(0, track.length, 0.1) - target_speed = 10.0 / 3.6 - - # Note: disable backward direction temporary - #sp = calc_speed_profile(track, target_speed, s) - sp = target_speed - - t, x, y, yaw, v, goal_flag = closed_loop_prediction(track, sp, goal) + t, x, y, yaw, v, goal_flag = simulate(track, goal) # Test assert goal_flag, "Cannot goal" @@ -232,14 +217,14 @@ def main(): plt.legend() plt.subplots(1) - plt.plot(s, np.rad2deg(track.yaw(s)), "-r", label="yaw") + plt.plot(s, np.rad2deg(track.calc_yaw(s)), "-r", label="yaw") plt.grid(True) plt.legend() plt.xlabel("line length[m]") plt.ylabel("yaw angle[deg]") plt.subplots(1) - plt.plot(s, track.curvature(s), "-r", label="curvature") + plt.plot(s, track.calc_curvature(s), "-r", label="curvature") plt.grid(True) plt.legend() plt.xlabel("line length[m]") From 7005a664e1b65d6751d1cb323833acff7c7505ca Mon Sep 17 00:00:00 2001 From: Chachay Date: Mon, 23 Mar 2020 16:10:11 +0100 Subject: [PATCH 4/7] - Changed Class name (CubicSplinePath) - remove unnecessary else case --- PathTracking/rear_wheel_feedback/rear_wheel_feedback.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py index 9b69f8ea..8f1079b0 100644 --- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py +++ b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py @@ -36,7 +36,7 @@ class State: self.yaw = self.yaw + self.v / L * math.tan(delta) * dt self.v = self.v + a * dt -class TrackSpline: +class CubicSplinePath: def __init__(self, x, y): x, y = map(np.asarray, (x, y)) s = np.append([0],(np.cumsum(np.diff(x)**2) + np.cumsum(np.diff(y)**2))**0.5) @@ -187,8 +187,8 @@ def calc_target_speed(state, yaw_r): if state.direction != 1: return -target_speed - else: - return target_speed + + return target_speed def main(): print("rear wheel feedback tracking start!!") @@ -196,7 +196,7 @@ def main(): ay = [0.0, 0.0, 5.0, 6.5, 3.0, 5.0, -2.0] goal = [ax[-1], ay[-1]] - track = TrackSpline(ax, ay) + track = CubicSplinePath(ax, ay) s = np.arange(0, track.length, 0.1) t, x, y, yaw, v, goal_flag = simulate(track, goal) From 6d178e1feb3740c4b7dc67136b9b701f22c6e6e6 Mon Sep 17 00:00:00 2001 From: Chachay Date: Tue, 24 Mar 2020 16:33:43 +0100 Subject: [PATCH 5/7] rename yaw_r to yaw_ref --- .../rear_wheel_feedback/rear_wheel_feedback.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py index 8f1079b0..f6df94ac 100644 --- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py +++ b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py @@ -104,9 +104,9 @@ def pi_2_pi(angle): return angle -def rear_wheel_feedback_control(state, e, k, yaw_r): +def rear_wheel_feedback_control(state, e, k, yaw_ref): v = state.v - th_e = pi_2_pi(state.yaw - yaw_r) + th_e = pi_2_pi(state.yaw - yaw_ref) omega = v * k * math.cos(th_e) / (1.0 - k * e) - \ KTH * abs(v) * th_e - KE * v * math.sin(th_e) * e / th_e @@ -134,14 +134,14 @@ def simulate(track, goal): goal_flag = False s = np.arange(0, track.length, 0.1) - e, k, yaw_r, s0 = track.calc_track_error(state.x, state.y, 0.0) + e, k, yaw_ref, s0 = track.calc_track_error(state.x, state.y, 0.0) while T >= time: - e, k, yaw_r, s0 = track.calc_track_error(state.x, state.y, s0) - di = rear_wheel_feedback_control(state, e, k, yaw_r) + e, k, yaw_ref, s0 = track.calc_track_error(state.x, state.y, s0) + di = rear_wheel_feedback_control(state, e, k, yaw_ref) - speed_r = calc_target_speed(state, yaw_r) - ai = pid_control(speed_r, state.v) + speed_ref = calc_target_speed(state, yaw_ref) + ai = pid_control(speed_ref, state.v) state.update(ai, di, dt) time = time + dt From 9b22d3fb88cde649a0f7b0d20b7d4a5f08cc3a99 Mon Sep 17 00:00:00 2001 From: Chachay Date: Tue, 24 Mar 2020 20:12:26 +0100 Subject: [PATCH 6/7] rename yaw_r to yaw_ref --- PathTracking/rear_wheel_feedback/rear_wheel_feedback.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py index f6df94ac..4141cfbf 100644 --- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py +++ b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py @@ -175,10 +175,10 @@ def simulate(track, goal): return t, x, y, yaw, v, goal_flag -def calc_target_speed(state, yaw_r): +def calc_target_speed(state, yaw_ref): target_speed = 10.0 / 3.6 - dyaw = yaw_r - state.yaw + dyaw = yaw_ref - state.yaw switch = math.pi / 4.0 <= dyaw < math.pi / 2.0 if switch: From b6e89c6d8b15a4907b47ec8bfee145e36a54c783 Mon Sep 17 00:00:00 2001 From: Chachay Date: Wed, 25 Mar 2020 16:42:00 +0100 Subject: [PATCH 7/7] rename a variable --- .../rear_wheel_feedback.py | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py index 4141cfbf..52b4a11a 100644 --- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py +++ b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py @@ -119,7 +119,7 @@ def rear_wheel_feedback_control(state, e, k, yaw_ref): return delta -def simulate(track, goal): +def simulate(path_ref, goal): T = 500.0 # max simulation time goal_dis = 0.3 @@ -133,11 +133,11 @@ def simulate(track, goal): t = [0.0] goal_flag = False - s = np.arange(0, track.length, 0.1) - e, k, yaw_ref, s0 = track.calc_track_error(state.x, state.y, 0.0) + s = np.arange(0, path_ref.length, 0.1) + e, k, yaw_ref, s0 = path_ref.calc_track_error(state.x, state.y, 0.0) while T >= time: - e, k, yaw_ref, s0 = track.calc_track_error(state.x, state.y, s0) + e, k, yaw_ref, s0 = path_ref.calc_track_error(state.x, state.y, s0) di = rear_wheel_feedback_control(state, e, k, yaw_ref) speed_ref = calc_target_speed(state, yaw_ref) @@ -165,9 +165,9 @@ def simulate(track, goal): # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) - plt.plot(track.X(s), track.Y(s), "-r", label="course") + plt.plot(path_ref.X(s), path_ref.Y(s), "-r", label="course") plt.plot(x, y, "ob", label="trajectory") - plt.plot(track.X(s0), track.Y(s0), "xg", label="target") + plt.plot(path_ref.X(s0), path_ref.Y(s0), "xg", label="target") plt.axis("equal") plt.grid(True) plt.title("speed[km/h]:{:.2f}, target s-param:{:.2f}".format(round(state.v * 3.6, 2), s0)) @@ -196,10 +196,10 @@ def main(): ay = [0.0, 0.0, 5.0, 6.5, 3.0, 5.0, -2.0] goal = [ax[-1], ay[-1]] - track = CubicSplinePath(ax, ay) - s = np.arange(0, track.length, 0.1) + reference_path = CubicSplinePath(ax, ay) + s = np.arange(0, reference_path.length, 0.1) - t, x, y, yaw, v, goal_flag = simulate(track, goal) + t, x, y, yaw, v, goal_flag = simulate(reference_path, goal) # Test assert goal_flag, "Cannot goal" @@ -208,7 +208,7 @@ def main(): plt.close() plt.subplots(1) plt.plot(ax, ay, "xb", label="input") - plt.plot(track.X(s), track.Y(s), "-r", label="spline") + plt.plot(reference_path.X(s), reference_path.Y(s), "-r", label="spline") plt.plot(x, y, "-g", label="tracking") plt.grid(True) plt.axis("equal") @@ -217,14 +217,14 @@ def main(): plt.legend() plt.subplots(1) - plt.plot(s, np.rad2deg(track.calc_yaw(s)), "-r", label="yaw") + plt.plot(s, np.rad2deg(reference_path.calc_yaw(s)), "-r", label="yaw") plt.grid(True) plt.legend() plt.xlabel("line length[m]") plt.ylabel("yaw angle[deg]") plt.subplots(1) - plt.plot(s, track.calc_curvature(s), "-r", label="curvature") + plt.plot(s, reference_path.calc_curvature(s), "-r", label="curvature") plt.grid(True) plt.legend() plt.xlabel("line length[m]")