do_mpc_example.py

gistfile1.txt

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],]))