Beispiel #1
0
def export_quad_ode_model():

    model_name = 'quad_ode'

    # set up states & controls
    q0 = SX.sym('q0')
    q1 = SX.sym('q1')
    q2 = SX.sym('q2')
    q3 = SX.sym('q3')
    omegax = SX.sym('omegax')
    omegay = SX.sym('omegay')
    omegaz = SX.sym('omegaz')

    x = vertcat(q0, q1, q2, q3, omegax, omegay, omegaz)

    # controls
    w1 = SX.sym('w1')
    w2 = SX.sym('w2')
    w3 = SX.sym('w3')
    w4 = SX.sym('w4')

    u = vertcat(w1, w2, w3, w4)

    # xdot
    q0_dot = SX.sym('q0_dot')
    q1_dot = SX.sym('q1_dot')
    q2_dot = SX.sym('q2_dot')
    q3_dot = SX.sym('q3_dot')
    omegax_dot = SX.sym('omegax_dot')
    omegay_dot = SX.sym('omegay_dot')
    omegaz_dot = SX.sym('omegaz_dot')

    xdot = vertcat(q0_dot, q1_dot, q2_dot, q3_dot, omegax_dot, omegay_dot,
                   omegaz_dot)

    # algebraic variables
    # z = None

    # parameters
    # set up parameters
    rho = SX.sym('rho')  # air density
    A = SX.sym('A')  # propeller area
    Cl = SX.sym('Cl')  # lift coefficient
    Cd = SX.sym('Cd')  # drag coefficient
    m = SX.sym('m')  # mass of quad
    g = SX.sym('g')  # gravity
    J1 = SX.sym('J1')  # mom inertia
    J2 = SX.sym('J2')  # mom inertia
    J3 = SX.sym('J3')  # mom inertia
    J = diag(vertcat(J1, J2, J3))

    # This comes from ref[6] pg 449.
    # dq = 1/2 * G(q)' * Ω
    S = SX.zeros(3, 4)
    S[:, 0] = vertcat(-q1, -q2, -q3)
    S[:, 1:4] = q0 * np.eye(3) - skew(vertcat(q1, q2, q3))

    print("S:", S, S.shape)
    #Define angular velocity vector
    OMG = vertcat(omegax, omegay, omegaz)

    p = vertcat(rho, A, Cl, Cd, m, g, J1, J2, J3)

    #Define torque applied to the system
    T = SX(3, 1)

    T[0, 0] = 0.5 * A * Cl * rho * (w2 * w2 - w4 * w4)
    T[1, 0] = 0.5 * A * Cl * rho * (w1 * w1 - w3 * w3)
    T[2, 0] = 0.5 * A * Cd * rho * (w1 * w1 - w2 * w2 + w3 * w3 - w4 * w4)

    # Reference generation (quaternion to eul) used in cost function.
    Eul = SX(3, 1)
    Eul[0, 0] = arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2))
    Eul[1, 0] = arcsin(2 * (q0 * q2 - q3 * q1))
    Eul[2, 0] = arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2))

    # dynamics
    f_expl = vertcat(0.5 * mtimes(transpose(S), OMG),
                     mtimes(inv(J), (T - cross(OMG, mtimes(J, OMG)))))

    f_impl = xdot - f_expl
    print("dynamics shape: ", f_expl.shape)
    model = AcadosModel()

    model.f_impl_expr = f_impl
    model.f_expl_expr = f_expl
    model.x = x
    model.xdot = xdot
    model.u = u
    # model.z = []
    model.p = p
    model.name = model_name

    # add the nonlinear cost.
    # Cost defined in paper
    #            ---                    ---
    #            | roll(x)   - roll_ref    |
    #            | pitch(x)  - pitch_ref   |
    # l(x,u,z) = | yaw(x)    - yaw_ref     |
    #            | x         - x_ref       |
    #            | u         - u_ref       |
    #            ---                    ---  14x1
    #        ---                    ---
    #        | roll(x)   - roll_ref    |
    #        | pitch(x)  - pitch_ref   |
    # m(x) = | yaw(x)    - yaw_ref     |
    #        | x         - x_ref       |
    #        ---                    ---  10x1
    model.cost_y_expr = vertcat(
        Eul, x, u)  #: CasADi expression for nonlinear least squares
    model.cost_y_expr_e = vertcat(
        Eul, x)  #: CasADi expression for nonlinear least squares, terminal
    model.con_h_expr = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3
    # # yReference checking.
    # refCheck = Function('y_ref', [vertcat(x, u)], [model.cost_y_expr])
    # print(refCheck)
    # y_ref = refCheck(np.array([0.566, 0.571, 0.168, 0.571, 0.1, 0.2, 0.3, 1, 2, 3, 4]))
    # print(y_ref)
    #
    # dynCheck = Function('sys_dyn', [vertcat(x, u),p], [f_expl])
    # print(dynCheck)
    # dx_ref = dynCheck(np.array([0.566, 0.571, 0.168, 0.571, 0.1, 0.2, 0.3, 1, 2, 3, 4]),np.array([1, 1, 1, 1, 1, 1, 1, 1, 1]))
    # print(dx_ref)
    return model
