
    Rie"                         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 d dlmZmZ d dl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S )    )expand)symbols)cossin)Matrix)trigsimp)PinJointJointsMethodBodyKanesMethodPrismaticJointLagrangesMethodinertia)dynamicsymbolsReferenceFrame)raises)zeros)lambdify)solvec                     t          d          } t          d          }t          d| |          }t          d          \  }}t          d          \  }}|                     || j        z             t          | |          }|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   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	            Olib/python3.11/site-packages/sympy/physics/mechanics/tests/test_jointsmethod.pytest_jointsmethodr/      s   S		AS		A
4A

Cy!!HE1,''DAqMM!AC%!S!!F<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1    c                     t          d          t          d          } t          d          }t          d          \  }}t          d| |          t          d| ||          t	          t
          fd           t          d| |          t          d| ||          t	          t
          fd	           t          d| ||          t          d| |||          t	          t
          fd
           d S )Nr   r   Tzq ur   P2c                  &    t                     S Nr
   r   r   r3   s   r.   <lambda>z?test_jointmethod_duplicate_coordinates_speeds.<locals>.<lambda>-       |Ar266 r0   )speedsc                  &    t                     S r5   r6   r7   s   r.   r8   z?test_jointmethod_duplicate_coordinates_speeds.<locals>.<lambda>1   r9   r0   c                  &    t                     S r5   r6   r7   s   r.   r8   z?test_jointmethod_duplicate_coordinates_speeds.<locals>.<lambda>5   r9   r0   )r   r   r	   r   r   
ValueError)r   r2   r!   r"   r   r   r3   s       @@@r.   -test_jointmethod_duplicate_coordinates_speedsr>   &   s   S		AS		AS		A%  DAq	$1a	 	 B	aA	&	&B
:666666777	$1Q	'	'	'B	a1	-	-	-B
:666666777	$1a	#	#B	aAq	)	)B
:66666677777r0   c                  *   t          d          \  } }t          d          \  }}t          d          \  }}}t          d          }t          d|          }t          d|          }	t          d|||| | |j        z  |j        	          }
t          d
||	||| |	j        z  |j        	          }|                    ||z  |j        z             |	                    ||z  |j        z             t          ||
|          }|                                 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   massRJ1r:   coordinateschild_point
joint_axisJ2)r   r   r   r   )r   r   r   r   r         )r   r   r   r	   xzr   r
   r%   r   r'   r   r   r   r&   r   )q1q2u1u2mlr,   r   PartPPartRrC   rH   r)   s                r.   $test_complete_simple_double_pendulumrU   7   s.   G$$FBG$$FBgGAq!S		A1E1E	$5 bjQS
: 
: 
:B	$uRR bjUW
> 
> 
>B 
ac!#g	ac!#g!R$$F
&)**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r0   c                     t          d          \  } }}}t          d          \  }}}}}t          d          }	t          d|          }
t          d|          }t          d|	|
| |          }t          d	|
|||          }|	                    || z  |	j        z  |

           |	                    ||z  |	j        z  |

           |
                    ||z  |	j        z  |
           |
                    ||z  |	j        z  |
           t          |	||          }|                                 |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B1r@   B2rC   )rE   r:   rH   )reaction_bodyr   r   rI   )r   r   r   r   r   rK   r
   r%   mass_matrixforcingLUsolver   )rM   rN   rO   rP   rQ   c1c2k1k2rW   rX   rY   rC   rH   r)   MMr\   rhss                     r.   test_two_dof_jointsrd   Q   s   #M22NBB00Ar2r2S		A	d			B	d			B	aB	?	?	?B	b""R	@	@	@BMM"R%)2M...MM"R%)2M...NN2b59BN///NN2b59BN///!R$$F
		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 2r0   c            	         t          d          \  } }}t          d          }t          d|          }t          d          }t          d|||                    t
                    ||  |j        z  |j                  }| |z  | z  t          |          z  |_	        t          ||          }|                    t                     |                                }|d         | t          |          z  | z  k    sJ d S )	Nzl m gr   br@   r!   r   rD   r   )r   r   r   r	   r$   trK   rL   r   potential_energyr
   r%   r   rc   r   )	rR   rQ   r,   r   rf   r!   r   r)   rc   s	            r.   test_simple_pedulumri   f   s    gGAq!S		ASqAsAa166!99!R!#X!#	7 	7 	7Aq1s1vv-A!QF
