From b4de4a1db7dc498b0c37885083524506f7d66935 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sat, 2 Feb 2019 15:23:12 +0900 Subject: [PATCH] code clean up --- Bipedal/bipedal_planner/bipedal_planner.py | 6 +- .../kmeans_clustering/kmeans_clustering.py | 2 +- PathPlanning/AStar/a_star.py | 2 +- .../closed_loop_rrt_star_car.py | 19 +- PathPlanning/Dijkstra/dijkstra.py | 2 +- .../Eta3SplinePath/eta3_spline_path.py | 4 +- .../frenet_optimal_trajectory.py | 2 +- PathPlanning/HybridAStar/hybrid_a_star.py | 222 ------------------ .../motion_model.py | 5 +- .../quintic_polynomials_planner.py | 10 +- PathTracking/cgmres_nmpc/cgmres_nmpc.py | 28 ++- PathTracking/move_to_pose/move_to_pose.py | 8 +- 12 files changed, 46 insertions(+), 264 deletions(-) delete mode 100644 PathPlanning/HybridAStar/hybrid_a_star.py diff --git a/Bipedal/bipedal_planner/bipedal_planner.py b/Bipedal/bipedal_planner/bipedal_planner.py index 1d537c31..3cf2936e 100644 --- a/Bipedal/bipedal_planner/bipedal_planner.py +++ b/Bipedal/bipedal_planner/bipedal_planner.py @@ -52,10 +52,10 @@ class BipedalPlanner(object): self.ref_p = [] # reference footstep positions self.act_p = [] # actual footstep positions - px, py = 0., 0. # reference footstep position + px, py = 0.0, 0.0 # reference footstep position px_star, py_star = px, py # modified footstep position - xi, xi_dot, yi, yi_dot = 0., 0., 0.01, 0. # TODO yi should be set as +epsilon, set xi, yi as COM - time = 0. + xi, xi_dot, yi, yi_dot = 0.0, 0.0, 0.01, 0.0 + time = 0.0 n = 0 self.ref_p.append([px, py, 0]) self.act_p.append([px, py, 0]) diff --git a/Mapping/kmeans_clustering/kmeans_clustering.py b/Mapping/kmeans_clustering/kmeans_clustering.py index 5d29aa11..9d333451 100644 --- a/Mapping/kmeans_clustering/kmeans_clustering.py +++ b/Mapping/kmeans_clustering/kmeans_clustering.py @@ -124,7 +124,7 @@ def calc_association(cx, cy, clusters): inds = [] - for ic in range(len(cx)): + for ic, _ in enumerate(cx): tcx = cx[ic] tcy = cy[ic] diff --git a/PathPlanning/AStar/a_star.py b/PathPlanning/AStar/a_star.py index 1d4a186a..a1e36aaf 100644 --- a/PathPlanning/AStar/a_star.py +++ b/PathPlanning/AStar/a_star.py @@ -85,7 +85,7 @@ def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr): closedset[c_id] = current # expand search grid based on motion model - for i in range(len(motion)): + for i, _ in enumerate(motion): node = Node(current.x + motion[i][0], current.y + motion[i][1], current.cost + motion[i][2], c_id) diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py index 4f558b22..86bcf8b3 100644 --- a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py +++ b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py @@ -5,9 +5,6 @@ author: AtsushiSakai(@Atsushi_twi) """ -import sys -sys.path.append("../ReedsSheppPath/") - import random import math import copy @@ -15,8 +12,14 @@ import numpy as np import pure_pursuit import matplotlib.pyplot as plt -import reeds_shepp_path_planning -import unicycle_model +import sys +sys.path.append("../ReedsSheppPath/") + +try: + import reeds_shepp_path_planning + import unicycle_model +except: + raise show_animation = True @@ -130,8 +133,8 @@ class RRT(): fy.append(self.end.y) fyaw.append(self.end.yaw) return True, fx, fy, fyaw, fv, ft, fa, fd - else: - return False, None, None, None, None, None, None, None + + return False, None, None, None, None, None, None, None def calc_tracking_path(self, path): path = np.array(path[::-1]) @@ -194,7 +197,7 @@ class RRT(): return find_goal, x, y, yaw, v, t, a, d def choose_parent(self, newNode, nearinds): - if len(nearinds) == 0: + if not nearinds: return newNode dlist = [] diff --git a/PathPlanning/Dijkstra/dijkstra.py b/PathPlanning/Dijkstra/dijkstra.py index c1c865e1..889336dd 100644 --- a/PathPlanning/Dijkstra/dijkstra.py +++ b/PathPlanning/Dijkstra/dijkstra.py @@ -67,7 +67,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, reso, rr): closedset[c_id] = current # expand search grid based on motion model - for i in range(len(motion)): + for i, _ in enumerate(motion): node = Node(current.x + motion[i][0], current.y + motion[i][1], current.cost + motion[i][2], c_id) n_id = calc_index(node, xw, minx, miny) diff --git a/PathPlanning/Eta3SplinePath/eta3_spline_path.py b/PathPlanning/Eta3SplinePath/eta3_spline_path.py index 263909a9..072b0bb4 100644 --- a/PathPlanning/Eta3SplinePath/eta3_spline_path.py +++ b/PathPlanning/Eta3SplinePath/eta3_spline_path.py @@ -185,8 +185,8 @@ class eta3_path_segment(object): assert(order > 0 and order <= 2) if order == 1: return self.coeffs[:, 1:].dot(np.array([1, 2. * u, 3. * u**2, 4. * u**3, 5. * u**4, 6. * u**5, 7. * u**6])) - else: - return self.coeffs[:, 2:].dot(np.array([2, 6. * u, 12. * u**2, 20. * u**3, 30. * u**4, 42. * u**5])) + + return self.coeffs[:, 2:].dot(np.array([2, 6. * u, 12. * u**2, 20. * u**3, 30. * u**4, 42. * u**5])) def test1(): diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py index 9d6c62e5..2f31f0c6 100644 --- a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py +++ b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py @@ -257,7 +257,7 @@ def check_collision(fp, ob): def check_paths(fplist, ob): okind = [] - for i in range(len(fplist)): + for i, _ in enumerate(fplist): if any([v > MAX_SPEED for v in fplist[i].s_d]): # Max speed check continue elif any([abs(a) > MAX_ACCEL for a in fplist[i].s_dd]): # Max accel check diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py deleted file mode 100644 index a7ac4784..00000000 --- a/PathPlanning/HybridAStar/hybrid_a_star.py +++ /dev/null @@ -1,222 +0,0 @@ -""" - -Hybrid A* path planning - -author: Atsushi Sakai (@Atsushi_twi) - -""" - -import sys -sys.path.append("../ReedsSheppPath/") - -import math -import numpy as np -import scipy.spatial -import matplotlib.pyplot as plt -# import reeds_shepp_path_planning as rs -# import heapq - -EXTEND_AREA = 5.0 # [m] -H_COST = 1.0 - -show_animation = True - - -class Node: - - def __init__(self, xind, yind, yawind, direction, x, y, yaw, directions, steer, cost, pind): - # store kd-tree - self.xind = xind - self.yind = yind - self.yawind = yawind - self.direction = direction - self.xlist = x - self.ylist = y - self.yawlist = yaw - self.directionlist = directions - self.steer = steer - self.cost = cost - self.pind = pind - - -class KDTree: - """ - Nearest neighbor search class with KDTree - """ - - def __init__(self, data): - # store kd-tree - self.tree = scipy.spatial.cKDTree(data) - - def search(self, inp, k=1): - """ - Search NN - inp: input data, single frame or multi frame - """ - - if len(inp.shape) >= 2: # multi input - index = [] - dist = [] - - for i in inp.T: - idist, iindex = self.tree.query(i, k=k) - index.append(iindex) - dist.append(idist) - - return index, dist - else: - dist, index = self.tree.query(inp, k=k) - return index, dist - - def search_in_distance(self, inp, r): - """ - find points with in a distance r - """ - - index = self.tree.query_ball_point(inp, r) - return index - - -class Config: - - def __init__(self, ox, oy, xyreso, yawreso): - min_x_m = min(ox) - EXTEND_AREA - min_y_m = min(oy) - EXTEND_AREA - max_x_m = max(ox) + EXTEND_AREA - max_y_m = max(oy) + EXTEND_AREA - - ox.append(min_x_m) - oy.append(min_y_m) - ox.append(max_x_m) - oy.append(max_y_m) - - self.minx = int(min_x_m / xyreso) - self.miny = int(min_y_m / xyreso) - self.maxx = int(max_x_m / xyreso) - self.maxy = int(max_y_m / xyreso) - - self.xw = int(self.maxx - self.minx) - self.yw = int(self.maxy - self.miny) - - self.minyaw = int(- math.pi / yawreso) - 1 - self.maxyaw = int(math.pi / yawreso) - self.yaww = int(self.maxyaw - self.minyaw) - - -def analytic_expantion(current, ngoal, c, ox, oy, kdtree): - - return False, None # no update - - -def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso): - """ - start - goal - ox: x position list of Obstacles [m] - oy: y position list of Obstacles [m] - xyreso: grid resolution [m] - yawreso: yaw angle resolution [rad] - """ - - # start[2], goal[2] = rs.pi_2_pi(start[2]), rs.pi_2_pi(goal[2]) - # tox, toy = ox[:], oy[:] - - # obkdtree = KDTree(np.vstack((tox, toy)).T) - - # c = Config(tox, toy, xyreso, yawreso) - - # nstart = Node(int(start[0] / xyreso), int(start[1] / xyreso), int(start[2] / yawreso), - # True, [start[0]], [start[1]], [start[2]], [True], 0.0, 0.0, -1) - # ngoal = Node(int(goal[0] / xyreso), int(goal[1] / xyreso), int(goal[2] / yawreso), - # True, [goal[0]], [goal[1]], [goal[2]], [True], 0.0, 0.0, -1) - - # openList, closedList = {}, {} - # h = [] - # # goalqueue = queue.PriorityQueue() - # pq = [] - # openList[calc_index(nstart, c)] = nstart - # heapq.heappush(pq, (calc_index(nstart, c), calc_cost(nstart, h, ngoal, c))) - - # while True: - # if not openList: - # print("Error: Cannot find path, No open set") - # return [], [], [] - - # c_id, cost = heapq.heappop(pq) - # current = openList.pop(c_id) - # closedList[c_id] = current - - # isupdated, fpath = analytic_expantion( - # current, ngoal, c, ox, oy, obkdtree) - - # # print(current) - - rx, ry, ryaw = [], [], [] - - return rx, ry, ryaw - - -def calc_cost(n, h, ngoal, c): - - hcost = 1.0 - - return (n.cost + H_COST * hcost) - - -def calc_index(node, c): - ind = (node.yawind - c.minyaw) * c.xw * c.yw + \ - (node.yind - c.miny) * c.xw + (node.xind - c.minx) - - if ind <= 0: - print("Error(calc_index):", ind) - - return ind - - -def main(): - print("Start Hybrid A* planning") - - ox, oy = [], [] - - for i in range(60): - ox.append(i) - oy.append(0.0) - for i in range(60): - ox.append(60.0) - oy.append(i) - for i in range(61): - ox.append(i) - oy.append(60.0) - for i in range(61): - ox.append(0.0) - oy.append(i) - for i in range(40): - ox.append(20.0) - oy.append(i) - for i in range(40): - ox.append(40.0) - oy.append(60.0 - i) - - # Set Initial parameters - start = [10.0, 10.0, np.deg2rad(90.0)] - goal = [50.0, 50.0, np.deg2rad(-90.0)] - - xyreso = 2.0 - yawreso = np.deg2rad(15.0) - - rx, ry, ryaw = hybrid_a_star_planning( - start, goal, ox, oy, xyreso, yawreso) - - plt.plot(ox, oy, ".k") - # rs.plot_arrow(start[0], start[1], start[2]) - # rs.plot_arrow(goal[0], goal[1], goal[2]) - - plt.grid(True) - plt.axis("equal") - plt.show() - - print(__file__ + " start!!") - - -if __name__ == '__main__': - main() diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py index 39675198..bb66d1fb 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py @@ -18,7 +18,7 @@ class State: def pi_2_pi(angle): - return (angle + math.pi) % (2*math.pi) - math.pi + return (angle + math.pi) % (2 * math.pi) - math.pi def update(state, v, delta, dt, L): @@ -74,5 +74,6 @@ def generate_last_state(s, km, kf, k0): state = State() - [update(state, v, ikp, dt, L) for ikp in kp] + _ = [update(state, v, ikp, dt, L) for ikp in kp] + return state.x, state.y, state.yaw diff --git a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py index ffa7a88e..383b0e92 100644 --- a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py +++ b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py @@ -146,17 +146,17 @@ def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_a break if show_animation: - for i in range(len(rx)): + for i, _ in enumerate(rx): plt.cla() plt.grid(True) plt.axis("equal") plot_arrow(sx, sy, syaw) plot_arrow(gx, gy, gyaw) plot_arrow(rx[i], ry[i], ryaw[i]) - plt.title("Time[s]:" + str(time[i])[0:4] + - " v[m/s]:" + str(rv[i])[0:4] + - " a[m/ss]:" + str(ra[i])[0:4] + - " jerk[m/sss]:" + str(rj[i])[0:4], + plt.title("Time[s]:" + str(time[i])[0:4] + + " v[m/s]:" + str(rv[i])[0:4] + + " a[m/ss]:" + str(ra[i])[0:4] + + " jerk[m/sss]:" + str(rj[i])[0:4], ) plt.pause(0.001) diff --git a/PathTracking/cgmres_nmpc/cgmres_nmpc.py b/PathTracking/cgmres_nmpc/cgmres_nmpc.py index dc72cb75..cc6c1563 100644 --- a/PathTracking/cgmres_nmpc/cgmres_nmpc.py +++ b/PathTracking/cgmres_nmpc/cgmres_nmpc.py @@ -127,8 +127,8 @@ class NMPCSimulatorSystem(): pre_lam_3 = lam_3 + dt * \ (- lam_1 * math.sin(yaw) * v + lam_2 * math.cos(yaw) * v) pre_lam_4 = lam_4 + dt * \ - (lam_1 * math.cos(yaw) + lam_2 * - math.sin(yaw) + lam_3 * math.sin(u_2) / WB) + (lam_1 * math.cos(yaw) + lam_2 + * math.sin(yaw) + lam_3 * math.sin(u_2) / WB) return pre_lam_1, pre_lam_2, pre_lam_3, pre_lam_4 @@ -275,6 +275,8 @@ class NMPCController_with_CGMRES(): e = np.zeros((self.max_iteration + 1, 1)) e[0] = 1.0 + ys_pre = None + for i in range(self.max_iteration): du_1 = vs[::self.input_num, i] * self.ht du_2 = vs[1::self.input_num, i] * self.ht @@ -358,8 +360,8 @@ class NMPCController_with_CGMRES(): for i in range(N): # ∂H/∂u(xi, ui, λi) F.append(u_1s[i] + lam_4s[i] + 2.0 * raw_1s[i] * u_1s[i]) - F.append(u_2s[i] + lam_3s[i] * v_s[i] / - WB * math.cos(u_2s[i])**2 + 2.0 * raw_2s[i] * u_2s[i]) + F.append(u_2s[i] + lam_3s[i] * v_s[i] + / WB * math.cos(u_2s[i])**2 + 2.0 * raw_2s[i] * u_2s[i]) F.append(-PHI_V + 2.0 * raw_1s[i] * dummy_u_1s[i]) F.append(-PHI_OMEGA + 2.0 * raw_2s[i] * dummy_u_2s[i]) @@ -420,13 +422,13 @@ def plot_figures(plant_system, controller, iteration_num, dt): u_2_fig.set_xlabel("time [s]") u_2_fig.set_ylabel("u_omega") - dummy_1_fig.plot(np.arange(iteration_num - 1) * - dt, controller.history_dummy_u_1) + dummy_1_fig.plot(np.arange(iteration_num - 1) + * dt, controller.history_dummy_u_1) dummy_1_fig.set_xlabel("time [s]") dummy_1_fig.set_ylabel("dummy u_1") - dummy_2_fig.plot(np.arange(iteration_num - 1) * - dt, controller.history_dummy_u_2) + dummy_2_fig.plot(np.arange(iteration_num - 1) + * dt, controller.history_dummy_u_2) dummy_2_fig.set_xlabel("time [s]") dummy_2_fig.set_ylabel("dummy u_2") @@ -476,8 +478,8 @@ def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"): [WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]]) fr_wheel = np.array([[WHEEL_LEN, -WHEEL_LEN, -WHEEL_LEN, WHEEL_LEN, WHEEL_LEN], - [-WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD, WHEEL_WIDTH - - TREAD, WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD]]) + [-WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD, WHEEL_WIDTH - + TREAD, WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD]]) rr_wheel = np.copy(fr_wheel) @@ -549,9 +551,9 @@ def animation(plant, controller, dt): plot_car(x, y, yaw, steer=steer) plt.axis("equal") plt.grid(True) - plt.title("Time[s]:" + str(round(time, 2)) + - ", accel[m/s]:" + str(round(accel, 2)) + - ", speed[km/h]:" + str(round(v * 3.6, 2))) + plt.title("Time[s]:" + str(round(time, 2)) + + ", accel[m/s]:" + str(round(accel, 2)) + + ", speed[km/h]:" + str(round(v * 3.6, 2))) plt.pause(0.0001) plt.close("all") diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py index c500ba55..2fe18817 100644 --- a/PathTracking/move_to_pose/move_to_pose.py +++ b/PathTracking/move_to_pose/move_to_pose.py @@ -48,11 +48,9 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal): x_diff = x_goal - x y_diff = y_goal - y - """ - Restrict alpha and beta (angle differences) to the range - [-pi, pi] to prevent unstable behavior e.g. difference going - from 0 rad to 2*pi rad with slight turn - """ + # Restrict alpha and beta (angle differences) to the range + # [-pi, pi] to prevent unstable behavior e.g. difference going + # from 0 rad to 2*pi rad with slight turn rho = np.sqrt(x_diff**2 + y_diff**2) alpha = (np.arctan2(y_diff, x_diff) -