Mismatching dimension for field "yref" for terminal node

Hello!
Being this my first acados post, pardon me for any mistakes. I am using python interface to create an mpc problem. I have a question mainly regarding the acados formulation. I want to achieve an optimal pushing force for an object on the ground to move to a certain distance defined by a reference positions given state and control constraints. I have defined cost functions separately for yref and yref_e as ocp.cost.yref = np.zeros((ny,)) and ocp.cost.yref_e = np.zeros((ny_e,)) respectively.

I am facing problem when I am setting the last node as yref_e as below:

for i in range(N):
        if i<N-1:
            # yref = p_ref[i]
            # Update the OCP cost function's reference for the current time step
            ocp_solver.set(i, "yref", p_ref[i,:])
            # print(i)
        else:
            yref_e = p_ref[i,:][:4]
            ocp_solver.set(i, "yref", yref_e)
            print('finally made it!')

But I getting the error as: Exception: AcadosOcpSolver.set(): mismatching dimension for field "yref" with dimension 6 (you have 4). I saw several examples but still not able to figure out the problem. I see that other examples also have the dimension mismatch. I have attached the code below. Any help will be appreciated. Thanks!

code:

from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
from casadi import SX, vertcat, sin, cos, Function
import casadi as ca
import numpy as np
import pandas as pd
import scipy.linalg

def peg_model() -> AcadosModel:

    model_name = 'sliding_peg'

    #Parameters
    m_peg = SX.sym('m_peg')

    p = vertcat(m_peg) #Parameter Vector

    # constants
    m_peg = 5 # mass of the cart [kg]
    g = np.array([0,0,-9.81]) # gravity constant [m/s^2]

    # Define state & control variables
    x = SX.sym('x', 2,1)  # Position of COM [x, y, z] (3,1)
    v = SX.sym('v', 2,1)  # Velocity of COM [vx, vy, vz] (3,1)

    X = vertcat(x, v)

    #Force
    F = SX.sym('F',2,1)
    u = vertcat(F)

    # xdot
    x_dot = SX.sym('xdot',2,1)
    # x_dot = v
    v_dot = SX.sym('vdot',2,1)
    # v_dot = F/m_peg  # Acceleration is just gravity
    # print(xdot)

    xdot = vertcat(x_dot, v_dot)

    # Define the explicit ODE
    f_expl = vertcat(v, F/m_peg) #qdot & wdot is excluded
    f_impl = xdot - f_expl

    #set paramters
    #reference trajectories
    # p_ref = SX.sym('p_ref', 4, 1)  # Position reference (2D in this case)

    # Create the Acados model
    model = AcadosModel()
    model.f_impl_expr = f_impl
    model.f_expl_expr = f_expl
    model.x = X
    model.xdot = xdot
    model.u = u
    model.p = p
    # print(model.x)
    model.name = model_name

    return model

