
    g)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#dS )    )solve)
cosexpandMatrixsinsymbolstansqrtSzeroseye)simplify)	dynamicsymbolsReferenceFramePoint	RigidBodyKanesMethodinertiaParticledotfind_dynamicsymbols)raisesc            	      B  	 t          d          \  } }}t          d          \  	                    t          j                  	z
  gt	          d          t          d          c}|                    d           t          dt          d          |          j        	                    || t                    j        z  t                    j        z  z
  z             j        | |z  j        z  ft          t          	fd           d S )Nzl m gq uNOr   Pc                  6    t          ggg g          S )Nbodies	forcelistr   )Fr   r   kdqus   c/var/www/html/ai-engine/env/lib/python3.11/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OOOeCjj!$$AGOOAqCFFQSL3q66AC<78999	
1"q&13,A
: : : : : : : : : : ; ; ; ; ;r)   c                     t          d          \  } }t          dd          \  }}t          d          \  }}}t          d          }t          d          }|                    |||j        z             ||z
  g}	|| | z  ||z  z
  |j        z  fg}
t          d||          }|g}t          || g|g|	          }|                    ||
           |j	        |k    sJ |j
        |
k    sJ |j        }|j        }|                                |z  }t          |d                   t          | |z  ||z  z    |z            k    sJ t          |                                |j                            |j                  z
            t)          dd          k    sJ |                    d	
          d         t-          ddg| |z  | |z  gg          k    sJ d S )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QS
q&BrAvA~$
%	&B	$1		B
B	QaS"	%	%Br29????8r>>>>	BjG
&&((W
C#a&>>Va!ea!em$4q$8999999BFFHH'//@@A B BEJ1a[[Q Q Q Q LLL((+v1v1qbd|6L/M/MMMMMMMr)   c            	         t          d          \  t          dd          \  } }}}t          d          \  }}}}}t          d          t          d          }	t          d          }
|	                    j        z             |
                    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
        }|                                |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 ]^}||_        t%          |                                |j                            |j                  z
            t/          dd          k    sJ _| dz  z
  t1          |          t3                    z
  gt5          t6          fd           d S )Nq1 q2 u1 u2r7   m c1 c2 k1 k2r   P1P2r:   pa1pa2q_indu_indkd_eqsr   F)FT   c                  2    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  F  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22NBB'q99Cc300Ar2r2sA	tB	tBJJq"qs(JJq27ac/""" 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	B	B	BBr2	BjG
&&((W
C#a&>>VbS2XR%7"r'%ABG%KQ$NOOOOOO#a&>>VR"WrBw%6R"%Dq$&H')+H, &,/0%1 2 2 2 2 2 2 !!!!Q'''' #B!A$$q&!q!f(=!>!>>>>>  - U U!4+33BODDE F FINqRSU U U U U
 A+s3xx#b'')	*B
: F F F F F F F F F G G G G Gr)   c                     t          d          \  } }t          dd          \  }}t          d          \  }}}t          d          }t          d          }|                    || |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        }|                                |z  }|                                 t!          |d                   t!          | |z  t          |           z            k    sJ t          |                                |j                            |j                  z
            t+          dd          k    sJ d S )	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&&13&QQ!#)==>>>
q&Ba!eack
	B	$1		B
B	QaS"	%	%Br2	BjG
&&((W
CLLNNN#a&>>VQBFSVVO444444BFFHH'//@@A B BEJ1a[[Q Q Q Q Q Qr)   c                     !" t          d          \  } }}}}}t          dd          \  }}}}	}
}t          d          \  }}}t          d                                dd|  j        g          }|                    dd||j        g          }|                    dd||j        g          !!                               "!                     ||j        z  ||j        z  z   ||j        z  z              t          d	          }|
                     d
           |                    d||j        z            }|                    | !           t          ||dz  |dz  z  |dz  |dz  z  |dz  |dz  z            } !"fd|D             }|| |z  |j        z  fg}t          d|!|||f          }|g}t           | ||g|||g|          }|                    ||           |j        }|j        }|                                |z  }|                                }|                    |          }|                                 |                                t1          d|z  |z  |z  |dz  |z  t3          |          z  z
  d|z  t5          |          z  z   d|z  z  d|z  |z  dz  |d|z  |t3          |          z  z   z  g                                          k    sJ t-          |                                |j                            |j                  z
            t?          dd          k    sJ |                     d          d
         }|                    |d|d|di                              | d
