
    MZd+                        d dl mZmZmZmZmZmZmZmZ d dl	m
Z
mZmZ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mZmZ d dlmZ  ed      \  ZZ Z!Z"Z# ed      Z$e$jK                  dd	ee$jL                  g      Z'e'jK                  d
d	e e'jP                  g      Z)e)jK                  dd	e!e)jT                  g      Z+d Z,d Z-d Z.d Z/d Z0d Z1d Z2d Z3d Z4d Z5d Z6d Z7y)    )sincostanpisymbolsMatrixSFunction)ParticlePointReferenceFrame	RigidBody)angular_momentumdynamicsymbolsinertiainertia_of_point_masskinetic_energylinear_momentumouterpotential_energymsubsfind_dynamicsymbols
Lagrangian)gravitycenter_of_mass_validate_coordinates)raiseszq1 q2 q3 q4 q5NAAxisBCc            	         t        d      } t        d      \  }}}t        d      \  }}}t        | |||      || j                  | j                  z  z  || j                  | j                  z  z  z   || j
                  | j
                  z  z  z   k(  sJ t        | ddd      d| j                  | j                  z  z  k(  sJ t        t        d        t        | ||||||      || j                  | j                  z  z  || j                  | j                  z  z  z   || j                  | j
                  z  z  z   || j                  | j                  z  z  z   || j                  | j                  z  z  z   || j                  | j
                  z  z  z   || j
                  | j                  z  z  z   || j
                  | j                  z  z  z   || j
                  | j
                  z  z  z   k(  sJ y )Nr   zixx iyy izzzixy iyz izxr   c                      t        dddd      S )Nr   )r        N/usr/lib/python3/dist-packages/sympy/physics/mechanics/tests/test_functions.py<lambda>ztest_inertia.<locals>.<lambda>   s    gaAq1 r&   )r   r   r   xyzr   	TypeError)r   ixxiyyizzixyiyzizxs          r'   test_inertiar3      s   sAM*MCcM*MCc1c3$acc	):SSS133Y> *qss+*, - - -1aA!qssQSSy/111
9121c3S#s3qssQSSy8I13399 #qssQSSy 1924713394E9FHK	
qssI9QSS133Y'9(*-qss*;9<>AQSSCCF ?9 !##)$9% & & &r&   c                  j   t        d      \  } }}}t        d      }| |j                  z  }t        |||      }||| dz  z  |j                  |j                  z  z  || dz  z  |j
                  |j
                  z  z  z   k(  sJ ||j                  z  }t        |||      }|||dz  z  |j                  |j                  z  z  ||dz  z  |j
                  |j
                  z  z  z   k(  sJ ||j
                  z  }t        |||      }|||dz  z  |j                  |j                  z  z  ||dz  z  |j                  |j                  z  z  z   k(  sJ ||z   |z   }	t        ||	|      }|||dz  |dz  z   z  |j                  |j                  z  z  || z  |z  |j                  |j                  z  z  z
  || z  |z  |j                  |j
                  z  z  z
  || z  |z  |j                  |j                  z  z  z
  || dz  |dz  z   z  |j                  |j                  z  z  z   ||z  |z  |j                  |j
                  z  z  z
  || z  |z  |j
                  |j                  z  z  z
  ||z  |z  |j
                  |j                  z  z  z
  || dz  |dz  z   z  |j
                  |j
                  z  z  z   k(  sJ y )Nzr s t mr      )r   r   r)   r   r*   r+   )
rstmr   pxIpypzps
             r'   test_inertia_of_point_massr?   $   s   #JAq!QsA	
QSSBaQ'AAqDACC!##I&QTQSS133Y)?????	
QSSBaQ'AAqDACC!##I&QTQSS133Y)?????	
QSSBaQ'AAqDACC!##I&QTQSS133Y)?????
R"AaA&AadQTk"accACCi0QaccACCi()QaccACCi() QaccACCi() adQTk"accACCi0	1
 QaccACCi() QaccACCi() QaccACCi() adQTk"accACCi01 2 2 2r&   c                     t        d      t        d      } | j                  dj                  z         t	        j
                  j
                        }t        d| d|| f      t        d      }t        d|d      j                  j                  d	j
                  z         t        t        fd
       t        t        fd       t              d	j
                  z  dj                  z  z   k(  sJ y )Nr   Ac   r      PPa   
   c                      t                S Nr   r   rE   s   r'   r(   z&test_linear_momentum.<locals>.<lambda>J       oaB7 r&   c                      t                S rI   rJ   r   rE   s   r'   r(   z&test_linear_momentum.<locals>.<lambda>K   rL   r&   i  )r   r   set_velr*   r   r)   r   r   pointr   r,   r   )rA   r;   rD   r   r   rE   s      @@@r'   test_linear_momentumrQ   A   s    sA	tBJJq"qss(acc133A#r1b1b'*Ac