def export_quaternion_ode_model():

    model_name = 'quaternion_ode'

    # set up states & controls
    q0 = SX.sym('q0')
    q1 = SX.sym('q1')
    q2 = SX.sym('q2')
    q3 = SX.sym('q3')
    omegax = SX.sym('omegax')
    omegay = SX.sym('omegay')
    omegaz = SX.sym('omegaz')

    x = vertcat(q0, q1, q2, q3, omegax, omegay, omegaz)

    # controls
    omegax_cont = SX.sym('omegax_cont')
    omegay_cont = SX.sym('omegay_cont')
    omegaz_cont = SX.sym('omegaz_cont')

    u = vertcat(omegax_cont, omegay_cont, omegaz_cont)

    # xdot
    q0_dot = SX.sym('q0_dot')
    q1_dot = SX.sym('q1_dot')
    q2_dot = SX.sym('q2_dot')
    q3_dot = SX.sym('q3_dot')
    omegax_dot = SX.sym('omegax_dot')
    omegay_dot = SX.sym('omegay_dot')
    omegaz_dot = SX.sym('omegaz_dot')

    xdot = vertcat(q0_dot, q1_dot, q2_dot, q3_dot, omegax_dot, omegay_dot,
                   omegaz_dot)

    # algebraic variables
    # z = None

    # parameters
    p = []
    # This comes from ref[6] pg 449.
    # dq = 1/2 * G(q)' * Ω
    S = SX.zeros(3, 4)
    S[:, 0] = vertcat(-q1, -q2, -q3)
    S[:, 1:4] = q0 * np.eye(3) - skew(vertcat(q1, q2, q3))

    print("S:", S, S.shape)

    OMG = vertcat(omegax, omegay, omegaz)
    # dynamics
    f_expl = vertcat(0.5 * mtimes(S.T, OMG), u)

    f_impl = xdot - f_expl
    model = AcadosModel()

    model.f_impl_expr = f_impl
    model.f_expl_expr = f_expl
    model.x = x
    model.xdot = xdot
    model.u = u
    # model.z = []
    model.p = p
    model.name = model_name

    # Reference generation (quaternion to eul) used in cost function.
    Eul = SX(3, 1)
    Eul[0, 0] = arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2))
    Eul[1, 0] = arcsin(2 * (q0 * q2 - q3 * q1))
    Eul[2, 0] = arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2))
    quat_to_eul = Function("quat_to_eul", [x, u], [Eul])

    model.cost_y_expr = vertcat(
        Eul, x[4:, :], u)  #: CasADi expression for nonlinear least squares
    model.cost_y_expr_e = vertcat(
        Eul,
        x[4:, :])  #: CasADi expression for nonlinear least squares, terminal

    model.con_h_expr = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3

    return model