diff --git a/WheeledRobot.py b/WheeledRobot.py index 48b81d9..f9d5797 100755 --- a/WheeledRobot.py +++ b/WheeledRobot.py @@ -141,6 +141,9 @@ class WheeledRobotEnv(gym.Env): # 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()