Skip to content

Instantly share code, notes, and snippets.

@jeff-hykin
Created April 11, 2024 17:18
Show Gist options
  • Save jeff-hykin/91e8d65ae046bfc7bc904e26db9569a2 to your computer and use it in GitHub Desktop.
Save jeff-hykin/91e8d65ae046bfc7bc904e26db9569a2 to your computer and use it in GitHub Desktop.
do_mpc_example.py
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