start refactoring pose

get_line_follow_sim/RobotSimulationManager.py

@@ -116,7 +116,8 @@ class Environment:
 
 class Robot(ABC):
     def __init__(self):
-        self.pose = np.array([[0.0, 0.0, 0.0, 1.0]]).T  # x, y, theta, 1 (homogeneous coordinates)
+        self.pos = np.array([[0.0, 0.0, 1.0]]).T  # x, y, 1 (homogeneous coordinates)
+        self.pos_th = 0.0  # orientation
 
         # all units in mm
         self.wheel_base = 110.0
@@ -130,7 +131,6 @@ class Robot(ABC):
         self.inertia = 0.1  # kg.m^2
         self.max_tire_angular_velocity = 10.0  # radians per second
         self.motor_response_coefficient = 1.0  # Scaling factor for motor delay
-        self.pose = np.array([0.0, 0.0, 0.0])  # [x, y, theta]
         self.robot_vx = 0.0  # Linear velocity in x-direction
         self.robot_omega = 0.0  # Angular velocity (yaw rate)
         self.tire_omega_left = 0.0  # Current angular velocity of left tire
@@ -183,18 +183,18 @@ class Robot(ABC):
         rect_h = self.wheel_base / 6
 
         for patch, sign in zip(self.__tire_patches, [-1, 1]):
-            patch_pose = rotate(float(self.pose[2])) \
+            patch_pose = rotate(float(self.pos[2])) \
                 @ translate(-0.5 * rect_w, 0.5 * sign * self.wheel_base - 0.5 * rect_h) \
                 @ np.array([[0, 0, 0, 1]]).T \
-                + self.pose
+                + self.pos
 
             patch.set_xy(patch_pose[:2].flatten())
-            patch.set_angle(np.rad2deg(self.pose[2]))
+            patch.set_angle(np.rad2deg(self.pos[2]))
 
-        self.__chassis_patch.set_xy(self.pose[:2].T.tolist())
+        self.__chassis_patch.set_xy(self.pos[:2].T.tolist())
 
-        self.__left_sensor.update_ui(self.pose)
-        self.__right_sensor.update_ui(self.pose)
+        self.__left_sensor.update_ui(self.pos)
+        self.__right_sensor.update_ui(self.pos)
 
     def set_left_sensor(self, sensor):
         """Set the left sensor instance."""
@@ -207,7 +207,7 @@ class Robot(ABC):
     def set_pose(self, pose: np.ndarray):
         """Set the robot's pose."""
         assert pose.shape == (4, 1), f'pose must be (4, 1) array, got {pose.shape}'
-        self.pose = pose
+        self.pos = pose
 
     def update_state(self, environment: Environment, dt: float) -> np.ndarray:
         """
@@ -217,8 +217,8 @@ class Robot(ABC):
         :param environment: Data provided by the left and right sensors.
         :param dt: Time step.
         """
-        left = self.__left_sensor.evaluate(self.pose, environment)
-        right = self.__right_sensor.evaluate(self.pose, environment)
+        left = self.__left_sensor.evaluate(self.pos, environment)
+        right = self.__right_sensor.evaluate(self.pos, environment)
         left_input, right_input = self.update((left, right), dt)
 
         target_v_left = left_input * self.max_tire_angular_velocity
@@ -233,15 +233,15 @@ class Robot(ABC):
         self.robot_omega = (self.tire_radius / self.wheel_base) * (self.tire_omega_right - self.tire_omega_left)
 
         # Update pose based on velocities
-        self.pose[0] += self.robot_vx * np.cos(self.pose[2]) * dt  # x position
-        self.pose[1] += self.robot_vx * np.sin(self.pose[2]) * dt  # y position
-        self.pose[2] += self.robot_omega * dt  # orientation theta
+        self.pos[0] += self.robot_vx * np.cos(self.pos[2]) * dt  # x position
+        self.pos[1] += self.robot_vx * np.sin(self.pos[2]) * dt  # y position
+        self.pos[2] += self.robot_omega * dt  # orientation theta
 
         # Normalize theta to range [-π, π]
-        self.pose[2] = (self.pose[2] + np.pi) % (2 * np.pi) - np.pi
+        self.pos[2] = (self.pos[2] + np.pi) % (2 * np.pi) - np.pi
 
         # return np.array([[0,0,0,1]]).T
-        return self.pose
+        return self.pos
 
     @abstractmethod
     def update(self, sensors_data: Tuple[int, int], dt: float) -> Tuple[float, float]: