
    Rie2.                         d dl mZmZmZmZmZmZmZmZ d dl	m
Z
 d dlmZ d dlmZ d dlmZmZmZmZmZmZmZmZmZmZmZ d dlmZ ed             Zd Zd	 Zd
 Z d Z!d Z"dS )    )symbolsMatrixcossinatansqrtRational_simplify_matrix)sympify)simplify)solve)dynamicsymbolsReferenceFramePointdotcrossinertiaKanesMethodParticle	RigidBody
LagrangianLagrangesMethod)slowc                    ;<=>?@A t          d          \  ?} }}}t          d          x\  }}}}}}	}
?fd|
D             x\  }}}}}}}t          d          }t          d          x\  }}}}}}}?fd|D             \  }}}}}}t          d          =t          d          }=                    d	d
|=j        g          } |                     dd
|| j        g          }!|!                    dd
||!j        g          ;|                    d|=j        z  |=j        z  z   |	=j        z  z             <;	                    =          A|!	                    =          }";
                    =||!j        z  ||!j        z  z   ||!j        z  z              <                    |                              =          @<                    =|;j        z  |;j        z  z   |;j        z  z              <                    d| |!j        z            >>                    <=;           t          |	t!          <                    >          =j                  z
  g          }#t          =>fd;D                       }$t          ;=Afd|!D             <=@fd=D             z             }%t#          |%|          }&|!
                    =|"                    |&                     ;                    =;	                    =                              |!          t)          |!	                    =          ;	                    =                    z              ||z  | j        z  }'|| dz  z  dz  }(|| dz  z  dz  })t+          ;|(|)|(          }*t-          d<;||*<f          }+|+g},<|'fg}-t/          =|||||g|||g|%|	g|#|||g|$          }.|.                    |,|-          \  }/}0|.                                }1|1j        |#k    sJ |1j        |$k    sJ |1j        |$                    ?                              |.                                          k    sJ t#          |1j        |1j         z   |          }2|&!                                D ]}3|2|3         |&|3         k    sJ tE          |1j#        |1j$        z   |/z
  |0z
            t          g d          k    sJ |	|  tK          |          z  i}4|d|tM          |          |z  |z   |tK          |          |z  ||  tM          |          |z  |z   z  tK          |          z  |d||  tM          |          |z  |z   z  tM          |          z  i}5|d||  tM          |          |z  |z   z  tK          |          z  ||  tM          |          |z  |z   z  tM          |          z  |di}6|d|z  tM          |          z  d| z  z  tM          d|z            |dz  z  dz  z   dtK          |          z  |z  |z  dz  z   |d|d|| tM          |          tM          |          z  |z  |z  tM          |          |dz  z  z   z  || d|z  tM          |          z  d| z  z  tM          d|z            |dz  z  dz  z   dtK          |          z  |z  |z  dz  z   z  ||  tM          |          tK          |          z  |z  |z  tK          |          |dz  z  z   z  i}7|1'                    |4|5|6|7gdd          \  } }!|d|d|d| d|di}8t          g dg dg dtM          |          |z  ddddtM          |           tK          |           dgtK          |           |z  ddddtK          |          tM          |           dgdtQ          dd          dddddd|z  dz  gg d dddddd!|z  ddgg          }9t          g           }:|                     |8          |9k    sJ |!                    |8          |:k    sJ tS          |                     |8                              |dtU          d"          z                      +                                dd#ik    sJ d S )$Nz	t r m g vzq1:7c                 :    g | ]}|                               S  diff).0qits     Llib/python3.11/site-packages/sympy/physics/mechanics/tests/test_linearize.py
<listcomp>z4test_linearize_rolling_disc_kane.<locals>.<listcomp>   s#    (@(@(@(@(@(@    zu:6zu1:7c                 :    g | ]}|                               S r   r   )r   uir!   s     r"   r#   z4test_linearize_rolling_disc_kane.<locals>.<listcomp>   s#    #;#;#;2BGGAJJ#;#;#;r$   NNOAAxisBCCOPc                 V    g | ]%}t                                        |          &S r   r   vel)r   uvr'   r.   s     r"   r#   z4test_linearize_rolling_disc_kane.<locals>.<listcomp>7   s-    000#aeeAhh##000r$   c                 \    g | ](}t                                        z
  |          )S r   )r   
