omega_z_terms_expr = sympyutils.collect_into_dict_include_zero_and_constant_terms( R_dot_R_T_expr[2,1], [theta_dot_expr,psi_dot_expr,phi_dot_expr] ) omega_y_terms_expr = sympyutils.collect_into_dict_include_zero_and_constant_terms( -R_dot_R_T_expr[2,0], [theta_dot_expr,psi_dot_expr,phi_dot_expr] ) omega_x_terms_expr = sympyutils.collect_into_dict_include_zero_and_constant_terms( R_dot_R_T_expr[1,0], [theta_dot_expr,psi_dot_expr,phi_dot_expr] ) A_expr = sympy.Matrix( [ \ [ omega_z_terms_expr[theta_dot_expr], omega_z_terms_expr[psi_dot_expr], omega_z_terms_expr[phi_dot_expr] ], \ [ omega_y_terms_expr[theta_dot_expr], omega_y_terms_expr[psi_dot_expr], omega_y_terms_expr[phi_dot_expr] ], \ [ omega_x_terms_expr[theta_dot_expr], omega_x_terms_expr[psi_dot_expr], omega_x_terms_expr[phi_dot_expr] ] ] ) A_dot_expr = sympy.trigsimp(A_expr.diff(t_expr)) syms = [ theta_expr, psi_expr, phi_expr, theta_dot_expr, psi_dot_expr, phi_dot_expr ] print "Dummifying sympy expressions..." A_expr_dummy, A_expr_dummy_syms = sympyutils.dummify( A_expr, syms ) A_dot_expr_dummy, A_dot_expr_dummy_syms = sympyutils.dummify( A_dot_expr, syms ) print "Saving 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", "w") as f: f.write(sympy.srepr(A_expr_dummy)) with open(current_source_file_path+"/data/sympy/quadrotorcamera3d_A_dot_expr_dummy.dat", "w") as f: f.write(sympy.srepr(A_dot_expr_dummy)) with open(current_source_file_path+"/data/sympy/quadrotorcamera3d_A_expr_dummy_syms.dat", "w") as f: f.write(sympy.srepr(sympy.Matrix(A_expr_dummy_syms))) with open(current_source_file_path+"/data/sympy/quadrotorcamera3d_A_dot_expr_dummy_syms.dat", "w") as f: f.write(sympy.srepr(sympy.Matrix(A_dot_expr_dummy_syms))) else: print "Loading sympy expressions..."
x_syms = hstack( [ matrix(q_expr).A1, matrix(q_dot_expr).A1 ] ) x_and_u_syms = hstack( [ matrix(q_expr).A1, matrix(q_dot_expr).A1, matrix(u_expr).A1 ] ) print "Substituting physical constants into sympy expressions..." const_vals = hstack( [ d, alpha, beta, gamma, m, I.A1, f_external_world.A1 ] ) const_subs = dict(zip(const_syms,const_vals)) A_expr = sympyutils.nsimplify_matrix(A_expr.subs(const_subs)) A_dot_expr = sympyutils.nsimplify_matrix(A_dot_expr.subs(const_subs)) dqdotdot_dq_expr = sympyutils.nsimplify_matrix(dqdotdot_dq_expr.subs(const_subs)) dqdotdot_dqdot_expr = sympyutils.nsimplify_matrix(dqdotdot_dqdot_expr.subs(const_subs)) print "Dummifying sympy expressions..." A_expr_dummy, A_expr_dummy_syms = sympyutils.dummify( A_expr, x_syms ) A_dot_expr_dummy, A_dot_expr_dummy_syms = sympyutils.dummify( A_dot_expr, x_syms ) dqdotdot_dq_expr_dummy, dqdotdot_dq_expr_dummy_syms = sympyutils.dummify( dqdotdot_dq_expr, x_and_u_syms ) dqdotdot_dqdot_expr_dummy, dqdotdot_dqdot_expr_dummy_syms = sympyutils.dummify( dqdotdot_dqdot_expr, x_and_u_syms ) print "Saving sympy expressions..." current_source_file_path = pathutils.get_current_source_file_path() with open(current_source_file_path+"/data/sympy/quadrotor3d_A_expr_dummy.dat", "w") as f: f.write(sympy.srepr(A_expr_dummy)) with open(current_source_file_path+"/data/sympy/quadrotor3d_A_dot_expr_dummy.dat", "w") as f: f.write(sympy.srepr(A_dot_expr_dummy)) with open(current_source_file_path+"/data/sympy/quadrotor3d_dqdotdot_dq_expr_dummy.dat", "w") as f: f.write(sympy.srepr(dqdotdot_dq_expr_dummy)) with open(current_source_file_path+"/data/sympy/quadrotor3d_dqdotdot_dqdot_expr_dummy.dat", "w") as f: f.write(sympy.srepr(dqdotdot_dqdot_expr_dummy)) with open(current_source_file_path+"/data/sympy/quadrotor3d_A_expr_dummy_syms.dat", "w") as f: f.write(sympy.srepr(sympy.Matrix(A_expr_dummy_syms))) with open(current_source_file_path+"/data/sympy/quadrotor3d_A_dot_expr_dummy_syms.dat", "w") as f: f.write(sympy.srepr(sympy.Matrix(A_dot_expr_dummy_syms)))
print "Substituting physical constants into sympy expressions..." const_vals = hstack([d, alpha, beta, gamma, m, I.A1, f_external_world.A1]) const_subs = dict(zip(const_syms, const_vals)) A_expr = sympyutils.nsimplify_matrix(A_expr.subs(const_subs)) A_dot_expr = sympyutils.nsimplify_matrix(A_dot_expr.subs(const_subs)) dqdotdot_dq_expr = sympyutils.nsimplify_matrix( dqdotdot_dq_expr.subs(const_subs)) dqdotdot_dqdot_expr = sympyutils.nsimplify_matrix( dqdotdot_dqdot_expr.subs(const_subs)) print "Dummifying sympy expressions..." A_expr_dummy, A_expr_dummy_syms = sympyutils.dummify(A_expr, x_syms) A_dot_expr_dummy, A_dot_expr_dummy_syms = sympyutils.dummify( A_dot_expr, x_syms) dqdotdot_dq_expr_dummy, dqdotdot_dq_expr_dummy_syms = sympyutils.dummify( dqdotdot_dq_expr, x_and_u_syms) dqdotdot_dqdot_expr_dummy, dqdotdot_dqdot_expr_dummy_syms = sympyutils.dummify( dqdotdot_dqdot_expr, x_and_u_syms) print "Saving sympy expressions..." current_source_file_path = pathutils.get_current_source_file_path() with open( current_source_file_path + "/data/sympy/quadrotor3d_A_expr_dummy.dat", "w") as f: f.write(sympy.srepr(A_expr_dummy))