
    g=                     d    d dl mZmZmZmZmZmZmZ d dlm	Z	m
Z
mZmZmZmZ d Zd	dZd Zd ZdS )
    )zerosMatrixsymbolslambdifysqrtpisimplify)dynamicsymbolscrossinertia	RigidBodyReferenceFrameKanesMethodc            
      D   t           j        } t          d          \
  }}}}}}}}}	}
t          d          \  }}}t          d          }t          d|          }||dz  z  dz  t	          |j        ddd          z  |j        f|_        |j                            |j        d	           |j                            |j        d	           t          d
          }|	                    |j        ||d	fd           |j        
                    ||j        |           |j                            |j                  }|j                            |j        ||j        z  ||j        z  z   ||j        z  z              |j                            d||j        z  ||j        z  z             }|                    |j        |	|j        z  |
|j        z  z              |j                            |||j        z             |j                            |j        t%          |j                            |j                  |j                            |                               |                    |j                  |z
  |                    |j                  |z
  |                    |j                  |z
  |                    |           |	z
  |                    |           |
z
  g}|                    |j                  t%          |j                            |          |                    |j                            z   }|                    |j                  |                    |j                  g}|j        |j         |z  |j        z  fg}|g}|j        |||||g|||g|	|
g||||dS )Nz	q1:6 u1:6g r mgrounddisk)mass         r   	int_framezxycontact_point)frameq_indu_indu_depkdesfnhbodiesloads)r
   _tr   r   r   r   
masscenterset_velr   orient_body_fixedorient_axisy
ang_vel_inset_ang_velxz	locatenewset_posr   pos_fromdotdiffvelr   )tq1q2q3q4q5u1u2u3u4u5grmr   discr   g_w_dcpr   v0r    r"   r!   s                           d/var/www/html/ai-engine/env/lib/python3.11/site-packages/sympy/physics/mechanics/tests/test_kane5.py_create_rolling_discrF      sr   A-;K-H-H*BBBBBgGAq!x  FV!$$$DQJNWTZAq%A%AAO%DL
flA...ODJ***{++Ir2qk5AAAJ9ik2666J!!&,//EJ6<;df4rDF{BD D D 
		$	$_%'&(]R&(]%B
D 
DBJJv|R&(]R&(]:;;;OBIK000OFL%
fl++T_-E-Eb-I-I+K +K L L L IIdf"EIIdf$5$5$:IIdf"BGGAJJORWWQZZ"_FD 
		
i(("++do*F*F!H !H 
HB66&(RVVFH--
.Co	zA~89:EVFb"b"%b"b	 	 	    Fc           	      .   t          d          t          d          t          d          }}}t          |||f| j        | j        fd          fd}t          ||d d         |f| j        t          |d d                   z  d          }dd	}d
}g d}	|	                     |||	|          d ddf                    t          g d          }
t          fd |||	|          |
z
  D                       sJ d}g d}	|	                     |||	|          d ddf                    t          g d          }
t          fd |||	|          |
z
  D                       sJ |r%d}d}	 |||	|          t          dd          k    sJ d S d S )Nq1:6u1:6r   T)csec                 J    t          j        d  | ||          D              S )Nc              3   4   K   | ]}t          |          V  d S N)r   ).0mats     rE   	<genexpr>zE_verify_rolling_disc_numerically.<locals>.<lambda>.<locals>.<genexpr><   s(      	3	3#&++	3	3	3	3	3	3rG   )r   LUsolve)qupeval_syss      rE   <lambda>z2_verify_rolling_disc_numerically.<locals>.<lambda>;   s-    	3	3!Q!2!2	3	3	3!5 rG      绽|=)Q#@gp=
ף?gq=
ףp@)g333333?g?gQ?ffffffֿ)\(@)gɿg?g333333?r   r   )
g3Ԏ4?g5k?g8{?ga@w?gHyR?gZg	gyU@g?Ugtu#?c              3   >   K   | ]}t          |          k     V  d S rN   absrO   r+   epss     rE   rQ   z3_verify_rolling_disc_numerically.<locals>.<genexpr>I   =       ? ?s1vv| ? ? ? ? ? ?rG   )g(\@gQѿgffffff @r[   r\   )g      пgףp=
?)
g7?gWw`?gF-g[!?gz?gs4?g4;eg^] gw{Dÿg-}¿c              3   >   K   | ]}t          |          k     V  d S rN   r^   r`   s     rE   rQ   z3_verify_rolling_disc_numerically.<locals>.<genexpr>T   rb   rG   r   r   r   r   r   
   r   )
