
    g'                         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 d dlmZ d dlmZ d dlmZmZmZ d dlmZ d dlmZ d dlmZ d	 Zd
 Zd Zd Zd Zd ZdS )    )dynamicsymbolsReferenceFramePoint	RigidBodyLagrangesMethodParticleinertia
Lagrangian)
DerivativeFunction)pi)symbols)cossintan)Matrix)simplify)raisesc                  X   t          d          \  } }}t          d          t          d          t          d          }}|                    |d           t	          dt          d          |          j                            || t                    |j        z  t                    |j
        z  z
  z             ||z  j                            |                              |j
                  z  _        t          |          t          t           fd           d S )Nl m gqNOr   Pc                  *    t           g          S )Nbodies)r   )Lr   r   s   g/var/www/html/ai-engine/env/lib/python3.11/site-packages/sympy/physics/mechanics/tests/test_lagrange.py<lambda>z*test_invalid_coordinates.<locals>.<lambda>   s    q1#a@@@     )r   r   r   set_velr   pointset_posr   xr   ypos_fromdotpotential_energyr
   r   
ValueError)lmgr   r   r   r   r   s        @@@r   test_invalid_coordinatesr.      s    gGAq!A#c

qAIIaOOOeCjj!$$AGOOAqCFFQSL3q66AC<78999Q!1!1!!4!4!8!8!=!==A1aA
:@@@@@@AAAAAr!   c                  @   t          d          \  } }t          dd          \  }}t          d          \  }}}}}t          d          }	|	                    ddt          dz  |z
  |	j        g          }
|
                    dd| |
j        g          }t          d	          }|                    |	||
j        z             ||dz  z  dz  |j        z  |j        z  }t          d
|||||f          }||z  || z
  z  t          |          z  |_        t          |	|          }| |g}| ||z  z
  g}t          |||          }|                                 |                                }|                                 |d         d|z  t          |          z  dz  k    sJ d S )Nzy theta   zm g R l alphar   AAxis   BDoD)
hol_coneqs   )r   r   r   	orientnewr   zr   r"   r%   r   r   r)   r
   r   form_lagranges_equationsrhsr   )r&   thetaydthetadr,   r-   Rr+   alphar   r1   r4   r5   Ir6   r   r   r7   r<   s                      r   test_disc_on_an_incline_planerC      s    i((HAu	1--JB00Aq!Q 	sA	C"Q$,!455A	C5&!#//A
 
tBJJq"qs(	AqD
QS13A#r1a!R))A
 Q!a%3u::5A1aA 
E
Aa%i-J1444A   
%%''CLLNNNq6QqSU^A%%%%%%%r!   c                     t          d          \  } }t          dd          \  }}t          d          \  }}}t          d          }|                    dd| |j        g          }|                    |||j        z             t          d          }	|	                    |d	           |	                    d
||j	        z            }
|

                    |	||           t          d|
|          }| |z  |z  t          |           z  |_        t          ||          }t          || g          }|                                 |                                }|d         | t%          |           z  |z  k    sJ d S )Nzq uzq u r0   r   r   r1   r2   r   r   r   Pa)r   r   r   r9   r:   set_ang_velr   r"   	locatenewr%   v2pt_theoryr   r   r)   r
   r   r;   r<   r   )r   uqdudr+   r,   r-   r   r1   r   r   rE   r   lmRHSs                  r   test_simp_penrN   J   sh    %  DAqFA&&FBgGAq!
 	sA	C!QS**AMM!R!#X
 	c

AIIaOOO	CQS!!AMM!Q 
$1		B#'A+A.B1bA 
QC	 	 B!!!
&&((Cq6aRAYq[      r!   c            	      V   t          d          \  } }t          dd          \  }}t          d          \  }}}d}t          d          }t          d          }	|	                    |d           |	                    d	| |j        z  ||j        z  z             }
|
                    ||
                    |	          	                    |                     t          d
|
|          }t          | dz  |dz  z   |dz  z
  g          }t          ||          }t          || |g||
||z  |j        z  fg|          }|                                 |j        d         }t          |t!          | ||          z  d|z  z
  d|z  | z  z   g|t!          |||          z  d|z  |z  z   gg          }|j        |k    sJ t          d| z  d|dz  z  z   d|dz  z  z   d| dz  z  |z  d|dz  z  |z  z   z  g          }t%          |                    d                    t%          |          k    sJ d S )Nzq1:3r0   )levelzL, m, tg#@r   zN*r   P1pPr3   )r7   	forcelistframe)r   r   g3@   r   )sol_type)r   r   r   r   r"   rG   r%   r&   r'   dtr   r   r
   r   r;   lam_vecr   eomr   solve_multipliers)q1q2q1dq2dr   r,   tr-   r   pNr   rR   f_cLagLMlam1eom_sollam_sols                     r   test_nonminimal_pendulumrg   s   sQ   F##FBfA...HCi  GAq!AsA	tBJJq!
T2ac6BqsF?++AIIaB""1%%&&&	$1		B
"a%"a%-!Q$&'
(
(C
Q

C	r2h31Q3qs7|nA
/ 
/ 
/B!!!:dDqB1---A5$r	ABB1---$r	9:< = =G6WtBw36)Ac1fH4qQwqy1RU7197LMNOOGB((((;;<<@Q@QQQQQQQr!   c                     t          d          \  } }t          dd          \  }}t          dd          \  }}t          d          \  }}t          dd          \  }}	t          d          \  }
}}t          d          }|                    dd| |j        g          }|                    d	d||j        g          }|                    |||j        z             |                    |||j        z             t          d
          }|                    d|
