
    g!&h)T                         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 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 d dlmZ d Zd Zd Zd	 Zd
 Zd Zd Z d Z!d Z"d Z#y)    )solve)
cosexpandMatrixsinsymbolstansqrtSzeroseye)simplify)	dynamicsymbolsReferenceFramePoint	RigidBodyKanesMethodinertiaParticledotfind_dynamicsymbols)raisesc            	        	 t        d      \  } }}t        d      \  	j                  t        j                        	z
  gt	        d      t        d      c}|j                  d       t        dt        d      |      j                  j                  || t              j                  z  t              j                  z  z
  z         j                  | |z  j                  z  ft        t        	fd       y )Nzl m gq uNOr   Pc                  .    t        ggg g      S )Nbodies	forcelistr   )Fr   r   kdqus   }/var/www/pru.catia.catastroantioquia-mas.com/valormas/lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_kane.py<lambda>z*test_invalid_coordinates.<locals>.<lambda>   s!    {1qcA3A367S :     )r   diffr   _tr   r   set_velr   pointset_posr   xr   yr   
ValueError)
lmgr   r#   r   r   r$   r%   r&   s
       @@@@@@r'   test_invalid_coordinatesr5      s    gGAq!5>DAq
&&""
#a
'	(B#c
DAqIIaOeCj!$AGGOOAqCFQSSL3q6ACC<789	
1"q&133,A
: : : ;r)   c                  X   t        d      \  } }t        dd      \  }}t        d      \  }}}t        d      }t        d      }|j	                  |||j
                  z         ||z
  g}	|| | z  ||z  z
  |j
                  z  fg}
t        d||      }|g}t        || g|g|	      }|j                  ||
       |j                  |k(  sJ |j                  |
k(  sJ |j                  }|j                  }|j                         |z  }t        |d         t        | |z  ||z  z    |z        k(  sJ t        |j!                         |j"                  j%                  |j&                        z
        t)        dd      k(  sJ |j+                  d	
      d   t-        ddg| |z  | |z  gg      k(  sJ y )Nr      m c kr   r   par      TA_and_B)r   r   r   r   r,   r/   r   r   kanes_equationsr    loadsmass_matrixforcinginvr   r   rhsmass_matrix_fullLUsolveforcing_fullr   	linearizer   )r%   r&   qdudr3   ckr   r   r$   FLr9   BLKMMMr@   rB   s                    r'   test_one_dofrO      s    % DAqE1%FBgGAq!sAc
AIIaQSS
q&BrAvA~$
%	&B	$1	B
B	QaS"	%Br299??88r>>	BjjG
&&(W
C#a&>Va!ea!em$4q$89999BFFH''//@A BEJ1a[Q Q Q LLL(+v1v1qbd|6L/MMNMr)   c            	      ~   t        d      \  t        dd      \  } }}}t        d      \  }}}}}t        d      t        d      }	t        d      }
|	j	                  j
                  z         |
j	                  z   j
                  z         | dz  dz  z
  d|z  dz  z
  g|	| z  |z  z
  |z  z   |z  z   j
                  z  f|
| z  |z  z
  j
                  z  fg}t        d|	|      }t        d	|