r
   r   r   mass_matrix_fullforcing_full_Arsr   extendallr   )kaneall_zerorS   rT   rU   	solve_syssolve_u_depp_valsq_valsu_valsexpectedra   rV   s              @@rE    _verify_rolling_disc_numericallyrt   7   sa   V$$nV&<&<gg>N>N!qAAq	D$94;L#M " " "H5 5 5 5IAq!ua=$)fQrrUmm*CNNNK
CF*FF
MM++fff55bqb!e<===     H
  ? ? ? ?	&&&11H<? ? ? ? ? ? ? ? -F   F
MM++fff55bqb!e<===     H
  ? ? ? ?	&&&11H<? ? ? ? ? ? ? ? A  y00E"aLL@@@@A A A@rG   c                      t                      } t          | d         | d         | d         | d         | d         | d         | d         | d         d	d

  
        }|                                 t          |           d S )Nr   r   r   r   r   r    r!   r"   FLU)u_dependentvelocity_constraintsr!   	forcelistexplicit_kinematicsconstraint_solver)rF   r   kanes_equationsrt   )propsrl   s     rE   test_kane_rolling_disc_lur~   \   s     ""EuW~uW~uW~V}%.,1%L#How+0D	J J JD
 	$T*****rG   c                  R   t                      } t          | d         | d         | d         | d         | d         | d         | d         | d         d	d
 
  
        }t          d          t          d          t          d          }}}t          dd          }t	          ||||ft          |                                                                                    }dd}d}t          fd |||||          D                       sJ t          t          t          t          dz  t          dz  t          dz  ddf                    }	t          t          t          ddddt          d          z  z
  ddt          d          z  dz
  z  ddt          d          z
  z  f                    }
t          t          t          ddddddt          d          z  z   z  dt          d           dz   z  f                    }t          fd ||	|
||          D                       sJ d S )Nr   r   r   r   r   r    r!   r"   Fc                 F    t          |                     |                    S rN   )r	   rR   )Abs     rE   rW   z6test_kane_rolling_disc_kdes_callable.<locals>.<lambda>n   s    8AIIaLL#9#9 rG   )rw   rx   r!   ry   rz   kd_eqs_solverrI   rJ   r   r   rY   )rZ         ?g      @re   c              3   J   K   | ]\  }}t          ||z
            k     V  d S rN   r^   rO   qdifuira   s      rE   rQ   z7test_kane_rolling_disc_kdes_callable.<locals>.<genexpr>v   sN       C CSs39~~# C C C C C CrG      rX   r   gzG?rc   r   gUUUUUU?r   c              3   J   K   | ]\  }}t          ||z
            k     V  d S rN   r^   r   s      rE   rQ   z7test_kane_rolling_disc_kdes_callable.<locals>.<genexpr>   sF       ; ;Ss39~~# ; ; ; ; ; ;rG   )rF   r   r
   r   r   tuplekindiffdictitemsrk   mapfloatr   r   )r}   rl   rS   rT   rU   qd	eval_kdesrp   	zero_valsrq   qd_valsrr   ra   s               @rE   $test_kane_rolling_disc_kdes_callabler   g   sv    ""Egggf'NuX%.!99; ; ;D V$$nV&<&<gg>N>N!qA		"	"B!RAd.>.>.@.@.F.F.H.H(I(IJJI
CFI C C C CyIy&AAC C C C C C C C 3urAvrAvrAvtTBCCDDFC5!a$q''k/ $DGGa 8 $DGG 4 6 7 7 8 8G 3ur1eTR!d1gg+5E-F#Qx!|46 7 7 8 8F ; ; ; ;y&&99; ; ; ; ; ; ; ; ; ;rG   N)F)sympyr   r   r   r   r   r   r	   sympy.physics.mechanicsr
   r   r   r   r   r   rF   rt   r~   r    rG   rE   <module>r      s  * * * * * * * * * * * * * * * * * *B B B B B B B B B B B B B B B B- - -`"A "A "A "AJ+ + +; ; ; ; ;rG   