External Cost Module: How to Set The Reference?

eBunny · February 28, 2022, 3:44am

Hi

I am trying to build a model predictive controller using the python interface. I use the external cost module but I have trouble setting the reference trajectory.

To be specific, I would like to use the set method provided by acados_ocp_solver.py, but I am not sure to set yref or y_ref. What is the difference? Is there any more information about using the external cost module?

I see in the code that it is recommended to use cost_set, in which only yref is the available field. But how to use yref in the “external cost” way? I don’t pick up any instruction or documentation.

Any kind help is sincerely appreciated. Thanks.

Cost formulation pasted below:

ocp.model.cost_expr_ext_cost = e(x, y_ref).T @ Q @ e(x, y_ref) + u.T @ R @ u
ocp.model.cost_expr_ext_cost_e = e(x, y_ref).T @ Q_e @ e(x, y_ref)
# https://docs.acados.org/python_interface/index.html#acados_template.acados_ocp.AcadosOcpCost
ocp.cost.cost_type = 'EXTERNAL'
ocp.cost.cost_type_e = 'EXTERNAL'

where Q, e, and R is defined as:


def e(x, y_ref):

    p_ref = y_ref[0:3]
    q_ref = y_ref[3:7]
    v_ref = y_ref[7:10]
    w_ref = y_ref[10:13]

    p_err = x[0:3] - p_ref
    q_err = quat_err(x[3:7], q_ref)
    v_err = x[7:10] - v_ref
    w_err = x[10:13] - w_ref
    f = x[13:16]
    t = x[16:19]
    
    return vertcat(p_err, q_err, v_err, w_err, f, t)


# cost weighting matrices
Q_p = SX([1, 1, 1])
Q_q = SX([1, 1, 1])
Q_v = SX([1, 1, 1])
Q_w = SX([1, 1, 1])
Q_f = SX([1, 1, 1])
Q_t = SX([1, 1, 1])
Q = diag(vertcat(Q_p, Q_q, Q_v, Q_w, Q_f, Q_t))

R_f_dot = SX([1, 1, 1]) # small R -> aggressive moves, large R -> large error
R_t_dot = SX([1, 1, 1])
R = diag(vertcat(R_f_dot, R_t_dot))

Q_p_e = SX([1, 1, 1])
Q_q_e = SX([1, 1, 1])
Q_v_e = SX([1, 1, 1])
Q_w_e = SX([1, 1, 1])
Q_f_e = SX([1, 1, 1])
Q_t_e = SX([1, 1, 1])
Q_e = diag(vertcat(Q_p_e, Q_q_e, Q_v_e, Q_w_e, Q_f_e, Q_t_e))

# reference trajectory, with y_ref being the interface
# set to all 'zero' for now, can be set later when using the solver
# y_ref = [p_ref, q_ref, v_ref, w_ref]
y_ref = np.array([0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0])

Full code (WIP) is pasted here.

from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver, AcadosSimSolver
from casadi import SX, vertcat, Function, dot, cross, norm_2, inv, diag, sqrt
import numpy as np
from os import environ, system, removedirs, path
import shutil

def sx_quat_multiply(q, p):

    s1 = q[0]
    v1 = q[1:4]
    s2 = p[0]
    v2 = p[1:4]
    s = s1 * s2 - dot(v1, v2)
    v = s1 * v2 + s2 * v1 + cross(v1, v2)
    return vertcat(s, v)


def sx_quat_inverse(q):

    return SX([1, -1, -1, -1]) * q / (norm_2(q)**2)


def quat_derivative(q, w):

    return sx_quat_multiply(q, vertcat(SX(0), w)) / 2


def rotate_vec3(v, q):

    p = vertcat(SX(0), v)
    p_rotated = sx_quat_multiply(sx_quat_multiply(q, p), sx_quat_inverse(q))
    return p_rotated[1:4]