ang_vel_in)r   r2   r,   r'   w_c_n_qds     r"   r#   z4test_linearize_rolling_disc_kane.<locals>.<listcomp>:   s2    GGGrs8all1oo5r::GGGr$   c                 \    g | ](}t                                        z
  |          )S r   r0   )r   r2   r-   r'   	v_co_n_qds     r"   r#   z4test_linearize_rolling_disc_kane.<locals>.<listcomp>;   s2    DDDBY2B77DDDr$         Disc)kd_eqsq_dependentconfiguration_constraintsu_dependentvelocity_constraints)r   r   r   r         Top_pointA_and_Br      )r   r   r   r   r   r   r   rE   )r   r   r   r   r   rE   r   r   )r   r   r   r   r   r   rE   r   )r   r   r   r   r   r   r   r         ),r   r   r   r   	orientnewzxy	locatenewr4   set_ang_velpos_fromdtset_velv2pt_theoryr   r   r   subsset_ang_accr   r   r   r   kanes_equationsto_linearizerf_cf_vf_ar   kindiffdictf_0f_1keysr   f_2f_3r   r   	linearizer	   r   r   	eigenvals)Brmgvq1q2q3q4q5q6qq1dq2dq3dq4dq5dq6dqduu1u2u3u4u5u6u1du2du3du4du5du6dr(   r)   r+   w_b_n_qdrW   rX   kindiffsqdotsF_COIJI_C_COr:   BLFLKMfrfr_star
linearizersolr    q_opu_opqd_opud_opupright_nominalA_solB_solr,   r-   r'   r.   r!   r7   r5   sB                                                              @@@@@@@r"    test_linearize_rolling_disc_kaner      s    K((MAq!Q "0!7!77BBBQ(@(@(@(@a(@(@(@@ Cc3S2 	uA!/!7!77BBBQ#;#;#;#;#;#;#; Cc3S 	sA	tB	C"ac++A	C"ac++A	C"ac++A	dBqsFRVObf4	5	5B ||AH||AH MM!RVbf_r!#v-... B""1%%I JJq"QS&2ac6/BqsF*+++ 	S!AC%  AMM"a "s2;;q>>13///0
1
1C 00000a000
1
1C GGGGGGQGGGDDDDDD!DDDE F FH(BE MM!X]]5))***MM!Q\\!__''**U1<<??ALLQROO-T-TTUUU Q3qs7D 
QTQA	
QTQAQ1a  FVRA|44D
Bt*B	QRR,r2rl8R3
@ 
@ 
@B &&r2..MR !!##J>S    >S    >SXXa[[--bnn.>.>??????

/
4
4Cjjll $ $2w%)#####JNZ^3b87BCCviiiGXGXXXXX 3r77
DBc!BCGGCK#%&s2ww.CGGCK#%&s2ww.0D !1"c"ggckC'(R01"c"ggckC'(R0!E !A#c"gg+qs#c!B$iiQ&6q&881SWW9S=;LQ;NN!!!SWWSWW_S(,s2wwsAv~=>!QqSR[!A#&QrT36)9!);;aBimC>OPQ>QQR1"c"ggc"ggoc)#-BQ>?AE $eU)CT\`aaDAqAr1aAq!Q7O ,,,,,,,,,WWS[!Q1s2wwhR!D"ggXc\1aAs2wwR!DA1aAquQw?,,,1aBsFAq13 4 4E 2JJE 66/""e++++66/""e++++ 166/**//QtAwwY??@@JJLLQRTUPVVVVVVVr$   c                  X   t          d          } t          d          }t          dd          }t          d          \  }}}d}t          d          }t          d          }|                    |d           |                    d	d
| |j        g          }	|	                    |||j        z             |                    d||	j	        z            }
|

                    |||	           t          d|
|          }t          ||z
  g          }||z  |j	        z  }t          || g|g|          }|                    |g|
