From 4880986bb5973fd666734afe2868e59d33f7ff6e Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sat, 2 Feb 2019 16:51:00 +0900 Subject: [PATCH] fix code for coveralls --- .../Quadrotor.py | 6 +- .../rocket_powered_landing.py | 86 +++++++++---------- .../arm_obstacle_navigation.py | 2 +- .../arm_obstacle_navigation_2.py | 2 +- .../n_joint_arm_to_point_control/NLinkArm.py | 2 +- .../n_joint_arm_to_point_control.py | 4 +- .../two_joint_arm_to_point_control.py | 6 +- .../extended_kalman_filter.py | 2 +- .../particle_filter/particle_filter.py | 2 +- .../unscented_kalman_filter.py | 2 +- Mapping/circle_fitting/circle_fitting.py | 2 +- .../batch_informed_rrtstar.py | 14 +-- PathPlanning/BezierPath/bezier_path.py | 11 ++- .../DubinsPath/dubins_path_planning.py | 10 +-- .../dynamic_window_approach.py | 2 +- .../InformedRRTStar/informed_rrt_star.py | 22 ++--- .../model_predictive_trajectory_generator.py | 2 +- .../probabilistic_road_map.py | 2 +- .../quintic_polynomials_planner.py | 10 +-- .../RRTStarDubins/dubins_path_planning.py | 10 +-- .../VoronoiRoadMap/voronoi_road_map.py | 2 +- PathTracking/cgmres_nmpc/cgmres_nmpc.py | 26 +++--- ...odel_predictive_speed_and_steer_control.py | 11 +-- PathTracking/move_to_pose/move_to_pose.py | 6 +- 24 files changed, 122 insertions(+), 122 deletions(-) diff --git a/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py b/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py index fd27c2f9..9b38677c 100644 --- a/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py +++ b/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py @@ -51,12 +51,12 @@ class Quadrotor(): yaw = self.yaw return np.array( [[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll), x], - [sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) * - sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y], + [sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) + * sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y], [-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw), z] ]) - def plot(self): + def plot(self): # pragma: no cover T = self.transformation_matrix() p1_t = np.matmul(T, self.p1) diff --git a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py index a3ee21b5..64143499 100644 --- a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py +++ b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py @@ -129,12 +129,12 @@ class Rocket_Model_6DoF: [vx], [vy], [vz], - [(-1.0 * m - ux * (2 * q2**2 + 2 * q3**2 - 1) - 2 * uy * - (q0 * q3 - q1 * q2) + 2 * uz * (q0 * q2 + q1 * q3)) / m], - [(2 * ux * (q0 * q3 + q1 * q2) - uy * (2 * q1**2 + - 2 * q3**2 - 1) - 2 * uz * (q0 * q1 - q2 * q3)) / m], - [(-2 * ux * (q0 * q2 - q1 * q3) + 2 * uy * - (q0 * q1 + q2 * q3) - uz * (2 * q1**2 + 2 * q2**2 - 1)) / m], + [(-1.0 * m - ux * (2 * q2**2 + 2 * q3**2 - 1) - 2 * uy + * (q0 * q3 - q1 * q2) + 2 * uz * (q0 * q2 + q1 * q3)) / m], + [(2 * ux * (q0 * q3 + q1 * q2) - uy * (2 * q1**2 + + 2 * q3**2 - 1) - 2 * uz * (q0 * q1 - q2 * q3)) / m], + [(-2 * ux * (q0 * q2 - q1 * q3) + 2 * uy + * (q0 * q1 + q2 * q3) - uz * (2 * q1**2 + 2 * q2**2 - 1)) / m], [-0.5 * q1 * wx - 0.5 * q2 * wy - 0.5 * q3 * wz], [0.5 * q0 * wx + 0.5 * q2 * wz - 0.5 * q3 * wy], [0.5 * q0 * wy - 0.5 * q1 * wz + 0.5 * q3 * wx], @@ -154,12 +154,12 @@ class Rocket_Model_6DoF: [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0], - [(ux * (2 * q2**2 + 2 * q3**2 - 1) + 2 * uy * (q0 * q3 - q1 * q2) - 2 * uz * (q0 * q2 + q1 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q2 * uz - - q3 * uy) / m, 2 * (q2 * uy + q3 * uz) / m, 2 * (q0 * uz + q1 * uy - 2 * q2 * ux) / m, 2 * (-q0 * uy + q1 * uz - 2 * q3 * ux) / m, 0, 0, 0], - [(-2 * ux * (q0 * q3 + q1 * q2) + uy * (2 * q1**2 + 2 * q3**2 - 1) + 2 * uz * (q0 * q1 - q2 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (-q1 * uz + - q3 * ux) / m, 2 * (-q0 * uz - 2 * q1 * uy + q2 * ux) / m, 2 * (q1 * ux + q3 * uz) / m, 2 * (q0 * ux + q2 * uz - 2 * q3 * uy) / m, 0, 0, 0], - [(2 * ux * (q0 * q2 - q1 * q3) - 2 * uy * (q0 * q1 + q2 * q3) + uz * (2 * q1**2 + 2 * q2**2 - 1)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q1 * uy - - q2 * ux) / m, 2 * (q0 * uy - 2 * q1 * uz + q3 * ux) / m, 2 * (-q0 * ux - 2 * q2 * uz + q3 * uy) / m, 2 * (q1 * ux + q2 * uy) / m, 0, 0, 0], + [(ux * (2 * q2**2 + 2 * q3**2 - 1) + 2 * uy * (q0 * q3 - q1 * q2) - 2 * uz * (q0 * q2 + q1 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q2 * uz + - q3 * uy) / m, 2 * (q2 * uy + q3 * uz) / m, 2 * (q0 * uz + q1 * uy - 2 * q2 * ux) / m, 2 * (-q0 * uy + q1 * uz - 2 * q3 * ux) / m, 0, 0, 0], + [(-2 * ux * (q0 * q3 + q1 * q2) + uy * (2 * q1**2 + 2 * q3**2 - 1) + 2 * uz * (q0 * q1 - q2 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (-q1 * uz + + q3 * ux) / m, 2 * (-q0 * uz - 2 * q1 * uy + q2 * ux) / m, 2 * (q1 * ux + q3 * uz) / m, 2 * (q0 * ux + q2 * uz - 2 * q3 * uy) / m, 0, 0, 0], + [(2 * ux * (q0 * q2 - q1 * q3) - 2 * uy * (q0 * q1 + q2 * q3) + uz * (2 * q1**2 + 2 * q2**2 - 1)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q1 * uy + - q2 * ux) / m, 2 * (q0 * uy - 2 * q1 * uz + q3 * ux) / m, 2 * (-q0 * ux - 2 * q2 * uz + q3 * uy) / m, 2 * (q1 * ux + q2 * uy) / m, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, -0.5 * wx, -0.5 * wy, - 0.5 * wz, -0.5 * q1, -0.5 * q2, -0.5 * q3], [0, 0, 0, 0, 0, 0, 0, 0.5 * wx, 0, 0.5 * wz, @@ -184,12 +184,12 @@ class Rocket_Model_6DoF: [0, 0, 0], [0, 0, 0], [0, 0, 0], - [(-2 * q2**2 - 2 * q3**2 + 1) / m, 2 * - (-q0 * q3 + q1 * q2) / m, 2 * (q0 * q2 + q1 * q3) / m], - [2 * (q0 * q3 + q1 * q2) / m, (-2 * q1**2 - 2 * - q3**2 + 1) / m, 2 * (-q0 * q1 + q2 * q3) / m], - [2 * (-q0 * q2 + q1 * q3) / m, 2 * (q0 * q1 + q2 * q3) / - m, (-2 * q1**2 - 2 * q2**2 + 1) / m], + [(-2 * q2**2 - 2 * q3**2 + 1) / m, 2 + * (-q0 * q3 + q1 * q2) / m, 2 * (q0 * q2 + q1 * q3) / m], + [2 * (q0 * q3 + q1 * q2) / m, (-2 * q1**2 - 2 + * q3**2 + 1) / m, 2 * (-q0 * q1 + q2 * q3) / m], + [2 * (-q0 * q2 + q1 * q3) / m, 2 * (q0 * q1 + q2 * q3) + / m, (-2 * q1**2 - 2 * q2**2 + 1) / m], [0, 0, 0], [0, 0, 0], [0, 0, 0], @@ -227,12 +227,12 @@ class Rocket_Model_6DoF: def dir_cosine(self, q): return np.matrix([ - [1 - 2 * (q[2] ** 2 + q[3] ** 2), 2 * (q[1] * q[2] + - q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])], - [2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2 * - (q[1] ** 2 + q[3] ** 2), 2 * (q[2] * q[3] + q[0] * q[1])], - [2 * (q[1] * q[3] + q[0] * q[2]), 2 * (q[2] * q[3] - - q[0] * q[1]), 1 - 2 * (q[1] ** 2 + q[2] ** 2)] + [1 - 2 * (q[2] ** 2 + q[3] ** 2), 2 * (q[1] * q[2] + + q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])], + [2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2 + * (q[1] ** 2 + q[3] ** 2), 2 * (q[2] * q[3] + q[0] * q[1])], + [2 * (q[1] * q[3] + q[0] * q[2]), 2 * (q[2] * q[3] + - q[0] * q[1]), 1 - 2 * (q[1] ** 2 + q[2] ** 2)] ]) def omega(self, w): @@ -459,16 +459,16 @@ class SCProblem: # Dynamics: # x_t+1 = A_*x_t+B_*U_t+C_*U_T+1*S_*sigma+zbar+nu constraints += [ - self.var['X'][:, k + 1] - == cvxpy.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x)) - * self.var['X'][:, k] - + cvxpy.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u)) - * self.var['U'][:, k] - + cvxpy.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u)) - * self.var['U'][:, k + 1] - + self.par['S_bar'][:, k] * self.var['sigma'] - + self.par['z_bar'][:, k] - + self.var['nu'][:, k] + self.var['X'][:, k + 1] == + cvxpy.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x)) * + self.var['X'][:, k] + + cvxpy.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u)) * + self.var['U'][:, k] + + cvxpy.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u)) * + self.var['U'][:, k + 1] + + self.par['S_bar'][:, k] * self.var['sigma'] + + self.par['z_bar'][:, k] + + self.var['nu'][:, k] for k in range(K - 1) ] @@ -486,10 +486,10 @@ class SCProblem: # Objective: sc_objective = cvxpy.Minimize( - self.par['weight_sigma'] * self.var['sigma'] - + self.par['weight_nu'] * cvxpy.norm(self.var['nu'], 'inf') - + self.par['weight_delta'] * self.var['delta_norm'] - + self.par['weight_delta_sigma'] * self.var['sigma_norm'] + self.par['weight_sigma'] * self.var['sigma'] + + self.par['weight_nu'] * cvxpy.norm(self.var['nu'], 'inf') + + self.par['weight_delta'] * self.var['delta_norm'] + + self.par['weight_delta_sigma'] * self.var['sigma_norm'] ) objective = sc_objective @@ -550,8 +550,8 @@ class SCProblem: def axis3d_equal(X, Y, Z, ax): - max_range = np.array([X.max() - X.min(), Y.max() - - Y.min(), Z.max() - Z.min()]).max() + max_range = np.array([X.max() - X.min(), Y.max() + - Y.min(), Z.max() - Z.min()]).max() Xb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, - 1:2:2][0].flatten() + 0.5 * (X.max() + X.min()) Yb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, @@ -563,7 +563,7 @@ def axis3d_equal(X, Y, Z, ax): ax.plot([xb], [yb], [zb], 'w') -def plot_animation(X, U): +def plot_animation(X, U): # pragma: no cover fig = plt.figure() ax = fig.gca(projection='3d') @@ -582,8 +582,8 @@ def plot_animation(X, U): CBI = np.array([ [1 - 2 * (qy ** 2 + qz ** 2), 2 * (qx * qy + qw * qz), 2 * (qx * qz - qw * qy)], - [2 * (qx * qy - qw * qz), 1 - 2 * - (qx ** 2 + qz ** 2), 2 * (qy * qz + qw * qx)], + [2 * (qx * qy - qw * qz), 1 - 2 + * (qx ** 2 + qz ** 2), 2 * (qy * qz + qw * qx)], [2 * (qx * qz + qw * qy), 2 * (qy * qz - qw * qx), 1 - 2 * (qx ** 2 + qy ** 2)] ]) diff --git a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py index 9f4edf68..2bcc33d0 100644 --- a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py +++ b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py @@ -241,7 +241,7 @@ class NLinkArm(object): self.end_effector = np.array(self.points[self.n_links]).T - def plot(self, obstacles=[]): + def plot(self, obstacles=[]): # pragma: no cover plt.cla() for obstacle in obstacles: diff --git a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py index a93260c3..b2645155 100644 --- a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py +++ b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py @@ -272,7 +272,7 @@ class NLinkArm(object): self.end_effector = np.array(self.points[self.n_links]).T - def plot(self, myplt, obstacles=[]): + def plot(self, myplt, obstacles=[]): # pragma: no cover myplt.cla() for obstacle in obstacles: diff --git a/ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py b/ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py index 36a7d429..e8f558db 100644 --- a/ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py +++ b/ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py @@ -49,7 +49,7 @@ class NLinkArm(object): if self.show_animation: self.plot() - def plot(self): + def plot(self): # pragma: no cover plt.cla() for i in range(self.n_links + 1): diff --git a/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py b/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py index 3491b977..a04c64cc 100644 --- a/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py +++ b/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py @@ -159,5 +159,5 @@ def ang_diff(theta1, theta2): if __name__ == '__main__': - main() - # animation() \ No newline at end of file + # main() + animation() \ No newline at end of file diff --git a/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py b/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py index 1468fc6d..23ade401 100644 --- a/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py +++ b/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py @@ -39,8 +39,8 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0): try: theta2_goal = np.arccos( (x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2)) - theta1_goal = np.math.atan2(y, x) - np.math.atan2(l2 * - np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal))) + theta1_goal = np.math.atan2(y, x) - np.math.atan2(l2 + * np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal))) if theta1_goal < 0: theta2_goal = -theta2_goal @@ -61,7 +61,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0): return theta1, theta2 -def plot_arm(theta1, theta2, x, y): +def plot_arm(theta1, theta2, x, y): # pragma: no cover shoulder = np.array([0, 0]) elbow = shoulder + np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)]) wrist = elbow + \ diff --git a/Localization/extended_kalman_filter/extended_kalman_filter.py b/Localization/extended_kalman_filter/extended_kalman_filter.py index fd288d57..b2ce6235 100644 --- a/Localization/extended_kalman_filter/extended_kalman_filter.py +++ b/Localization/extended_kalman_filter/extended_kalman_filter.py @@ -134,7 +134,7 @@ def ekf_estimation(xEst, PEst, z, u): return xEst, PEst -def plot_covariance_ellipse(xEst, PEst): +def plot_covariance_ellipse(xEst, PEst): # pragma: no cover Pxy = PEst[0:2, 0:2] eigval, eigvec = np.linalg.eig(Pxy) diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py index 7e92f3fe..8e93893e 100644 --- a/Localization/particle_filter/particle_filter.py +++ b/Localization/particle_filter/particle_filter.py @@ -156,7 +156,7 @@ def resampling(px, pw): return px, pw -def plot_covariance_ellipse(xEst, PEst): +def plot_covariance_ellipse(xEst, PEst): # pragma: no cover Pxy = PEst[0:2, 0:2] eigval, eigvec = np.linalg.eig(Pxy) diff --git a/Localization/unscented_kalman_filter/unscented_kalman_filter.py b/Localization/unscented_kalman_filter/unscented_kalman_filter.py index 1bd924f9..cb59a57e 100644 --- a/Localization/unscented_kalman_filter/unscented_kalman_filter.py +++ b/Localization/unscented_kalman_filter/unscented_kalman_filter.py @@ -161,7 +161,7 @@ def ukf_estimation(xEst, PEst, z, u, wm, wc, gamma): return xEst, PEst -def plot_covariance_ellipse(xEst, PEst): +def plot_covariance_ellipse(xEst, PEst): # pragma: no cover Pxy = PEst[0:2, 0:2] eigval, eigvec = np.linalg.eig(Pxy) diff --git a/Mapping/circle_fitting/circle_fitting.py b/Mapping/circle_fitting/circle_fitting.py index 39ed3e73..b3de66e4 100644 --- a/Mapping/circle_fitting/circle_fitting.py +++ b/Mapping/circle_fitting/circle_fitting.py @@ -90,7 +90,7 @@ def ray_casting_filter(xl, yl, thetal, rangel, angle_reso): return rx, ry -def plot_circle(x, y, size, color="-b"): +def plot_circle(x, y, size, color="-b"): # pragma: no cover deg = list(range(0, 360, 5)) deg.append(0) xl = [x + size * math.cos(np.deg2rad(d)) for d in deg] diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index 5f45a8fe..f8246738 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -172,8 +172,8 @@ class BITStar(object): cBest = self.g_scores[self.goalId] # Computing the sampling space - cMin = math.sqrt(pow(self.start[0] - self.goal[1], 2) + - pow(self.start[0] - self.goal[1], 2)) / 1.5 + cMin = math.sqrt(pow(self.start[0] - self.goal[1], 2) + + pow(self.start[0] - self.goal[1], 2)) / 1.5 xCenter = np.array([[(self.start[0] + self.goal[0]) / 2.0], [(self.goal[1] - self.start[1]) / 2.0], [0]]) a1 = np.array([[(self.goal[0] - self.start[0]) / cMin], @@ -413,8 +413,8 @@ class BITStar(object): def bestVertexQueueValue(self): if(len(self.vertex_queue) == 0): return float('inf') - values = [self.g_scores[v] + - self.computeHeuristicCost(v, self.goalId) for v in self.vertex_queue] + values = [self.g_scores[v] + + self.computeHeuristicCost(v, self.goalId) for v in self.vertex_queue] values.sort() return values[0] @@ -422,8 +422,8 @@ class BITStar(object): if(len(self.edge_queue) == 0): return float('inf') # return the best value in the queue by score g_tau[v] + c(v,x) + h(x) - values = [self.g_scores[e[0]] + self.computeDistanceCost(e[0], e[1]) + - self.computeHeuristicCost(e[1], self.goalId) for e in self.edge_queue] + values = [self.g_scores[e[0]] + self.computeDistanceCost(e[0], e[1]) + + self.computeHeuristicCost(e[1], self.goalId) for e in self.edge_queue] values.sort(reverse=True) return values[0] @@ -549,7 +549,7 @@ class BITStar(object): plt.grid(True) plt.pause(0.01) - def plot_ellipse(self, xCenter, cBest, cMin, etheta): + def plot_ellipse(self, xCenter, cBest, cMin, etheta): # pragma: no cover a = math.sqrt(cBest**2 - cMin**2) / 2.0 b = cBest / 2.0 diff --git a/PathPlanning/BezierPath/bezier_path.py b/PathPlanning/BezierPath/bezier_path.py index 5188ad81..ee4bd547 100644 --- a/PathPlanning/BezierPath/bezier_path.py +++ b/PathPlanning/BezierPath/bezier_path.py @@ -94,7 +94,8 @@ def bezier_derivatives_control_points(control_points, n_derivatives): w = {0: control_points} for i in range(n_derivatives): n = len(w[i]) - w[i + 1] = np.array([(n - 1) * (w[i][j + 1] - w[i][j]) for j in range(n - 1)]) + w[i + 1] = np.array([(n - 1) * (w[i][j + 1] - w[i][j]) + for j in range(n - 1)]) return w @@ -111,7 +112,7 @@ def curvature(dx, dy, ddx, ddy): return (dx * ddy - dy * ddx) / (dx ** 2 + dy ** 2) ** (3 / 2) -def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): +def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover """Plot arrow.""" if not isinstance(x, float): for (ix, iy, iyaw) in zip(x, y, yaw): @@ -155,7 +156,8 @@ def main(): tangent = np.array([point, point + dt]) normal = np.array([point, point + [- dt[1], dt[0]]]) curvature_center = point + np.array([- dt[1], dt[0]]) * radius - circle = plt.Circle(tuple(curvature_center), radius, color=(0, 0.8, 0.8), fill=False, linewidth=1) + circle = plt.Circle(tuple(curvature_center), radius, + color=(0, 0.8, 0.8), fill=False, linewidth=1) assert path.T[0][0] == start_x, "path is invalid" assert path.T[1][0] == start_y, "path is invalid" @@ -165,7 +167,8 @@ def main(): if show_animation: fig, ax = plt.subplots() ax.plot(path.T[0], path.T[1], label="Bezier Path") - ax.plot(control_points.T[0], control_points.T[1], '--o', label="Control Points") + ax.plot(control_points.T[0], control_points.T[1], + '--o', label="Control Points") ax.plot(x_target, y_target) ax.plot(tangent[:, 0], tangent[:, 1], label="Tangent") ax.plot(normal[:, 0], normal[:, 1], label="Normal") diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index f8b94817..dc7485ad 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -201,10 +201,10 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c): lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin( lex, ley, leyaw, c) - px = [math.cos(-syaw) * x + math.sin(-syaw) * - y + sx for x, y in zip(lpx, lpy)] - py = [- math.sin(-syaw) * x + math.cos(-syaw) * - y + sy for x, y in zip(lpx, lpy)] + px = [math.cos(-syaw) * x + math.sin(-syaw) + * y + sx for x, y in zip(lpx, lpy)] + py = [- math.sin(-syaw) * x + math.cos(-syaw) + * y + sy for x, y in zip(lpx, lpy)] pyaw = [pi_2_pi(iyaw + syaw) for iyaw in lpyaw] return px, py, pyaw, mode, clen @@ -251,7 +251,7 @@ def generate_course(length, mode, c): return px, py, pyaw -def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): +def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover """ Plot arrow """ diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index 489197e0..40db9a96 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -159,7 +159,7 @@ def dwa_control(x, u, config, goal, ob): return u, traj -def plot_arrow(x, y, yaw, length=0.5, width=0.1): +def plot_arrow(x, y, yaw, length=0.5, width=0.1): # pragma: no cover plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), head_length=width, head_width=width) plt.plot(x, y) diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index bead4eec..2a999fd0 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -44,8 +44,8 @@ class InformedRRTStar(): path = None # Computing the sampling space - cMin = math.sqrt(pow(self.start.x - self.goal.x, 2) + - pow(self.start.y - self.goal.y, 2)) + cMin = math.sqrt(pow(self.start.x - self.goal.x, 2) + + pow(self.start.y - self.goal.y, 2)) xCenter = np.array([[(self.start.x + self.goal.x) / 2.0], [(self.start.y + self.goal.y) / 2.0], [0]]) a1 = np.array([[(self.goal.x - self.start.x) / cMin], @@ -129,8 +129,8 @@ class InformedRRTStar(): def findNearNodes(self, newNode): nnode = len(self.nodeList) r = 50.0 * math.sqrt((math.log(nnode) / nnode)) - dlist = [(node.x - newNode.x) ** 2 + - (node.y - newNode.y) ** 2 for node in self.nodeList] + dlist = [(node.x - newNode.x) ** 2 + + (node.y - newNode.y) ** 2 for node in self.nodeList] nearinds = [dlist.index(i) for i in dlist if i <= r ** 2] return nearinds @@ -175,8 +175,8 @@ class InformedRRTStar(): node1_y = path[i][1] node2_x = path[i - 1][0] node2_y = path[i - 1][1] - pathLen += math.sqrt((node1_x - node2_x) ** - 2 + (node1_y - node2_y)**2) + pathLen += math.sqrt((node1_x - node2_x) + ** 2 + (node1_y - node2_y)**2) return pathLen @@ -184,8 +184,8 @@ class InformedRRTStar(): return math.sqrt((node1.x - node2.x)**2 + (node1.y - node2.y)**2) def getNearestListIndex(self, nodes, rnd): - dList = [(node.x - rnd[0])**2 + - (node.y - rnd[1])**2 for node in nodes] + dList = [(node.x - rnd[0])**2 + + (node.y - rnd[1])**2 for node in nodes] minIndex = dList.index(min(dList)) return minIndex @@ -220,8 +220,8 @@ class InformedRRTStar(): for i in nearInds: nearNode = self.nodeList[i] - d = math.sqrt((nearNode.x - newNode.x)**2 + - (nearNode.y - newNode.y)**2) + d = math.sqrt((nearNode.x - newNode.x)**2 + + (nearNode.y - newNode.y)**2) scost = newNode.cost + d @@ -275,7 +275,7 @@ class InformedRRTStar(): plt.grid(True) plt.pause(0.01) - def plot_ellipse(self, xCenter, cBest, cMin, etheta): + def plot_ellipse(self, xCenter, cBest, cMin, etheta): # pragma: no cover a = math.sqrt(cBest**2 - cMin**2) / 2.0 b = cBest / 2.0 diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py index e2e2d40f..5dad3c0b 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py @@ -19,7 +19,7 @@ cost_th = 0.1 show_animation = False -def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): +def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover """ Plot arrow """ diff --git a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py index d4e21467..d42592a0 100644 --- a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py +++ b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py @@ -230,7 +230,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y): return rx, ry -def plot_road_map(road_map, sample_x, sample_y): +def plot_road_map(road_map, sample_x, sample_y): # pragma: no cover for i, _ in enumerate(road_map): for ii in range(len(road_map[i])): diff --git a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py index 383b0e92..b0a42115 100644 --- a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py +++ b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py @@ -153,17 +153,17 @@ def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_a 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) return time, rx, ry, ryaw, rv, ra, rj -def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): +def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover """ Plot arrow """ diff --git a/PathPlanning/RRTStarDubins/dubins_path_planning.py b/PathPlanning/RRTStarDubins/dubins_path_planning.py index a6bf295f..b2946520 100644 --- a/PathPlanning/RRTStarDubins/dubins_path_planning.py +++ b/PathPlanning/RRTStarDubins/dubins_path_planning.py @@ -200,10 +200,10 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c): lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin( lex, ley, leyaw, c) - px = [math.cos(-syaw) * x + math.sin(-syaw) * - y + sx for x, y in zip(lpx, lpy)] - py = [- math.sin(-syaw) * x + math.cos(-syaw) * - y + sy for x, y in zip(lpx, lpy)] + px = [math.cos(-syaw) * x + math.sin(-syaw) + * y + sx for x, y in zip(lpx, lpy)] + py = [- math.sin(-syaw) * x + math.cos(-syaw) + * y + sy for x, y in zip(lpx, lpy)] pyaw = [pi_2_pi(iyaw + syaw) for iyaw in lpyaw] # print(syaw) # pyaw = lpyaw @@ -258,7 +258,7 @@ def generate_course(length, mode, c): return px, py, pyaw -def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): +def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover """ Plot arrow """ diff --git a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py index 0327b723..1e18d21c 100644 --- a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py +++ b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py @@ -229,7 +229,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y): return rx, ry -def plot_road_map(road_map, sample_x, sample_y): +def plot_road_map(road_map, sample_x, sample_y): # pragma: no cover for i, _ in enumerate(road_map): for ii in range(len(road_map[i])): diff --git a/PathTracking/cgmres_nmpc/cgmres_nmpc.py b/PathTracking/cgmres_nmpc/cgmres_nmpc.py index 279db72d..164c6895 100644 --- a/PathTracking/cgmres_nmpc/cgmres_nmpc.py +++ b/PathTracking/cgmres_nmpc/cgmres_nmpc.py @@ -126,8 +126,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 @@ -359,8 +359,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]) @@ -371,7 +371,7 @@ class NMPCController_with_CGMRES(): return np.array(F) -def plot_figures(plant_system, controller, iteration_num, dt): +def plot_figures(plant_system, controller, iteration_num, dt): # pragma: no cover # figure # time history fig_p = plt.figure() @@ -421,13 +421,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") @@ -462,7 +462,7 @@ def plot_figures(plant_system, controller, iteration_num, dt): plt.show() -def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"): +def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"): # pragma: no cover # Vehicle parameters LENGTH = 0.4 # [m] @@ -550,9 +550,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/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py index ef6577a5..ec3e8e65 100644 --- a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py +++ b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py @@ -105,9 +105,6 @@ def get_linear_model_matrix(v, phi, delta): return A, B, C - -def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"): - outline = np.array([[-BACKTOWHEEL, (LENGTH - BACKTOWHEEL), (LENGTH - BACKTOWHEEL), -BACKTOWHEEL, -BACKTOWHEEL], [WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]]) @@ -276,8 +273,8 @@ def linear_mpc_control(xref, xbar, x0, dref): if t < (T - 1): cost += cvxpy.quad_form(u[:, t + 1] - u[:, t], Rd) - constraints += [cvxpy.abs(u[1, t + 1] - u[1, t]) - <= MAX_DSTEER * DT] + constraints += [cvxpy.abs(u[1, t + 1] - u[1, t]) <= + MAX_DSTEER * DT] cost += cvxpy.quad_form(xref[:, T] - x[:, T], Qf) @@ -438,8 +435,8 @@ def do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state): plot_car(state.x, state.y, state.yaw, steer=di) plt.axis("equal") plt.grid(True) - plt.title("Time[s]:" + str(round(time, 2)) + - ", speed[km/h]:" + str(round(state.v * 3.6, 2))) + plt.title("Time[s]:" + str(round(time, 2)) + + ", speed[km/h]:" + str(round(state.v * 3.6, 2))) plt.pause(0.0001) return t, x, y, yaw, v, d, a diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py index 2fe18817..c03ef21b 100644 --- a/PathTracking/move_to_pose/move_to_pose.py +++ b/PathTracking/move_to_pose/move_to_pose.py @@ -53,8 +53,8 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal): # 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) - - theta + np.pi) % (2 * np.pi) - np.pi + alpha = (np.arctan2(y_diff, x_diff) + - theta + np.pi) % (2 * np.pi) - np.pi beta = (theta_goal - theta - alpha + np.pi) % (2 * np.pi) - np.pi v = Kp_rho * rho @@ -76,7 +76,7 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal): plot_vehicle(x, y, theta, x_traj, y_traj) -def plot_vehicle(x, y, theta, x_traj, y_traj): +def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover # Corners of triangular vehicle when pointing to the right (0 radians) p1_i = np.array([0.5, 0, 1]).T p2_i = np.array([-0.5, 0.25, 1]).T