Unable to change my initial position x0, after setting the h func

User Questions
1

Hello, Acados Team,

I encountered an issue: after setting the h function, I am unable to change my initial position x0. Even when I use:

solver.constraints_set(0, 'ubx', initial_state)
solver.constraints_set(0, 'lbx', initial_state)

It does not modify the initial x0 that was initially set in the OCP.

Below is my code:

N = 20
dt = 0.1 

nx_current = 7  #  x, y, vx, vy, ax, ay, yaw
nx_pre = 3      #  x_pre, y_pre, yaw_pre
nx = nx_current + nx_pre  # 10

nu = 3   #  jerk_x, jerk_y, w

ny = nx_current + nu # ：x, y, vx, vy, ax, ay, yaw, jerk_x, jerk_y, w
ny_e = nx_current

nh = 6

def define_track_model():
    x = SX.sym('x')         
    y = SX.sym('y')       
    vx = SX.sym('vx')      
    vy = SX.sym('vy')      
    ax = SX.sym('ax')      
    ay = SX.sym('ay')      
    yaw = SX.sym('yaw')    

    x_pre = SX.sym('x_pre')  
    y_pre = SX.sym('y_pre')
    yaw_pre = SX.sym('yaw_pre')
    
    jerk_x = SX.sym('jerk_x')  # 
    jerk_y = SX.sym('jerk_y')  # 
    w = SX.sym('w')            # 

    x_aug = vertcat(x, y, vx, vy, ax, ay, yaw, x_pre, y_pre, yaw_pre)
    x_next = x + vx * dt + 0.5 * ax * dt**2
    y_next = y + vy * dt + 0.5 * ay * dt**2
    vx_next = vx + ax * dt
    vy_next = vy + ay * dt
    ax_next = ax + jerk_x * dt
    ay_next = ay + jerk_y * dt
    yaw_next = yaw + w * dt
    # 
    f_discrete = vertcat(
        x_next,
        y_next,
        vx_next,
        vy_next,
        ax_next,
        ay_next,
        yaw_next,
        x,
        y,
        yaw
    )
    model = AcadosModel()
    model.name = 'augmented_yaw_model'
    model.x = x_aug  # 
    model.u = vertcat(jerk_x, jerk_y, w)   
    model.disc_dyn_expr = f_discrete   

    d_min = SX.sym('d_min')
    dx = SX.sqrt((x - x_pre)**2 + (y - y_pre)**2)
    con_h_expr = vertcat(
        d_min - dx
    )
    model.con_h_expr = con_h_expr
    model.con_h_expr_e = con_h_expr
    model.p = vertcat(d_min)

    return model
