Beispiel #1
0
else:

    print "Loading sympy expressions..."

    current_source_file_path = pathutils.get_current_source_file_path()

    with open(current_source_file_path+"/data/sympy/quadrotorcamera3d_A_expr_dummy.dat",     "r") as f: A_expr_dummy     = sympy.sympify(f.read())
    with open(current_source_file_path+"/data/sympy/quadrotorcamera3d_A_dot_expr_dummy.dat", "r") as f: A_dot_expr_dummy = sympy.sympify(f.read())

    with open(current_source_file_path+"/data/sympy/quadrotorcamera3d_A_expr_dummy_syms.dat",     "r") as f: A_expr_dummy_syms     = array(sympy.sympify(f.read())).squeeze()
    with open(current_source_file_path+"/data/sympy/quadrotorcamera3d_A_dot_expr_dummy_syms.dat", "r") as f: A_dot_expr_dummy_syms = array(sympy.sympify(f.read())).squeeze()

print "Compiling sympy functions..."

A_anon_funcs_ufuncify     = sympyutils.construct_matrix_anon_funcs_ufuncify(A_expr_dummy,     A_expr_dummy_syms)
A_dot_anon_funcs_ufuncify = sympyutils.construct_matrix_anon_funcs_ufuncify(A_dot_expr_dummy, A_dot_expr_dummy_syms)

print "Finished compiling sympy functions."



def unpack_state(x):

    p_body         = matrix( [ x.item(0), x.item(1), x.item(2) ] ).T
    theta_body     = x.item(3)
    psi_body       = x.item(4)
    phi_body       = x.item(5)
    theta_cam      = x.item(6)
    psi_cam        = x.item(7)
    phi_cam        = x.item(8)
Beispiel #2
0
    current_source_file_path = pathutils.get_current_source_file_path()

    with open(current_source_file_path+"/data/sympy/quadrotor3d_A_expr_dummy.dat",              "r") as f: A_expr_dummy              = sympy.sympify(f.read())
    with open(current_source_file_path+"/data/sympy/quadrotor3d_A_dot_expr_dummy.dat",          "r") as f: A_dot_expr_dummy          = sympy.sympify(f.read())
    with open(current_source_file_path+"/data/sympy/quadrotor3d_dqdotdot_dq_expr_dummy.dat",    "r") as f: dqdotdot_dq_expr_dummy    = sympy.sympify(f.read())
    with open(current_source_file_path+"/data/sympy/quadrotor3d_dqdotdot_dqdot_expr_dummy.dat", "r") as f: dqdotdot_dqdot_expr_dummy = sympy.sympify(f.read())

    with open(current_source_file_path+"/data/sympy/quadrotor3d_A_expr_dummy_syms.dat",              "r") as f: A_expr_dummy_syms              = array(sympy.sympify(f.read())).squeeze()
    with open(current_source_file_path+"/data/sympy/quadrotor3d_A_dot_expr_dummy_syms.dat",          "r") as f: A_dot_expr_dummy_syms          = array(sympy.sympify(f.read())).squeeze()
    with open(current_source_file_path+"/data/sympy/quadrotor3d_dqdotdot_dq_expr_dummy_syms.dat",    "r") as f: dqdotdot_dq_expr_dummy_syms    = array(sympy.sympify(f.read())).squeeze()
    with open(current_source_file_path+"/data/sympy/quadrotor3d_dqdotdot_dqdot_expr_dummy_syms.dat", "r") as f: dqdotdot_dqdot_expr_dummy_syms = array(sympy.sympify(f.read())).squeeze()

print "Compiling sympy functions..."

A_anon_funcs_ufuncify              = sympyutils.construct_matrix_anon_funcs_ufuncify(A_expr_dummy,              A_expr_dummy_syms)
A_dot_anon_funcs_ufuncify          = sympyutils.construct_matrix_anon_funcs_ufuncify(A_dot_expr_dummy,          A_dot_expr_dummy_syms)
dqdotdot_dq_anon_funcs_ufuncify    = sympyutils.construct_matrix_anon_funcs_ufuncify(dqdotdot_dq_expr_dummy,    dqdotdot_dq_expr_dummy_syms)
dqdotdot_dqdot_anon_funcs_ufuncify = sympyutils.construct_matrix_anon_funcs_ufuncify(dqdotdot_dqdot_expr_dummy, dqdotdot_dqdot_expr_dummy_syms)

print "Finished compiling sympy functions."



def unpack_state(x):

    p         = matrix( [ x.item(0), x.item(1), x.item(2) ] ).T
    theta     = x.item(3)
    psi       = x.item(4)
    phi       = x.item(5)
    p_dot     = matrix( [ x.item(6), x.item(7), x.item(8) ] ).T
Beispiel #3
0
    with open(
            current_source_file_path +
            "/data/sympy/quadrotor2d_dqdotdot_dq_expr_dummy_syms.dat",
            "r") as f:
        dqdotdot_dq_expr_dummy_syms = array(sympy.sympify(f.read())).squeeze()
    with open(
            current_source_file_path +
            "/data/sympy/quadrotor2d_dqdotdot_dqdot_expr_dummy_syms.dat",
            "r") as f:
        dqdotdot_dqdot_expr_dummy_syms = array(sympy.sympify(
            f.read())).squeeze()

print "Compiling sympy functions..."

dqdotdot_dq_anon_funcs_ufuncify = sympyutils.construct_matrix_anon_funcs_ufuncify(
    dqdotdot_dq_expr_dummy, dqdotdot_dq_expr_dummy_syms)
dqdotdot_dqdot_anon_funcs_ufuncify = sympyutils.construct_matrix_anon_funcs_ufuncify(
    dqdotdot_dqdot_expr_dummy, dqdotdot_dqdot_expr_dummy_syms)

print "Finished compiling sympy functions."


def unpack_state(x):

    p = matrix([x.item(0), x.item(1)]).T
    theta = x.item(2)
    p_dot = matrix([x.item(3), x.item(4)]).T
    theta_dot = x.item(5)
    q = matrix([p.item(0), p.item(1), theta]).T
    q_dot = matrix([p_dot.item(0), p_dot.item(1), theta_dot]).T