From 4d714706312735ce46a118bb6782c2aef0ba333f Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sat, 1 Jul 2023 15:30:32 +0900 Subject: [PATCH] upgrade numpy to 1.25.0 and fix bugs (#861) --- .../drone_3d_trajectory_following.py | 13 ++++--- .../two_joint_arm_to_point_control.py | 11 +++--- .../inverted_pendulum_lqr_control.py | 6 +-- .../inverted_pendulum_mpc_control.py | 6 +-- .../histogram_filter/histogram_filter.py | 5 ++- .../unscented_kalman_filter.py | 4 +- Mapping/circle_fitting/circle_fitting.py | 6 +-- .../batch_informed_rrtstar.py | 6 +-- .../InformedRRTStar/informed_rrt_star.py | 4 +- .../lookup_table_generator.py | 2 +- .../motion_model.py | 39 +++++++++++-------- .../trajectory_generator.py | 6 +-- .../state_lattice_planner.py | 2 +- SLAM/FastSLAM1/fast_slam1.py | 2 +- SLAM/FastSLAM2/fast_slam2.py | 2 +- SLAM/GraphBasedSLAM/graph_based_slam.py | 6 +-- requirements/requirements.txt | 2 +- tests/test_codestyle.py | 2 +- 18 files changed, 66 insertions(+), 58 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 e00b3fff..029e82be 100644 --- a/AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py +++ b/AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py @@ -8,7 +8,6 @@ from math import cos, sin import numpy as np from Quadrotor import Quadrotor from TrajectoryGenerator import TrajectoryGenerator -from mpl_toolkits.mplot3d import Axes3D show_animation = True @@ -128,7 +127,7 @@ def calculate_position(c, t): Calculates a position given a set of quintic coefficients and a time. Args - c: List of coefficients generated by a quintic polynomial + c: List of coefficients generated by a quintic polynomial trajectory generator. t: Time at which to calculate the position @@ -143,7 +142,7 @@ def calculate_velocity(c, t): Calculates a velocity given a set of quintic coefficients and a time. Args - c: List of coefficients generated by a quintic polynomial + c: List of coefficients generated by a quintic polynomial trajectory generator. t: Time at which to calculate the velocity @@ -158,7 +157,7 @@ def calculate_acceleration(c, t): Calculates an acceleration given a set of quintic coefficients and a time. Args - c: List of coefficients generated by a quintic polynomial + c: List of coefficients generated by a quintic polynomial trajectory generator. t: Time at which to calculate the acceleration @@ -168,7 +167,7 @@ def calculate_acceleration(c, t): return 20 * c[0] * t**3 + 12 * c[1] * t**2 + 6 * c[2] * t + 2 * c[3] -def rotation_matrix(roll, pitch, yaw): +def rotation_matrix(roll_array, pitch_array, yaw): """ Calculates the ZYX rotation matrix. @@ -180,6 +179,8 @@ def rotation_matrix(roll, pitch, yaw): Returns 3x3 rotation matrix as NumPy array """ + roll = roll_array[0] + pitch = pitch_array[0] 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)], [sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) * @@ -190,7 +191,7 @@ def rotation_matrix(roll, pitch, yaw): def main(): """ - Calculates the x, y, z coefficients for the four segments + Calculates the x, y, z coefficients for the four segments of the trajectory """ x_coeffs = [[], [], [], []] 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 012a7f72..66d0ebeb 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 @@ -14,9 +14,10 @@ Ref: P. I. Corke, "Robotics, Vision & Control", Springer 2017, import matplotlib.pyplot as plt import numpy as np +import math -# Similation parameters +# Simulation parameters Kp = 15 dt = 0.01 @@ -50,15 +51,15 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0): else: theta2_goal = np.arccos( (x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2)) - tmp = np.math.atan2(l2 * np.sin(theta2_goal), + tmp = math.atan2(l2 * np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal))) - theta1_goal = np.math.atan2(y, x) - tmp + theta1_goal = math.atan2(y, x) - tmp if theta1_goal < 0: theta2_goal = -theta2_goal - tmp = np.math.atan2(l2 * np.sin(theta2_goal), + tmp = math.atan2(l2 * np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal))) - theta1_goal = np.math.atan2(y, x) - tmp + theta1_goal = math.atan2(y, x) - tmp theta1 = theta1 + Kp * ang_diff(theta1_goal, theta1) * dt theta2 = theta2 + Kp * ang_diff(theta2_goal, theta2) * dt diff --git a/Control/inverted_pendulum/inverted_pendulum_lqr_control.py b/Control/inverted_pendulum/inverted_pendulum_lqr_control.py index fb68c91c..c4380530 100644 --- a/Control/inverted_pendulum/inverted_pendulum_lqr_control.py +++ b/Control/inverted_pendulum/inverted_pendulum_lqr_control.py @@ -50,14 +50,14 @@ def main(): if show_animation: plt.clf() - px = float(x[0]) - theta = float(x[2]) + px = float(x[0, 0]) + theta = float(x[2, 0]) plot_cart(px, theta) plt.xlim([-5.0, 2.0]) plt.pause(0.001) print("Finish") - print(f"x={float(x[0]):.2f} [m] , theta={math.degrees(x[2]):.2f} [deg]") + print(f"x={float(x[0, 0]):.2f} [m] , theta={math.degrees(x[2, 0]):.2f} [deg]") if show_animation: plt.show() diff --git a/Control/inverted_pendulum/inverted_pendulum_mpc_control.py b/Control/inverted_pendulum/inverted_pendulum_mpc_control.py index 6b966b13..9a5fa2ab 100644 --- a/Control/inverted_pendulum/inverted_pendulum_mpc_control.py +++ b/Control/inverted_pendulum/inverted_pendulum_mpc_control.py @@ -55,14 +55,14 @@ def main(): if show_animation: plt.clf() - px = float(x[0]) - theta = float(x[2]) + px = float(x[0, 0]) + theta = float(x[2, 0]) plot_cart(px, theta) plt.xlim([-5.0, 2.0]) plt.pause(0.001) print("Finish") - print(f"x={float(x[0]):.2f} [m] , theta={math.degrees(x[2]):.2f} [deg]") + print(f"x={float(x[0, 0]):.2f} [m] , theta={math.degrees(x[2, 0]):.2f} [deg]") if show_animation: plt.show() diff --git a/Localization/histogram_filter/histogram_filter.py b/Localization/histogram_filter/histogram_filter.py index a4aeb419..17cfc2e1 100644 --- a/Localization/histogram_filter/histogram_filter.py +++ b/Localization/histogram_filter/histogram_filter.py @@ -15,6 +15,7 @@ import copy import math import matplotlib.pyplot as plt +import matplotlib as mpl import numpy as np from scipy.ndimage import gaussian_filter from scipy.stats import norm @@ -114,7 +115,7 @@ def motion_model(x, u): def draw_heat_map(data, mx, my): max_value = max([max(i_data) for i_data in data]) plt.grid(False) - plt.pcolor(mx, my, data, vmax=max_value, cmap=plt.cm.get_cmap("Blues")) + plt.pcolor(mx, my, data, vmax=max_value, cmap=mpl.colormaps["Blues"]) plt.axis("equal") @@ -194,7 +195,7 @@ def motion_update(grid_map, u, yaw): y_shift = grid_map.dy // grid_map.xy_resolution if abs(x_shift) >= 1.0 or abs(y_shift) >= 1.0: # map should be shifted - grid_map = map_shift(grid_map, int(x_shift), int(y_shift)) + grid_map = map_shift(grid_map, int(x_shift[0]), int(y_shift[0])) grid_map.dx -= x_shift * grid_map.xy_resolution grid_map.dy -= y_shift * grid_map.xy_resolution diff --git a/Localization/unscented_kalman_filter/unscented_kalman_filter.py b/Localization/unscented_kalman_filter/unscented_kalman_filter.py index dbcdeef0..4af748ec 100644 --- a/Localization/unscented_kalman_filter/unscented_kalman_filter.py +++ b/Localization/unscented_kalman_filter/unscented_kalman_filter.py @@ -69,8 +69,8 @@ def motion_model(x, u): [0, 0, 1.0, 0], [0, 0, 0, 0]]) - B = np.array([[DT * math.cos(x[2]), 0], - [DT * math.sin(x[2]), 0], + B = np.array([[DT * math.cos(x[2, 0]), 0], + [DT * math.sin(x[2, 0]), 0], [0.0, DT], [1.0, 0.0]]) diff --git a/Mapping/circle_fitting/circle_fitting.py b/Mapping/circle_fitting/circle_fitting.py index c331d567..2eba5501 100644 --- a/Mapping/circle_fitting/circle_fitting.py +++ b/Mapping/circle_fitting/circle_fitting.py @@ -40,9 +40,9 @@ def circle_fitting(x, y): T = np.linalg.inv(F).dot(G) - cxe = float(T[0] / -2) - cye = float(T[1] / -2) - re = math.sqrt(cxe**2 + cye**2 - T[2]) + cxe = float(T[0, 0] / -2) + cye = float(T[1, 0] / -2) + re = math.sqrt(cxe**2 + cye**2 - T[2, 0]) error = sum([np.hypot(cxe - ix, cye - iy) - re for (ix, iy) in zip(x, y)]) diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index b5eabbb3..92c3a587 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -23,7 +23,7 @@ import numpy as np show_animation = True -class RTree(object): +class RTree: # Class to represent the explicit tree created # while sampling through the state space @@ -132,7 +132,7 @@ class RTree(object): self.node_id_to_grid_coord(nid)) -class BITStar(object): +class BITStar: def __init__(self, start, goal, obstacleList, randArea, eta=2.0, @@ -189,7 +189,7 @@ class BITStar(object): [(self.start[1] + self.goal[1]) / 2.0], [0]]) a1 = np.array([[(self.goal[0] - self.start[0]) / cMin], [(self.goal[1] - self.start[1]) / cMin], [0]]) - eTheta = math.atan2(a1[1], a1[0]) + eTheta = math.atan2(a1[1, 0], a1[0, 0]) # first column of identity matrix transposed id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3) M = np.dot(a1, id1_t) diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index 4f0ec319..b6f9d234 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -58,7 +58,7 @@ class InformedRRTStar: a1 = np.array([[(self.goal.x - self.start.x) / c_min], [(self.goal.y - self.start.y) / c_min], [0]]) - e_theta = math.atan2(a1[1], a1[0]) + e_theta = math.atan2(a1[1, 0], a1[0, 0]) # first column of identity matrix transposed id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3) m = a1 @ id1_t @@ -136,7 +136,7 @@ class InformedRRTStar: def find_near_nodes(self, new_node): n_node = len(self.node_list) - r = 50.0 * math.sqrt((math.log(n_node) / n_node)) + r = 50.0 * math.sqrt(math.log(n_node) / n_node) d_list = [(node.x - new_node.x) ** 2 + (node.y - new_node.y) ** 2 for node in self.node_list] near_inds = [d_list.index(i) for i in d_list if i <= r ** 2] diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py index 6cb226ad..4c3b32f2 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py @@ -81,7 +81,7 @@ def generate_lookup_table(): if x is not None: print("find good path") lookup_table.append( - [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])]) + [x[-1], y[-1], yaw[-1], float(p[0, 0]), float(p[1, 0]), float(p[2, 0])]) print("finish lookup table generation") diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py index f6c78d3b..531bf91e 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py @@ -1,10 +1,10 @@ import math import numpy as np -import scipy.interpolate +from scipy.interpolate import interp1d # motion parameter L = 1.0 # wheel base -ds = 0.1 # course distanse +ds = 0.1 # course distance v = 10.0 / 3.6 # velocity [m/s] @@ -22,7 +22,6 @@ def pi_2_pi(angle): def update(state, v, delta, dt, L): - state.v = v state.x = state.x + state.v * math.cos(state.yaw) * dt state.y = state.y + state.v * math.sin(state.yaw) * dt @@ -33,18 +32,20 @@ def update(state, v, delta, dt, L): def generate_trajectory(s, km, kf, k0): - n = s / ds time = s / v # [s] - - if isinstance(time, type(np.array([]))): time = time[0] - if isinstance(km, type(np.array([]))): km = km[0] - if isinstance(kf, type(np.array([]))): kf = kf[0] - + + if isinstance(time, type(np.array([]))): + time = time[0] + if isinstance(km, type(np.array([]))): + km = km[0] + if isinstance(kf, type(np.array([]))): + kf = kf[0] + tk = np.array([0.0, time / 2.0, time]) kk = np.array([k0, km, kf]) t = np.arange(0.0, time, time / n) - fkp = scipy.interpolate.interp1d(tk, kk, kind="quadratic") + fkp = interp1d(tk, kk, kind="quadratic") kp = [fkp(ti) for ti in t] dt = float(time / n) @@ -64,18 +65,22 @@ def generate_trajectory(s, km, kf, k0): def generate_last_state(s, km, kf, k0): - n = s / ds time = s / v # [s] - - if isinstance(time, type(np.array([]))): time = time[0] - if isinstance(km, type(np.array([]))): km = km[0] - if isinstance(kf, type(np.array([]))): kf = kf[0] - + + if isinstance(n, type(np.array([]))): + n = n[0] + if isinstance(time, type(np.array([]))): + time = time[0] + if isinstance(km, type(np.array([]))): + km = km[0] + if isinstance(kf, type(np.array([]))): + kf = kf[0] + tk = np.array([0.0, time / 2.0, time]) kk = np.array([k0, km, kf]) t = np.arange(0.0, time, time / n) - fkp = scipy.interpolate.interp1d(tk, kk, kind="quadratic") + fkp = interp1d(tk, kk, kind="quadratic") kp = [fkp(ti) for ti in t] dt = time / n diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py index a44e1ac9..97c0ad8b 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py @@ -106,7 +106,7 @@ def show_trajectory(target, xc, yc): # pragma: no cover def optimize_trajectory(target, k0, p): for i in range(max_iter): - xc, yc, yawc = motion_model.generate_trajectory(p[0], p[1], p[2], k0) + xc, yc, yawc = motion_model.generate_trajectory(p[0, 0], p[1, 0], p[2, 0], k0) dc = np.array(calc_diff(target, xc, yc, yawc)).reshape(3, 1) cost = np.linalg.norm(dc) @@ -135,7 +135,7 @@ def optimize_trajectory(target, k0, p): return xc, yc, yawc, p -def test_optimize_trajectory(): # pragma: no cover +def optimize_trajectory_demo(): # pragma: no cover # target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(00.0)) target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(90.0)) @@ -155,7 +155,7 @@ def test_optimize_trajectory(): # pragma: no cover def main(): # pragma: no cover print(__file__ + " start!!") - test_optimize_trajectory() + optimize_trajectory_demo() if __name__ == '__main__': diff --git a/PathPlanning/StateLatticePlanner/state_lattice_planner.py b/PathPlanning/StateLatticePlanner/state_lattice_planner.py index e26fb828..f4b1461b 100644 --- a/PathPlanning/StateLatticePlanner/state_lattice_planner.py +++ b/PathPlanning/StateLatticePlanner/state_lattice_planner.py @@ -70,7 +70,7 @@ def generate_path(target_states, k0): if x is not None: print("find good path") result.append( - [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])]) + [x[-1], y[-1], yaw[-1], float(p[0, 0]), float(p[1, 0]), float(p[2, 0])]) print("finish path generation") return result diff --git a/SLAM/FastSLAM1/fast_slam1.py b/SLAM/FastSLAM1/fast_slam1.py index c4beaba2..7b89f52d 100644 --- a/SLAM/FastSLAM1/fast_slam1.py +++ b/SLAM/FastSLAM1/fast_slam1.py @@ -194,7 +194,7 @@ def compute_weight(particle, z, Q_cov): print("singular") return 1.0 - num = math.exp(-0.5 * dx.T @ invS @ dx) + num = np.exp(-0.5 * (dx.T @ invS @ dx))[0, 0] den = 2.0 * math.pi * math.sqrt(np.linalg.det(Sf)) w = num / den diff --git a/SLAM/FastSLAM2/fast_slam2.py b/SLAM/FastSLAM2/fast_slam2.py index 4c6c1033..aa77aa95 100644 --- a/SLAM/FastSLAM2/fast_slam2.py +++ b/SLAM/FastSLAM2/fast_slam2.py @@ -193,7 +193,7 @@ def compute_weight(particle, z, Q_cov): except np.linalg.linalg.LinAlgError: return 1.0 - num = math.exp(-0.5 * dz.T @ invS @ dz) + num = np.exp(-0.5 * dz.T @ invS @ dz)[0, 0] den = 2.0 * math.pi * math.sqrt(np.linalg.det(Sf)) w = num / den diff --git a/SLAM/GraphBasedSLAM/graph_based_slam.py b/SLAM/GraphBasedSLAM/graph_based_slam.py index 5510a411..4d66ef67 100644 --- a/SLAM/GraphBasedSLAM/graph_based_slam.py +++ b/SLAM/GraphBasedSLAM/graph_based_slam.py @@ -117,9 +117,9 @@ def calc_edges(x_list, z_list): for iz2 in range(len(z_list[t2][:, 0])): if z_list[t1][iz1, 3] == z_list[t2][iz2, 3]: d1 = z_list[t1][iz1, 0] - angle1, phi1 = z_list[t1][iz1, 1], z_list[t1][iz1, 2] + angle1, _ = z_list[t1][iz1, 1], z_list[t1][iz1, 2] d2 = z_list[t2][iz2, 0] - angle2, phi2 = z_list[t2][iz2, 1], z_list[t2][iz2, 2] + angle2, _ = z_list[t2][iz2, 1], z_list[t2][iz2, 2] edge = calc_edge(x1, y1, yaw1, x2, y2, yaw2, d1, angle1, d2, angle2, t1, t2) @@ -188,7 +188,7 @@ def graph_based_slam(x_init, hz): for i in range(nt): x_opt[0:3, i] += dx[i * 3:i * 3 + 3, 0] - diff = dx.T @ dx + diff = (dx.T @ dx)[0, 0] print("iteration: %d, diff: %f" % (itr + 1, diff)) if diff < 1.0e-5: break diff --git a/requirements/requirements.txt b/requirements/requirements.txt index 9db68984..4f226fac 100644 --- a/requirements/requirements.txt +++ b/requirements/requirements.txt @@ -1,4 +1,4 @@ -numpy == 1.24.3 +numpy == 1.25.0 scipy == 1.10.1 matplotlib == 3.7.1 cvxpy == 1.3.1 diff --git a/tests/test_codestyle.py b/tests/test_codestyle.py index 2ad3816e..a7d11d27 100644 --- a/tests/test_codestyle.py +++ b/tests/test_codestyle.py @@ -126,7 +126,7 @@ def test(): branch_commit = find_branch_point("origin/master") files = diff_files(branch_commit) print(files) - rc, errors = run_ruff(files, fix=False) + rc, errors = run_ruff(files, fix=True) if errors: print(errors) else: