Skip to content

refactor: ♻️ Use the shared rot_mat_22 helper for 2D rotations #156

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Tektrix wants to merge 1 commit into
base: main
Choose a base branch
from
Open

refactor: ♻️ Use the shared rot_mat_22 helper for 2D rotations #156

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 7 additions & 9 deletions src/components/array/xy_array.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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)

Expand Down
5 changes: 5 additions & 0 deletions src/components/common/matrix_lib.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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],
Expand Down
6 changes: 3 additions & 3 deletions src/components/plan/informed_rrt_star/informed_rrt_star_path_planner.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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__(
Expand Down Expand Up @@ -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)
Expand Down