_%%%
**,,Cq6aRAYq[      r0   c                  f   t          d          \	  } }}}}}}}}t          d          \  }	}
}}t          d          }t          d          }t          d| |t	          |||d                    }t          d||t	          ||||                    }t          d	          }t          d
|||	|| |j        z  |j                  }t          d|||
|||z
  |j        z  |j                  }|                    | |z  |j        z             |                    ||z  |j        z             t          |||          }|
                                 |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   )rA   r   central_inertiaplater   rC   )rE   r:   rF   rG   rH   )rE   r:   parent_pointrG   rI   r   )r   r   r   r   r   r	   rL   r   r   r
   r%   r[   r\   r]   r   r   simplify)mAmBlAlBIAxxIBxxIByyIBzzr,   thetaphiomegaalphark   rl   rm   ro   r   rC   rH   r)   rb   r\   rc   xds                            r.   test_chaos_pendulumr   s   s   078c0d0d-BBD$dA-.EFFE3usAsA
u2Q4q8Q8Q
R
R
RCrGAtTSW<X<XYYYES		A	$3E%!cCEkac
; 
; 
;B	$UE "R350SU
D 
D 
DB OOBqDH	bd13h!R$$F
		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((((((r0   c                  .  &'( t          d          \  } }}}}}t          d          \  }}}}	(t          d          &&(fd||||	fD             }
t          d&(|z  |
d                   }t          d(|z  |
d	         
          }t          d(|z  |
d         
          }t          d(|	z  |
d         
          }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          ||||          }t           j        }t          |j        |                     |          |                    |          |                    |          g          }|                    |                              |          }t%          |j        | g|g||g||g|j        |||j        |j        
  
        }|                                \  }}|t/          d	          k    sJ t          ||||	(g          }t          | ||g          }t          |||g          }t1          ||f|j                  }t1          |||f|j                  }t1          |||f|          } g d}!g d}"g d}#t          g dg dg dg          }$t          dgdgdgg          }%d't7          'fd | |"|#|!          D                       sJ t7          'fdt           ||"|!                    |$z
  D                       sJ t7          '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   rJ      )r   ).0rR   r   rhos     r.   
<listcomp>zAtest_four_bar_linkage_with_manual_constraints.<locals>.<listcomp>   s4    NNN1aqAv!233NNNr0   Link1r   )r   rA   rn   Link2r   )rA   rn   Link3rI   Link4rJ   rC   )rE   r:   rG   rp   rF   rH   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 r5   absr   rK   epss     r.   	<genexpr>z@test_four_bar_linkage_with_manual_constraints.<locals>.<genexpr>   s-      FFs1vv|FFFFFFr0   c              3   >   K   | ]}t          |          k     V  d S r5   r   r   s     r.   r   z@test_four_bar_linkage_with_manual_constraints.<locals>.<genexpr>   s=       > >s1vv| > > > > > >r0   c              3   >   K   | ]}t          |          k     V  d S r5   r   r   s     r.   r   z@test_four_bar_linkage_with_manual_constraints.<locals>.<genexpr>   sE       I Is1vv| I I I I I Ir0   )r   r   r   r   r	   rL   rK   r    pos_fromr   dotr   r
   _tr   r#   r$   subsr   r   r   r   kanes_equationsr   r   r[   r\   all))rM   rN   q3rO   rP   u3l1l2l3l4inertiaslink1link2link3link4joint1joint2joint3loopfhr)   rg   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)                                         @@@r.   -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b(1+NNNEsRx!EEEEsRx!EEEEsRx!EEEE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%88F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r0   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   sympy.physics.vectorr   r   sympy.testing.pytestr   sympy.core.backendr   sympy.utilities.lambdifyr   sympy.solvers.solversr   r   rg   r/   r>   rU   rd   ri   r   r    r0   r.   <module>r      s   & & & & & & % % % % % % ? ? ? ? ? ? ? ? ' ' ' ' ' ' , , , , , ,N N N N N N N N N N N N N N N N N N ? ? ? ? ? ? ? ? ' ' ' ' ' ' $ $ $ $ $ $ - - - - - - ' ' ' ' ' ' 2 2 2(8 8 8"Z Z Z42 2 2*! ! !) ) )@AI AI AI AI AIr0   