
    g3                         d dl 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)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$%          }1|1'                    |4|5|6|7gdd          \  } }!|                     |8          |9k    sJ |!                    |8          |:k    sJ d S )&Nz	t r m g vzq1:7c                 :    g | ]}|                               S  diff).0qits     h/var/www/html/ai-engine/env/lib/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>6   s-    000#aeeAhh##000r#   c                 \    g | ](}t                                        z
  |          )S r   )r   
ang_vel_in)r   r1   r+   r&   w_c_n_qds     r!   r"   z4test_linearize_rolling_disc_kane.<locals>.<listcomp>9   s2    GGGrs8all1oo5r::GGGr#   c                 \    g | ](}t                                        z
  |          )S r   r/   )r   r1   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   rD   )r   r   r   r   r   rD   r   r   )r   r   r   r   r   r   rD   r   )r   r   r   r   r   r   r   r         GJ)linear_solver),r   r   r   r   	orientnewzxy	locatenewr3   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_qdrX   rY   kindiffsqdotsF_COIJI_C_COr9   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    r6   r4   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 !!!55J$eU)C(,t   = =DAq66/""e++++66/""e++++++r#   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 )Nrg   rv   rD   L, m, t皙#@r&   N*r   r(   axisr-   pP)q_indu_indr:   T)rC   r   皙#)r   r   r   r   rR   rJ   rK   rO   rN   rL   rS   r   r   r   rV   ra   r   )rg   rv   rn   Lrd   r    re   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 |                    |||gddd          \  }}}|                                t          d	dgd|z  d	gg          k    sJ |t          g           k    sJ |                    |||gddd           \  }}}|                                t          d	dgd|z  d	gg          k    sJ |t          g           k    sJ d S )Nq1:3rD   levelzu1:3r   r   r&   r   r   r(   r   P1r   )r   r   r;   r=   r<   r>   acceleration_constraintsr:   TrA   r   rH   )rB   rC   r   rI   c                 ,    |                      |          S NLUsolver(   bs     r!   <lambda>z9test_linearize_pendulum_kane_nonminimal.<locals>.<lambda>   s    AIIaLL r#   )r   r   r   r   rR   r   rJ   rK   rN   rL   rM   r   r   r   rP   rQ   rT   	magnituder0   expressr   r   r   r   rV   ra   expand) rg   rh   rn   ro   rv   rw   r|   r}   r   rd   r    re   r&   r   theta1r(   r-   r   r   dq_dictrX   rY   rZ   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

???? LL4u*=t)-T ! C CMAq' 88::!Q$q&! 5666666r

????LL4u*=)-*./H/H ! J JMAq'
 88::!Q$q&! 5666666r

??????r#   c                      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 |                    | g|gdd          \  }}}t!          |          t#          ddgdt%          |           z  |z  dgg          k    sJ |t#          g           k    sJ d S )Nrg   rD   r   r   r&   r   r   r(   r   r-   r   )	forcelistframeT)rC   r   rH   )rC   rI   )r   r   r   r   rR   rJ   rK   rO   rN   rL   rS   r   r   r   form_lagranges_equationsra   r   r   r   )rg   rn   r   rd   r    re   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<>1*=!>??????r

???? LL"ud$LOOMAq'A;;&1a&4B<>1*=!>??????r

??????r#   c                  L   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 |                    |g|g| g|g|dd           \  }}}t-          |          t          ddgd|z  dgg          k    sJ |t          g           k    sJ d S )Nr   rD   r   r   r   r&   r   r   r(   r   r   r   r7   )
hol_coneqsr   r   )rB   T)rB   rC   r   c                 ,    |                      |          S r   r   r   s     r!   r   z=test_linearize_pendulum_lagrange_nonminimal.<locals>.<lambda>L  s    !"1 r#   )rB   rC   rI   )r   r   r   r   rR   r   rJ   rK   rN   rL   rM   rP   rQ   r   r   r   r   r   r   solve_multipliersupdatera   r   )rg   rh   rn   ro   r   rd   r    re   r&   r   r   r(   r-   r   rX   r   r   rB   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+!6777777r

???? LL"urdSEH)- >. >. ! / /MAq' A;;&1a&461+!6777777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 q3rD   zr m gr&   Yr)   r   r   r+   r   Dmcr8   r7   BodyDT)r   qd_indrB   rC   )r   r   r   rD   r   r   )r   r   r   r   rD   r   )r   r   r   r   r   rD   ir?   r@   )r   r   r   r   r   r   )r   r   r   rJ   rK   rL   rM   r   rR   rN   rS   r   r   r   potential_energyr   r   r   r   ra   r   )rg   rh   ri   rm   rn   ro   rp   rt   rc   rd   re   r&   r   r   r   r+   r   r   r   r   lrB   r(   r   s                           r!   $test_linearize_rolling_disc_lagranger   Q  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)"sympyr   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   A A A A A A A A A A A A A A A A A A & & & & & & , , , , , , ' ' ' ' ' '                          & % % % % % , , ,B# # #JN N Nb# # #L& & &P# # # # #r#   