
    g'                         d dl mZ d dl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mZmZmZmZmZmZ d dl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  ej!        Z"d Z#d Z$d Z%d Z&d Z'd Z(d Z)d Z*dS )    )expand)symbols)cossin)Matrix)trigsimp)	PinJointJointsMethod	RigidBodyParticleBodyKanesMethodPrismaticJointLagrangesMethodinertia)dynamicsymbolsReferenceFrame)raiseswarns_deprecated_sympy)zeros)lambdify)solvec                  &   t                      5  t          d          } t          d          }d d d            n# 1 swxY w Y   t          d| |          }t          d          \  }}t	          d          \  }}|                     || j        z             t                      5  t          | |          }d d d            n# 1 swxY w Y   |j        | j        k    sJ |j	        || gk    sJ |j
        | j        || j        j        z  fgk    sJ |j        t          |g          k    sJ |j        t          |g          k    sJ |j        t          ||                                z
  g          k    sJ |                                }|t          | |                                z  gg          k    sJ |j        t          |gdgg          k    sJ |j        t          ddgd|gg          k    sJ t)          |j        t,                    sJ d S )NPCP1zC_ixx gz
q_P1, u_P1r      )r   r   r	   r   r   apply_forceyr
   framebodiesloads
masscenterqr   ukdesdiff	form_eomsforcing_fullmass_matrix_full
isinstancemethodr   )	r   r   PinC_ixxgr$   r%   r,   solns	            k/var/www/html/ai-engine/env/lib/python3.11/site-packages/sympy/physics/mechanics/tests/test_jointsmethod.pytest_jointsmethodr2      s   		!	!  IIII               4A

Cy!!HE1,''DAqMM!AC%		!	! & &a%%& & & & & & & & & & & & & & &<17""""=QF""""<Q\1QWY;7888888vqc{{""""8vqc{{"""";&!affhh,000000D6UF16688O,-......&1#s"4"44444"fq!fq%j-A&B&BBBBBfm[1111111s   :>>%CC	Cc            
      P   t          d          \  } }}t          d          }t          d|          }t          d          }t	          d          \  }}t          d||||||  |j        z  |j                  }t                      5  t          ||          }	d d d            n# 1 swxY w Y   |	j
                            |j        ||z  |j        z  f           |	                                 |	                                }
|
d	         | t          |          z  | z  k    sJ d S )
Nl m gr   bmassb_frameq ur   )coordinatesspeedschild_interframechild_point
joint_axisr   )r   r   r   r   r   r	   xzr   r
   r"   appendr#   r(   rhsr   )lmr/   r   r5   r8   r$   r%   r   r,   rB   s              r1   &test_rigid_body_particle_compatibilityrE   *   s`   gGAq!#A1AY''G%  DAqa!gR')^	= 	= 	=A		!	! $ $a##$ $ $ $ $ $ $ $ $ $ $ $ $ $ $
Lq1uqs{3444

**,,Cq6aRAYq[      s   B%%B),B)c                  8   t                      5  t          d          t          d          } t          d          }d d d            n# 1 swxY w Y   t          d          \  }}t          d| |          t	          d| ||          t                      5  t          t          fd           d d d            n# 1 swxY w Y   t          d| |          t	          d| ||          t                      5  t          t          fd	           d d d            n# 1 swxY w Y   t          d| ||          t	          d| |||          t                      5  t          t          fd
           d d d            d S # 1 swxY w Y   d S )Nr   r   Tr9   r   P2c                  &    t                     S Nr
   r   r   rH   s   r1   <lambda>z?test_jointmethod_duplicate_coordinates_speeds.<locals>.<lambda>C       <2r#:#:     )r;   c                  &    t                     S rJ   rK   rL   s   r1   rM   z?test_jointmethod_duplicate_coordinates_speeds.<locals>.<lambda>H   rN   rO   c                  &    t                     S rJ   rK   rL   s   r1   rM   z?test_jointmethod_duplicate_coordinates_speeds.<locals>.<lambda>M   rN   rO   )r   r   r   r	   r   r   