|      }||g}t        gg
      }|j                  ||       |j                  }|j                  }|j                         |z  }t        |d         t        | z  |z  z
  |z  z   |z  z   |z        k(  sJ t        |d         t        |z  |z  z   d|z  z  z
  d|z  z  z
  |z        k(  sJ |j                  sJ |j                  t        d      k(  sJ d|_        |j                  t!        t#        d      dz  dgddgg      k(  sJ dD ]W  }||_        t%        |j'                         |j(                  j+                  |j,                        z
        t/        dd      k(  rWJ  | dz  z
  t1        |      t3              z
  gt5        t6        fd       y )Nq1 q2 u1 u2r7   m c1 c2 k1 k2r   P1P2r:   pa1pa2q_indu_indkd_eqsr   F)FT   c                  *    t         gg      S )NrW   r"   )r   r$   q1q2u1u2s   r'   r(   ztest_two_dof.<locals>.<lambda>p   s    {1RH24b" F r)   )r   r   r   r   r,   r/   r   r   r=   r?   r@   rA   r   explicit_kinematicsmass_matrix_kinr   r   r   r   rB   rC   rD   rE   r   r   r   r   r1   )q1dq2du1du2dr3   c1c2k1k2rS   rT   rK   rU   rV   rL   rM   rN   r@   rB   ra   r   r$   r]   r^   r_   r`   s                       @@@@@@r'   test_two_dofrk   9   s+   
 $M2NBB'q9Cc30Ar2r2sA	tB	tBJJq"qss(JJq27acc/" a%"Q$,#"	%B b27"R"W,rBw6!##=
>rc
G"WGF A 
B
5"a
 C
5"a
 C
sB
 
Qr2hr2hr	BBr2	BjjG
&&(W
C#a&>VbS2XR%7"r'%ABG%KQ$NOOOO#a&>VR"WrBw%6R"%Dq$&H')+H, &,/0%1 2 2 2 2 !!!!Q''' #B!A$q&!q!f(=!>>>>  - U!4++33BOODE FINqRSU 	U UU A+s3x#b')	*B
: F F Gr)   c                  (   t        d      \  } }t        dd      \  }}t        d      \  }}}t        d      }t        d      }|j	                  || |z  t        |       z  |j                  z  ||z  t        |       z  |j                  z  z          ||z
  g}	|||z  |j                  z  fg}
t        d||      }|g}t        || g|g|	      }|j                  ||
       |j                  }|j                  }|j                         |z  }|j                          t!        |d         t!        | |z  t        |       z        k(  sJ t        |j#                         |j$                  j'                  |j(                        z
        t+        dd      k(  sJ y )	Nr   r7   zm l gr   r   r9   r   r:   )r   r   r   r   r,   r   r/   r   r0   r   r   r=   r?   r@   rA   r   r   rB   rC   rD   rE   r   )r%   r&   rG   rH   r3   r2   r4   r   r   r$   rK   r9   rL   rM   rN   r@   rB   s                    r'   	test_pendrm   s   s}   % DAqE1%FBgGAq!sAc
AIIa!a#a&133&QQ!##)==>
q&Ba!eacck
	B	$1	B
B	QaS"	%Br2	BjjG
&&(W
CLLN#a&>VQBFSVO4444BFFH''//@A BEJ1a[Q Q Qr)   c                  \   t        d      \  } }}}}}t        dd      \  }}}}	}
}t        d      \  }}}t        d      }|j                  dd| |j                  g      }|j                  dd||j
                  g      }|j                  dd||j                  g      }|j                  |      }|j                  |||j
                  z  ||j                  z  z   ||j                  z  z          t        d	      }|j                  |d
       |j                  d||j                  z        }|j                  |||       t        ||dz  |dz  z  |dz  |dz  z  |dz  |dz  z        }|D cg c]   }t        |j                  |      |z
  |      " }}|| |z  |j                  z  fg}t        d|||||f      }|g}t!        || ||g|||g|      }|j#                  ||       |j$                  }|j&                  }|j)                         |z  }|j+                         } |j-                  |       }|j/                          |j1                         t3        d|z  |z  |z  |dz  |z  t5        |      z  z
  d|z  t7        |      z  z   d|z  z  d|z  |z  dz  |d|z  |t5        |      z  z   z  g      j1                         k(  sJ t/        |j9                         |j:                  j=                  |j>                        z
        tA        dd      k(  sJ |jC                  d      d
   }!|!j-                  |d|d|di      j-                  | d
|d
|d
|d
|d
i      }"d
d l"}#|#jG                  |"j-                  |dtI        d      z  i            jK                         tL        jN                  dik(  sJ y c c}w )Nq1 q2 q3 u1 u2 u3r7   r m gr   YAxisLRCr   Dmcr[   r:   BodyDrW            Tr;   )(r   r   r   	orientnewzr/   r0   
ang_vel_inset_ang_velr   r,   	locatenewv2pt_theoryr   r   r   r   r=   r?   r@   rA   kindiffdictsubsr   r   r   r	   r   rB   rC   rD   rE   r   rF   sympysympifyr
   	eigenvalsr   Zero)$r]   r^   q3r_   r`   u3rc   rd   q3dre   rf   u3drr3   r4   r   rq   rs   rt   w_R_N_qdru   rv   Iuvr$   	ForceListrw   BodyListrM   rN   r@   rB   kddA	A_uprightr   s$                                       r'   test_rolling_discr      s    ,,?@BBB#12Eq#I Cc3SgGAq! 	sA	C"acc+A	C"acc+A	C"acc+A||AHMM!R!##XQSS(2834 	c
