Hi
In AS-RTI-acados/pendulum_benchmark.py at 4349509154b127b4c36ef95ebef839351e494692 路 FreyJo/AS-RTI-acados 路 GitHub , may I ask what terminal_cost_state is here? And why does cost_total need to add terminal_cost_state item?
def add_total_cost_to_results(results):
x_end = results['x_traj'][-1]
terminal_cost_state = x_end[-1]
x_og_vec = np.expand_dims(x_end[:dummy_model_params.nx_original], axis=1)
terminal_cost_term = (x_og_vec.T @ dummy_mpc_params.P @ x_og_vec)[0][0]
# intermediate terminal cost
# i_mid = int(results['x_traj'].shape[0]/2)
# x_mid = results['x_traj'][i_mid]
# x_og_vec = np.expand_dims(x_mid[:dummy_model_params.nx_original], axis=1)
# intermediate_terminal_cost_term = (x_og_vec.T @ dummy_mpc_params.P @ x_og_vec)[0][0]
results['cost_total'] = terminal_cost_state + terminal_cost_term
return results
Looking forward to your response锛乀hank you!
FreyJo
February 21, 2025, 3:34pm
2
Hi,terminal_cost_state here.terminal_cost_term, which is the terminal cost of the OCP evaluated at the final state of the simulation.
Hi锛學hat is the mathematical formula for a cost state? (may be the 0 step to N-1 cost, like
FreyJo
February 22, 2025, 11:07am
4
It is implemented here:
model.x = ca.vertcat(model.x, t)
model.xdot = ca.vertcat(model.xdot, tdot)
model.f_expl_expr = ca.vertcat(model.f_expl_expr, 1)
model.f_impl_expr = model.f_expl_expr - model.xdot
model.clock_state = t
return model
def augment_model_with_cost_state(model: AcadosModel, params: ModelParameters, mpc_params: MpcParameters):
cost_state = ca.SX.sym('cost_state')
cost_state_dot = ca.SX.sym('cost_state_dot')
x_ref = ca.SX.sym('x_ref', params.nx_original)
u_ref = ca.SX.sym('u_ref', params.nu_original)
xdiff = x_ref - model.x[:params.nx_original]
udiff = u_ref - model.u[:params.nu_original]
cost_state_dyn = .5 * (xdiff.T @ mpc_params.Q @xdiff + udiff.T @mpc_params.R @udiff)
In addition to the quadratic cost, we also integrate the cost corresponding to the (soft) constraint violation.