ValueError)r   rG   r$   r%   r   r   rH   s       @@@r1   -test_jointmethod_duplicate_coordinates_speedsrS   :   s   		!	!  IIIIII               %  DAq	$1a	 	 B	aA	&	&B		!	! < <z::::::;;;< < < < < < < < < < < < < < < 
$1Q	'	'	'B	a1	-	-	-B		!	! < <z::::::;;;< < < < < < < < < < < < < < < 
$1a	#	#B	aAq	)	)B		!	! < <z::::::;;;< < < < < < < < < < < < < < < < < <sG   .AAACC	CD((D,/D,'FFFc                     t          d          \  } }t          d          \  }}t          d          \  }}}t                      5  t          d          }t          d|          }t          d|          }	d d d            n# 1 swxY w Y   t	          d|||| | |j        z  |j        	          }
t	          d
||	||| |	j        z  |j        	          }|                    ||z  |j        z             |	                    ||z  |j        z             t                      5  t          ||
|          }d d d            n# 1 swxY w Y   |	                                 t          |j                  t          g dg dddd|dz  z  |z  t          |          z  d|dz  z  |z  z   |dz  |z  t          |          z  |dz  |z  z   gdd|dz  |z  t          |          z  |dz  |z  z   |dz  |z  gg          k    sJ t          |j                  t          t          |g|g| |z  |z  t!          | |z             t!          |           z   z  ||z  |z  t!          |           z  z
  |dz  |z  d|z  |z   z  |z  t!          |          z  z   g| |z  |z  t!          | |z             z  |dz  |z  |dz  z  t!          |          z  z
  gg                    k    sJ d S )Nzq1 q2zu1 u2zm l gr   r   r6   RJ1r;   r:   r=   r>   J2)r   r   r   r   )r   r   r   r   r         )r   r   r   r   r	   r?   r@   r   r
   r(   r   r*   r   r   r   r)   r   )q1q2u1u2rD   rC   r/   r   PartPPartRrV   rX   r,   s                r1   $test_complete_simple_double_pendulumra   O   s   G$$FBG$$FBgGAq!		!	! " "IISq!!!Sq!!!" " " " " " " " " " " " " " " 
$5 bjQS
: 
: 
:B	$uRR bjUW
> 
> 
>B 
ac!#g	ac!#g		!	! ) )aR(() ) ) ) ) ) ) ) ) ) ) ) ) ) )
&)**flll6Bll78!Qq!tVAXc"gg=MPQRSUVRVPVWXPX=XZ[]^Z^_`Z`adegahahZhklnokopqkqZq6r78!QT!VCGG^aQRdSTf=TVWYZVZ[\V\6]6_ /` /` ` ` ` ` F'((HVbTB41"Q$q&RUVX[]V]R^R^adegahahRhJi+,Q3q5R=K9;<a461R4"9;Mb;PQTUWQXQX;XKY JZ,-2a46#b2g,,+>AaAcRTggAU+U*V=X 6Y 6Y -Z -Z Z Z Z Z Z Zs$   2BBB%EE
Ec                  P   t          d          \  } }}}t          d          \  }}}}}t                      5  t          d          }	t          d|          }
t          d|          }d d d            n# 1 swxY w Y   t	          d|	|
| |          }t	          d	|
|||          }|	                    || z  |	j        z  |

           |	                    ||z  |	j        z  |

           |
                    ||z  |	j        z  |
           |
                    ||z  |	j        z  |
           t                      5  t          |	||          }d d d            n# 1 swxY w Y   |                                 |j	        }|j
        }|                    |          }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 )Nzq1 q2 u1 u2zm c1 c2 k1 k2WB1r6   B2rV   )r:   r;   rX   )reaction_bodyr   r   rY   )r   r   r   r   r   r   r?   r
   r(   mass_matrixforcingLUsolver   )r[   r\   r]   r^   rD   c1c2k1k2rc   rd   re   rV   rX   r,   MMrh   rB   s                     r1   test_two_dof_jointsro   k   s   #M22NBB00Ar2r2		!	!    II$Q$Q                              
aB	?	?	?B	b""R	@	@	@BMM"R%)2M...MM"R%)2M...NN2b59BN///NN2b59BN///		!	! ) )aR(() ) ) ) ) ) ) ) ) ) ) ) ) ) )
		BnG
