Initial commit

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()

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()

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()

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()

environment.yml

@@ -0,0 +1,10 @@
+name: wheeled-robot
+channels:
+  - conda-forge
+  - defaults
+dependencies:
+  - python>=3.8
+  - numpy
+  - matplotlib
+  - gymnasium
+  - pip