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)