From 55bb2734d08c7dff77765f405c11b5ac99897d4e Mon Sep 17 00:00:00 2001 From: Eric Seuret Date: Tue, 23 Sep 2025 05:58:04 +0200 Subject: [PATCH] Initial commit --- Ex1_omni.py | 60 ++++++++++++++++++++++++++++++++++++++++++++++ Ex2_tank.py | 59 +++++++++++++++++++++++++++++++++++++++++++++ Ex3_mecanum.py | 63 +++++++++++++++++++++++++++++++++++++++++++++++++ Ex4_ackerman.py | 58 +++++++++++++++++++++++++++++++++++++++++++++ environment.yml | 10 ++++++++ 5 files changed, 250 insertions(+) create mode 100755 Ex1_omni.py create mode 100755 Ex2_tank.py create mode 100755 Ex3_mecanum.py create mode 100755 Ex4_ackerman.py create mode 100644 environment.yml diff --git a/Ex1_omni.py b/Ex1_omni.py new file mode 100755 index 0000000..8a8ec31 --- /dev/null +++ b/Ex1_omni.py @@ -0,0 +1,60 @@ +#!/usr/bin/env python + +import numpy as np +from WheeledRobot import OmniRobotEnv + +# OmniRobot kinematics +a1 = 0.0; b1 = 0.0; l1 = 0.5 +a2 = (2/3)*np.pi; b2 = 0.0; l2 = 0.5 +a3 = (4/3)*np.pi; b3 = 0.0; l3 = 0.5 +r = 0.1 + +# Kinematics matrix +J = ... + +F = np.linalg.inv(J) + +def controller_omni(t, X_I, dX_I): + """Controller Logic""" + # example controller outputs + radius = 2; period = 10 + dX_R_des = np.array([[2*np.pi*radius/period],[0],[2*np.pi/period]]) + # dX_R_des = np.array([[1],[0],[0]]) + # dX_R_des = np.array([[0],[1],[0]]) + # dX_R_des = np.array([[0],[0],[1]]) + + U = J @ dX_R_des + return U + +def run_simulation(): + """Run simulation using Gymnasium environment""" + env = OmniRobotEnv(render_mode="human") + observation, _ = env.reset() + + print("Starting Omnidirectional Robot Simulation") + + for step in range(200): + # Extract controller inputs from observation + time = observation[6] # Current time + X_I = observation[:3].reshape(-1, 1) # State [x, y, theta] + dX_I = observation[3:6].reshape(-1, 1) # Derivatives [dx, dy, dtheta] + + # Call original controller + U = controller_omni(time, X_I, dX_I) + + # Step environment + observation, reward, terminated, truncated, _ = env.step(U.flatten()) + + # Render the environment + env.render() + + if terminated or truncated: + print(f"Simulation ended at step {step}") + break + + input("Press Enter to close the simulation window...") + env.close() + print("Simulation completed") + +if __name__ == "__main__": + run_simulation() diff --git a/Ex2_tank.py b/Ex2_tank.py new file mode 100755 index 0000000..77bee44 --- /dev/null +++ b/Ex2_tank.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python + +import numpy as np +from WheeledRobot import TankRobotEnv + +# TankRobot kinematics +b = 1 +r = 0.1 + +# wheel equations +J = ... + +F = np.linalg.pinv(J) + +def controller_tank(t, X_I, dX_I): + """Controller Logic""" + # example controller outputs + radius = 2; + period = 10 + # dX_R_des = np.array([[2*np.pi*radius/period],[0],[2*np.pi/period]]) + dX_R_des = np.array([[1],[0],[0]]) + # dX_R_des = np.array([[0],[1],[0]]) + # dX_R_des = np.array([[0],[0],[1]]) + + U = J @ dX_R_des + return U + +def run_simulation(): + """Run simulation using Gymnasium environment""" + env = TankRobotEnv(render_mode="human") + observation, _ = env.reset() + + print("Starting Tank Robot Simulation") + + for step in range(200): + # Extract controller inputs from observation + time = observation[6] # Current time + X_I = observation[:3].reshape(-1, 1) # State [x, y, theta] + dX_I = observation[3:6].reshape(-1, 1) # Derivatives [dx, dy, dtheta] + + # Call original controller + U = controller_tank(time, X_I, dX_I) + + # Step environment + observation, reward, terminated, truncated, _ = env.step(U.flatten()) + + # Render the environment + env.render() + + if terminated or truncated: + print(f"Simulation ended at step {step}") + break + + input("Press Enter to close the simulation window...") + env.close() + print("Simulation completed") + +if __name__ == "__main__": + run_simulation() diff --git a/Ex3_mecanum.py b/Ex3_mecanum.py new file mode 100755 index 0000000..0827ce1 --- /dev/null +++ b/Ex3_mecanum.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python + +import numpy as np +from WheeledRobot import MecanumRobotEnv + +# MecanumRobot kinematics +l1 = np.array([+0.5, +0.5]); f1 = np.array([+1, +1])/np.sqrt(2); d1 = np.array([+1, 0]) +l2 = np.array([-0.5, +0.5]); f2 = np.array([-1, +1])/np.sqrt(2); d2 = np.array([+1, 0]) +l3 = np.array([-0.5, -0.5]); f3 = np.array([-1, -1])/np.sqrt(2); d3 = np.array([-1, 0]) +l4 = np.array([+0.5, -0.5]); f4 = np.array([+1, -1])/np.sqrt(2); d4 = np.array([-1, 0]) +r = 0.1 + +# wheel equations +J = ... + +# forward differential kinematics +F = np.linalg.pinv(J) + +def controller_mecanum(t, X_I, dX_I): + # example controller outputs + radius = 2; period = 10 + dX_R_des = np.array([[2*np.pi*radius/period],[0],[2*np.pi/period]]) + #dX_R_des = np.array([[1],[0],[0]]) + #dX_R_des = np.array([[0],[1],[0]]) + #dX_R_des = np.array([[0],[0],[1]]) + + U = J @ dX_R_des + return U + +def run_simulation(): + """Run simulation using Gymnasium environment""" + env = MecanumRobotEnv(render_mode="human") + observation, _ = env.reset() + + print("Starting Mecanum Robot Simulation") + print("Controller: Circular motion") + print(f"Jacobian matrix J:\n{J}") + + for step in range(200): + # Extract controller inputs from observation + time = observation[6] # Current time + X_I = observation[:3].reshape(-1, 1) # State [x, y, theta] + dX_I = observation[3:6].reshape(-1, 1) # Derivatives [dx, dy, dtheta] + + # Call original controller + U = controller_mecanum(time, X_I, dX_I) + + # Step environment + observation, reward, terminated, truncated, _ = env.step(U.flatten()) + + # Render the environment + env.render() + + if terminated or truncated: + print(f"Simulation ended at step {step}") + break + + input("Press Enter to close the simulation window...") + env.close() + print("Simulation completed") + +if __name__ == "__main__": + run_simulation() diff --git a/Ex4_ackerman.py b/Ex4_ackerman.py new file mode 100755 index 0000000..6bb0c56 --- /dev/null +++ b/Ex4_ackerman.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python + +import numpy as np +from WheeledRobot import AckermanRobotEnv + +# AckermanRobot kinematics +r = 0.1 + +# wheel equations function +def J(dX_R): + """Transform desired robot velocities to control inputs""" + U = ... + return U + +def controller_ackerman(t, X_I, dX_I): + # example controller outputs + radius = 2; period = 10 + dX_R_des = np.array([[2*np.pi*radius/period],[0],[2*np.pi/period]]) + #dX_R_des = np.array([[1],[0],[0]]) + #dX_R_des = np.array([[0],[1],[0]]) + #dX_R_des = np.array([[0],[0],[1]]) + + U = J(dX_R_des) + return U + +def run_simulation(): + """Run simulation using Gymnasium environment""" + env = AckermanRobotEnv(render_mode="human") + observation, _ = env.reset() + + print("Starting Ackermann Robot Simulation") + print("Controller: Circular motion") + + for step in range(200): + # Extract controller inputs from observation + time = observation[6] # Current time + X_I = observation[:3].reshape(-1, 1) # State [x, y, theta] + dX_I = observation[3:6].reshape(-1, 1) # Derivatives [dx, dy, dtheta] + + # Call original controller + U = controller_ackerman(time, X_I, dX_I) + + # Step environment + observation, reward, terminated, truncated, _ = env.step(U.flatten()) + + # Render the environment + env.render() + + if terminated or truncated: + print(f"Simulation ended at step {step}") + break + + input("Press Enter to close the simulation window...") + env.close() + print("Simulation completed") + +if __name__ == "__main__": + run_simulation() diff --git a/environment.yml b/environment.yml new file mode 100644 index 0000000..b02d93c --- /dev/null +++ b/environment.yml @@ -0,0 +1,10 @@ +name: wheeled-robot +channels: + - conda-forge + - defaults +dependencies: + - python>=3.8 + - numpy + - matplotlib + - gymnasium + - pip \ No newline at end of file