**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 2s#   2A66A:=A:?EE!$E!c            	         t          d          \  } }}t                      5  t          d          }t          d|          }d d d            n# 1 swxY w Y   t          d          }t	          d|||                    t                    ||  |j        z  |j                  }| |z  | z  t          |          z  |_
        t                      5  t          ||          }d d d            n# 1 swxY w Y   |                    t                     |                                }|d         | t          |          z  | z  k    sJ d S )	Nr4   r   r5   r6   r$   r   rW   r   )r   r   r   r   r	   r'   tr?   r@   r   potential_energyr
   r(   r   rB   r   )	rC   rD   r/   r   r5   r$   r   r,   rB   s	            r1   test_simple_pedulumrs      s   gGAq!		!	!  II1               	sAa166!99!R!#X!#	7 	7 	7Aq1s1vv-A		!	! $ $a##$ $ $ $ $ $ $ $ $ $ $ $ $ $ $
_%%%
**,,Cq6aRAYq[      s#   !AAAC..C25C2c                     t          d          \	  } }}}}}}}}t          d          \  }	}
}}t          d          }t          d          }t                      5  t	          d| |t          |||d                    }t	          d||t          ||||                    }t	          d	          }d d d            n# 1 swxY w Y   t          d
|||	|| |j        z  |j                  }t          d|||
|||z
  |j        z  |j                  }|	                    | |z  |j        z             |	                    ||z  |j        z             t                      5  t          |||          }d d d            n# 1 swxY w Y   |                                 |j        }|j        }|                    |          }d|z  |z  |z  t          |
          z  t!          |
          z  d|z  |z  |z  t          |
          z  t!          |
          z  z   ||z  | z  t          |	          z  z
  ||z  |z  t          |	          z  z
  ||t          |
          dz  z  z   |t!          |
          dz  z  z   |dz  | z  z   |dz  |z  z   z  }|d         |z
                                  dk    sJ ||z
  |dz  z  t          |
          z  t!          |
          z  |z  }|d         |z
                                  dk    sJ d S )Nz)mA, mB, lA, lB, IAxx, IBxx, IByy, IBzz, gztheta phi omega alphaABrodr   )r7   r    central_inertiaplater   rV   )r:   r;   r=   r>   rX   )r:   r;   parent_pointr>   rY   r   )r   r   r   r   r   r   r	   r@   r   r   r
   r(   rg   rh   ri   r   r   simplify)mAmBlAlBIAxxIBxxIByyIBzzr/   thetaphiomegaalpharu   rv   rw   ry   r   rV   rX   r,   rn   rh   rB   xds                            r1   test_chaos_pendulumr      s   078c0d0d-BBD$dA-.EFFE3usAsA		!	!  5r#*1dD!#<#<> > >W2Q%,QdD%A%AC C CII               
$3E%!cCEkac
; 
; 
;B	$UE "R350SU
D 
D 
DB OOBqDH	bd13h		!	! ) )aR(() ) ) ) ) ) ) ) ) ) ) ) ) ) )
		BnG
**W

C
t)e
e
#c#hh
.S
9AHu<Lu<TWZ[^W_W_<_HH= 2v{SZZ/023b&2+E

2JKOSVZC!W P"SXXq[0P135q52:P>@BA
PK
LB FRK!!##q((((
+	!CHH	,s3xx	7$	>BFRK!!##q((((((s%   AB<<C C E==FFc                    &'( t          d          \  } }}}}}t          d          \  }}}}	(t          d          &&(fd||||	fD             }
t                      5  t	          d&(|z  |
d                   }t	          d(|z  |
d	         
          }t	          d(|z  |
d         
          }t	          d(|	z  |
d         
          }d d d            n# 1 swxY w Y   t          d||| ||j        |dz  |j        z  | dz  |j        z            }t          d|||||j        |dz  |j        z  | dz  |j        z            }t          d|||||j        |dz  |j        z  |	 dz  |j        z            }|j        	                    |j                  |dz  |j        z  z   |	dz  |j        z  z   }t          |                    |j                  |                    |j                  g          }t                      5  t          ||||          }d d d            n# 1 swxY w Y   t           j        }t          |j        |                     |          |                    |          |                    |          g          }|                    |                              |          }t'          |j        | g|g||g||g|j        |||j        |j        
  
        }|                                \  }}|t1          d	          k    sJ t          ||||	(g          }t          | ||g          }t          |||g          }t3          ||f|j                  }t3          |||f|j                  }t3          |||f|          } g d}!g d}"g d}#t          g dg dg dg          }$t          dgdgdgg          }%d't9          'fd | |"|#|!          D                       sJ t9          'fdt           ||"|!                    |$z
  D                       sJ t9          'fd t           ||"|#|!                    |%z
  D                       sJ d S )!Nz