AIIaO
++eQW
%COOAq! 	1q51a4<QAq1uq!t|<A 9:	:"#all1o("
-	:B	: sQw}%&IgsAq1c(3EwH 
Qr2rl2r2,r	JBx+	BjjG
&&(W
C
..
C
((3-CLLN::<6AbDGAIAaB$?	!CG%c$rE"HQJBrEBs2wJ,>(?#A BBH&(K K KBFFH''//@A BEJ1a[Q Q Q 	T"1%A1aAq)*//QAr1b!RQR0STI==Qa[(9:;EEGAFFTU;VVVG 
;s   )%N)c            	      P   t        d      \  } }}}}}t        dd      \  }}}}	}
}t        d      \  }}}}t        dd      \  }}t        d      \  }}}t        d      }|j                  dd| |j                  g      }|j                  d	d||j
                  g      }|j                  d
d||j                  g      }|j                  |      }|j                  |||j
                  z  ||j                  z  z   ||j                  z  z          t        d      }|j                  |||j
                  z  ||j                  |j
                  z  z  z          |j                  d||j                  z        }|j                  |||       |j                  |||       t        ||dz  |dz  z  |dz  |dz  z  |dz  |dz  z        }|D cg c]   }t        |j                  |      |z
  |      " }}|| |z  |j                  z  f|||j
                  z  ||j                  |j
                  z  z  z   fg}t!        d|||||f      } | g}!t#        || ||g|||||g|      }"|"j%                  |!|      \  }#}$|#j'                  |d|di      j'                  |d|di      }#|$j'                  |d|di      j'                  |d|di      }$t#        || ||g|||g|||g      }%|%j%                  |!|      \  }&}'|&j'                  |d|di      j'                  |d|di      }&|'j'                  |d|di      j'                  |d|di      }'|$j)                          |'j)                          |#|&z
  j+                         t-        g d      k(  sJ |$|'z
  j+                         t-        g d      k(  sJ y c c}w )Nro   r7   zu4, u5, f1, f2zu4, u5rp   r   rq   rr   rs   rt   ru   rv   r[   r:   rw   rW   r   )rX   rY   rZ   u_auxiliary)r   r   r   r   r   )r   r   r   r|   r}   r/   r0   r~   r   r   r,   r   r   a2pt_theoryr   r   r   r   r=   r   r   r   r   )(r]   r^   r   r_   r`   r   rc   rd   r   re   rf   r   u4u5f1f2u4du5dr   r3   r4   r   rq   rs   rt   r   ru   rv   r   r   r$   r   rw   r   rM   frfrstarKM2fr2frstar2s(                                           r'   test_auxr      s    ,,?@BBB#12Eq#I Cc3S#$45NBBh*HCgGAq!sA	C"acc+A	C"acc+A	C"acc+A||AHMM!R!##XQSS(2834c
AIIaaccB!##),,-
++eQW
%COOAq!OOAq!1q51a4<QAq1uq!t|<A89	:"#all1o("
-	:B	:sQw}%28bACC!##I6F+F'GHIgsAq1c(3EwH	Qr2rl2r2r22F
 B%%h	:LR	#q#q!	"	'	'QA	7B[[#q#q)*//QA?F
aB|BB<#%r(,C((9=NS'
((CC#
$
)
)2q"a.
9CllCC+,112q"a.AG
OOH&"9999W$$&&*AAAA- 
;s   %N#c                  N   t        d      } t        d      \  }}t        d      \  }}}t        d      }t        d      \  }}}	t        d      \  }
}t        dd      \  }}t        d      \  }}t        dd      \  }}t        d	      }t        d
      }|j                  |d| |j                  g       |j                  || |j                  z         t        d      }|j                  d|
|j                  z        }|j                  d||j                  z        }|j                  |d       |j                  |||j                  z         |j                  |||       t        d||      }t        d|||t        ||||	      |f      }||z
  ||z
  g}||g}||j                   | z  |z  f||j                   | z  |z  f||j                   |z  |
|z
  z  f||j                  |z  fg}t        ||
|g||g|      }|j!                  ||      \  }}|j"                  }|d   |	k(  sJ y )Nr4   zk lsza mA mCr#   zIx Iy Izzq1 q2r7   zu1 u2r   r   rr   r   ru   Aor   CartPendulum)r{   r{   )r   r   r   orientr}   r   r   r   r/   r0   r,   r   r   r   r   r   r=   rC   ) gravityrJ   lsamAmCr#   IxIyIzr]   r^   rc   rd   r_   r`   re   rf   r   r   r   ru   r   r   r   kindiffsbodyList	forceListkmr   r   mms                                    r'   test_parallel_axisr   	  s4    clGFOEAr	"IAr2sA$JBB G$FBgq)HCG$FBgq)HC 	sAsAHHQ"acc
#MM!bS133Y 	c
A 	
Cacc"A	
T1qss7	#B IIaOIIaaccNN1aFAr"DRBBB1G0KLH b#(#HhHqssdWnr)*accTG^b()accTAXb)*QSS1WI
 
