<s> from sympy import ( symbols , ccode , Symbol , cse , numbered_symbols , solve ) <EOL> from sympy . physics . mechanics import ( dynamicsymbols , ReferenceFrame , Vector , <EOL> Point , inertia , dot , cross ) <EOL> Vector . simp = False <EOL> t , g = symbols ( '<STR_LIT>' ) <EOL> a , b , c = symbols ( '<STR_LIT>' ) <EOL> d , e , f = symbols ( '<STR_LIT>' ) <EOL> m , Ixx , Iyy , Izz , Ixy , Iyz , Ixz = symbols ( '<STR_LIT>' ) <EOL> q = dynamicsymbols ( '<STR_LIT>' ) <EOL> qd = [ qi . diff ( t ) for qi in q ] <EOL> wx , wy , wz = symbols ( '<STR_LIT>' ) <EOL> rx , ry , rz = symbols ( '<STR_LIT>' ) <EOL> Fx , Fy , Fz = symbols ( '<STR_LIT>' ) <EOL> mu_x , mu_y , mu_z = symbols ( '<STR_LIT>' ) <EOL> N = ReferenceFrame ( '<STR_LIT:N>' ) <EOL> Y = N . orientnew ( '<STR_LIT:Y>' , '<STR_LIT>' , [ q [ <NUM_LIT:0> ] , N . z ] ) <EOL> L = Y . orientnew ( '<STR_LIT:L>' , '<STR_LIT>' , [ q [ <NUM_LIT:1> ] , Y . x ] ) <EOL> R = L . orientnew ( '<STR_LIT:R>' , '<STR_LIT>' , [ q [ <NUM_LIT:2> ] , L . y ] ) <EOL> print ( R . dcm ( Y ) ) <EOL> I = inertia ( R , Ixx , Iyy , Izz , Ixy , Iyz , Ixz ) <EOL> print ( I . express ( Y ) ) <EOL> P = Point ( '<STR_LIT:P>' ) <EOL> O = P . locatenew ( '<STR_LIT:O>' , - rx * R . x - rx * R . y - rx * R . z ) <EOL> RO = O . locatenew ( '<STR_LIT>' , d * R . x + e * R . y + f * R . z ) <EOL> w_r_n = wx * R . x + wy * R . y + wz * R . z <EOL> omega_dict = { wx : dot ( qd [ <NUM_LIT:0> ] * Y . z , R . x ) , <EOL> wy : dot ( qd [ <NUM_LIT:0> ] * Y . z , R . y ) , <EOL> wz : dot ( qd [ <NUM_LIT:0> ] * Y . z , R . z ) } <EOL> v_ro_n = cross ( w_r_n , RO . pos_from ( P ) ) <EOL> a_ro_n = cross ( w_r_n , v_ro_n ) <EOL> mu_dict = { mu_x : dot ( R . x , Y . z ) , mu_y : dot ( R . y , Y . z ) , mu_z : dot ( R . z , Y . z ) } <EOL> F_RO = Fx * R . x + Fy * R . y + Fz * R . z + m * g * ( mu_x * R . x + mu_y * R . y + mu_z * R . z ) <EOL> newton_eqn = F_RO - m * a_ro_n <EOL> force_scalars = solve ( [ dot ( newton_eqn , uv ) . expand ( ) for uv in R ] , [ Fx , Fy , Fz ] ) <EOL> euler_eqn = cross ( P . pos_from ( RO ) , F_RO ) - cross ( w_r_n , dot ( I , w_r_n ) ) <EOL> print ( dot ( euler_eqn , R . x ) . subs ( omega_dict ) . expand ( ) ) <EOL> print ( dot ( euler_eqn , R . y ) . subs ( omega_dict ) . expand ( ) ) <EOL> print ( dot ( euler_eqn , R . z ) . subs ( omega_dict ) . expand ( ) . subs ( force_scalars ) . expand ( ) . subs ( mu_dict ) . expand ( ) ) <EOL> RO = O . locatenew ( '<STR_LIT>' , d * R . x + e * R . y + f * R . z ) <EOL> RO . set_vel ( N , v_ro_n ) <EOL> O . v2pt_theory ( RO , N , R ) <EOL> partial_w = [ R . ang_vel_in ( N ) . diff ( qdi , N ) for qdi in qd ] <EOL> partial_v_RO = [ RO . vel ( N ) . diff ( qdi , N ) for qdi in qd ] <EOL> steady_dict = { qd [ <NUM_LIT:1> ] : <NUM_LIT:0> , qd [ <NUM_LIT:2> ] : <NUM_LIT:0> , <EOL> qd [ <NUM_LIT:0> ] . diff ( t ) : <NUM_LIT:0> , qd [ <NUM_LIT:1> ] . diff ( t ) : <NUM_LIT:0> , qd [ <NUM_LIT:2> ] . diff ( t ) : <NUM_LIT:0> } <EOL> O . set_vel ( N , O . vel ( N ) . subs ( steady_dict ) ) <EOL> RO . set_vel ( N , RO . vel ( N ) . subs ( steady_dict ) ) <EOL> R . set_ang_vel ( N , R . ang_vel_in ( N ) . subs ( steady_dict ) ) <EOL> assert ( RO . vel ( N ) . diff ( t , R ) . subs ( steady_dict ) == <NUM_LIT:0> ) <EOL> RO . set_acc ( N , cross ( R . ang_vel_in ( N ) , RO . vel ( N ) ) ) <EOL> F_RO = m * g * Y . z <EOL> gaf_RO = [ dot ( F_RO , pv ) for pv in partial_v_RO ] <EOL> R_star = - m * RO . acc ( N ) <EOL> T_star = - cross ( R . ang_vel_in ( N ) , dot ( I , R . ang_vel_in ( N ) ) ) <EOL> f_r = [ <NUM_LIT:0> ] * <NUM_LIT:3> <EOL> f_r_star = [ <NUM_LIT:0> ] * <NUM_LIT:3> <EOL> f_dyn = [ <NUM_LIT:0> ] * <NUM_LIT:3> <EOL> for i in range ( <NUM_LIT:3> ) : <EOL> f_r [ i ] = gaf_RO [ i ] . expand ( ) . collect ( m * g ) <EOL> f_r_star [ i ] = ( dot ( R_star , partial_v_RO [ i ] ) + dot ( T_star , partial_w [ i ] ) ) . expand ( ) . collect ( qd [ <NUM_LIT:0> ] ** <NUM_LIT:2> ) <EOL> f_dyn [ i ] = f_r [ i ] + f_r_star [ i ] <EOL> assert ( f_dyn [ i ] . diff ( q [ <NUM_LIT:0> ] ) == <NUM_LIT:0> ) <EOL> assert ( f_dyn [ <NUM_LIT:0> ] == <NUM_LIT:0> ) <EOL> h = [ <NUM_LIT:0> ] * <NUM_LIT:4> <EOL> for i in range ( <NUM_LIT:2> ) : <EOL> h [ <NUM_LIT:2> * i ] = f_dyn [ i + <NUM_LIT:1> ] . subs ( { qd [ <NUM_LIT:0> ] : <NUM_LIT:0> } ) <EOL> h [ <NUM_LIT:2> * i + <NUM_LIT:1> ] = f_dyn [ i + <NUM_LIT:1> ] . coeff ( qd [ <NUM_LIT:0> ] ** <NUM_LIT:2> ) <EOL> dh = [ <NUM_LIT:0> ] * <NUM_LIT:8> <EOL> for i in range ( <NUM_LIT:4> ) : <EOL> for j in range ( <NUM_LIT:2> ) : <EOL> dh [ <NUM_LIT:2> * i + j ] = h [ i ] . diff ( q [ j + <NUM_LIT:1> ] ) <EOL> symbol_dict = { q [ <NUM_LIT:1> ] : Symbol ( '<STR_LIT>' ) , q [ <NUM_LIT:2> ] : Symbol ( '<STR_LIT>' ) , qd [ <NUM_LIT:0> ] : Symbol ( '<STR_LIT>' ) } <EOL> for i in range ( <NUM_LIT:4> ) : <EOL> h [ i ] = h [ i ] . subs ( symbol_dict ) <EOL> for i in range ( <NUM_LIT:8> ) : <EOL> dh [ i ] = dh [ i ] . subs ( symbol_dict ) <EOL> z , h_red = cse ( h , numbered_symbols ( '<STR_LIT:z>' ) ) <EOL> output_code = "<STR_LIT>" <EOL> for zi_lhs , zi_rhs in z : <EOL> output_code += "<STR_LIT>" . format ( zi_lhs , ccode ( zi_rhs ) ) <EOL> output_code += "<STR_LIT>" <EOL> for i in range ( <NUM_LIT:4> ) : <EOL> output_code += "<STR_LIT>" . format ( i , ccode ( h_red [ i ] ) ) <EOL> z , dh_red = cse ( dh , numbered_symbols ( '<STR_LIT:z>' ) ) <EOL> output_code += "<STR_LIT>" <EOL> for zi_lhs , zi_rhs in z : <EOL> output_code += "<STR_LIT>" . format ( zi_lhs , ccode ( zi_rhs ) ) <EOL> output_code += "<STR_LIT>" <EOL> for i in range ( <NUM_LIT:8> ) : <EOL> output_code += "<STR_LIT>" . format ( i , ccode ( dh_red [ i ] ) ) <EOL> import re <EOL> output_code = re . sub ( r"<STR_LIT>" , r"<STR_LIT>" , output_code ) <EOL> with open ( "<STR_LIT>" , '<STR_LIT:w>' ) as f : <EOL> f . write ( output_code ) </s>