def quat_err(q, p):

    # assume q = q_delta * p, q_delta = [w, q_err] = q * p^(-1)
    q_d = sx_quat_multiply(q, sx_quat_inverse(p))
    return q_d[1:4]


def e(x, y_ref):

    p_ref = y_ref[0:3]
    q_ref = y_ref[3:7]
    v_ref = y_ref[7:10]
    w_ref = y_ref[10:13]

    p_err = x[0:3] - p_ref
    q_err = quat_err(x[3:7], q_ref)
    v_err = x[7:10] - v_ref
    w_err = x[10:13] - w_ref
    f = x[13:16]
    t = x[16:19]
    
    return vertcat(p_err, q_err, v_err, w_err, f, t)


def h(x):

    # fields in the state variable
    v = x[7:10]
    w = x[10:13]
    f = x[13:16]
    t = x[16:19]
    
    # allocation: y = pinv(B) * wrench
    # y = [sin(a1)f1, cos(a1)f1, ... sin(a6)f6, cos(a6)f6]
    # wrench = [f, t]
    # pinv(B) see matlab script allocation.m, with torque = thrust * 0.06
    B_inv = SX([
        [-1/3,            0     ,         0  ,          -25/149,          0     ,       425/894],
        [ 0  ,            0     ,        -1/6,         -425/447,          0     ,       -25/298],
        [ 1/3,            0     ,         0  ,          -25/149,          0     ,       425/894],
        [ 0  ,            0     ,        -1/6,          425/447,          0     ,        25/298],
        [ 1/6,          390/1351,         0  ,           25/298,        695/4783,       425/894],
        [ 0  ,            0     ,        -1/6,          425/894,        760/923 ,       -25/298],
        [-1/6,         -390/1351,         0  ,           25/298,        695/4783,       425/894],
        [ 0  ,            0     ,        -1/6,         -425/894,       -760/923 ,        25/298],
        [-1/6,          390/1351,         0  ,           25/298,       -695/4783,       425/894],
        [ 0  ,            0     ,        -1/6,         -425/894,        760/923 ,        25/298],
        [ 1/6,         -390/1351,         0  ,           25/298,       -695/4783,       425/894],
        [ 0  ,            0     ,        -1/6,          425/894,       -760/923 ,       -25/298]
    ])
    y = B_inv @ vertcat(f, t)
    f_unit_actuator = vertcat(
        sqrt(y[0]**2 + y[1]**2),
        sqrt(y[2]**2 + y[3]**2),
        sqrt(y[4]**2 + y[5]**2),
        sqrt(y[6]**2 + y[7]**2),
        sqrt(y[8]**2 + y[9]**2),
        sqrt(y[10]**2 + y[11]**2)
    )

    # h = [v, w, f_unit_actuator]
    return vertcat(v, w, f_unit_actuator)
    

