Ex5..6

Ex5_omni_bangbang.py

@@ -0,0 +1,102 @@
+#!/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 = np.array([
+    [1/r*np.sin(a1+b1), -1/r*np.cos(a1+b1), -l1/r*np.cos(b1)],
+    [1/r*np.sin(a2+b2), -1/r*np.cos(a2+b2), -l2/r*np.cos(b2)],
+    [1/r*np.sin(a3+b3), -1/r*np.cos(a3+b3), -l3/r*np.cos(b3)]
+])
+F = np.linalg.inv(J)
+
+def controller_omni_bangbang(t, X_I, dX_I, target_position):
+    """Bang-bang controller with dynamic target position"""
+    
+    # maximum control velocity
+    dX_I_max = np.array([[2], [2], [1]])
+
+    # Use target position from parameters instead of hardcoded values
+    X_I_des = target_position.reshape(-1, 1)
+    pos_err = X_I_des-X_I
+
+    # part 1 (separate X,Y,theta)
+    dX_I_des = np.array([ 
+        [dX_I_max[0,0] * np.sign(pos_err[0,0])], 
+        [dX_I_max[1,0] * np.sign(pos_err[1,0])], 
+        [dX_I_max[2,0] * np.sign(pos_err[2,0])] 
+    ])
+    
+    # part 3 (combined XY control)
+    # dir_err = pos_err[0:2,:] / np.linalg.norm(pos_err[0:2,:])
+    # dX_I_des = np.array([ 
+    #     [dX_I_max[0,0] * dir_err[0,0]], 
+    #     [dX_I_max[1,0] * dir_err[1,0]], 
+    #     [dX_I_max[2,0] * np.sign(pos_err[2,0])] 
+    # ])
+
+    # part 2 (stopping condition)
+    if np.abs(pos_err[0,0]) < 0.1: dX_I_des[0,0] = 0
+    if np.abs(pos_err[1,0]) < 0.1: dX_I_des[1,0] = 0
+    if np.abs(pos_err[2,0]) < 0.01: dX_I_des[2,0] = 0
+
+    # coordinate transform
+    R_RI = np.array([[ np.cos(X_I[2,0]), np.sin(X_I[2,0]), 0.0],
+                     [-np.sin(X_I[2,0]), np.cos(X_I[2,0]), 0.0],
+                     [             0.0,               0.0, 1.0]])
+    dX_R_des = R_RI @ dX_I_des
+    U = J @ dX_R_des
+    return U
+
+def run_simulation():
+    """Run simulation using Gymnasium environment with bang-bang control to dynamic target"""
+    # Initialize environment with fixed target to match Sol6 behavior
+    # You can set random_target=True for random target generation
+    env = OmniRobotEnv(render_mode="human", random_target=False)
+    observation, _ = env.reset()
+    
+    # Expand render bounds to show target position (default target is at [10,5])
+    env.set_render_bounds((-2, 12), (-2, 8))
+    
+    print("Starting Omnidirectional Robot Bang-Bang Control Simulation")
+    print("Controller: Bang-bang control to dynamic target position")
+    print(f"Target position: [{observation[7]:.2f}, {observation[8]:.2f}, {observation[9]:.2f}]")
+    
+    for step in range(1000):
+        # Extract controller inputs from observation
+        # New observation format: [x, y, theta, dx, dy, dtheta, time, target_x, target_y, target_theta]
+        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]
+        target_position = observation[7:10]      # Target [target_x, target_y, target_theta]
+        
+        # Call bang-bang controller with dynamic target
+        U = controller_omni_bangbang(time, X_I, dX_I, target_position)
+        
+        # Step environment
+        observation, reward, terminated, truncated, _ = env.step(U.flatten())
+        
+        # Render the environment
+        env.render()
+        
+        # Check if target reached
+        if terminated:
+            print(f"Target reached at step {step}! Reward: {reward:.2f}")
+            break
+        elif truncated:
+            print(f"Maximum steps reached at step {step}")
+            break
+    
+    input("Press Enter to close the simulation window...")
+    env.close()
+    print("Simulation completed")
+
+if __name__ == "__main__":
+    run_simulation()

Ex6_omni_pid.py