|d
|d
|d
|d
i          }d
d l!}|"                    |                    |dtG          d          z  i                    $                                tJ          j&        dik    sJ d S )Nq1 q2 q3 u1 u2 u3r7   r m gr   YAxisLRCr   Dmcr[   r:   c                 \    g | ](}t                                        z
  |          )S  r   
ang_vel_in.0uvr   rt   w_R_N_qds     r'   
<listcomp>z%test_rolling_disc.<locals>.<listcomp>   2    	:	:	:"#all1oo("
-
-	:	:	:r)   BodyDrW            Tr;   )'r   r   r   	orientnewzr/   r0   rz   set_ang_velr   r,   	locatenewv2pt_theoryr   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   rq   rs   ru   rv   Ir$   	ForceListr   BodyListrM   rN   r@   rB   kddA	A_uprightr   r   rt   r~   s#                                   @@@r'   test_rolling_discr      s    ,,?@@BBB#12Eq#I#I Cc3SgGAq! 	sA	C"ac++A	C"ac++A	C"ac++A||AHMM!R!#XQS(283444 	c

AIIaOOO
++eQW
%
%COOAq! 	1q51a4<QAq1uq!t|<<A 
;	:	:	:	:	:	:	:	:B sQw}%&IgsAq1c(33EwH 
Qr2rl2r2,r	J	J	JBx+++	BjG
&&((W
C
..

C
((3--CLLNNN::<<6AbDGAIAaB$?	!CGG%c$rE"HQJBrEBs2wwJ,>(?#A B BBH&((K K K KBFFHH'//@@A B BEJ1a[[Q Q Q Q 	T""1%A1aAq)**//QAr1b!RQR0STTILLL==Qa[(9::;;EEGGAFTU;VVVVVVr)   c            	        $%& t          d          \  } }}}}}t          dd          \  }}}}	}
}t          d          \  }}}}t          dd          \  }}t          d          \  }}}t          d          $$                    dd| $j        g          }|                    d	d||j        g          }|                    d
d||j        g          %%                    $          &%                    $||j        z  ||j        z  z   ||j        z  z              t          d          }|
                    $||j        z  ||j        |j        z  z  z              |                    d||j        z            }|                    |$%           |                    |$%           t          ||dz  |dz  z  |dz  |dz  z  |dz  |dz  z            }$%&fd|D             }|| |z  |j        z  f|||j        z  ||j        |j        z  z  z   fg}t          d|%|||f          }|g}t!          $| ||g|||||g|          }|                    ||          \  }} |                    |d|di                              |d|di          }|                     |d|di                              |d|di          } t!          $| ||g|||g|||g          }!|!                    ||          \  }"}#|"                    |d|di                              |d|di          }"|#                    |d|di                              |d|di          }#|                                  |#                                 ||"z
                                  t+          g d          k    sJ | |#z
                                  t+          g d          k    sJ d S )Nro   r7   zu4, u5, f1, f2zu4, u5rp   r   rq   rr   rs   rt   ru   rv   r[   r:   c                 \    g | ](}t                                        z
  |          )S rx   ry   r{   s     r'   r   ztest_aux.<locals>.<listcomp>   r   r)   r   rW   r   )rX   rY   rZ   u_auxiliary)r   r   r   r   r   )r   r   r   r   r   r/   r0   rz   r   r   r,   r   r   a2pt_theoryr   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   rq   rs   ru   rv   r   r$   r   r   r   rM   frfrstarKM2fr2frstar2r   rt   r~   s'                                       @@@r'   test_auxr      s    ,,?@@BBB#12Eq#I#I Cc3S#$455NBBh**HCgGAq!sA	C"ac++A	C"ac++A	C"ac++A||AHMM!R!#XQS(283444c

AIIaacB!#),,---
++eQW
%
%COOAq!OOAq!1q51a4<QAq1uq!t|<<A	:	:	:	:	:	:	:	:	:BsQw}%28bAC!#I6F+F'GHIgsAq1c(33EwH	Qr2rl2r2r22F
  
  
 B%%h	::LR	#q#q!	"	"	'	'QA	7	7B[[#q#q)**//QA??F