def get_omnihex_ode_model():

    # nomial value of parameters
    m = 4.0
    g = SX([0, 0, 9.8])
    com = SX([0, 0, 0.03]) # center of mass position in body FRD frame
    J_xx = 0.08401764152
    J_xy = -0.00000445135
    J_xz = 0.00014163105
    J_yy = 0.08169689992
    J_yz = -0.00000936372
    J_zz = 0.14273598018
    J = SX([
        [J_xx, J_xy, J_xz],
        [J_xy, J_yy, J_yz],
        [J_xz, J_yz, J_zz]
    ])
    J_inv = inv(J)

    # states and controls
    p = SX.sym('p', 3) # position in world NED frame (m)
    q = SX.sym('q', 4) # attitude expressed in quaternion
    v = SX.sym('v', 3) # velocity in body frame (m/s)
    w = SX.sym('w', 3) # angular velocity in body FRD frame (rad/s)
    f = SX.sym('f', 3) # actuator force in body frame (N)
    t = SX.sym('t', 3) # actuator torque in body frame (Nm)

    p_dot = SX.sym('p_dot', 3)
    q_dot = SX.sym('q_dot', 4)
    v_dot = SX.sym('v_dot', 3)
    w_dot = SX.sym('w_dot', 3)
    f_dot_x = SX.sym('f_dot_x', 3)
    t_dot_x = SX.sym('t_dot_x', 3)
    f_dot_u = SX.sym('f_dot_u', 3)
    t_dot_u = SX.sym('t_dot_u', 3)

    x = vertcat(p, q, v, w, f, t)
    u = vertcat(f_dot_u, t_dot_u)

    # dynamics
    x_dot = vertcat(p_dot, q_dot, v_dot, w_dot, f_dot_x, t_dot_x)
    f_expl = vertcat(
        rotate_vec3(v, q),
        quat_derivative(q, w),
        f / m + rotate_vec3(g, sx_quat_inverse(q)) - cross(w, v),
        J_inv @ (t + cross(com, f) - cross(w, J @ w)),
        f_dot_u,
        t_dot_u
    )
    
    f_impl = x_dot - f_expl

    # acados model
    model = AcadosModel()
    model.f_expl_expr = f_expl
    model.f_impl_expr = f_impl
    model.x = x
    model.xdot = x_dot
    model.u = u
    model.name = 'omnihex_ode'
    return model


def formulate_omnihex_ocp():

    # prediction horizon and dt
    N = 20
    Tf = 1.0

    # cost weighting matrices
    Q_p = SX([1, 1, 1])
    Q_q = SX([1, 1, 1])
    Q_v = SX([1, 1, 1])
    Q_w = SX([1, 1, 1])
    Q_f = SX([1, 1, 1])
    Q_t = SX([1, 1, 1])
    Q = diag(vertcat(Q_p, Q_q, Q_v, Q_w, Q_f, Q_t))

    R_f_dot = SX([1, 1, 1]) # small R -> aggressive moves, large R -> large error
    R_t_dot = SX([1, 1, 1])
    R = diag(vertcat(R_f_dot, R_t_dot))

    Q_p_e = SX([1, 1, 1])
    Q_q_e = SX([1, 1, 1])
    Q_v_e = SX([1, 1, 1])
    Q_w_e = SX([1, 1, 1])
    Q_f_e = SX([1, 1, 1])
    Q_t_e = SX([1, 1, 1])
    Q_e = diag(vertcat(Q_p_e, Q_q_e, Q_v_e, Q_w_e, Q_f_e, Q_t_e))

    # constraints, u = [f_dot, t_dot], h = [v, w, f_unit_actuator]
    u_lb = np.array([-1, -1, -1, -1, -1, -1])
    u_ub = np.array([1, 1, 1, 1, 1, 1])
    h_lb = np.array([-10, -10, -10, -60, -60, -60, 0, 0, 0, 0, 0, 0])
    h_ub = np.array([10, 10, 10, 60, 60, 60, 10, 10, 10, 10, 10, 10])

    # reference trajectory, with y_ref being the interface
    # set to all 'zero' for now, can be set later when using the solver
    # y_ref = [p_ref, q_ref, v_ref, w_ref]
    y_ref = np.array([0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0])
    
    # optimal control problem
    # https://docs.acados.org/python_interface/index.html#acados_template.acados_ocp.AcadosOcp
    ocp = AcadosOcp()

    # https://docs.acados.org/python_interface/index.html#acados_template.acados_ocp.AcadosOcpDims
    ocp.dims.N = N # prediction horizon, other fields in dim are set automatically

    # https://docs.acados.org/python_interface/index.html#acados_template.acados_model.AcadosModel
    ocp.model = get_omnihex_ode_model() # dynamics
    x = ocp.model.x
    u = ocp.model.u
    ocp.model.con_h_expr = h(x)
    ocp.model.con_h_expr_e = h(x)
    ocp.model.cost_expr_ext_cost = e(x, y_ref).T @ Q @ e(x, y_ref) + u.T @ R @ u
    ocp.model.cost_expr_ext_cost_e = e(x, y_ref).T @ Q_e @ e(x, y_ref)

    # https://docs.acados.org/python_interface/index.html#acados_template.acados_ocp.AcadosOcpCost
    ocp.cost.cost_type = 'EXTERNAL'
    ocp.cost.cost_type_e = 'EXTERNAL'

    # https://docs.acados.org/python_interface/index.html#acados_template.acados_ocp.AcadosOcpConstraints
    ocp.constraints.constr_type = 'BGH'
    ocp.constraints.lbu = u_lb
    ocp.constraints.ubu = u_ub
    ocp.constraints.idxbu = np.array([0, 1, 2, 3, 4, 5])
    ocp.constraints.lh = h_lb
    ocp.constraints.uh = h_ub
    ocp.constraints.lh_e = h_lb
    ocp.constraints.uh_e = h_ub

    # https://docs.acados.org/python_interface/index.html#acados_template.acados_ocp.AcadosOcpOptions
    ocp.solver_options.tf = Tf
    ocp.solver_options.integrator_type = 'IRK'
    ocp.solver_options.nlp_solver_type = 'SQP_RTI'
    ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'

    ocp.acados_include_path = environ['ACADOS_SOURCE_DIR'] + '/include'
    ocp.acados_lib_path = environ['ACADOS_SOURCE_DIR'] + '/lib'
    ocp.code_export_directory = '../acados_export'

    return ocp