A	$1	BHHQQSS!
978
9781a$QSS39(<<<<r&   c                    	
 t        d      \  } }}}t        d      }t        d      
t        d      }t        d      j	                  d|
j
                  z        }|j	                  d|
j
                  z        }j                  
d
j
                  z         |j                  
|
j                  z         |j                  
|       |j                  
|       t        d	||       t        d
||||t        
j                  
j                        z  |f      	d| z  |z  |z  
j                  z  ||z  |z  
j                  z  z   }t        
	      |k(  sJ t        t         	
fd       t        t         	fd       t        t         
fd       |||dz  z  z   d| z  |dz  z  z   |z  
j                  z  }t#        
	      |k(  sJ y)a   A rod with length 2l, centroidal inertia I, and mass M along with a
    particle of mass m fixed to the end of the rod rotate with an angular rate
    of omega about point O which is fixed to the non-particle end of the rod.
    The rod's reference frame is A and the inertial frame is N.z
m, M, l, Iomegar   aOrA   rD   r   rE   r   r5   c                       t               S rI   r   )r   r   rE   s   r'   r(   z;test_angular_momentum_and_linear_momentum.<locals>.<lambda>c       .q!Q; r&   c                       t               S rI   rW   )r   rU   rE   s   r'   r(   z;test_angular_momentum_and_linear_momentum.<locals>.<lambda>d   rX   r&   c                       t               S rI   rW   )r   rU   rE   s   r'   r(   z;test_angular_momentum_and_linear_momentum.<locals>.<lambda>e   rX   r&      N)r   r   r   r   	locatenewr)   rO   set_ang_velr+   v2pt_theoryr   r   r   r*   r   r   r,   r   )r9   Mlr;   rS   rT   rA   rD   expectedr   r   rU   rE   s            @@@@r'   )test_angular_momentum_and_linear_momentumrb   O   s   
 &JAq!Q7#EsAsAc
A	
T1qss7	#B
S!acc'"AIIaQSSMM!UQSS[!NN1aMM!Q	$1	B#r1a!eACCo"5r!:;A1uu}q 133&Q)<<H1a$000
9;<
9;<
9;<A1Hq1uq!t|+u4qss:HAq!R(H444r&   c                  *  	
 t        d      \  } }}t        d      }t        d      
t        d      }|j	                  
d
j
                  z         |j                  d|
j
                  z        }|j                  d|
j
                  z        }t        d      }|j                  
|
j                  z         |j                  |
|       |j                  |
|       t        d	||       t        
j                  
j                        }t        d
|||||f      	t        t        	fd       t        t        	
fd       dt        
	      ||dz  z  |dz  z  dz  d|dz  z  | z  |dz  z  z   |dz  dz  z   z
  j!                         k(  sJ y )Nzm M l1rS   r   rU   r   rA   rD   rT   rE   r   c                      t               S rI   r   rK   s   r'   r(   z%test_kinetic_energy.<locals>.<lambda>y   s    nRQ7 r&   c                      t               S rI   re   )r   r   s   r'   r(   z%test_kinetic_energy.<locals>.<lambda>z   s    nQ15 r&   r5   )r   r   r   r   rO   r)   r\   r]   r+   r^   r   r   r   r   r,   r   expand)r9   r_   l1rS   rU   rA   rD   rT   r;   r   r   rE   s            @@@r'   test_kinetic_energyri   j   sq   x HAq"7#EsAc
AIIaQSS	
T28	$B
S"qss(#AsAMM!UQSS[!NN1aMM!Q	$1	Bacc133A#r1a!R)A
978
9562q)Qr1uWUAX-=a-?Agaiq .!#(!8A:.. /069 9 9r&   c                     t        d      \  } }}}}}t        d      }t        d      }t        d      }|j	                  |d|j
                  z         |j                  d||j
                  z        }	|	j                  d||j
                  z        }
t        d      }|j                  |||j                  z         |	j                  |||       |
j                  |||       t        d	|
|       }t        |j                  |j                        }t        d
|	||||	f      }| |z  |z  |_        ||z  |z  |_        t        ||      | |z  |z  ||z  |z  z   k(  sJ y )Nzm M l1 g h HrS   r   rU   r   rA   rD   rT   rE   r   )r   r   r   r   rO   r)   r\   r]   r+   r^   r   r   r   r   )r9   r_   rh   ghHrS   r   rU   rA   rD   rT   rE   r;   r   s                  r'   test_potential_energyrn      s=   /Aq"aA7#EsAc
AIIaQSS	
T28	$B
S"qss(#AsAMM!UQSS[!NN1aMM!Q	$1	Bacc133A#r1a!R)Aa%!)BQAAr"a!eai!a%!)&;;;;r&   c                    	
 t        d      \  } }}}t        d      
