code clean up

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
+        xi, xi_dot, yi, yi_dot = 0.0, 0.0, 0.01, 0.0
-        time = 0.
+        time = 0.0
         n = 0
         self.ref_p.append([px, py, 0])
         self.act_p.append([px, py, 0])

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]
 

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)

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 sys
-import unicycle_model
+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 = []

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)

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():

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

PathPlanning/HybridAStar/hybrid_a_star.py

@@ -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()

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

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] +
+            plt.title("Time[s]:" + str(time[i])[0:4]
-                      " v[m/s]:" + str(rv[i])[0:4] +
+                      + " v[m/s]:" + str(rv[i])[0:4]
-                      " a[m/ss]:" + str(ra[i])[0:4] +
+                      + " a[m/ss]:" + str(ra[i])[0:4]
-                      " jerk[m/sss]:" + str(rj[i])[0:4],
+                      + " jerk[m/sss]:" + str(rj[i])[0:4],
                       )
             plt.pause(0.001)
 

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 *
+            (lam_1 * math.cos(yaw) + lam_2
-             math.sin(yaw) + lam_3 * math.sin(u_2) / WB)
+             * 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] /
+            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])
+                     / 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) *
+    dummy_1_fig.plot(np.arange(iteration_num - 1)
-                     dt, controller.history_dummy_u_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) *
+    dummy_2_fig.plot(np.arange(iteration_num - 1)
-                     dt, controller.history_dummy_u_2)
+                     * 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
+                         [-WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD, WHEEL_WIDTH -
-                          - TREAD, WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD]])
+                          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)) +
+        plt.title("Time[s]:" + str(round(time, 2))
-                  ", accel[m/s]:" + str(round(accel, 2)) +
+                  + ", accel[m/s]:" + str(round(accel, 2))
-                  ", speed[km/h]:" + str(round(v * 3.6, 2)))
+                  + ", speed[km/h]:" + str(round(v * 3.6, 2)))
         plt.pause(0.0001)
 
     plt.close("all")

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
-        Restrict alpha and beta (angle differences) to the range
+        # [-pi, pi] to prevent unstable behavior e.g. difference going
-        [-pi, pi] to prevent unstable behavior e.g. difference going
+        # from 0 rad to 2*pi rad with slight turn
-        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) -