seureteric/lawarob-wheeled
1
0
Fork 0
Code Pull Requests Activity

Solutions 7, 8 and 9

Browse Source
This commit is contained in:
Eric Seuret 2025-10-28 06:37:27 +01:00
parent 5eef695e83
commit bed3444e19
3 changed files with 296 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
Sol7_omni_pid_pom.py Executable file
View File

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

91
Sol8_tank_linear.py Executable file
View File

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

100
Sol9_tank_l1.py Executable file
View File

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