t        d      }|j                  
d
j                  z         |j                  dd
j                  z        }|j                  
d
j                  z         t        d|d      |j                  d	d

j                  z        }|j                  
d
j                  z         t        d      }|j                  
d
j                  z         t        
j                  
j                        }t        d||d||f      	||z  |z  _        | |z  |z  	_        t        t        	fd       t        t        
fd       y )NzM m g hr   rU   r   rD   rF   rG   rE   rA   r5      rT   r   rC   c                      t                S rI   r   rK   s   r'   r(   z!test_Lagrangian.<locals>.<lambda>       jAr2 r&   c                      t                S rI   rr   rN   s   r'   r(   z!test_Lagrangian.<locals>.<lambda>   rs   r&   )r   r   r   rO   r)   r\   r   r*   r]   r+   r   r   r   r   r,   )r_   r9   rk   rl   rU   rD   rA   rT   r;   r   r   rE   s            @@@r'   test_Lagrangianru      s'   #JAq!QsAc
AIIaQSS	CQSS!AIIaacc	$1	B	
T1qss7	#BJJq!acc'sAMM!R!##Xacc133A#r1b1b'*Aa%!)BQA
923
923r&   c            	         t        d      \  } }t        d      \  }}}t        | |z  |z   ||j                         z  |z   g|j                         j                         |t	        |j                               z   gg      }t        | |z   |g|j                         j                         dgg      }|d|d|j                         d|j                         di}t        ||      |k(  sJ t        ||z         t        ||z         z  ||j                         z  z   }|d|t        dz  |j                         di}t        ||d      |dz   k(  sJ t        d      }||j                  z  ||j                  z  z   }	||j                  |j                  z  z  ||j                  |j                  z  z  z   }
d|j                  z  }d|j                  |j                  z  z  }|d|di}t        |	|      |k(  sJ t        |
|      |k(  sJ y )	Na, bx, y, zrF   r   r5   T)smartr   )r   r   r   diffr   r   r   r   r   r   r)   r*   )rT   br)   r*   r+   exprsolsdr   vdv_sold_sols                r'   
test_msubsr      s   6?DAqY'GAq!AaC!GQqvvxZ!^,FFHMMOQQVVX%679 :D
1q5!*668==?A&( )C
Q1affh1668Q	/Br?c!!!q1u:c!a%j 1QVVX:-D
Q2a41	%Br&!a%///sA	!##!##A	133qss7aQSSk!AaccEEqss133wKE
Q1BB<5   B<5   r&   c            	      0   t        d      \  } }t        d      \  }}}t        | |z  |z   ||j                         z  |z   g|j                         j                         |t	        |j                               z   gg      }||j                         ||j                         j                         ||j                         h}t        |      |k(  sJ |||g}|j                         |j                         j                         |j                         h}t        ||      |k(  sJ t        d      \  }}	}
t        d      }||j                  z  |	|j                  z  z   |
|j                  z  z   ||	|
h}t        |      |k(  sJ t        t        fd       y )Nrw   rx   )excludezd, e, fr   )reference_framec                      t               S rI   )r   )r   s   r'   r(   z*test_find_dynamicsymbols.<locals>.<lambda>   s    215 r&   )r   r   r   rz   r   r   r   r)   r*   r+   r   
ValueError)rT   r{   r)   r*   r+   r|   r}   exclude_listr   efr   r   s               @r'   test_find_dynamicsymbolsr      s^   6?DAqY'GAq!AaC!GQqvvxZ!^,FFHMMOQQVVX%679 :D affh1668==?Aqvvx
8Ct$+++q!9L668QVVX]]_affh
/Ct\:cAAAY'GAq!sA	ACC!acc'AG#Aa)Cq!4;;;
:56r&   c                  `   t        d      } t        d      \  }}}t        d      \  }}t        d      }t	        d||      }t        d      }t        d      }	t        |j                  |j                        }
t        d|	|||
|	f      }||f|	|fg}|j                  t        || j                  z  ||             ||f|	|f|||z  | j                  z  f|	||z  | j                  z  fg}t        t        |            D ]1  }t        t        ||               D ]  }||   |   ||   |   k(  rJ  3 y )	Nr   zm M gzF1 F2popar   rD   r!   )r   r   r   r   r   r   r)   r   extendr   r*   rangelen)r   r9   r_   rk   F1F2r   r   r   rD   r;   r!   	forceListr`   ijs                   r'   test_gravityr      s5   sAgGAq!G$FB	tB	$A	BsAc
Aacc133A#q!QA'AbAr7#IWQqssUB*+
bAr7R1QSSMAqs133w<8A3q6] .s1Q4y! 	.AQ<?ad1g---	..r&   c                     t        d      } t        dd      }t        dt        d      t        j
                        }t        dt        d      t	        d	            }t        d
