seureteric/lawarob-wheeled
1
0
Fork 0
Code Pull Requests Activity

Added speed limit on Wheels directly

Browse Source
This commit is contained in:
Eric Seuret 2025-10-01 16:39:25 +02:00
parent a8b69d1a0e
commit 7bb4d79c8f
2 changed files with 40 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Ex6_omni_pid.py
View File

@ -35,7 +35,7 @@ def run_simulation():
# Initialize environment with fixed target for reproducible results # Initialize environment with fixed target for reproducible results
# You can set random_target=True for random target generation # 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() observation, _ = env.reset()
# Expand render bounds to show target position (default target is at [10,5]) # Expand render bounds to show target position (default target is at [10,5])

55
WheeledRobot.py
View File

@ -31,7 +31,7 @@ class WheeledRobotEnv(gym.Env):
""" """
metadata = {"render_modes": ["human", "rgb_array"], "render_fps": 20} 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__() super().__init__()
self.dt = 0.05 self.dt = 0.05
@ -54,6 +54,10 @@ class WheeledRobotEnv(gym.Env):
self.velocity_tolerance = velocity_tolerance # units/step for "stopped" detection self.velocity_tolerance = velocity_tolerance # units/step for "stopped" detection
self.target_reached_reward = 100.0 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] # Expanded observation space: [x, y, theta, dx, dy, dtheta, time, target_x, target_y, target_theta]
self.observation_space = spaces.Box( 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), 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() 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): def _define_action_space(self):
return spaces.Box(low=-5.0, high=5.0, shape=(3,), dtype=np.float32) 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) 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) robot_velocity = np.linalg.norm(self.state_derivative)
return (pos_error[0] < self.position_tolerance and return bool((pos_error[0] < self.position_tolerance) and
pos_error[1] < self.position_tolerance and (pos_error[1] < self.position_tolerance) and
angle_error < self.orientation_tolerance and (angle_error < self.orientation_tolerance) and
robot_velocity < self.velocity_tolerance) (robot_velocity < self.velocity_tolerance))
def reset(self, *, seed: Optional[int] = None, options: Optional[Dict[str, Any]] = None) -> Tuple[np.ndarray, Dict[str, Any]]: def reset(self, *, seed: Optional[int] = None, options: Optional[Dict[str, Any]] = None) -> Tuple[np.ndarray, Dict[str, Any]]:
if seed is not None: if seed is not None:
@ -138,12 +153,12 @@ class WheeledRobotEnv(gym.Env):
U = action.reshape(-1, 1) U = action.reshape(-1, 1)
X = self.state.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 # U represents wheel velocities from controllers
# F converts wheel velocities to robot velocities # F converts wheel velocities to robot velocities
robot_velocities = self.F @ U 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 Xp = X + self._R(X[2, 0]) @ robot_velocities * self.dt
previous_state = self.state.copy() previous_state = self.state.copy()
@ -327,8 +342,9 @@ class OmniRobotEnv(WheeledRobotEnv):
Action space: Box(3,) representing wheel velocities [-100.0, 100.0] 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): 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) 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): def _define_action_space(self):
return spaces.Box(low=-100.0, high=100.0, shape=(3,), dtype=np.float32) 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] 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): 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) 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): def _define_action_space(self):
return spaces.Box(low=-160.0, high=160.0, shape=(4,), dtype=np.float32) 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] 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): 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) 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): def _define_action_space(self):
return spaces.Box(low=-100.0, high=100.0, shape=(2,), dtype=np.float32) 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. 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): 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) 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): 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) 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): # if isinstance(self.action_space, spaces.Box):
# action = np.clip(action, self.action_space.low, self.action_space.high) # 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 r = 0.1
dU = action.reshape(-1, 1) dU = action.reshape(-1, 1)
X = self.state.reshape(-1, 1) X = self.state.reshape(-1, 1)