@@ -0,0 +1,99 @@
+#!/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 = np.array([
+    [1/r*np.sin(a1+b1), -1/r*np.cos(a1+b1), -l1/r*np.cos(b1)],
+    [1/r*np.sin(a2+b2), -1/r*np.cos(a2+b2), -l2/r*np.cos(b2)],
+    [1/r*np.sin(a3+b3), -1/r*np.cos(a3+b3), -l3/r*np.cos(b3)]
+])
+F = np.linalg.inv(J)
+
+def controller_omni_pid(t, X_I, dX_I, target_position):
+    """PID Controller with dynamic target position"""
+    global int_err
+
+    # PID parameters
+    Kp_t = 2; Kp_r = 2
+    Kd_t = 0.5; Kd_r = 0.5
+    Ki_t = 0.1; Ki_r = 0.1
+
+    # Use target position from parameters instead of hardcoded values
+    X_I_des = target_position.reshape(-1, 1)
+    pos_err = X_I_des - X_I
+    vel_err = np.zeros((3, 1)) - dX_I
+    int_err = int_err + pos_err
+
+    # PID control output
+    dX_I_des = np.array([ 
+        [Kp_t*pos_err[0,0] + Kd_t*vel_err[0,0] + Ki_t*int_err[0,0]], 
+        [Kp_t*pos_err[1,0] + Kd_t*vel_err[1,0] + Ki_t*int_err[1,0]], 
+        [Kp_r*pos_err[2,0] + Kd_r*vel_err[2,0] + Ki_r*int_err[2,0]] 
+    ])
+
+    # Coordinate transform
+    R_RI = np.array([[ np.cos(X_I[2,0]), np.sin(X_I[2,0]), 0.0],
+                     [-np.sin(X_I[2,0]), np.cos(X_I[2,0]), 0.0],
+                     [             0.0,               0.0, 1.0]])
+    dX_R_des = R_RI @ dX_I_des
+    U = J @ dX_R_des
+    return U
+
+def run_simulation():
+    """Run simulation using Gymnasium environment with PID control to dynamic target"""
+    global int_err
+    
+    # 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)
+    observation, _ = env.reset()
+    
+    # Expand render bounds to show target position (default target is at [10,5])
+    env.set_render_bounds((-2, 12), (-2, 8))
+    
+    # Initialize integral error
+    int_err = np.zeros((3, 1))
+    
+    print("Starting Omnidirectional Robot PID Control Simulation")
+    print("Controller: PID control to dynamic target position")
+    print(f"Target position: [{observation[7]:.2f}, {observation[8]:.2f}, {observation[9]:.2f}]")
+    
+    for step in range(1000):
+        # Extract controller inputs from observation
+        # New observation format: [x, y, theta, dx, dy, dtheta, time, target_x, target_y, target_theta]
+        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]
+        target_position = observation[7:10]      # Target [target_x, target_y, target_theta]
+        
+        # Call PID controller with dynamic target
+        U = controller_omni_pid(time, X_I, dX_I, target_position)
+        
+        # Step environment
+        observation, reward, terminated, truncated, _ = env.step(U.flatten())
+        
+        # Render the environment
+        env.render()
+        
+        # Check if target reached
+        if terminated:
+            print(f"Target reached at step {step}! Reward: {reward:.2f}")
+            break
+        elif truncated:
+            print(f"Maximum steps reached at step {step}")
+            break
+    
+    input("Press Enter to close the simulation window...")
+    env.close()
+    print("Simulation completed")
+
+if __name__ == "__main__":
+    run_simulation()

Sol1_omni.py

@@ -0,0 +1,63 @@
+#!/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 = np.array([
+    [1/r*np.sin(a1+b1), -1/r*np.cos(a1+b1), -l1/r*np.cos(b1)],
+    [1/r*np.sin(a2+b2), -1/r*np.cos(a2+b2), -l2/r*np.cos(b2)],
+    [1/r*np.sin(a3+b3), -1/r*np.cos(a3+b3), -l3/r*np.cos(b3)]
+])
+F = np.linalg.inv(J)
+
+def controller_omni(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 = OmniRobotEnv(render_mode="human")
+    observation, _ = env.reset()
+    
+    print("Starting Omnidirectional 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_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()

Sol2_tank.py

@@ -0,0 +1,61 @@
+#!/usr/bin/env python
+
+import numpy as np
+from WheeledRobot import TankRobotEnv
+
+# TankRobot kinematics 
+b = 1
+r = 0.1
+
+# wheel equations
+J = np.array([
+    [+1/r, 0, -b/(2*r)],
+    [-1/r, 0, -b/(2*r)]
+])
+F = np.linalg.pinv(J)
+
+def controller_tank(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 = TankRobotEnv(render_mode="human")
+    observation, _ = env.reset()
+    
+    print("Starting Tank Robot Simulation")
+    print("Controller: Circular motion")
+    
+    for step in range(1000):
+        # 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()

Sol3_mecanum.py

@@ -0,0 +1,75 @@
+#!/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
+J11 = f1[1]/(r*d1[0]*f1[1]-r*d1[1]*f1[0])
+J21 = f2[1]/(r*d2[0]*f2[1]-r*d2[1]*f2[0])
+J31 = f3[1]/(r*d3[0]*f3[1]-r*d3[1]*f3[0])
+J41 = f4[1]/(r*d4[0]*f4[1]-r*d4[1]*f4[0])
+J12 = -f1[0]/(r*d1[0]*f1[1]-r*d1[1]*f1[0])
+J22 = -f2[0]/(r*d2[0]*f2[1]-r*d2[1]*f2[0])
+J32 = -f3[0]/(r*d3[0]*f3[1]-r*d3[1]*f3[0])
+J42 = -f4[0]/(r*d4[0]*f4[1]-r*d4[1]*f4[0])
+J13 = (-f1[1]*l1[1]-f1[0]*l1[0])/(r*d1[0]*f1[1]-r*d1[1]*f1[0])
+J23 = (-f2[1]*l2[1]-f2[0]*l2[0])/(r*d2[0]*f2[1]-r*d2[1]*f2[0])
+J33 = (-f3[1]*l3[1]-f3[0]*l3[0])/(r*d3[0]*f3[1]-r*d3[1]*f3[0])
+J43 = (-f4[1]*l4[1]-f4[0]*l4[0])/(r*d4[0]*f4[1]-r*d4[1]*f4[0])
+J = np.array([[J11, J12, J13], [J21, J22, J23], [J31, J32, J33], [J41, J42, J43]])
+
+# 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(1000):
+        # 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()

Sol4_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 = np.array([[1/r*dX_R[0,0]], [dX_R[0,0]/dX_R[2,0]]])
+    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(1000):
+        # 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()