def main():
    # max_values, min_values, ref_p, ref_v, initial_state = ref_traj()
    # print(min_values)
    # create ocp object to formulate the OCP
    ocp = AcadosOcp() #The ocp object contains the full problem setup: model equations, cost functions, constraints, time horizon, etc.
    model = peg_model()
    ocp.model = model

    nx = model.x.rows() #4 states
    print(f'np. of states: {nx}')
    nu = model.u.rows() #2 controls
    print(f'np. of controls: {nu}')
    ny = nx + nu #6
    ny_e = nx

    m_peg = 5

    ocp.parameter_values = np.array([m_peg]) #expects numpy array


    Tf = 1 #horizon for the entire optimisation: here the plot is only for optimal sol for one horizon
    N = 10

    ocp.cost.cost_type = "LINEAR_LS"
    ocp.cost.cost_type_e = "LINEAR_LS"

    # N = 10#len(p_ref) #dt
    # Fmax = np.array([0,50]) #N

    #set cost
    Q_pos = 10*np.diag(np.ones((2))) #weights on the psoition error
    Q_vel = 0.1*np.diag(np.ones((2))) #weights on the velocity error
    Q = scipy.linalg.block_diag(Q_pos, Q_vel)
    R = np.zeros((2,2)) #weights on the control input

    ocp.cost.W_e = Q
    ocp.cost.W = scipy.linalg.block_diag(Q, R)

    # Set the weighting matrix for state tracking
    Vx = np.zeros((ny, nx))  #tracking 2D position-->(6,4)
    Vx[0, 0] = 1  # Track the x-position
    Vx[1, 1] = 1  # Track the y-position
    Vx[2, 2] = 1  # Track the x-velocity
    Vx[3, 3] = 1  # Track the y-velocity
    
    ocp.cost.Vx = Vx

    ocp.cost.Vx_e = np.eye(ny_e)

    Vu = np.zeros((ny,nu))   # (6,2)
    Vu[0, 0] = 1  # Track the x-position
    Vu[1, 1] = 1  # Track the y-position
  

    ocp.cost.Vu = Vu  # Assign it to the cost function

    ocp.cost.yref = np.zeros((ny,))
    ocp.cost.yref_e = np.zeros((ny_e,))

    # y_ref = model.p
    p_ref = np.array([[0,0,0,0,0,0],[0,0.01,0,0,0,0],[0,0.02,0,0,0,0],[0,0.03,0,0,0,0],
                      [0,0.04,0,0,0,0],[0,0.05,0,0,0,0],[0,0.06,0,0,0,0],[0,0.07,0,0,0,0],
                      [0,0.08,0,0,0,0],[0,0.09,0,0,0,0]])

    # set constraints
    terminal_state = np.array([0.0, 0.09, 0,0])
    ocp.constraints.x0 = np.array([0,0,0,0]) #bounds on the initial state vector
    ocp.constraints.lbx_e = terminal_state #lower bounds on x (states) at terminal shooting node N
    ocp.constraints.ubx_e = terminal_state #lower bounds on x (states) at terminal shooting node N
    ocp.constraints.idxbx_e = np.arange(ny_e) #indices in x vector which needed to be bounded at terminal shooting node N

    ocp.constraints.lbu = np.array([0,0]) #lower bound on control input
    ocp.constraints.ubu = np.array([0,50]) #upper bound on control input
    ocp.constraints.idxbu = np.arange(nu) #indices in x vector which needed to be bounded at terminal shooting node N

    # set options
    ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # 'FULL_CONDENSING_QPOASES', 'PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_HPIPM'
    ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # 'GAUSS_NEWTON', 'EXACT'
    ocp.solver_options.integrator_type = 'ERK'
    ocp.solver_options.print_level = 1
    ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP, DDP
    ocp.solver_options.nlp_solver_max_iter = 1000
    ocp.solver_options.globalization = 'MERIT_BACKTRACKING'
    ocp.solver_options.with_adaptive_levenberg_marquardt = True
    ocp.solver_options.tf = Tf
    ocp.solver_options.N_horizon = N #timesteps for a predcited horizon
    # NOTE: this is needed for PARTIAL_CONDENSING_HPIPM to get expected behavior
    qp_tol = 5e-7
    ocp.solver_options.qp_solver_tol_stat = qp_tol
    ocp.solver_options.qp_solver_tol_eq = qp_tol
    ocp.solver_options.qp_solver_tol_ineq = qp_tol
    ocp.solver_options.qp_solver_tol_comp = qp_tol


    # ocp.translate_to_feasibility_problem()

    ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') #This file contains settings such as solver options, numerical tolerances, and possibly specific configuration data for the solver backend (Acados).
    
    for i in range(N):
        if i<N-1:
            # yref = p_ref[i]
            # Update the OCP cost function's reference for the current time step
            ocp_solver.set(i, "yref", p_ref[i,:])
            # print(i)
        else:
            yref_e = p_ref[i,:][:4]
            ocp_solver.set(i, "yref", yref_e)
            print('finally made it!')

    status = ocp_solver.solve() #Solve the ocp with current input.

    # ocp_solver.print_statistics()
    ocp_solver.get_status()

    if status != 0:
        raise Exception(f'acados returned status {status}.')


if __name__ == '__main__':
    main()

Hi,

if the prediction horizon is set to N there will be N+1 nodes in total (the initial one is included).
To set values at the last node, you should try ocp_solver.set(N, "yref", yref_e), which doesn’t happen in your code since the loop only goes to N-1.

An example can be found here.

Hope this helps!

Kind regards,
Josip

1 Like

Thanks @josipkh, yes, I will change that.