Ed<dgZddlmZmZmZddlmZddlmZddl m Z ddl m Z ddl mZddlmZGd dZd Zd S) Linearizer)MatrixeyezerosDummy)flatten)dynamicsymbols)msubs) namedtuple)Iterablec<eZdZdZ d dZdZdZdZdZd d Z dS) raThis object holds the general model form for a dynamic system. This model is used for computing the linearized form of the system, while properly dealing with constraints leading to dependent coordinates and speeds. Attributes ========== f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a : Matrix Matrices holding the general system form. q, u, r : Matrix Matrices holding the generalized coordinates, speeds, and input vectors. q_i, u_i : Matrix Matrices of the independent generalized coordinates and speeds. q_d, u_d : Matrix Matrices of the dependent generalized coordinates and speeds. perm_mat : Matrix Permutation matrix such that [q_ind, u_ind]^T = perm_mat*[q, u]^T Nc|t||_t||_t||_t||_t||_t||_t||_t||_t| |_ t| |_ d}|| |_ || |_ || |_ |||_|||_|||_|j t$j|_|j t$j|_t-|j|j tfd|jD|_t3|j}t3|j}t3|j }t3|j }t3|j}t3|j}t5dgd}||||||||_d|_d|_d|_d|_d|_ d|_!d|_"d|_#d|_$d|_%d|_&dS)a Parameters ========== f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a : array_like System of equations holding the general system form. Supply empty array or Matrix if the parameter does not exist. q : array_like The generalized coordinates. u : array_like The generalized speeds q_i, u_i : array_like, optional The independent generalized coordinates and speeds. q_d, u_d : array_like, optional The dependent generalized coordinates and speeds. r : array_like, optional The input variables. lams : array_like, optional The lagrange multipliers c@|rt|n tS)N)r)xs Alib/python3.11/site-packages/sympy/physics/mechanics/linearize.pyz%Linearizer.__init__..Gsa!=VXXc8g|]}|vr|n tSr).0vardup_varss r z'Linearizer.__init__..Vs;!!!'*&9Fssuww!!!rdims)lmnoskNF)'rf_0f_1f_2f_3f_4f_cf_vf_aquq_iq_du_iu_drlamsdiffr _t_qd_udset intersection_qd_duplenr _dims_Pq_Pqi_Pqd_Pu_Pui_Pud_C_0_C_1_C_2perm_mat _setup_done)selfr"r#r$r%r&r'r(r)r*r+r,r-r.r/r0r1 none_handlerrrrrr r!rrs @r__init__zLinearizer.__init__"sT2#;;#;;#;;#;;#;;#;;#;;#;;== <$$<$$<$$<$$a L&& 6;;~0116;;~011tx==--df55!!!!x!!!""  MM MM KK KK KK  NN&"@"@"@AAT!Q1a++          rc|||d|_dS)NT)_form_permutation_matrices_form_block_matrices_form_coefficient_matricesrE)rFs r_setupzLinearizer._setuppsH ''))) !!### '')))rc |j\}}}}}}|dkrt|jt|j|jg|_|dkr3|jddd| f|_|jdd| df|_n|j|_t|_|dkrt|j t|j |j g|_ |dkr3|j ddd| f|_ |j dd| df|_n|j |_ t|_t|jt||z||z g}tt|||z |j t|||z g}|r'|r|||_dS||_dS||_dS)z(Form the permutation matrices Pq and Pu.rN)r:permutation_matrixr*rr,r-r;r<r=r+r.r/r>r?r@rrow_joinrD) rFrrrrr r!P_col1P_col2s rrJz%Linearizer._form_permutation_matricesxs :1aAq 6 %)$&&$(DH9M2N2NOODH1u % HQQQ!V,  HQQQV,  H "HH 6 %)$&&$(DH9M2N2NOODH1u % HQQQ!V,  HQQQV,  H "HH E!a%Q$7$7899q!a%$)U1a!e__EFF  # ' & 7 7 & "DMMMrc|j\}}}}}}|dkrb|j|j}t ||j||jz|zz |jz|_nt ||_|dkr|j |j }||j z} |dkrC|j |j} |j | | z|_ nt|||_ t ||j | |zz |jz|_dSt|||_ t ||_dS)z0Form the coefficient matrices C_0, C_1, and C_2.rN)r:r'jacobianr*rr=LUsolver<rAr(r+r@rBrr?rC) rFrrrrr r! f_c_jac_q f_v_jac_utemp f_v_jac_qs rrLz%Linearizer._form_coefficient_matricessg :1aAq q5 ))$&11IQ$)yI0&wy11#2259Y?DIIADI q5 ))$&11Ity(DAv ( H--df55 !YJi)@)@@ !!QKK Q$)LL++#,,/3y9DIIIa DIADIIIrc|j\}}}}}}|dkrR|j|j|_|j|jz|j |_n&t|_t|_|dkrP|dkrJ|j |j |_ |j |j |_ n&t|_ t|_ |dkrf||z |zdkrZ|j |j |_|j|j z|jz|j |_n&t|_t|_|dkrP|dkrJ|j |j|_|j |j |_n&t|_t|_|dkr^||z |zdkrR|j|j|_|j|j z|j |_n&t|_t|_|dkr,|dkr&|j|j |_nt|_|dkr1||z |zdkr%|j|j|_nt|_|dkr3||z |zdkr'|j |j |_dSt|_dS)z2Form the block matrices for composing M, A, and B.rN)r:r"rTr4_M_qqr#r*_A_qqrr)r8_M_uqc_A_uqcr%_M_uqdr$r&_A_uqdr5_M_uucr+_A_uuc_M_uud_A_uud_A_qur1_M_uldr0_B_u)rFrrrrr r!s rrKzLinearizer._form_block_matricess :1aAq 6 "**4844DJ8dh.88@@@DJJDJDJ 6 #a1f #(++DL99DK8,,TV444DKK ((DK ((DK 6 #a!eai1n #(++DL99DK Htx/$(:DDTVLLLDKK ((DK ((DK 6 #a1f #(++DH55DK8,,TV444DKK ((DK ((DK 6 #a!eai1n #(++DH55DK Htx/99$&AAADKK ((DK ((DK 6 "a1f "(++DF333DJJDJ 6 #a!eai1n #(++DI66DKK ((DK 6 !a!eai1n !**46222DIIIDIIIrFcB|js|t|tr|}n4t|tri}|D]}||ni}|j\}}}} } } |j} |j} |j }|j }|j }|j }|j }|j}|j}|j}|j}|j}|j}|j}|j}|j}| dkr t/t1|| ||g}| dkr"t/t1||z| |g}|dkrft/| | |g}| dkr/| dkr)|||}n=| dkr||}n!|}n| dkr||}n|}t5||} |dkr|}!| dkr|!||zz }!|!|z}!|dkr|}"| dkr|"||zz }"|"|z}"nt/}"| |z | zdkr|}#| dkr|#||zz }#|#|z}#nt/}#t/|!|"|#g}nt/}| dkrg|dkr||z}$nt/}$|dkr||z}%nt/}%| |z | zdkr||z}&nt/}&t/|$|%|&g}nt/}|r|r||}'n|}'n|}'t5|'|}(| dkrC| |z | zdkr7t1||z| |})t5|)|}*nt/}*|rw|jj| |(z}+|*r#|jj| |*z},n|*},|r(|+|,|+|,fS|r<| |(|*| |(|*fS)aLinearize the system about the operating point. Note that q_op, u_op, qd_op, ud_op must satisfy the equations of motion. These may be either symbolic or numeric. Parameters ========== op_point : dict or iterable of dicts, optional Dictionary or iterable of dictionaries containing the operating point conditions. These will be substituted in to the linearized system before the linearization is complete. Leave blank if you want a completely symbolic form. Note that any reduction in symbols (whether substituted for numbers or expressions with a common parameter) will result in faster runtime. A_and_B : bool, optional If A_and_B=False (default), (M, A, B) is returned for forming [M]*[q, u]^T = [A]*[q_ind, u_ind]^T + [B]r. If A_and_B=True, (A, B) is returned for forming dx = [A]x + [B]r, where x = [q_ind, u_ind]^T. simplify : bool, optional Determines if returned values are simplified before return. For large expressions this may be time consuming. Default is False. Potential Issues ================ Note that the process of solving with A_and_B=True is computationally intensive if there are many symbolic parameters. For this reason, it may be more desirable to use the default A_and_B=False, returning M, A, and B. More values may then be substituted in to these matrices later on. The state space form can then be found as A = P.T*M.LUsolve(A), B = P.T*M.LUsolve(B), where P = Linearizer.perm_mat. r) rErM isinstancedictr updater:r[r]r_rarcrfr\r^r`rerbrdrgrArBrCrrrPr col_joinrDTrUsimplify)-rFop_pointA_and_Brn op_point_dictoprrrrr r!M_qqM_uqcM_uqdM_uucM_uudM_uldA_qqA_uqcA_uqdA_quA_uucA_uudB_uC_0C_1C_2col2col3col1MM_eqr1c1r2c1r3c1r1c2r2c2r3c2AmatAmat_eqBmatBmat_eqA_contB_conts- r linearizezLinearizer.linearizesN  KKMMM h % % $MM ( + + M ) )$$R(((( )M :1aAqz     z  z  iiii 6 75A;;u566D 6 45Q??E233D 6 4.//DAv !q& MM$''0066a MM$'' !V  d##AAAQ && 6 DAv %$#:DAv 6*US[)Dczxx1uqyA~ 6*US[)Dczxx4t,--DD88D 6 Av czxxAv s{xx1uqyA~ s{xx4t,--DD88D   }}T**Dm,, 6 a!eai1n Q??++C00DD-00GGhhG  *]_t||G'<'< ! #   """  """') )r)NNNNNN)NFF) __name__ __module__ __qualname____doc__rHrMrJrLrKrrrrrr s,BFL!L!L!L!\   ###B>0!0!0!dq*q*q*q*q*q*rctttfstt|ttfst|}t t |krt dfd|D}t t}t|D] \}}d|||f< |S)akCompute the permutation matrix to change order of orig_vec into order of per_vec. Parameters ========== orig_vec : array_like Symbols in original ordering. per_vec : array_like Symbols in new ordering. Returns ======= p_matrix : Matrix Permutation matrix such that orig_vec == (p_matrix * per_vec). zKorig_vec and per_vec must be the same length, and contain the same symbols.c:g|]}|Sr)index)riorig_vecs rrz&permutation_matrix..s%333aq!!333r) rilisttupler r6 ValueErrorrr9 enumerate)rper_vecind_listp_matrixrjs` rrOrOs$ hu . .%8$$ ge} - -#'"" 8}}G $1011 133337333HS]]##H(##1A OrN)__all__sympy.core.backendrrrsympy.core.symbolrsympy.utilities.iterablesr sympy.physics.vectorr !sympy.physics.mechanics.functionsr collectionsr collections.abcr rrOrrrrs .1111111111######------//////333333""""""$$$$$$O*O*O*O*O*O*O*O*d r