aB|BB<#%r(, , ,C((9==NS'
((CC#
$
$
)
)2q"a.
9
9CllCC+,,112q"a.AAG
OOH&"9"99999W$$&&&*A*AAAAAAAr)   c                     t          d          } t          d          \  }}t          d          \  }}}t          d          }t          d          \  }}}	t          d          \  }
}t          dd          \  }}t          d          \  }}t          dd          \  }}t          d	          }t          d
          }|                    |d| |j        g           |                    || |j        z             t          d          }|                    d|
|j        z            }|                    d||j	        z            }|
                    |d           |
                    |||j        z             |                    |||           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        }|d         |	k    sJ d S )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   	  sv    cllGFOOEAr	""IAr2sA$$JBB G$$FBgq))HCG$$FBgq))HC 	sAsAHHQ"ac
###MM!bS13Y 	c

A 	
Cac""A	
T1qs7	#	#B IIaOOOIIaacNN1aFAr""DRBBB1G1G0KLLH b#(#HhHqsdWnr)*acTG^b()acTAXb)*QS1WI
 
QR2r(H	5	5B%%h	::LR		Bd8r>>>>>>r)   c                      t          d          \  } }t          dd          \  }}t          d          \  }}}t          d          }t          d          }|                    |||j        z             ||z
  g}	|| | z  ||z  z
  |j        z  fg}
t          d||          }|g}t          || g|g|	                                |          d         t          dg          k    sJ                      |d           d         t          dg          k    sJ                      |d 	          d         t          dg          k    sJ                      |          d         t          dg          k    sJ                      |g           d         t          dg          k    sJ t          t           fd
           t          || g|g|	||
                                            d         t          | |z  || z  z
  g          k    sJ t          d          \  }}}}t          dd          \  }}}}t          d          \  }}}}}t          d          }t          d          }t          d          }|                    |||j        z             |                    |||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        }|                                |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 d S )Nr   r7   r8   r   r   r9   r   )r    r>   )r>   c                  .                          d          S )Nz	bad input)_form_fr)rM   s   r'   r(   z#test_input_format.<locals>.<lambda>[  s    r{{;7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  sz   %  DAqE1%%FBgGAq!sAc

AIIaQS
q&BrAvA~$
%	&B	$1		B
B	QaS"	%	%Bb!!!$s3333Rt44Q761#;;FFFFb--a0FA3KK????b!!!$s3333b"%%a(FA3KK7777
:7777888 
QaS"R2	>	>	>B"fqbdQqSj\&:&::::: $M22NBB'q99Cc300Ar2r2sA	tB	tBJJq"qs(JJq27ac/"""
(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	B	B	BB r2	BjG
&&((W
C#a&>>VbS2XR%7"r'%ABG%KQ$NOOOOOO#a&>>VR"WrBw%6R"%Dq$&H')+H, &,/0%1 2 2 2 2 2 2 2 2r)   c                  	   t          d          } t          d          }|                    | d           t          dd          }|                     dd|          }t          d	          }|                    d
|d         |j        z  |d         |j        z  z   |d         |j        z  z             }|dd          |z   }|d         g}g }|	                    |           }	t          d          \  }
}}|
                    | |
|j        z  ||j        z  z   ||j        z  z              |	                    |           |	z
                                  }|||j        z  ||j        z  ||j        z  gz  }|                    |           }t          d          \  }}}|                    | ||j        z  ||j        z  z   ||j        z  z              |                    |           |z
  }|||j        z  ||j        z  ||j        z  gz  }||||
