Created
April 11, 2024 17:18
-
-
Save jeff-hykin/91e8d65ae046bfc7bc904e26db9569a2 to your computer and use it in GitHub Desktop.
do_mpc_example.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
import numpy | |
import numpy as np | |
import matplotlib.pyplot as plt | |
import math | |
from casadi import cos, sin | |
import do_mpc | |
def linear_steps(*, start, end, quantity): | |
""" | |
Example: | |
assert [4, 11, 18, 24, 31] == list(linear_steps(start=4, end=31, quantity=5)) | |
""" | |
import math | |
assert quantity > -1 | |
if quantity != 0: | |
quantity = math.ceil(quantity) | |
if start == end: | |
for each in range(quantity): | |
yield start | |
else: | |
x0 = 1 | |
x1 = quantity | |
y0 = start | |
y1 = end | |
interpolater = lambda x: y0 if (x1 - x0) == 0 else y0 + (y1 - y0) / (x1 - x0) * (x - x0) | |
for x in range(quantity-1): | |
yield interpolater(x+1) | |
yield end | |
# | |
# model | |
# | |
if True: | |
model_type = 'continuous' # either 'discrete' or 'continuous' | |
model = do_mpc.model.Model(model_type) | |
x_position = model.set_variable(var_type='_x', var_name='x_position', shape=(1,1)) | |
y_position = model.set_variable(var_type='_x', var_name='y_position', shape=(1,1)) | |
# Two states for the desired (set) motor position: | |
desired_total_velocity = model.set_variable(var_type='_u', var_name='desired_total_velocity', shape=(1,1)) | |
desired_absolute_angle = model.set_variable(var_type='_u', var_name='desired_absolute_angle', shape=(1,1)) | |
# Uncertainity | |
additive_velocity = model.set_variable(var_type='parameter', var_name='additive_velocity', shape=(1,1)) | |
additive_angle = model.set_variable(var_type='parameter', var_name='additive_angle', shape=(1,1)) | |
# link var with its deriviative | |
model.set_rhs('x_position', (desired_total_velocity+additive_velocity) * cos(desired_absolute_angle+additive_angle)) | |
model.set_rhs('y_position', (desired_total_velocity+additive_velocity) * sin(desired_absolute_angle+additive_angle)) | |
# model.set_rhs('actual_absolute_angle', y_velocity) | |
model.setup() | |
mpc = do_mpc.controller.MPC(model) | |
mpc.set_param( | |
n_horizon=10, | |
t_step=0.25, | |
n_robust=1, | |
store_full_solution=True, | |
) | |
# I don't fully understand this part, I believe it is minimizing these functions | |
# but I don't think it is the objective function that optimizes the unknowns | |
mpc.set_objective( | |
mterm=additive_velocity**2 + additive_angle**2, | |
lterm=additive_velocity**2 + additive_angle**2, | |
) | |
mpc.bounds['lower','_u', 'desired_total_velocity'] = 0 | |
mpc.bounds['upper','_u', 'desired_total_velocity'] = 100 | |
mpc.bounds['lower','_u', 'desired_absolute_angle'] = -math.radians(90) | |
mpc.bounds['upper','_u', 'desired_absolute_angle'] = math.radians(90) | |
# mpc.bounds['lower','_p_est', 'additive_velocity'] = -100 # AttributeError: 'MPC' object has no attribute '_p_est_lb' , 'parameter' also give error | |
# mpc.bounds['upper','_p_est', 'additive_velocity'] = 50 | |
# mpc.bounds['lower','_p_est', 'additive_angle'] = -math.pi | |
# mpc.bounds['upper','_p_est', 'additive_angle'] = math.pi | |
# provide Uncertainity possibilities | |
mpc.set_uncertainty_values( | |
additive_velocity=numpy.array(tuple( | |
linear_steps( | |
start=-100, | |
end=50, | |
quantity=8 | |
) | |
)), | |
additive_angle=numpy.array(tuple( | |
linear_steps( | |
start=-math.radians(90), | |
end=math.radians(90), | |
quantity=8 | |
) | |
)), | |
) | |
mpc.setup() | |
# | |
# simulator | |
# | |
if True: | |
simulator = do_mpc.simulator.Simulator(model) | |
simulator.settings.t_step = 0.25 | |
p_template = simulator.get_p_template() | |
def p_fun(t_now): | |
print(f'''t_now = {t_now}''') | |
return p_template | |
simulator.set_p_fun(p_fun) | |
simulator.setup() | |
x_position = 0 | |
y_position = 0 | |
inital_state = np.array([x_position, y_position,]) | |
simulator.x0 = inital_state | |
mpc.x0 = inital_state | |
# mpc.set_inital_guess() # AttributeError: 'MPC' object has no attribute 'set_inital_guess' | |
velocity = 2 | |
angle = math.radians(30) | |
new_x, new_y = simulator.make_step(numpy.array([[velocity],[angle],])) | |
mpc.set_initial_guess() | |
output = mpc.make_step(numpy.array([[velocity],[angle],])) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment