
    \h#/                         S SK JrJrJrJrJrJrJr  S SKJ	r	J
r
JrJr  S SKJrJr  S SKJrJr  S rS rS rS rS	 rS
 rS rS rS rS rS rS rS rS r S r!S r"S r#g)    )SymbolsymbolssincosMatrixzerossimplify)PointReferenceFramedynamicsymbolsDyadic)inertiaBody)raiseswarns_deprecated_sympyc            
      L   [        5          [        S5      n S S S 5        W R                  S:X  d   eU R                  / :X  d   e[	        S5      nUR                  U R                  S5        U R                  nU R                  nUR                  U5      UR                  U5      :X  d   eU R                  [        S5      :X  d   e[        S5      u  pEn[        S5      u  pxn	U R                  [        U R                  XEXgX5      U R                  4:X  d   eg ! , (       d  f       GN= f)Nbodybody_masscenterr   	body_masszbody_ixx body_iyy body_izzzbody_ixy body_iyz body_izx)r   r   nameloadsr
   set_velframe
masscentervelmassr   r   r   )
r   pointcomr   ixxiyyizzixyiyzizxs
             _/var/www/auris/envauris/lib/python3.13/site-packages/sympy/physics/mechanics/tests/test_body.pytest_defaultr&      s    		!F| 
"99::#$E	MM$**a 
//CJJE775>UYYu----99{++++89MCc89MCc<<GDJJ#CM OO- - - - 
"	!s   D
D#c                     [        S5      n [        S5      n[        S5      n[        USSS5      n[	        5          [        SU XU5      nS S S 5        WR                  nUR                  nU R                  US5        UR                  U5      U R                  U5      :X  d   eUR                  U5      U R                  U5      :X  d   eUR                  U:X  d   eUR                  X04:X  d   eUR                  (       d   e[        US5      (       d   e[        US5      (       d   e[        US	5      (       d   e[        US
5      (       d   eg ! , (       d  f       GN
= f)Nrigidbody_masscenterrigidbody_massrigidbody_frame   r   rigidbody_bodyr   r   r   r   )r
   r   r   r   r   r   r   r   r   r   pos_fromr   is_rigidbodyhasattr)r(   r)   r*   body_inertia
rigid_bodyr   r   s          r%   test_custom_rigid_bodyr2      sO    !78,-N$%67O?Aq!4L		!*,@(<I
 
" 

CE  !4775>155e<<<<<< 4 = =c BBBB??n,,,,!EEEE"""":|,,,,:v&&&&:w'''':y))))# 
"	!s   E
Ec                     [        S5      n [        S5      n[        S5      n[        5          [	        SU X5      nS S S 5        WR
                  nUR                  nU R                  US5        UR                  U5      U R                  U5      :X  d   eUR                  U5      U R                  U5      :X  d   eUR                  U:X  d   e[        US5      (       a   e[        US5      (       d   e[        US5      (       d   e[        US	5      (       d   eUR                  [        S5      UR
                  4:X  d   eUR                  [        S5      :X  d   eUR                  (       a   e[        US
S
S
5      Ul        UR                  [        US
S
S
5      :X  d   eUR                  (       d   e[        5          [	        SUS9nS S S 5        UR                  (       a   eUR
                  R!                  SUR"                  5      nU[        UR                  SS
S
5      -  nXv4Ul        UR                  Xv4:X  d   eUR                  [        S5      :X  d   eUR                  (       d   eg ! , (       d  f       GN7= f! , (       d  f       N= f)Nparticle_masscenterparticle_massparticle_frameparticle_bodyr   _inertiar   r   r   r+   )r   r   )r
   r   r   r   r   r   r   r   r   r-   r   r/   r   r   central_inertiar.   	locatenewx)r4   r5   r6   r7   r   r   r   point_inertias           r%   test_particle_bodyr=   3   sP    56?+M#$45N		!_.A*< 
" 
"
"CE2775>044U;;;;<< 3 < <S AAAA...}j1111='****=,////=&))))  VAY0H0H$IIII((F1I555))))$+NAq!$DM!((GNAq!,LLLL%%%%		!_=A 
"))))$$..wHE!GM,?,?Aq$IIM*2M  ]$::::((F1I555%%%%= 
"	!, 
"	!s   I*9I<*
I9<
J
c                     [        S5      n [        S5      n[        S5      n[        5          [	        SU X5      nS S S 5        [        S5      nUWR
                  R                  -  nUR                  XSR                  5        [        UR                  5      S:X  d   eUR                  R                  UR                  S-   S5      nUR                  UR
                  S5        UR                  S   S   nUR
                  nUR                  U5      UR                  U5      :X  d   eUR                  U5      UR                  U5      :X  d   eUR                  S   S   U:X  d   eg ! , (       d  f       GN1= f)	Nr4   r5   r6   r7   ar+   _point0r   )r
   r   r   r   r   r   r;   apply_forcer   lenr   r:   _namer   r   r-   )	r4   r5   r6   r7   r?   force_vectorr   force_pointr   s	            r%   test_particle_body_add_forcerF   Y   sQ    56?+M#$45N		!_.A*< 
" 	sA}**,,,Ll,D,DE}""#q((($$..i',E	MM-%%q)%%a(+KE??5!UYYu%5555,{0KKKKq!!$444# 
"	!s   E$$
E3c                     ^	^
 [        S5      n [        S5      n[        S5      n[        USSS5      n[	        5          [        SU XU5      m
S S S 5        [        S5      n[        S5      nT
R                  R                  S	UT
R                  R                  -  5      nUR                  T
R                  S5        UT
R                  R                  -  m	T
R                  T	U5        [        T
R                  5      S:X  d   eT
R                  S   S   nT
R                  nUR                  U5      UR                  U5      :X  d   eUR!                  U5      UR!                  U5      :X  d   eT
R                  S   S   T	:X  d   eT
R                  T	5        [        T
R                  5      S
:X  d   eT
R                  S   S   T	:X  d   e[#        [$        U	U
4S j5        [#        [$        U
4S j5        g ! , (       d  f       GN= f)Nr(   r)   r*   r+   r   r,   lFarigidbody_body_point0   c                  (   > TR                  T S5      $ Nr   rA   )rD   r1   s   r%   <lambda>%test_body_add_force.<locals>.<lambda>   s    j44\AF    c                  &   > T R                  S5      $ rM   rN   )r1   s   r%   rO   rP      s    j44Q7rQ   )r
   r   r   r   r   r   r   r:   r   r;   r   zrA   rB   r   r   r-   r   	TypeError)r(   r)   r*   r0   rH   rI   r   rE   r   rD   r1   s            @@r%   test_body_add_forcerU   r   s    !78,-N$%67O?Aq!4L		!*,@(<I
 
" 	sA	B!!++	J E 
MM*""A&
((***L</z A%%%""1%a(KE??5!UYYu%5555,{0KKKKAq!\111<(z A%%%Aq!\111
9FG
9783 
"	!s   G..
G=c                  R  ^ [        5          [        S5      mS S S 5        TR                  R                  n TR	                  U 5        [        TR                  5      S:X  d   eTR                  S   TR                  U 4:X  d   e[        [        U4S j5        g ! , (       d  f       N= f)Nr   r+   r   c                  &   > T R                  S5      $ rM   )apply_torque)r   s   r%   rO   &test_body_add_torque.<locals>.<lambda>   s    d//2rQ   )	r   r   r   r;   rX   rB   r   r   rT   )torque_vectorr   s    @r%   test_body_add_torquer[      s{    		!F| 
"JJLLMm$tzz?a::a=TZZ7777
923 
"	!s   B
B&c                     [        5          [        S5      n S S S 5        [        S5      n[        5          [        SUS9nS S S 5        W R                  R	                  XR
                  5        U R                  W5      UR
                  :X  d   eU R                  U5      UR
                  :X  d   eg ! , (       d  f       N= f! , (       d  f       N= fNANBr   )r   r   r   r   r   rS   masscenter_velr^   r_   r`   s      r%   test_body_masscenter_velrd      s    		!I 
"sA		!A 
"LLCC A!##%%%A!##%%% 
"	! 
"	!s   B/C /
B= 
Cc                     [        5          [        S5      n S S S 5        [        S5      n[        5          [        SUS9nS S S 5        W R                  R	                  XR
                  5        U R                  W5      UR
                  :X  d   eUR                  U 5      UR
                  * :X  d   eU R                  U5      UR
                  :X  d   eg ! , (       d  f       N= f! , (       d  f       N= fr]   )r   r   r   r   set_ang_vely
ang_vel_inrc   s      r%   test_body_ang_velri      s    		!I 
"sA		!A 
"GG33<<?acc!!!<<?qssd"""<<?acc!!! 
"	! 
"	!s   CC"
C"
C0c                  B   [        5          [        S5      n [        S5      nS S S 5        W R                  R                  WR                  UR                  R                  S5        U R                  U5      [        [        S5      [        S5      S/[        S5      * [        S5      S// SQ/5      :X  d   eU R                  UR                  5      [        [        S5      [        S5      S/[        S5      * [        S5      S// SQ/5      :X  d   eg ! , (       d  f       N= f)Nr^   r`   
   r   )r   r   r+   )	r   r   r   orient_axisrS   dcmr   r   r   )r^   r`   s     r%   test_body_dcmrn      s    		!II 
" GGB/558vBR!4BxR!6LiXYYYY55>Vc"gs2w%:c"gXs2wPQ<RT]$^____ 
"	!s   D
Dc                     [        S5      n [        5          [        SU S9nS S S 5        WR                  U R                  :X  d   eUR                  U R                  :X  d   eUR
                  U R
                  :X  d   eg ! , (       d  f       Nc= f)Nr_   r`   ra   )r   r   r   r;   rg   rS   )r_   r`   s     r%   test_body_axisrp      sh    sA		!A 
"33!##::33!##::33!##::	 
"	!s   A>>
Bc                  f   [        S5      u  p[        S5      n[        5          [        S5      nS S S 5        U WR                  -  nXR
                  -  nUR                  XB5        UR                  X$4/:X  d   eUR                  XR5        UR                  X$U-   4/:X  d   eg ! , (       d  f       Ny= f)Nza bPr`   )r   r
   r   r   r;   rg   rA   r   )r?   brr   r`   f1f2s         r%   #test_apply_force_multiple_one_pointrv      s    5>DAc
A		!I 
"	
133B	
33BMM"77wiMM"77b5zl""" 
"	!s   B""
B0c                  0   [        S5      u  p[        S5      u  p#pE[        S5      n[        S5      n[        5          [	        S5      n[	        S5      n	S S S 5        [        S5      n
UR                  WR                  XHR                  -  5        UR                  W	R                  XYR                  -  5        X-  U
R                  -  nUR                  XX5        UR                  Xk4/:X  d   eU	R                  X{* 4/:X  d   eUR                  U-  U
R                  -  nU	R                  U-  U
R                  -  nUR                  U5        U	R                  U5        UR                  Xk4UR                  U4/:X  d   eU	R                  X{* 4U	R                  U4/:X  d   eX:R                  -  nUR                  XS9  UR                  Xk4UR                  X-   4/:X  d   eU	R                  X{* 4U	R                  U* U-   4/:X  d   eg ! , (       d  f       GN= f)	Nzf gz	q x v1 v2P1P2B1B2r_   reaction_body)r   r   r
   r   r   r   r   r   r;   rS   rA   r   r   rg   r   )fgqr;   v1v2rx   ry   rz   r{   r_   forceg1g2force2s                  r%   test_apply_forcer      s   5>DA!+.LA"	tB	tB		!$Z$Z 
" 	sAJJrxxDD!JJrxxDD!CGENN5b%88}$$$88V~%%%	133B	133BNN2NN288
R]]B$7888888Vr}}b&9::::ssUFNN6N,88bmmVY%?@@@@88Vr}}vgbj&ABBBB7 
"	!s   H
Hc                     [        S5      n [        S5      n[        5          [        S5      n[        S5      nS S S 5        [	        S5      nX-  UR
                  -  nWR                  UW5        UR                  UR                  U4/:X  d   eUR                  UR                  U* 4/:X  d   eXR                  -  nUR                  U5        UR                  UR                  XV-   4/:X  d   eg ! , (       d  f       N= f)Ntr   rz   r{   r_   )
r   r   r   r   r   r;   rX   r   r   rg   )r   r   rz   r{   r_   torquetorque2s          r%   test_apply_torquer      s    AsA		!$Z$Z 
" 	sASWFOOFB886*++++88F7+,,,,eGOOG886>23333 
"	!s   C..
C<c                  :   [        S5      n [        S5      n[        5          [        S5      nS S S 5        U WR                  -  nUR                  X15        UR                  X4/:X  d   eUR                  5         UR                  / :X  d   eg ! , (       d  f       Ne= f)Nr?   rr   r`   )r   r
   r   r   rS   rA   r   clear_loads)r?   rr   r`   r   s       r%   test_clear_loadr     sy    Ac
A		!I 
"accEEMM%77zl"""MMO77b== 
"	!s   B
Bc                  `   [        S5      n [        S5      n[        5          [        S5      nS S S 5        WR                  nUR                  nUR                  X05        UR                  XA5        UR                  X4X4/:X  d   eUR                  U5        UR                  X4/:X  d   eUR                  UR                  U5      5        UR                  X4UR                  UR                  U5      4/:X  d   eUR                  5         UR                  X4/:X  d   eg ! , (       d  f       N= f)Nrx   ry   r`   )r
   r   r   r;   rg   rA   r   remove_loadrX   crossr   )rx   ry   r`   rt   ru   s        r%   test_remove_loadr     s    	tB	tB		!I 
"	
B	
BMM"MM"77x"****MM"77xj   NN288B< 77x!''288B<!89999MMO77xj    
"	!s   D
D-c                  V   [        5          [        S5      n [        S5      n[        S5      n[        S5      nSSS5        [        S5      u  pE[        S5      u  pgp[        S5      u  pWR	                  U
W R
                  -  5        U R	                  Xd-  U R
                  -  US	9  U R	                  XtR                  5       -  U R
                  -  US	9  WR	                  UR                  U-  U R                  -  5        WR	                  UR                  U-  U R                  -  5        UR                  X-  U R                  -  US	9  UR                  XR                  -  5        UR                  UR                  XU-  -
  XtR                  5       -  -
  U R
                  -  4/:X  d   eUR                  UR                  UR                  U-  U R                  -  4UR                  XU-  -   U R                  -  4/:X  d   eUR                  UR                  UR                  U-  U R                  -  4UR                  U	* U-  U R                  -  4/:X  d   eU R                  U R                  XtR                  5       -  Xd-  -   U R
                  -  4/:X  d   eg! , (       d  f       GNh= f)
zXExample based on: https://pydy.readthedocs.io/en/latest/examples/multidof-holonomic.htmlWr`   rr   rs   Nzq1 q2zk c g kTzF Tr|   )r   r   r   r   rA   r;   diffr   rg   rX   rS   r   r   r   )r   r`   rr   rs   q1q2kcr   kTFTs               r%   9test_apply_loads_on_multi_degree_freedom_holonomic_systemr     s   		!IIII	 
"
 G$FB*%KA!% DA MM!ACC%MM!$qss(!M,MM!GGI+acc/M3MM!&&(133,MM!&&(133, NN259AN.NN1SS577qR4x!GGI+'=qss&BCDDDD77affQhqssl3aggrE	1335OPPPP77affQhqssl3aggs2vaccz5JKKKK77q{QT'9133&>?@@@@/ 
"	!s   -J
J(c            
         [        S5      n [        S5      u  pp4pV[        XX45      n[        S5      nUR	                  SXPR
                  -  X`R                  -  -   5      n	[        5          [        SXXS9n
S S S 5        W
R                  U	5      n[        XXS-  -  -   X1US-  -  -   XAUS-  US-  -   -  -   U* U-  U-  S9nX:X  d   e[        S	5      nUR                  X R                  S
5        [        U
R                  X5      U-
  R                  U5      5      [        SS5      :X  d   e[        S5      nUR	                  SXPR
                  -  X`R                  -  -   5      n	[        5          [        SXU S9nS S S 5        WR                  X5      n[        XUS-  -  XS-  -  XS-  US-  -   -  U* U-  U-  S9nUR                  (       a   eX:X  d   eg ! , (       d  f       GNf= f! , (       d  f       Nu= f)Nr_   zm, I_x, I_y, I_z, a, bopR)r   r   r   r9   rK   )r"   r^   r+      rr   )r   r   r   )r   r   r   r
   r:   r;   rg   r   r   parallel_axisrl   rS   r	   	to_matrixr   r.   )r_   mIxIyIzr?   rs   Ior   r   r   IpIp_expectedr^   rr   s                  r%   test_parallel_axisr   9  s   sA!":;A21		Bc
A	CSS1ss7*+A		!!H 
"	
	B!!d(]BQTMAqD1a4K00qb1fqjBKsAMM!SS!	
		,77:<?DQ{K K K 	c
A	CSS1ss7*+A		!!4 
"	
	B!aZ!VQq&16/5J b1fqj*K~~) 
"	! 
"	!s   )G+G&
G#&
G4N)$sympyr   r   r   r   r   r   r	   sympy.physics.vectorr
   r   r   r   sympy.physics.mechanicsr   r   sympy.testing.pytestr   r   r&   r2   r=   rF   rU   r[   rd   ri   rn   rp   rv   r   r   r   r   r   r    rQ   r%   <module>r      s{   * * * N N 1 ?-"*4#&L529B4&	"`
# CD4"	!"A8rQ   