From 5b4d82228b4910654ade5c19987b250f6e3802fe Mon Sep 17 00:00:00 2001 From: Tektrix Date: Tue, 27 Oct 2026 11:32:04 -0400 Subject: [PATCH] =?UTF-8?q?refactor:=20=E2=99=BB=EF=B8=8F=20Use=20the=20sh?= =?UTF-8?q?ared=20rot=5Fmat=5F22=20helper=20for=202D=20rotations?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replace the hand-written rotation matrices in XYArray.homogeneous_transformation() and InformedRrtStarPathPlanner.sample_informed() with rot_mat_22, add the translation in XYArray with plain broadcasting, and add a docstring to rot_mat_22. Behavior is unchanged. Closes #125 --- src/components/array/xy_array.py | 16 +++++++--------- src/components/common/matrix_lib.py | 5 +++++ .../informed_rrt_star_path_planner.py | 6 +++--- 3 files changed, 15 insertions(+), 12 deletions(-) diff --git a/src/components/array/xy_array.py b/src/components/array/xy_array.py index 767246b9..0569856a 100644 --- a/src/components/array/xy_array.py +++ b/src/components/array/xy_array.py @@ -4,8 +4,12 @@ Author: Shisato Yano """ -from math import sin, cos +import sys import numpy as np +from pathlib import Path + +sys.path.append(str(Path(__file__).absolute().parent) + "/../common") +from matrix_lib import rot_mat_22 class XYArray: @@ -30,15 +34,9 @@ def homogeneous_transformation(self, x, y, angle_rad): Return transformed XYArray object """ - angle_cos = cos(angle_rad) - angle_sin = sin(angle_rad) - - rotation_matrix = np.array([[angle_cos, -angle_sin], - [angle_sin, angle_cos]]) - - rotated_data = rotation_matrix @ self.data + rotated_data = rot_mat_22(angle_rad) @ self.data - translated_data = rotated_data + np.ones(rotated_data.shape) * np.array([[x], [y]]) + translated_data = rotated_data + np.array([[x], [y]]) return XYArray(translated_data) diff --git a/src/components/common/matrix_lib.py b/src/components/common/matrix_lib.py index 17637808..555ded3c 100644 --- a/src/components/common/matrix_lib.py +++ b/src/components/common/matrix_lib.py @@ -24,6 +24,11 @@ def hom_mat_33(x, y, yaw): def rot_mat_22(yaw): + """ + Rotation matrix 2x2 + yaw: yaw direction rotation[rad] + """ + cos_yaw, sin_yaw = cos(yaw), sin(yaw) return np.array([[cos_yaw, -sin_yaw], diff --git a/src/components/plan/informed_rrt_star/informed_rrt_star_path_planner.py b/src/components/plan/informed_rrt_star/informed_rrt_star_path_planner.py index d8d9e788..e4a1e484 100644 --- a/src/components/plan/informed_rrt_star/informed_rrt_star_path_planner.py +++ b/src/components/plan/informed_rrt_star/informed_rrt_star_path_planner.py @@ -20,6 +20,8 @@ abs_dir_path = str(Path(__file__).absolute().parent) relative_path = "/../../../components/" sys.path.append(abs_dir_path + relative_path + "mapping/grid") +sys.path.append(abs_dir_path + relative_path + "common") +from matrix_lib import rot_mat_22 class InformedRrtStarPathPlanner: def __init__( @@ -128,9 +130,7 @@ def sample_informed(self, c_best): theta = np.arctan2(self.goal[1] - self.start[1], self.goal[0] - self.start[0]) centre = (np.array(self.start) + np.array(self.goal)) / 2.0 - cos_theta, sin_theta = np.cos(theta), np.sin(theta) - C = np.array([[cos_theta, -sin_theta], - [sin_theta, cos_theta]]) + C = rot_mat_22(theta) r = np.sqrt(np.random.uniform(0, 1)) phi = np.random.uniform(0, 2 * np.pi)