def generate_ref(solver):

    # TODO: How to set reference trajectory?
    # TODO: y_ref and yref?
    # TODO: email, discourse, wechat?

    print('generate simple reference for testing only')

    
if __name__ == '__main__':

    ocp = formulate_omnihex_ocp()

    if path.exists(ocp.code_export_directory):
        shutil.rmtree(ocp.code_export_directory)

    json_path = ocp.code_export_directory + '/acados_ocp_' + ocp.model.name + '.json'
    ocpSolver = AcadosOcpSolver(ocp, json_file = json_path)
    simSolver = AcadosSimSolver(ocp, json_file = json_path)

    generate_ref(ocpSolver)

eBunny · March 3, 2022, 7:10am

Found the solution. Let me answer my question here.

According to Implementing cost functions which are nonlinear in y_ref? - #2 by FreyJo - User Questions - acados forum, one way is to use “parameters” as the state reference.

This idea is actually implicitly built into the problem formulation. But it is not obvious to new users. Please update 3.2 Cost module: external of this file. Thanks.

howai · February 21, 2023, 11:53am

No, that does not solve the problem completely. Since the parameter variable p is not a function of time \tau, it cannot change with time and will be a constant in one whole optimization horizon. So you can only set a reference point with this manner. However, you cannot assign a reference trajectory.

TPultke · February 21, 2023, 12:17pm

I hope I dont got you wrong @howai, but you can assign a different parameter variable for each shooting interval with .set(i…N, val_p). Thus you could assign a reference trajectory dependent on discretized timesteps for each shooting node.

howai · February 21, 2023, 12:59pm

Thank you for your reply. Since the parameter vector p does not have an index \tau as other variables like x, u and z in the cost function, I thought the parameters should be the same in every shooting nodes. I will try it.

TPultke · February 21, 2023, 1:20pm

Youre welcome! I assume p is not denoted as dependent on tau because is actually is not dependent. State x is a function of time (because of system dynamics) → p is a parameter given by the user, not calculated by the solver. Therefor it is the users choice to include e.g. an obstacle trajectory that is a function of time as parameters, but it must be precalculated since it is not part of the optimization variables nor of the system dynamics.

I hope you understand what I meant! Good luck with your implementation anyways

howai · February 21, 2023, 1:52pm

Very clear! So it is far more flexible than I thought. Thank you a lot!

Pratyush07 · November 2, 2024, 11:55am

hey i also have same doubt if you would have found solution can you post it here