From 7bb4d79c8fb5493b3d21cb7859d94cca532cd053 Mon Sep 17 00:00:00 2001 From: Eric Seuret Date: Wed, 1 Oct 2025 16:39:25 +0200 Subject: [PATCH] Added speed limit on Wheels directly --- Ex6_omni_pid.py | 2 +- WheeledRobot.py | 55 +++++++++++++++++++++++++++++++++++-------------- 2 files changed, 40 insertions(+), 17 deletions(-) diff --git a/Ex6_omni_pid.py b/Ex6_omni_pid.py index 3162d79..73b54a1 100755 --- a/Ex6_omni_pid.py +++ b/Ex6_omni_pid.py @@ -35,7 +35,7 @@ def run_simulation(): # Initialize environment with fixed target for reproducible results # You can set random_target=True for random target generation - env = OmniRobotEnv(render_mode="human", random_target=False) + env = OmniRobotEnv(render_mode="human", random_target=False, speed_limit_enabled=True) observation, _ = env.reset() # Expand render bounds to show target position (default target is at [10,5]) diff --git a/WheeledRobot.py b/WheeledRobot.py index f9d5797..ee2d6e6 100755 --- a/WheeledRobot.py +++ b/WheeledRobot.py @@ -31,7 +31,7 @@ class WheeledRobotEnv(gym.Env): """ metadata = {"render_modes": ["human", "rgb_array"], "render_fps": 20} - def __init__(self, render_mode: Optional[str] = None, random_target: bool = True, target_position: Optional[np.ndarray] = None, velocity_tolerance: float = 0.05): + def __init__(self, render_mode: Optional[str] = None, random_target: bool = True, target_position: Optional[np.ndarray] = None, velocity_tolerance: float = 0.05, speed_limit_enabled: bool = False): super().__init__() self.dt = 0.05 @@ -54,6 +54,10 @@ class WheeledRobotEnv(gym.Env): self.velocity_tolerance = velocity_tolerance # units/step for "stopped" detection self.target_reached_reward = 100.0 + # Speed limiting parameters + self.speed_limit_enabled = speed_limit_enabled + self._wheel_speed_limits = None # To be set by subclasses + # Expanded observation space: [x, y, theta, dx, dy, dtheta, time, target_x, target_y, target_theta] self.observation_space = spaces.Box( low=np.array([-15.0, -10.0, -np.pi, -15.0, -10.0, -10.0, 0.0, -15.0, -10.0, -np.pi], dtype=np.float32), @@ -73,6 +77,17 @@ class WheeledRobotEnv(gym.Env): self.F = self._F() + def _apply_wheel_speed_limits(self, wheel_speeds: np.ndarray) -> np.ndarray: + """Apply wheel speed limits if enabled""" + if not self.speed_limit_enabled or self._wheel_speed_limits is None: + return wheel_speeds + + # Apply symmetric limits to each wheel + limited_speeds = np.clip(wheel_speeds.flatten(), + -self._wheel_speed_limits, + self._wheel_speed_limits) + return limited_speeds.reshape(-1, 1) + def _define_action_space(self): return spaces.Box(low=-5.0, high=5.0, shape=(3,), dtype=np.float32) @@ -109,10 +124,10 @@ class WheeledRobotEnv(gym.Env): angle_error = np.abs(np.mod(self.state[2] - self.target_position[2] + np.pi, 2*np.pi) - np.pi) robot_velocity = np.linalg.norm(self.state_derivative) - return (pos_error[0] < self.position_tolerance and - pos_error[1] < self.position_tolerance and - angle_error < self.orientation_tolerance and - robot_velocity < self.velocity_tolerance) + return bool((pos_error[0] < self.position_tolerance) and + (pos_error[1] < self.position_tolerance) and + (angle_error < self.orientation_tolerance) and + (robot_velocity < self.velocity_tolerance)) def reset(self, *, seed: Optional[int] = None, options: Optional[Dict[str, Any]] = None) -> Tuple[np.ndarray, Dict[str, Any]]: if seed is not None: @@ -138,12 +153,12 @@ class WheeledRobotEnv(gym.Env): U = action.reshape(-1, 1) X = self.state.reshape(-1, 1) + # Apply wheel speed limits before kinematic conversion + U = self._apply_wheel_speed_limits(U) + # U represents wheel velocities from controllers # F converts wheel velocities to robot velocities robot_velocities = self.F @ U - robot_velocities = np.clip(robot_velocities, - a_min=np.array([-2, -2, -1]).reshape(-1, 1), - a_max=np.array([2, 2, 1]).reshape(-1, 1)) Xp = X + self._R(X[2, 0]) @ robot_velocities * self.dt previous_state = self.state.copy() @@ -327,8 +342,9 @@ class OmniRobotEnv(WheeledRobotEnv): Action space: Box(3,) representing wheel velocities [-100.0, 100.0] """ - def __init__(self, render_mode: Optional[str] = None, random_target: bool = True, target_position: Optional[np.ndarray] = None, velocity_tolerance: float = 0.05): - super().__init__(render_mode, random_target, target_position, velocity_tolerance) + def __init__(self, render_mode: Optional[str] = None, random_target: bool = True, target_position: Optional[np.ndarray] = None, velocity_tolerance: float = 0.05, speed_limit_enabled: bool = False): + super().__init__(render_mode, random_target, target_position, velocity_tolerance, speed_limit_enabled) + self._wheel_speed_limits = np.array([70.0, 70.0, 70.0]) def _define_action_space(self): return spaces.Box(low=-100.0, high=100.0, shape=(3,), dtype=np.float32) @@ -372,8 +388,9 @@ class MecanumRobotEnv(WheeledRobotEnv): Action space: Box(4,) representing wheel velocities [-160.0, 160.0] """ - def __init__(self, render_mode: Optional[str] = None, random_target: bool = True, target_position: Optional[np.ndarray] = None, velocity_tolerance: float = 0.05): - super().__init__(render_mode, random_target, target_position, velocity_tolerance) + def __init__(self, render_mode: Optional[str] = None, random_target: bool = True, target_position: Optional[np.ndarray] = None, velocity_tolerance: float = 0.05, speed_limit_enabled: bool = False): + super().__init__(render_mode, random_target, target_position, velocity_tolerance, speed_limit_enabled) + self._wheel_speed_limits = np.array([40.0, 40.0, 40.0, 40.0]) def _define_action_space(self): return spaces.Box(low=-160.0, high=160.0, shape=(4,), dtype=np.float32) @@ -426,8 +443,9 @@ class TankRobotEnv(WheeledRobotEnv): Action space: Box(2,) representing left and right wheel velocities [-100.0, 100.0] """ - def __init__(self, render_mode: Optional[str] = None, random_target: bool = True, target_position: Optional[np.ndarray] = None, velocity_tolerance: float = 0.05): - super().__init__(render_mode, random_target, target_position, velocity_tolerance) + def __init__(self, render_mode: Optional[str] = None, random_target: bool = True, target_position: Optional[np.ndarray] = None, velocity_tolerance: float = 0.05, speed_limit_enabled: bool = False): + super().__init__(render_mode, random_target, target_position, velocity_tolerance, speed_limit_enabled) + self._wheel_speed_limits = np.array([50.0, 50.0]) def _define_action_space(self): return spaces.Box(low=-100.0, high=100.0, shape=(2,), dtype=np.float32) @@ -471,8 +489,10 @@ class AckermanRobotEnv(WheeledRobotEnv): steering angle and forward velocity. """ - def __init__(self, render_mode: Optional[str] = None, random_target: bool = True, target_position: Optional[np.ndarray] = None, velocity_tolerance: float = 0.05): - super().__init__(render_mode, random_target, target_position, velocity_tolerance) + def __init__(self, render_mode: Optional[str] = None, random_target: bool = True, target_position: Optional[np.ndarray] = None, velocity_tolerance: float = 0.05, speed_limit_enabled: bool = False): + super().__init__(render_mode, random_target, target_position, velocity_tolerance, speed_limit_enabled) + # Set hardcoded limits with 10% margin above max observed (velocity: 1.3, steering: 0.6) + self._wheel_speed_limits = np.array([4, 2]) def _define_action_space(self): return spaces.Box(low=np.array([-10.0, -1.0]), high=np.array([10.0, 1.0]), dtype=np.float32) @@ -481,6 +501,9 @@ class AckermanRobotEnv(WheeledRobotEnv): # if isinstance(self.action_space, spaces.Box): # action = np.clip(action, self.action_space.low, self.action_space.high) + # Apply speed limits for Ackerman robot (velocity, steering angle) + action = self._apply_wheel_speed_limits(action.reshape(-1, 1)).flatten() + r = 0.1 dU = action.reshape(-1, 1) X = self.state.reshape(-1, 1)