From f0b6b7a94df847083aeb33cb3368564a8ec5409b Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 3 Feb 2019 10:00:09 +0900 Subject: [PATCH] code clean up for LGTM --- .../drone_3d_trajectory_following.py | 4 ++-- .../rocket_powered_landing.py | 6 ++---- .../arm_obstacle_navigation_2.py | 4 ++-- .../ClosedLoopRRTStar/pure_pursuit.py | 1 - PathPlanning/LQRPlanner/LQRplanner.py | 1 - .../potential_field_planning.py | 2 +- .../lqr_steer_control/lqr_steer_control.py | 20 ++++++++++--------- PathTracking/pure_pursuit/pure_pursuit.py | 3 ++- 8 files changed, 20 insertions(+), 21 deletions(-) diff --git a/AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py b/AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py index 3c980531..438494a4 100644 --- a/AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py +++ b/AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py @@ -73,8 +73,8 @@ def quad_sim(x_c, y_c, z_c): # des_x_pos = calculate_position(x_c[i], t) # des_y_pos = calculate_position(y_c[i], t) des_z_pos = calculate_position(z_c[i], t) - des_x_vel = calculate_velocity(x_c[i], t) - des_y_vel = calculate_velocity(y_c[i], t) + # des_x_vel = calculate_velocity(x_c[i], t) + # des_y_vel = calculate_velocity(y_c[i], t) des_z_vel = calculate_velocity(z_c[i], t) des_x_acc = calculate_acceleration(x_c[i], t) des_y_acc = calculate_acceleration(y_c[i], t) diff --git a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py index 64143499..2a07cb98 100644 --- a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py +++ b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py @@ -576,7 +576,7 @@ def plot_animation(X, U): # pragma: no cover axis3d_equal(X[2, :], X[3, :], X[1, :], ax) rx, ry, rz = X[1:4, k] - vx, vy, vz = X[4:7, k] + # vx, vy, vz = X[4:7, k] qw, qx, qy, qz = X[7:11, k] CBI = np.array([ @@ -618,8 +618,6 @@ def main(): integrator = Integrator(m, K) problem = SCProblem(m, K) - last_linear_cost = None - converged = False w_delta = W_DELTA for it in range(iterations): @@ -633,7 +631,7 @@ def main(): X_last=X, U_last=U, sigma_last=sigma, weight_sigma=W_SIGMA, weight_nu=W_NU, weight_delta=w_delta, weight_delta_sigma=W_DELTA_SIGMA) - info = problem.solve() + problem.solve() X = problem.get_variable('X') U = problem.get_variable('U') diff --git a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py index b2645155..b8c1ce89 100644 --- a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py +++ b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py @@ -53,7 +53,7 @@ def animate(grid, arm, route): theta2 = 2 * pi * node[1] / M - pi arm.update_joints([theta1, theta2]) plt.subplot(1, 2, 2) - arm.plot(plt, obstacles=obstacles) + arm.plot_arm(plt, obstacles=obstacles) plt.xlim(-2.0, 2.0) plt.ylim(-3.0, 3.0) plt.show() @@ -272,7 +272,7 @@ class NLinkArm(object): self.end_effector = np.array(self.points[self.n_links]).T - def plot(self, myplt, obstacles=[]): # pragma: no cover + def plot_arm(self, myplt, obstacles=[]): # pragma: no cover myplt.cla() for obstacle in obstacles: diff --git a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py index 099114b7..984c4a21 100644 --- a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py +++ b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py @@ -222,7 +222,6 @@ def extend_path(cx, cy, cyaw): def main(): # target course - import numpy as np cx = np.arange(0, 50, 0.1) cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in cx] diff --git a/PathPlanning/LQRPlanner/LQRplanner.py b/PathPlanning/LQRPlanner/LQRplanner.py index 21d7e1b0..78251a44 100644 --- a/PathPlanning/LQRPlanner/LQRplanner.py +++ b/PathPlanning/LQRPlanner/LQRplanner.py @@ -73,7 +73,6 @@ def solve_DARE(A, B, Q, R): Xn = A.T * X * A - A.T * X * B * \ la.inv(R + B.T * X * B) * B.T * X * A + Q if (abs(Xn - X)).max() < eps: - X = Xn break X = Xn diff --git a/PathPlanning/PotentialFieldPlanning/potential_field_planning.py b/PathPlanning/PotentialFieldPlanning/potential_field_planning.py index 0723a588..1e918fb1 100644 --- a/PathPlanning/PotentialFieldPlanning/potential_field_planning.py +++ b/PathPlanning/PotentialFieldPlanning/potential_field_planning.py @@ -157,7 +157,7 @@ def main(): plt.axis("equal") # path generation - rx, ry = potential_field_planning( + _, _ = potential_field_planning( sx, sy, gx, gy, ox, oy, grid_size, robot_radius) if show_animation: diff --git a/PathTracking/lqr_steer_control/lqr_steer_control.py b/PathTracking/lqr_steer_control/lqr_steer_control.py index 13e17d34..188cdf08 100644 --- a/PathTracking/lqr_steer_control/lqr_steer_control.py +++ b/PathTracking/lqr_steer_control/lqr_steer_control.py @@ -5,15 +5,18 @@ Path tracking simulation with LQR steering control and PID speed control. author Atsushi Sakai (@Atsushi_twi) """ - +import scipy.linalg as la +import matplotlib.pyplot as plt +import math +import numpy as np import sys sys.path.append("../../PathPlanning/CubicSpline/") -import numpy as np -import math -import matplotlib.pyplot as plt -import scipy.linalg as la -import cubic_spline_planner +try: + import cubic_spline_planner +except: + raise + Kp = 1.0 # speed proportional gain @@ -76,7 +79,6 @@ def solve_DARE(A, B, Q, R): Xn = A.T @ X @ A - A.T @ X @ B @ \ la.inv(R + B.T @ X @ B) @ B.T @ X @ A + Q if (abs(Xn - X)).max() < eps: - X = Xn break X = Xn @@ -206,8 +208,8 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal): plt.plot(cx[target_ind], cy[target_ind], "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]:" + str(round(state.v * 3.6, 2)) + + ",target index:" + str(target_ind)) plt.pause(0.0001) return t, x, y, yaw, v diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index 020df7f4..b407e6f2 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -140,6 +140,7 @@ def main(): assert lastIndex >= target_ind, "Cannot goal" if show_animation: + plt.cla() plt.plot(cx, cy, ".r", label="course") plt.plot(x, y, "-b", label="trajectory") plt.legend() @@ -148,7 +149,7 @@ def main(): plt.axis("equal") plt.grid(True) - flg, ax = plt.subplots(1) + plt.subplots(1) plt.plot(t, [iv * 3.6 for iv in v], "-r") plt.xlabel("Time[s]") plt.ylabel("Speed[km/h]")