|fg          \  }}|                    dd          \  }	}}|	t          ddgdt!          |           z  |z  dgg          k    sJ |t          g           k    sJ d S )Nrf   ru   rE   L, m, t皙#@r'   N*r   r)   axisr.   pP)q_indu_indr;   T)rD   r   皙#)r   r   r   r   rQ   rI   rJ   rN   rM   rK   rR   r   r   r   rU   r`   r   )rf   ru   rm   Lrc   r!   rd   r'   pNr)   r.   r   kdeRr   r   frstarr+   inp_vecs                      r"   $test_linearize_pendulum_kane_minimalr      s   			B			B
q
!
!Ci  GAq!A 	sA	tBJJq! 	
C"ac++AMM!RV 	S!AC%  AMM"a	$1		B #(

C 	
!ACA 
Qrd2$s	;	;	;B%%rdaVH55LR LLL==MAq'Ac"gga 34555555r

??????r$   c                     t          d          \  } }t          dd          \  }}t          d          \  }}t          dd          \  }}t          d          \  }}	}
d}t          d          }t          d          }|                    |d	           t          || z            }|                    d
d||j        g          }|                    d| |j	        z  ||j
        z  z             }t          d||	          }t          ||z
  ||z
  g          }t          |||g          }|                    ||                    |                              |                              |                     t          |                    |                                          |z
  g          }t          |                    |                              |                              |j	                  g          }|                                 |                    |
          }|                                 |	|z  |j	        z  }t/          ||g|g| g|g||||	  	        }|                    |g||fg          \  }}| ||d	i}|d	|d	i}|d	|d	i}|                    |||gdd          \  }}}|                                t          d	dgd|z  d	gg          k    sJ |t          g           k    sJ d S )Nq1:3rE   levelzu1:3r   r   r'   r   r   r)   r   P1r   )r   r   r<   r>   r=   r?   acceleration_constraintsr;   TrB   r   )r   r   r   r   rQ   r   rI   rJ   rM   rK   rL   r   r   r   rO   rP   rS   	magnituder1   expressr   r   r   r   rU   r`   expand) rf   rg   rm   rn   ru   rv   r{   r|   r   rc   r!   rd   r'   r   theta1r)   r.   r   r   dq_dictrW   rX   rY   r   r   r   r   r   r   r   r+   r   s                                    r"   'test_linearize_pendulum_kane_nonminimalr      s    F##FBfA...HCF##FBfA...HCi  GAq!A 	sA	tBJJq! "R%[[F	C&!#//A 	T2ac6BqsF?++A	$1		B #((  CC#s$$G IIaB""1%%**733444 !**R..**,,q01
2
2C !%%((""1%%))!#../
0
0CLLNNN ((1++CLLNNN 	
!ACA 
Qrd2$RD!$s3
P 
P 
PB %%rdaVH55LR 2q>D2q>D!S!ELL4u*=t*. ! 0 0MAq' 88::!Q$q&! 5666666r

??????r$   c                  B   t          d          } t          dd          }t          d          \  }}}d}t          d          }t          d          }|                    |d           |                    dd	| |j        g          }|                    |||j        z             |                    d
||j	        z            }	|	
                    |||           t          d|	|          }
t          ||
          }t          || g|	||z  |j	        z  fg|          }|                                 |                    | g|gd          \  }}}t!          |          t#          ddgdt%          |           z  |z  dgg          k    sJ |t#          g           k    sJ d S )Nrf   rE   r   r   r'   r   r   r)   r   r.   r   )	forcelistframeT)rD   r   )r   r   r   r   rQ   rI   rJ   rN   rM   rK   rR   r   r   r   form_lagranges_equationsr`   r
   r   r   )rf   rm   r   rc   r!   rd   r'   r   r)   r.   r   LagLMr+   r   s                  r"   (test_linearize_pendulum_lagrange_minimalr      s   			B
q
!
!Ci  GAq!A 	sA	tBJJq! 	
C"ac++AMM!SW 	S!AC%  AMM"a	$1		B Q

C	rd1Q3qs7|nA	F	F	FB!!! LL"udL;;MAq'A&1a&4B<>12E)F"G"GGGGGr