QR2r(H	5B%%h	:LR			Bd8r>>r)   c                      t        d      \  } }t        dd      \  }}t        d      \  }}}t        d      }t        d      }|j	                  |||j
                  z         ||z
  g}	|| | z  ||z  z
  |j
                  z  fg}
t        d||      }|g}t        || g|g|	        j                  |      d   t        dg      k(  sJ  j                  |d       d   t        dg      k(  sJ  j                  |d 	      d   t        dg      k(  sJ  j                  |      d   t        dg      k(  sJ  j                  |g       d   t        dg      k(  sJ t        t         fd
       t        || g|g|	||
        j                         d   t        | |z  || z  z
  g      k(  sJ t        d      \  }}}}t        dd      \  }}}}t        d      \  }}}}}t        d      }t        d      }t        d      }|j	                  |||j
                  z         |j	                  |||z   |j
                  z         ||z
  ||z
  g}	|| |z  ||z  z
  ||z  z   ||z  z   |j
                  z  f|| |z  ||z  z
  |j
                  z  ff}
t        d||      }t        d||      }||f}t        |||g||g|	        j                  ||
        j                  } j                  }|j                         |z  }t        |d         t        | |z  ||z  z
  ||z  z   ||z  z   |z        k(  sJ t        |d         t        ||z  ||z  z   d|z  |z  z
  d|z  |z  z
  |z        k(  sJ y )Nr   r7   r8   r   r   r9   r   )r    r>   )r>   c                  &     j                  d      S )Nz	bad input)_form_fr)rM   s   r'   r(   z#test_input_format.<locals>.<lambda>[  s    r{{;7 r)   r   rQ   rR   rS   rT   rU   rV   rW   r:   )r   r   r   r   r,   r/   r   r   r=   r   r   r1   r?   r@   rA   r   )!r%   r&   rG   rH   r3   rI   rJ   r   r   r$   rK   r9   rL   r]   r^   r_   r`   rc   rd   re   rf   rg   rh   ri   rj   rS   rT   rU   rV   rN   r@   rB   rM   s!                                   @r'   test_input_formatr   A  s   % DAqE1%FBgGAq!sAc