t        d      t	        d            }t        dt        d      |      }t        d      }t        d      }t        d      }t        d|||t        |j                  |j                        |f      }	|j                  j                  |j                  | j                         |j                  j                  |j                  | j                  | j                  z          |j                  j                  |j                  | j                         |	j                  j                  |j                  | j                  | j                  z          t        d      }
|
j                  |j                  t        |j                  |||||	             d||z   dz   z  | j                  z  ||z   dz   ||z   dz   z  | j                  z  z   |||z   dz   z  | j                  z  z   }|
j                  |j                        |z
  dk(  sJ y )NrT   r9   T)realp1p1_ptp2p2_ptr5   p3p3_pt   p4p4_ptb_fb_cmmbr{   orp      r   )r   r   r   r   r	   Oner   r   r)   rP   set_posr*   
masscenterr+   r   pos_from)rT   r9   r   r   r   r   r   r   r   r{   point_or|   s               r'   test_center_of_massr      s   sA$A	$g	.B	$g!	-B	$g!	-B	$g	*B

C=D	B#tS"uSUUCEE':D&ABAHHRXXqss#HHRXXqssQSSy)HHRXXqss#LL1339-#JGOOBHHnRXXr2r2qIJa"fqj>!##R!a"fqj 9!## ==AFQJPQPSPS@SSDBHH%d*a///r&   c                  $   	
 t        d      \  
t        d      \  t        g
g       t        g       t        g
g       t        
g       t        g
gd       t        t        
fd       t        g
gd       t        t        
fd       t        t        fd       t        z   gd	       t        t        fd
       t        gd
gd	       t        t        
fd       t        z   z   gd
gd	       t        t        
fd       t         j
                  	t        d       t        dt              \  t                        gd	       t        t         fd       t        t         fd        t         _        t                        g       t        t        	fd       	t         _        y )Nz	q1:4 u1:4zs1:4speedsFcheck_duplicatesc                  (    t         gg      S rI   r   )q1q2u1u2u3s   r'   r(   z+test_validate_coordinates.<locals>.<lambda>
  s    4	Rr2rl $ r&   c                  ,    t         ggd      S NTr   r   )r   r   q3r   r   s   r'   r(   z+test_validate_coordinates.<locals>.<lambda>       4	Rr2rlT ; r&   c                  ,    t         g gd      S r   r   )r   r   r   r   r   s   r'   r(   z+test_validate_coordinates.<locals>.<lambda>  r   r&   is_dynamicsymbolsc                  $    t         z   g      S rI   r   )r   r   r   s   r'   r(   z+test_validate_coordinates.<locals>.<lambda>  s    4b2gr]C r&   r   c                  ,    t         gdgd      S Nr   Tr   r   )r   r   s1r   r   s   r'   r(   z+test_validate_coordinates.<locals>.<lambda>  s     4	Rq"bkT ; r&   c                  4    t        z   z    gdgd      S r   r   )r   r   s2s3r   s   r'   r(   z+test_validate_coordinates.<locals>.<lambda>  s%    4	b2rQGt = r&   rT   zf1:3)clsc                  6    t                        g      S rI   r   rT   f1f2s   r'   r(   z+test_validate_coordinates.<locals>.<lambda>      4beRU^D r&   c                  8    t                        g      S )Nr   r   r   s   r'   r(   z+test_validate_coordinates.<locals>.<lambda>   s    4RUBqENK r&   c                  6    t                       g      S rI   r   )r   r   r8   s   r'   r(   z+test_validate_coordinates.<locals>.<lambda>#  r   r&   )r   r   r   r   r   _tr
   )rT   r   r   r   r   r   r   r   r   r8   r   r   r   s   @@@@@@@@@@@@@r'   test_validate_coordinatesr      s   +K8BBBJBB2r2,R52r(#2r(RD)"b*2r2,RuM
: $ %2r2,RuM
: ; <
: ; < 27B-5A
:CD2r2,BuM
: ; <27R<,q"gO
: = > 	AAV*FB2a5"Q%.EB
:DE
:KLN2a5"Q%.)
:DENr&   N)8sympy.core.backendr   r   r   r   r   r   r	   r
   sympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   r   r   r   r   r   !sympy.physics.mechanics.functionsr   r   r   sympy.testing.pytestr   r   r   r   q4q5r   	orientnewr+   r   r)   r!   r*   r"   r3   r?   rQ   rb   ri   rn   ru   r   r   r   r   r   r%   r&   r'   <module>r      s    N N N0 0F F F FF F ' -. BB3KKVb!##Y'KKVb!##Y'KKVb!##Y'&2:=569*<(4(!07,.*0*%r&   