||g}t          |          }|j        |z  d         dz
  g}||j        |                                z  d         gz   }	 t!          | |||||g g g d
  
        }n# t"          $ r}|d }~ww xY wt%          d          }t'          |gd dD             R  }|                    t+          d          dt+          d          di          }t-          d|||||f          }|g}|j        |j        t+          d          z  | j        z  f|j        t          d          |j        z  f|j        t          d          |j        z  fg}|                    ||           t7          d |j        D             d |j        D             z             }|j        }|j        }d|_         t7          d |j        D             d |j        D             z             } |j        }!|| z  dk     sJ ||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<   tE          |d         |d                   d         }$t          ||"z                                |d         |$i          }%|%|k    sJ d S )!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_Bc                 `    g | ]+}t          d |           |d         |d         k    rdndz  ,S )J_B_r   r7   )r   )r|   axs     r'   r   z,test_implicit_kinematics.<locals>.<listcomp>  sM     < < < ~~BqERUNNqqC < < <r)   )xxyyzzxyyzxzJ_B_xyJ_B_yzRBr4   T_zF_zc                 6    g | ]}|                                 S rx   	count_opsr|   r/   s     r'   r   z,test_implicit_kinematics.<locals>.<listcomp>       0001000r)   c                 6    g | ]}|                                 S rx   r   r   s     r'   r   z,test_implicit_kinematics.<locals>.<listcomp>       4441444r)   c                 6    g | ]}|                                 S rx   r   r   s     r'   r   z,test_implicit_kinematics.<locals>.<listcomp>  r   r)   c                 6    g | ]}|                                 S rx   r   r   s     r'   r   z,test_implicit_kinematics.<locals>.<listcomp>  r   r)   g?r   r   )#r   r   r,   r   r   r   r/   r0   r   rz   r   r   velr   Tr*   r   	Exceptionr   r   r   r   r   
masscentermassframer=   sumrE   rC   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   J_Br   rigid_bodies
force_list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  ~  s    

C'NNE	MM#q <d333Ec<//A 7##E??658AC<%(13,#>q!##MNND!""IE1XJEM S!!IW%%GAq!MM#ququ}qu,---LL%%	1;;==L qsqsqs M xx}}HW%%GAq!LLaeaemae+,,, HHSMMH,L "QS("QS("QS( M 1aAE uIK	)1-12K!ikINN4D4D&Da%H$IIMeU &1 " %
 
 
     %..C
! = < <:< < < = = =C
((AhKKAhKK3
4
4C	4q#T{	3	3B4L 
#su,-	>%((,-	u--ac12J |Z000 0000044 3444	5 N
  "1>!B0000044 3444	5 N >N*S0000 %rtyy{{25IIfUbNcNccccc
 *N	
QBQB	
QQB	
QBQ	
QQ	   
 

  +1-N2*1-N1*1-N1*1-N1 Q166q9L5FGGLLiXYl\hMijjM0000000s   I0 0
J:I<<Jc                     t          d          \  } }}}t          d          \  }}}}}}	t          d          }
t          d          }|                    |
|||fd           |                    |
          }|                    |j                  |z
  |                    |j                  |z
  |                    |j                  |	z
  g}t          d          }|
                    |
d           |                    d| |j        z            }|
                    |d           t          d	||          }t          |
|||g|||	g||g|| | z  |
j        z  fg
          }|                                 ||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                                      ||||||	h          sJ t+          |j        |z
            t/          dd          k    sJ t+          |j        |z
            t/          dd          k    sJ d S )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_fixedrz   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    s   ##JAq!Q+K88BBBsAsABB<///LLOOEIIacNNR13"!4eiinnr6IJDc

AIIaOOO	
TA28	$	$BJJq!b!Aq2r2,RdA3#%rAv|"4!57 7 7Da1f*vyyy)))YYY&GHHHK	
1aRR3r773r77?3r77#::;ad2gE	FG	
acA2s2wws2wwRRR)@@AAbDGKLMO P PK t|,,55r2r2r26NOOOOOD${233uQ{{BBBBDL;.//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  rx   r)   r'   <module>r     s        , , , , , , , , , , , , , , , , , , , , , , , , , , , , , ,? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ( ' ' ' ' '; ; ;O O O@8G 8G 8GtQ Q Q0GW GW GWT1B 1B 1Bh6 6 6p:2 :2 :2zQ1 Q1 Q1f? ? ? ? ?r)   