AIIaQSS
q&BrAvA~$
%	&B	$1	B
B	QaS"	%Bb!!$s333Rt4Q761#;FFFb-a0FA3K???b!!$s333b"%a(FA3K777
:78 
QaS"R2	>B"fqbdQqSj\&:::: $M2NBB'q9Cc30Ar2r2sA	tB	tBJJq"qss(JJq27acc/"
(C"H	Bb27"R"W,rBw6!##=
>rc
G"WGF A 
B
5"a
 C
5"a
 C
sB	Qr2hr2hr	BB r2	BjjG
&&(W
C#a&>VbS2XR%7"r'%ABG%KQ$NOOOO#a&>VR"WrBw%6R"%Dq$&H')+H, &,/0%1 2 2 2 2r)   c                  l
   t        d      } t        d      }|j                  | d       t        dd      }| j	                  dd|      }t        d	      }|j                  d
|d   |j                  z  |d   |j                  z  z   |d   |j                  z  z         }|dd  |z   }|d   g}g }|j                  |       }	t        d      \  }
}}|j                  | |
|j                  z  ||j                  z  z   ||j                  z  z          |j                  |       |	z
  j                         }|||j                  z  ||j                  z  ||j                  z  gz  }|j                  |       }t        d      \  }}}|j                  | ||j                  z  ||j                  z  z   ||j                  z  z          |j                  |       |z
  }|||j                  z  ||j                  z  ||j                  z  gz  }||||
||g}t        |      }|j                  |z  d   dz
  g}||j                  |j                         z  d   gz   }	 t!        | |||||g g g d
      }t%        d      }t'        |gdD cg c]   }t)        d|       |d   |d   k(  rdndz  " c} }|j+                  t)        d      dt)        d      di      }t-        d|||||f      }|g}|j.                  |j0                  t)        d      z  | j                  z  f|j2                  t        d      |j                  z  f|j.                  t        d      |j                  z  fg}|j5                  ||       t7        |j8                  D cg c]  }|j;                          c}|j<                  D cg c]  }|j;                          c}z         }|j>                  } |j@                  }!d|_!        t7        |j8                  D cg c]  }|j;                          c}|j<                  D cg c]  }|j;                          c}z         }"|j@                  }#||"z  dk  sJ | |jD                  j                         z  |!z
  t        |      k(  sJ |#}$t        d|
 | | g|
d|| g|| d|
g|||
 dgg      |z  dz  }%|%d   |$d<   |%d   |$d<   |%d   |$d<   |%d   |$d<   tG        |d   |d         d   }&t        | |$z        j+                  |d   |&i      }'|'|!k(  sJ y # t"        $ r}|d }~ww xY wc c}w c c}w c c}w c c}w c c}w )NNEDNED_or   z
lambda_0:4T)realB
QuaternionzB_x:zB_cmr7   r:   zP Q RzU V WF)q_dependentrZ   configuration_constraintsvelocity_constraintsu_dependentr   ra   M_B)xxyyzzxyyzxzJ_B_J_B_xyJ_B_yzRBr4   T_zF_zg?r{   )$r   r   r,   r   r|   r   r/   r0   r}   r~   r   r   velr   Tr*   r   	Exceptionr   r   r   r   r   
masscentermassframer=   sumrE   	count_opsrC   rb   forcing_kinra   r%   r   )(r   r   q_attr   q_posr   rX   q_depkinematic_eqs	B_ang_velr   Qrt   B_ang_vel_kdB_cm_velUVWB_ref_vel_kdrY   	q_att_vecconfig_consrM   er   axJ_Br   rigid_bodies
force_listr/   n_ops_implicitmass_matrix_kin_implicitforcing_kin_implicitn_ops_explicitforcing_kin_explicitqdot_candidatequat_dot_textbooklambda_0_sollhs_candidates(                                           r'   test_implicit_kinematicsr   ~  st    
C'NE	MM#q <d3Ec</A 7#E??658ACC<%(133,#>q!###MND!"IE1XJEM S!IW%GAq!MM#ququ}qu,-LL%	1;;=L qssqssqss M xx}HW%GAq!LLaeaemae+, HHSMH,L "QSS("QSS("QSS( M 1aAE uIKK	)1-12K!ikkINN4D&Da%H$IIMeU &1 " %
 %.C
! =:< 4t~BqERUNqC < =C
((AhKAhK3
4C	4q#T{	3B4L 
#suu,-	>%(,-	u-acc12J |Z0  "010 " 3 3414	5N
  "11>>!B "010 " 3 3414	5N >>N*S000 %rttyy{25IIfUbNcccc
 *N	
QBQB	
QQB	
QBQ	
QQ	  
 

  +1-N2*1-N1*1-N1*1-N1 Q16q9L5FGLLiXYl\hMijM0000Q  
<& 	14 	14s6   T
 7%T
:T"!T'
3T,T1

	TTTc                  "   t        d      \  } }}}t        d      \  }}}}}}	t        d      }
t        d      }|j                  |
|||fd       |j	                  |
      }|j                  |j                        |z
  |j                  |j                        |z
  |j                  |j                        |	z
  g}t        d      }|j                  |