??????r$   c                     t          d          \  } }t          dd          \  }}t          d          \  }}}d}t          d          }t          d          }	|	                    |d           t          || z            }
|                    d	d
|
|j        g          }|	                    d| |j	        z  ||j
        z  z             }|                    ||                    |	                              |                     t          d||          }t          | dz  |dz  z   |dz  z
  g          }t          ||          }t!          || |g||||z  |j	        z  fg|          }|                                 | ||d|d|d|                    |          d|                    |          di}|                    |          }|                    |           |                    |g|g| g|g|d          \  }}}t-          |          t          ddgd|z  dgg          k    sJ |t          g           k    sJ d S )Nr   rE   r   r   r   r'   r   r   r)   r   r   r   r8   )
hol_coneqsr   r   )rC   T)rC   rD   r   )r   r   r   r   rQ   r   rI   rJ   rM   rK   rL   rO   rP   r   r   r   r   r   r   solve_multipliersupdater`   r
   )rf   rg   rm   rn   r   rc   r!   rd   r'   r   r   r)   r.   r   rW   r   r   rC   lam_opr+   r   s                        r"   +test_linearize_pendulum_lagrange_nonminimalr   
  s@   F##FBfA...HCi  GAq!AsA	tBJJq!"R%[[F	C&!#//A
T2ac6BqsF?++AIIaB""1%%&&&	$1		B
"a%"a%-!Q$&'
(
(C
Q

C	r2h3Aqs13w<.XY	Z	Z	ZB!!!Ar1c1c1chhqkk1chhqkk1MH!!8!44FOOFLL"urdSEt ! - -MAq'A&1a&461+)>"?"?????r

??????r$   c                     t          d          x\  } }}}t          dd          x\  }}}}t          d          \  }}	}
t          d          }|                    dd| |j        g          }|                    dd||j        g          }|                    dd||j        g          }t          d	          }|                    |d
           |	                    d||j        z            }|
                    |||           t          ||	dz  |dz  z  |	dz  |dz  z  |	dz  |dz  z            }t          d|||	||f          }|	 |
z  |z  t          |          z  |_        t          ||          }t!          ||          }|                                 | d
|d
|d
|d
|d
|                                d
|                                d
|                                d
i}|                    |||d          d
         }t)          g dg dg dd
d
d
d
d|z  d
gd
d|
z  d|z  z  d
d|z  dz  d
d
gg dg          }||k    sJ d S )Nzq1 q2 q3rE   zr m gr'   Yr*   r   r   r,   r   Dmcr9   r8   BodyDT)r   qd_indrC   rD   )r   r   r   rE   r   r   )r   r   r   r   rE   r   )r   r   r   r   r   rE   ir@   rA   )r   r   r   r   r   r   )r   r   r   rI   rJ   rK   rL   r   rQ   rM   rR   r   r   r   potential_energyr   r   r   r   r`   r   )rf   rg   rh   rl   rm   rn   ro   rs   rb   rc   rd   r'   r   r   r   r,   r   r   r   r   lrC   r)   r   s                           r"   $test_linearize_rolling_disc_lagranger   +  st   #J///JBB'
A666MCcBgGAq!sA	C"ac++A	C"ac++A	C"ac++Ac

AIIaOOO
++eQW
%
%COOAq!1q51a4<QAq1uq!t|<<AgsAq1c(33E S1Wq[3r772E
Q

CQA    Ar1b!QQ

Asxxzz1chhjj!=H 	
!B4HHKA
$$$$$$$$$aAr#vq)bdAaCj!QsU1Wa3$$$& ' 'C 888888r$   N)#sympy.core.backendr   r   r   r   r   r   r	   r
   sympy.core.sympifyr   sympy.simplify.simplifyr   sympy.solvers.solversr   sympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   r   sympy.testing.pytestr   r   r   r   r   r   r   r   r$   r"   <module>r      s                                          & & & & & & , , , , , , ' ' ' ' ' '                          & % % % % % wW wW wWr# # #J> > >@  >  B# # # # #r$   