diff --git a/Sol7_omni_pid_pom.py b/Sol7_omni_pid_pom.py new file mode 100755 index 0000000..2f6fff8 --- /dev/null +++ b/Sol7_omni_pid_pom.py @@ -0,0 +1,105 @@ +#!/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_pom(t, X_I, dX_I, target_position): + """PID Controller with Point of Motion control and dynamic target position""" + global int_err + + # PID parameters + Kp_t = 4; Kp_r = 4 + Kd_t = 0.5; Kd_r = 0.5 + Ki_t = 0.2; Ki_r = 0.2 + + # 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]] + ]) + + # Add stopping condition when close to target + position_error = np.linalg.norm(pos_err[:2]) + orientation_error = abs(pos_err[2,0]) + if position_error < 0.15 and orientation_error < 0.08: + dX_I_des = np.array([[0], [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 PID POM 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 Point of Motion Control Simulation") + print("Controller: PID with Point of Motion 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 POM controller with dynamic target + U = controller_omni_pid_pom(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() diff --git a/Sol8_tank_linear.py b/Sol8_tank_linear.py new file mode 100755 index 0000000..9c853c6 --- /dev/null +++ b/Sol8_tank_linear.py @@ -0,0 +1,91 @@ +#!/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_linear(t, X_I, dX_I, target_position): + """Enhanced linear control with polar coordinates and dynamic target""" + + # Use target position from parameters instead of hardcoded values + X_I_des = target_position.reshape(-1, 1) + pos_err = X_I_des - X_I + + # Polar coordinates + rho = np.sqrt((pos_err[0,0])**2+(pos_err[1,0])**2) + alpha = -X_I[2,0] + np.arctan2((pos_err[1,0]), (pos_err[0,0])) + beta = -X_I[2,0]-alpha + + # Linear control + k_rho = 0.3; k_alpha = 0.8; k_beta = -0.15 + dX_R_des = np.array([[k_rho*rho], [0], [k_alpha*alpha + k_beta*beta]]) + + # Enhanced linear control + # dX_R_fix = 3 + # dtheta_R_des = dX_R_des[2,0] * dX_R_fix/dX_R_des[0,0] + # dX_R_des = np.array([[dX_R_fix], [0], [dtheta_R_des]]) + + # Stopping condition + if rho < 0.1: + dX_R_des = np.array([[0], [0], [0]]) + + # Calculate control input + U = J @ dX_R_des + return U + +def run_simulation(): + """Run simulation using Gymnasium environment with enhanced linear control to dynamic target""" + + # Initialize environment with fixed target for reproducible results + # You can set random_target=True for random target generation + env = TankRobotEnv(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 Tank Robot Enhanced Linear Control Simulation") + print("Controller: Enhanced linear control with polar coordinates to dynamic target") + 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 enhanced linear controller with dynamic target + U = controller_tank_linear(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() diff --git a/Sol9_tank_l1.py b/Sol9_tank_l1.py new file mode 100755 index 0000000..ec075ea --- /dev/null +++ b/Sol9_tank_l1.py @@ -0,0 +1,100 @@ +#!/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_l1(t, X_I, dX_I, target_position): + """L1 adaptive control with path planning and dynamic target""" + + # Controller parameters + dx_R_max = 3 + L0 = 4 + L1 = 4 + + # Points & vectors + A = np.array([[X_I[0,0]],[X_I[1,0]]]) + B = target_position[:2].reshape(-1, 1) # Use dynamic target (x, y only) + # d = np.array([[1],[1]]) + # d = np.array([[np.cos(X_I[2,0])],[np.sin(X_I[2,0])]]) + d = np.array([[-1],[0]]) + d = d / np.linalg.norm(d) # normalize + C = (np.transpose(A-B) @ d) * d + B + + # L0 controller + D = C + L0 * d + + # L1 controller + # D = C + np.sqrt( L1**2 - np.linalg.norm(C-A)**2 ) * d + + # Turning radius + eta = np.arctan2( (D-A)[1,0], (D-A)[0,0] ) - X_I[2,0] + omega = 2*np.sin(eta)*dx_R_max/np.linalg.norm(D-A) + dX_R_des = np.array([[dx_R_max], [0], [omega]]) + + # Stopping condition + if np.linalg.norm(A-B) < 0.2: + dX_R_des = np.array([[0], [0], [0]]) + + # Calculate control input + U = J @ dX_R_des + return U + +def run_simulation(): + """Run simulation using Gymnasium environment with L1 adaptive control to dynamic target""" + + # Initialize environment with fixed target for reproducible results + # You can set random_target=True for random target generation + env = TankRobotEnv(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 Tank Robot L1 Adaptive Control Simulation") + print("Controller: L1 adaptive control with path planning to dynamic target") + print(f"Target position: [{observation[7]:.2f}, {observation[8]:.2f}, {observation[9]:.2f}]") + + for step in range(10000): + # 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 L1 adaptive controller with dynamic target + U = controller_tank_l1(time, X_I, dX_I, target_position) + + print(X_I) + + # 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()