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)
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
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