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