d       |j                  d| |j                  z        }|j                  |d       t        d	||      }t        |
|||g|||	g||g|| | z  |
j                  z  fg
      }|j                          ||dz  z  t        g dg dg dg      z  }t        ||z  | t!        |      t!        |      z  t!        |      t#        |      z  t#        |      z  z
  z  ||z  |	z  z
  z  gdg||z  |  t!        |      t#        |      z  t!        |      t!        |      z  t#        |      z  z   z  ||z  |z  z   z  gg      }t%        |j&                        j)                  ||||||	h      sJ t+        |j,                  |z
        t/        dd      k(  sJ t+        |j&                  |z
        t/        dd      k(  sJ y )Nzg l m cz	q1:4 u1:4r   r   zxyr   r   Por   r   r:   )r7   r   r   )r   r   r   )r   r   r7   r{   r7   )r   r   r   orient_body_fixedr~   r   r/   r0   r}   r   r,   r   r   r   r=   r   r   r   r   r@   issubsetr   r?   r   )r4   r2   r3   rI   r]   r^   r   r_   r`   r   r   r   N_w_Akdesr   r   r   kaneexpected_mdexpected_fds                       r'   test_issue_24887r    sk   #JAq!Q+K8BBBsAsABB</LLOEIIaccNR133"!4eiinr6IJDc
AIIaO	
TA28	$BJJq!b!Aq2r2,RdA3#%rAv|"4!57Da1f*vy)Y&GHHK	
1aRR3r73r7?3r7#::;ad2gE	FG	
acA2s2ws2wRRR)@@AAbDGKLMO PK t||,55r2r2r26NOOOD$${23uQ{BBBDLL;./5A;>>>r)   N)$r   r   r   r   r   r   r   r	   r
   r   r   r   sympy.simplify.simplifyr   sympy.physics.mechanicsr   r   r   r   r   r   r   r   r   sympy.testing.pytestr   r5   rO   rk   rm   r   r   r   r   r   r   r)   r'   <module>r
     sn    , , , ,? ? ? (;O@8GtQ0GWT1Bh6p:2zQ1f?r)   