q1:4, u1:4z	l1:5, rhoNc           
      F    g | ]}t          d d |dz  z  dz            S )r   rZ      )r   ).0rC   r   rhos     r1   
<listcomp>zAtest_four_bar_linkage_with_manual_constraints.<locals>.<listcomp>   s4    NNN1aqAv!233NNNrO   Link1r   )r    r7   rx   Link2r   )r7   rx   Link3rY   Link4rZ   rV   )r:   r;   r>   rz   r=   rX   J3)	q_indu_indq_dependentu_dependentkd_eqsconfiguration_constraintsvelocity_constraints	forcelistr!   )gp=
ף?gQ?gzG?g(\?i  )g @gtM)L?gF!8@)g?g68[ƿgൄ?)gDwCA@g_'@gY>)g    gTB?gv1?)g_r=
ףg0 Sg;?gLgd`Egop]?g|=c              3   >   K   | ]}t          |          k     V  d S rJ   absr   r?   epss     r1   	<genexpr>z@test_four_bar_linkage_with_manual_constraints.<locals>.<genexpr>   s-      FFs1vv|FFFFFFrO   c              3   >   K   | ]}t          |          k     V  d S rJ   r   r   s     r1   r   z@test_four_bar_linkage_with_manual_constraints.<locals>.<genexpr>   s=       > >s1vv| > > > > > >rO   c              3   >   K   | ]}t          |          k     V  d S rJ   r   r   s     r1   r   z@test_four_bar_linkage_with_manual_constraints.<locals>.<genexpr>   sE       I Is1vv| I I I I I IrO   )r   r   r   r   r   r	   r@   r?   r#   pos_fromr   dotr   r
   _tr   r&   r'   subsr   r    r"   r!   kanes_equationsr   r   rg   rh   all))r[   r\   q3r]   r^   u3l1l2l3l4inertiaslink1link2link3link4joint1joint2joint3loopfhr,   rq   qdotsfhdkanefrfrspr$   r%   eval_meval_feval_fhdp_valsq_valsu_vals
mass_checkforcing_checkr   r   r   s)                                         @@@r1   -test_four_bar_linkage_with_manual_constraintsr      s   +L99BBB!+..BBCsANNNNNb"b"=MNNNH		!	! J JWAC"H%-a[2 2 2W38Xa[IIIW38Xa[IIIW38Xa[IIIJ J J J J J J J J J J J J J J eU2%'!Veg%B37UW3DF F FF eU2%'!Veg%B37UW3DF F FF eU2%'!Veg%B37UW3DF F FF $$U%566!Veg "Q 01D 
%'""DHHUW$5$56	7	7B		!	! = =eVVV<<= = = = = = = = = = = = = = = 	A&+

BGGAJJ

CDDE
''!**//%
 
 Cv|B4t$&8"b$kR,/6<$m	- - -D
 ""$$GBq>>>> 	BB$%%AB|AB|Aq!fd.//Fq!Qi..FAq	3''H***F999F<<<F 2 2 21 1 11 1 1	2 3 3J /0././1 2 2M CFFFFXXfff%E%EFFFFFFFF > > > >vvff--..;> > > > > > > > I I I Ivvfff5566FI I I I I I I I I Is%   A.CC CH&&H*-H*N)+sympy.core.functionr   sympy.core.symbolr   (sympy.functions.elementary.trigonometricr   r   sympy.matrices.denser   sympy.simplify.trigsimpr   sympy.physics.mechanicsr	   r
   r   r   r   r   r   r   r   sympy.physics.vectorr   r   sympy.testing.pytestr   r   sympyr   sympy.utilities.lambdifyr   sympy.solvers.solversr   r   rq   r2   rE   rS   ra   ro   rs   r   r    rO   r1   <module>r      s   & & & & & & % % % % % % ? ? ? ? ? ? ? ? ' ' ' ' ' ' , , , , , ,. . . . . . . . . . . . . . . . . . . . . . @ ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?       - - - - - - ' ' ' ' ' ' 2 2 2.! ! ! < < <*Z Z Z82 2 2.! ! !") ") ")HDI DI DI DI DIrO   