Getting the value of stage_cost and terminal_cost

[aabsz]

Hi,

Would you please let me know how I can get the value of stage_cost and terminal_cost at each time step of the simulation?

[brunomorampc]

Hi @aabsz,

unfortunately there's no direct way how to get that value, but what you can do is, since you know how these terms looks like, you can create a function that reproduce them and evaluate these functions at every time step

[aabsz]

As always, thanks for the prompt reply, Bruno.

Indeed, I wanted to do that. The issue is, that I only have access to the state variables and control actions at each time step. I lack the information across the prediction horizon, making it impossible to determine the precise value of the cost function at each step.

Am I wrong?

[brunomorampc]

You're right, but there's a way to extract that information (not documented so I can help you with that). Let me prepare a script for you, I'll post it here soon

[brunomorampc]

@aabsz can you tell me what type of stage and terminal cost do you have? are you using quadratic_stage_cost and quadratic_terminal_cost?

[brunomorampc]

Hi @aabsz

here's a simple example when using quadratic_stage_cost and quadratic_terminal_cost

from hilo_mpc import NMPC, Model
import casadi as ca
import numpy as np
model = Model(plot_backend='bokeh')
# Constants
M = 5.
m = 1.
l = 1.
h = .5
g = 9.81

# States and algebraic variables
x = model.set_dynamical_states(['x', 'v', 'theta', 'omega'])
model.set_measurements(['yx', 'yv', 'ytheta', 'tomega'])
model.set_measurement_equations([x[0], x[1], x[2], x[3]])
# y = model.set_algebraic_variables(['y'])
v = x[1]
theta = x[2]
omega = x[3]
# Inputs
F = model.set_inputs('F')

# ODE
dx = v
dv = 1. / (M + m - m * ca.cos(theta)) * (m * g * ca.sin(theta) - m * l * ca.sin(theta) * omega ** 2 + F)
dtheta = omega
domega = 1. / l * (dv * ca.cos(theta) + g * ca.sin(theta))

model.set_equations(ode=[dx, dv, dtheta, domega])

# Initial conditions
x0 = [2.5, 0., 0.1, 0.]
z0 = ca.sqrt(3.) / 2.
u0 = 0.

# Create model and run simulation
dt = .1
model.setup(dt=dt)

nmpc = NMPC(model)
nmpc.quad_stage_cost.add_states(names=['v', 'theta'], ref=[0, 0], weights=[10, 5])
nmpc.quad_stage_cost.add_inputs(names='F', weights=0.1)
nmpc.horizon = 25
nmpc.set_box_constraints(x_ub=[5, 10, 10, 10], x_lb=[-5, -10, -10, -10])
nmpc.set_initial_guess(x_guess=x0, u_guess=u0)
nmpc.setup()

n_steps = 10
model.set_initial_conditions(x0=x0)
sol =model.solution

stage_cost_fun = ca.Function('stage_cost',[model.x, model.u], [nmpc.quad_stage_cost.cost])
terminal_cost_fun = ca.Function('terminal_cost',[model.x, model.u], [nmpc.quad_terminal_cost.cost])
stage_cost_values = []
terminal_cost_values = []
for step in range(n_steps):
    u = nmpc.optimize(x0)
    model.simulate(u=u)
    x0 = sol['x:f']
    ## Compute cost values
    stage_cost = 0
    for i in range(nmpc.prediction_horizon):
        # Compute the function of the stage cost at every time of the prediciton horizon
        stage_cost += stage_cost_fun(nmpc._nlp_solution['x'][nmpc._x_ind[i]],nmpc._nlp_solution['x'][nmpc._u_ind[i]])
    stage_cost_values.append(float(stage_cost)) # Append the value to the list
    # Compute the terminal cost
    terminal_cost = terminal_cost_fun(nmpc._nlp_solution['x'][nmpc._x_ind[-1]],nmpc._nlp_solution['x'][nmpc._u_ind[-1]])
    terminal_cost_values.append(float(terminal_cost))


import matplotlib.pyplot as plt

plt.figure()

plt.plot(np.arange(0,n_steps), stage_cost_values, label='Stage cost')

# Plotting the second line
plt.plot(np.arange(0,n_steps), terminal_cost_values, label='Terminal cost')

# Adding a legend to the plot
plt.legend()

# Displaying the plot
plt.show()

[aabsz]

Hello Bruno,

Thank you for the script and your time and assistance.

I've used all three types of cost functions in my code - stage_cost, quadratic_stage_cost, and quadratic_terminal_cost.

I'll attempt to implement the generic stage cost using your script too. Given the complexity of my NMPC framework with various variable parameters, I'm uncertain about its direct applicability to my case. I'll do my best to test it, although my time is limited as I'm currently working on a paper due tomorrow.