def solver_model():
    model = define_track_model()
    ocp = AcadosOcp()
    ocp.model = model
    T_f = N * dt  
    ocp.solver_options.N_horizon = N
    ocp.solver_options.tf = T_f
    ocp.solver_options.integrator_type = 'DISCRETE'  #

    '''cost function'''
    # cost function
    ocp.cost.cost_type = 'LINEAR_LS'
    ocp.cost.cost_type_e = 'LINEAR_LS'
    ''' Vx Vu'''
    Vx = np.zeros((ny, nx))
    for i in range(nx_current) :
        Vx[i, i] = 1
    ocp.cost.Vx = Vx  # 
    # Vx_e = np.zeros((nx_current, nx_current))
    Vx_e = np.zeros((nx_current, nx))
    for i in range(nx_current):
        Vx_e[i, i] = 1 
    ocp.cost.Vx_e = Vx_e

    Vu = np.zeros((ny, nu))
    for i in range(nu) :
        Vu[i, i] = 1
    ocp.cost.Vu = Vu
    ''' Q R W '''
    Q = 1e2 * np.eye(nx_current)
    Q[0, 0] = 1e3  # x
    Q[1, 1] = 1e3  # y
    Q[2, 2] = 1e1  # vx
    Q[3, 3] = 1e1  # vy
    Q[4, 4] = 1e1  # ax
    Q[5, 5] = 1e1  # ay
    Q[6, 6] = 1e1  # yaw
    R = 1e1 * np.eye(nu)
    R[2, 2] = 1e0 
    W = np.block([
        [Q, np.zeros((nx_current, nu))],
        [np.zeros((nu, nx_current)), R]
    ])
    ocp.cost.W = W    
    ocp.cost.W_e = Q  
    ocp.cost.yref = np.zeros((nx_current + nu,))  # [x_ref, y_ref, vx_ref, vy_ref, ax_ref, ay_ref, yaw_ref, u_ref_x, u_ref_y]
    ocp.cost.yref_e = np.zeros((nx_current,))  
    '''constraints'''
    x0 = np.zeros((nx,))
    ocp.constraints.x0 = x0
    jerk_max = 5.0
    w_max = 5
    ocp.constraints.lbu = np.array([-jerk_max, -jerk_max, -w_max]) 
    ocp.constraints.ubu = np.array([jerk_max, jerk_max, w_max])  
    ocp.constraints.idxbu = np.arange(nu)

    default_p = np.array([1.0])
    ocp.parameter_values = default_p
    nh = 1
    ocp.constraints.lh = np.zeros(nh) 
    ocp.constraints.uh = 1e9 * np.ones(nh)
    ocp.constraints.lh_e = np.zeros(nh)
    ocp.constraints.uh_e = 1e9 * np.ones(nh)

    solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json')
    return solver


if __name__ == "__main__":
    initial_state = np.zeros((nx,))
    initial_state[0] = 10.0    
    initial_state[1] = 0.0     
    initial_state[2] = 0.0      
    initial_state[3] = np.pi   
    initial_state[4] = -np.pi**2 / 10.0  
    initial_state[5] = 0.0      
    initial_state[6] = np.pi / 2    
    initial_state[7] = 10.0     
    initial_state[8] = 0.0     
    initial_state[9] = np.pi / 2 

    solver = solver_model()
    solver.constraints_set(0, 'ubx', initial_state)
    solver.constraints_set(0, 'lbx', initial_state)

    ref_traj = define_ref_traj(N, dt=dt)
    for i in range(N) :
        yref =  np.hstack((ref_traj[i], np.zeros(nu)))
        solver.cost_set(i, 'yref', yref)
    solver.cost_set(N, 'yref', ref_traj[-1])

    status = solver.solve()
        # solver.
    if status != 0:
        print("false", status)
    else:
        print("true", status)
    solver.print_statistics()

    last_x = []
    for i in range(N+1):
        xi = solver.get(i, 'x')
        last_x.append(xi)
    last_x = np.array(last_x)
    last_u = []
    for i in range(N):
        ui = solver.get(i, 'u')
        last_u.append(ui)
    last_u = np.array(last_u)
    x_traj = last_x[:, 0]
    y_traj = last_x[:, 1]
    yaw_traj = last_x[:, 6]
    plot_traj_all([x_traj, y_traj, yaw_traj], ref_traj, alpha_value=0.4, FOV_length_value=5.0, N=N)
2

Implementation looks good at first glance. Please use ACADOS_INFTY for uh and uh_e to get one-sided constraints.

What solution do you get? Is the status 0? What is the output of print_statistics?

3

Hello, Kaethe,

Thank you for your suggestion! I’ve now reached state 4. After some trial and error, I found that adding the following code:

for i in range(N + 1):
    solver.set(i, 'x', initial_state)

allowed me to set the initial state. From what I deduce, it seems that the solver failure was caused by the initial state. In Acados, does setting ocp.constraints.x0 apply this value to all time steps? If so, that would explain the issue. When I defined the ocp model, I set the other time steps to the original state, and in the solver, I only changed ubx and lbx, which led to a poor initial state for the entire problem. My h function requires a better initial state to work properly.

4

The initial state constraint is only guaranteed to be satisfied if the problem is solved successfully, i.e. status 0.

The following initializes the x trajectory at the initial state which is typically a good idea if you have no initial guess available.

With this initial guess do you get status 0?