|j        z            }|                    d|
|j        z            }|	                    |d           |
                    |||           |
                    |||           t          d||          }t          d||          }| |z  |
z  t          |           z  |_        | |z  |
z  t          |           z  ||z  |
z  t          |          z  z
  |_        t          |||          }t          || |g||g          }|                                 t#          |
|z  d|z  t%          |           z  |
t%          |           z  t%          |          z  |z  z   |
t%          |           z  t          |          z  |dz  z  z   |
t%          |          z  t          |           z  |dz  z  z
  |
t          |           z  t          |          z  |z  z   d|
z  |z  z   z  |j        d         z
            dk    sJ t#          |
|z  |t%          |          z  |
t%          |           z  t%          |          z  |z  z   |
t%          |           z  t          |          z  |dz  z  z
  |
t%          |          z  t          |           z  |dz  z  z   |
t          |           z  t          |          z  |z  z   |
|z  z   z  |j        d         z
            dk    sJ |j        ||gk    sJ d S )Nzq1 q2r0   r3   zu1 u2r   r   r1   r2   r4   r   r   r@   r   ParPParRr   )r   r   r   r9   r:   rF   r   rG   r%   r"   rH   r   r   r)   r
   r   r;   r   r   rY   r   )r[   r\   r]   r^   q1ddq2ddu1u2u1du2dr+   r,   r-   r   r1   r4   r   r   r@   ri   rj   r   rL   s                          r   test_dub_penrq      s    G$$FBgq))HC++JD$G$$FBgq))HCgGAq!sA	C"ac++A	C"ac++AMM!S13YMM!S13Yc

A	CQS!!A	CQS!!AIIaOOOMM!QMM!QFAq!!DFAq!!DC!GaK#b''1DC!GaK#b''1AEAIB4GGD1dD!!A	RHdD\	:	:	:B!!!AaC1SWWqRyR'8'==
CGG)CGG
CF
"#%&s2wwYs2ww%6sAv%=>
CGG)CGG
D
 !#$Q3t8, -/1vay9 : :=>? ? ? ? AaC3r77Qs2wwYs2ww%6t%;;
CGG)CGG
CF
"#%&s2wwYs2ww%6sAv%=>
CGG)CGG
D
 !#$T6* +-/VAY7 8 8;<= = = = 9t$$$$$$r!   c            
      l   t          d          \  } }}t          dd          \  }}}t          d          \  }}}t          d          }	|	                    dd| |	j        g          }
|
                    dd||
j        g          }|                    dd||j        g          }t          d	          }|                    |	d
           |	                    d||j        z            }|
                    ||	|           t          ||dz  |dz  z  |dz  |dz  z  |dz  |dz  z            }t          d|||||f          }| |z  |z  t          |          z  |_        t          |	|          }| ||g}t!          d          } t!          d          }t!          d          }t#          ||          }|                                 |                                }|                                 t          d          }|j        dd         d
d|z  |dz  z  dz  d
gk    sJ |d                                         d|z  t-           ||                    z  |dt-          d ||          z            z  t/           | |          |          z  dt           ||                    z  t/           ||          |          z  z   z  t/           | |          |          z  z   d|z  z  k    sJ |d         dt           ||                    z  t/           | |          |          z  dt1           ||                    z  t/           ||          |          z  z   dt/           | |          |          z  t           ||                    z  z   t/           ||          |          z  k    sJ d S )Nzq1 q2 q3r0   zr m gr   Yr2   r   r@   Cr   DmcrU   r3   BodyDr[   r\   q3r_   r8         i   
   )r   r   r   r9   r:   r%   r&   r   r"   rG   rH   r	   r   r   r)   r
   r   r   r;   r<   r   mass_matrixr   r   r   )r[   r\   rw   r]   r^   q3drr,   r-   r   rs   r   r@   rt   ru   rB   rv   rb   r   r+   rM   r_   s                         r   test_rolling_discr      s     
++JBB":q11MCcgGAq! 	sA	C"ac++A	C"ac++A	C"ac++A 	c

AIIaOOO
++eQW
%
%COOAq! 	1Q3A:qsQTz1Q3A:66AgsAq1c(33E  !S1Wq[3r772E
Q

C	RA	$B	$B	$BQA   
%%''CLLNNNAM!A#1ac!Q$hqj!"44444q6??	Ac""Q%%jj1aAbbeeGnZ1q-A-AA
3rr!uu::jA***+ ,,6rr!uua,@,@A 
ACEa4	IJ J J J q6bRRUUmJrr!uua$8$881SA >
 >
 <

RRUUA

< !":bbeeQ#7#7!7BBqEE

!BC
RRUUA

      r!   N) sympy.physics.mechanicsr   r   r   r   r   r   r	   r
   sympy.core.functionr   r   sympy.core.numbersr   sympy.core.symbolr   (sympy.functions.elementary.trigonometricr   r   r   sympy.matrices.denser   sympy.simplify.simplifyr   sympy.testing.pytestr   r.   rC   rN   rg   rq   r    r!   r   <module>r      sh  9 9 9 9 9 9 9 9 9 9 9 9 9 9 9 9 9 9 9 9 7 6 6 6 6 6 6 6 ! ! ! ! ! ! % % % % % % D D D D D D D D D D ' ' ' ' ' ' , , , , , , ' ' ' ' ' '
B 
B 
B-& -& -&`&! &! &!RR R R<.% .% .%b5 5 5 5 5r!   