From cc3fd0c55e8df8637ffa1753c6a3d973db9ed2a1 Mon Sep 17 00:00:00 2001 From: Videh Patel <66770479+videh25@users.noreply.github.com> Date: Tue, 2 Jan 2024 19:09:48 +0530 Subject: [PATCH] Using util.angle_mod in all codes. #684 (#946) * switched to using utils.angle_mod() * switched to using utils.angle_mod() * renamed mod2pi to pi_2_pi * Removed linting errors * switched to using utils.angle_mod() * switched to using utils.angle_mod() * renamed mod2pi to pi_2_pi * Removed linting errors * annotation changes and round precision * Reverted to mod2pi --------- Co-authored-by: Videh Patel --- .../n_joint_arm_to_point_control.py | 5 ++-- .../two_joint_arm_to_point_control.py | 3 ++- Control/move_to_pose/move_to_pose.py | 19 +++++++-------- .../ensemble_kalman_filter.py | 3 ++- .../ClosedLoopRRTStar/unicycle_model.py | 3 ++- .../motion_model.py | 3 ++- .../trajectory_generator.py | 2 +- .../reeds_shepp_path_planning.py | 9 ++++---- .../lqr_speed_steer_control.py | 4 ++-- .../lqr_steer_control/lqr_steer_control.py | 3 ++- ...odel_predictive_speed_and_steer_control.py | 9 ++------ .../rear_wheel_feedback.py | 23 ++++++++----------- .../stanley_controller/stanley_controller.py | 13 ++++------- SLAM/EKFSLAM/ekf_slam.py | 3 ++- SLAM/FastSLAM1/fast_slam1.py | 3 ++- SLAM/FastSLAM2/fast_slam2.py | 3 ++- SLAM/GraphBasedSLAM/graph_based_slam.py | 3 ++- 17 files changed, 51 insertions(+), 60 deletions(-) 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 498ef41c..a2375233 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 @@ -11,6 +11,7 @@ sys.path.append(str(Path(__file__).parent.parent.parent)) import numpy as np from ArmNavigation.n_joint_arm_to_point_control.NLinkArm import NLinkArm +from utils.angle import angle_mod # Simulation parameters Kp = 2 @@ -159,9 +160,9 @@ def ang_diff(theta1, theta2): """ Returns the difference between two angles in the range -pi to +pi """ - return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi + return angle_mod(theta1 - theta2) if __name__ == '__main__': # main() - animation() \ No newline at end of file + animation() 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 66d0ebeb..c2227f18 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 @@ -15,6 +15,7 @@ Ref: P. I. Corke, "Robotics, Vision & Control", Springer 2017, import matplotlib.pyplot as plt import numpy as np import math +from utils.angle import angle_mod # Simulation parameters @@ -110,7 +111,7 @@ def plot_arm(theta1, theta2, target_x, target_y): # pragma: no cover def ang_diff(theta1, theta2): # Returns the difference between two angles in the range -pi to +pi - return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi + return angle_mod(theta1 - theta2) def click(event): # pragma: no cover diff --git a/Control/move_to_pose/move_to_pose.py b/Control/move_to_pose/move_to_pose.py index 73604ccd..cc296662 100644 --- a/Control/move_to_pose/move_to_pose.py +++ b/Control/move_to_pose/move_to_pose.py @@ -13,7 +13,7 @@ P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7 import matplotlib.pyplot as plt import numpy as np from random import random - +from utils.angle import angle_mod class PathFinderController: """ @@ -71,9 +71,8 @@ class PathFinderController: # from 0 rad to 2*pi rad with slight turn rho = np.hypot(x_diff, y_diff) - 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 + alpha = angle_mod(np.arctan2(y_diff, x_diff) - theta) + beta = angle_mod(theta_goal - theta - alpha) v = self.Kp_rho * rho w = self.Kp_alpha * alpha - controller.Kp_beta * beta @@ -173,16 +172,14 @@ def transformation_matrix(x, y, theta): def main(): for i in range(5): - x_start = 20 * random() - y_start = 20 * random() - theta_start = 2 * np.pi * random() - np.pi + x_start = 20.0 * random() + y_start = 20.0 * random() + theta_start: float = 2 * np.pi * random() - np.pi x_goal = 20 * random() y_goal = 20 * random() theta_goal = 2 * np.pi * random() - np.pi - print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" % - (x_start, y_start, theta_start)) - print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" % - (x_goal, y_goal, theta_goal)) + print(f"Initial x: {round(x_start, 2)} m\nInitial y: {round(y_start, 2)} m\nInitial theta: {round(theta_start, 2)} rad\n") + print(f"Goal x: {round(x_goal, 2)} m\nGoal y: {round(y_goal, 2)} m\nGoal theta: {round(theta_goal, 2)} rad\n") move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal) diff --git a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py index fc8280e7..2bab3b49 100644 --- a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py +++ b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py @@ -16,6 +16,7 @@ sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) import math import matplotlib.pyplot as plt import numpy as np +from utils.angle import angle_mod from utils.angle import rot_mat_2d @@ -179,7 +180,7 @@ def plot_covariance_ellipse(xEst, PEst): # pragma: no cover def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi + return angle_mod(angle) def main(): diff --git a/PathPlanning/ClosedLoopRRTStar/unicycle_model.py b/PathPlanning/ClosedLoopRRTStar/unicycle_model.py index 5a0fd17a..c05f76c8 100644 --- a/PathPlanning/ClosedLoopRRTStar/unicycle_model.py +++ b/PathPlanning/ClosedLoopRRTStar/unicycle_model.py @@ -8,6 +8,7 @@ author Atsushi Sakai import math import numpy as np +from utils.angle import angle_mod dt = 0.05 # [s] L = 0.9 # [m] @@ -39,7 +40,7 @@ def update(state, a, delta): def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi + return angle_mod(angle) if __name__ == '__main__': # pragma: no cover diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py index 531bf91e..5ef6d2e2 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py @@ -1,6 +1,7 @@ import math import numpy as np from scipy.interpolate import interp1d +from utils.angle import angle_mod # motion parameter L = 1.0 # wheel base @@ -18,7 +19,7 @@ class State: def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi + return angle_mod(angle) def update(state, v, delta, dt, L): diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py index 97c0ad8b..6084fc1a 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py @@ -18,7 +18,7 @@ import ModelPredictiveTrajectoryGenerator.motion_model as motion_model # optimization parameter max_iter = 100 -h = np.array([0.5, 0.02, 0.02]).T # parameter sampling distance +h: np.ndarray = np.array([0.5, 0.02, 0.02]).T # parameter sampling distance cost_th = 0.1 show_animation = True diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index c55d2629..4d8fb7d9 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -9,6 +9,7 @@ import math import matplotlib.pyplot as plt import numpy as np +from utils.angle import angle_mod show_animation = True @@ -40,6 +41,9 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): plt.plot(x, y) +def pi_2_pi(x): + return angle_mod(x) + def mod2pi(x): # Be consistent with fmod in cplusplus here. v = np.mod(x, np.copysign(2.0 * math.pi, x)) @@ -50,7 +54,6 @@ def mod2pi(x): v -= 2.0 * math.pi return v - def straight_left_straight(x, y, phi): phi = mod2pi(phi) # only take phi in (0.01*math.pi, 0.99*math.pi) for the sake of speed. @@ -296,10 +299,6 @@ def interpolate(dist, length, mode, max_curvature, origin_x, origin_y, return x, y, yaw, 1 if length > 0.0 else -1 -def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi - - def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size): q0 = [sx, sy, syaw] q1 = [gx, gy, gyaw] diff --git a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py index 7e57abb8..a70e30d3 100644 --- a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py +++ b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py @@ -11,6 +11,7 @@ import matplotlib.pyplot as plt import numpy as np import scipy.linalg as la import pathlib +from utils.angle import angle_mod sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) from PathPlanning.CubicSpline import cubic_spline_planner @@ -52,8 +53,7 @@ def update(state, a, delta): def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi - + return angle_mod(angle) def solve_dare(A, B, Q, R): """ diff --git a/PathTracking/lqr_steer_control/lqr_steer_control.py b/PathTracking/lqr_steer_control/lqr_steer_control.py index 95a1b92c..4078ea56 100644 --- a/PathTracking/lqr_steer_control/lqr_steer_control.py +++ b/PathTracking/lqr_steer_control/lqr_steer_control.py @@ -11,6 +11,7 @@ import math import numpy as np import sys import pathlib +from utils.angle import angle_mod sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) from PathPlanning.CubicSpline import cubic_spline_planner @@ -61,7 +62,7 @@ def PIDControl(target, current): def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi + return angle_mod(angle) def solve_DARE(A, B, Q, R): 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 30153131..c55018cf 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 @@ -11,6 +11,7 @@ import math import numpy as np import sys import pathlib +from utils.angle import angle_mod sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) from PathPlanning.CubicSpline import cubic_spline_planner @@ -69,13 +70,7 @@ class State: def pi_2_pi(angle): - while(angle > math.pi): - angle = angle - 2.0 * math.pi - - while(angle < -math.pi): - angle = angle + 2.0 * math.pi - - return angle + return angle_mod(angle) def get_linear_model_matrix(v, phi, delta): diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py index ff72454f..1702bd47 100644 --- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py +++ b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py @@ -8,6 +8,7 @@ author: Atsushi Sakai(@Atsushi_twi) import matplotlib.pyplot as plt import math import numpy as np +from utils.angle import angle_mod from scipy import interpolate from scipy import optimize @@ -51,21 +52,21 @@ class CubicSplinePath: self.ddY = self.Y.derivative(2) self.length = s[-1] - + def calc_yaw(self, s): dx, dy = self.dX(s), self.dY(s) return np.arctan2(dy, dx) - + def calc_curvature(self, s): dx, dy = self.dX(s), self.dY(s) ddx, ddy = self.ddX(s), self.ddY(s) return (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) - + def __find_nearest_point(self, s0, x, y): def calc_distance(_s, *args): _x, _y= self.X(_s), self.Y(_s) return (_x - args[0])**2 + (_y - args[1])**2 - + def calc_distance_jacobian(_s, *args): _x, _y = self.X(_s), self.Y(_s) _dx, _dy = self.dX(_s), self.dY(_s) @@ -76,7 +77,7 @@ class CubicSplinePath: def calc_track_error(self, x, y, s0): ret = self.__find_nearest_point(s0, x, y) - + s = ret[0][0] e = ret[1] @@ -96,13 +97,7 @@ def pid_control(target, current): return a def pi_2_pi(angle): - while(angle > math.pi): - angle = angle - 2.0 * math.pi - - while(angle < -math.pi): - angle = angle + 2.0 * math.pi - - return angle + return angle_mod(angle) def rear_wheel_feedback_control(state, e, k, yaw_ref): v = state.v @@ -170,7 +165,7 @@ def simulate(path_ref, goal): plt.plot(path_ref.X(s0), path_ref.Y(s0), "xg", label="target") plt.axis("equal") plt.grid(True) - plt.title("speed[km/h]:{:.2f}, target s-param:{:.2f}".format(round(state.v * 3.6, 2), s0)) + plt.title(f"speed[km/h]:{round(state.v * 3.6, 2):.2f}, target s-param:{s0:.2f}") plt.pause(0.0001) return t, x, y, yaw, v, goal_flag @@ -184,7 +179,7 @@ def calc_target_speed(state, yaw_ref): if switch: state.direction *= -1 return 0.0 - + if state.direction != 1: return -target_speed diff --git a/PathTracking/stanley_controller/stanley_controller.py b/PathTracking/stanley_controller/stanley_controller.py index 9c475cba..08aa0493 100644 --- a/PathTracking/stanley_controller/stanley_controller.py +++ b/PathTracking/stanley_controller/stanley_controller.py @@ -13,6 +13,7 @@ import numpy as np import matplotlib.pyplot as plt import sys import pathlib +from utils.angle import angle_mod sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) from PathPlanning.CubicSpline import cubic_spline_planner @@ -26,7 +27,7 @@ max_steer = np.radians(30.0) # [rad] max steering angle show_animation = True -class State(object): +class State: """ Class representing the state of a vehicle. @@ -38,7 +39,7 @@ class State(object): def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): """Instantiate the object.""" - super(State, self).__init__() + super().__init__() self.x = x self.y = y self.yaw = yaw @@ -106,13 +107,7 @@ def normalize_angle(angle): :param angle: (float) :return: (float) Angle in radian in [-pi, pi] """ - while angle > np.pi: - angle -= 2.0 * np.pi - - while angle < -np.pi: - angle += 2.0 * np.pi - - return angle + return angle_mod(angle) def calc_target_index(state, cx, cy): diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py index bb648ce9..a624e876 100644 --- a/SLAM/EKFSLAM/ekf_slam.py +++ b/SLAM/EKFSLAM/ekf_slam.py @@ -8,6 +8,7 @@ import math import matplotlib.pyplot as plt import numpy as np +from utils.angle import angle_mod # EKF state covariance Cx = np.diag([0.5, 0.5, np.deg2rad(30.0)]) ** 2 @@ -192,7 +193,7 @@ def jacob_h(q, delta, x, i): def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi + return angle_mod(angle) def main(): diff --git a/SLAM/FastSLAM1/fast_slam1.py b/SLAM/FastSLAM1/fast_slam1.py index 7b89f52d..17f6d5e5 100644 --- a/SLAM/FastSLAM1/fast_slam1.py +++ b/SLAM/FastSLAM1/fast_slam1.py @@ -10,6 +10,7 @@ import math import matplotlib.pyplot as plt import numpy as np +from utils.angle import angle_mod # Fast SLAM covariance Q = np.diag([3.0, np.deg2rad(10.0)]) ** 2 @@ -321,7 +322,7 @@ def motion_model(x, u): def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi + return angle_mod(angle) def main(): diff --git a/SLAM/FastSLAM2/fast_slam2.py b/SLAM/FastSLAM2/fast_slam2.py index aa77aa95..2b57e3bc 100644 --- a/SLAM/FastSLAM2/fast_slam2.py +++ b/SLAM/FastSLAM2/fast_slam2.py @@ -10,6 +10,7 @@ import math import matplotlib.pyplot as plt import numpy as np +from utils.angle import angle_mod # Fast SLAM covariance Q = np.diag([3.0, np.deg2rad(10.0)]) ** 2 @@ -346,7 +347,7 @@ def motion_model(x, u): def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi + return angle_mod(angle) def main(): diff --git a/SLAM/GraphBasedSLAM/graph_based_slam.py b/SLAM/GraphBasedSLAM/graph_based_slam.py index 4d66ef67..edd20a95 100644 --- a/SLAM/GraphBasedSLAM/graph_based_slam.py +++ b/SLAM/GraphBasedSLAM/graph_based_slam.py @@ -21,6 +21,7 @@ import math import matplotlib.pyplot as plt import numpy as np from scipy.spatial.transform import Rotation as Rot +from utils.angle import angle_mod # Simulation parameter Q_sim = np.diag([0.2, np.deg2rad(1.0)]) ** 2 @@ -249,7 +250,7 @@ def motion_model(x, u): def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi + return angle_mod(angle) def main():