o
    g=                     @   sj   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d ZdddZdd	 Zd
d ZdS )    )zerosMatrixsymbolslambdifysqrtpisimplify)dynamicsymbolscrossinertia	RigidBodyReferenceFrameKanesMethodc               
   C   s<  t j} t d\
}}}}}}}}}	}
td\}}}td}td|d}||d  d t|jddd |jf|_|j|jd	 |j|jd	 td
}|	|j||d	fd |j
||j| |j|j}|j|j||j ||j  ||j   |jd||j ||j  }||j|	|j |
|j   |j|||j  |j|jt|j|j|j| ||j| ||j| ||j| || |	 || |
 g}||jt|j|||j }||j||jg}|j|j | |j 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    rC   _/var/www/visachat/venv/lib/python3.10/site-packages/sympy/physics/mechanics/tests/test_kane5.py_create_rolling_disc   sV   
&rE   Fc                    s`  t dt dtd}}}t|||f| j| jfddfdd}t||d d |f| jt|d d  dd}d	 d
}d}g d}	|	|||	|d ddf  tg d}
t fdd|||	||
 D skJ d}g d}	|	|||	|d ddf  tg d}
t fdd|||	||
 D sJ |rd}d}	|||	|t	ddksJ d S d S )Nq1:6u1:6r   T)csec                    s   t jdd  | ||D  S )Nc                 s   s    | ]}t |V  qd S N)r   ).0matrC   rC   rD   	<genexpr><   s    zE_verify_rolling_disc_numerically.<locals>.<lambda>.<locals>.<genexpr>)r   LUsolve)qup)eval_sysrC   rD   <lambda>;   s    z2_verify_rolling_disc_numerically.<locals>.<lambda>   绽|=)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       | ]	}t | k V  qd S rI   absrJ   r)   epsrC   rD   rL   I       z3_verify_rolling_disc_numerically.<locals>.<genexpr>)g(\@gQѿgffffff @rV   rW   )g      пgףp=
?)
g7?gWw`?gF-g[!?gz?gs4?g4;eg^] gw{Dÿg-}¿c                 3   rX   rI   rY   r[   r\   rC   rD   rL   T   r^   r   r   r   r   r   
   r   )
r	   r   r   mass_matrix_fullforcing_full_Arsr   extendallr   )kaneall_zerorN   rO   rP   	solve_syssolve_u_depp_valsq_valsu_valsexpectedrC   )r]   rQ   rD    _verify_rolling_disc_numerically7   s8   .ro   c                  C   sV   t  } t| d | 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)rE   r   kanes_equationsro   )propsrg   rC   rC   rD   test_kane_rolling_disc_lu\   s   rx   c                     s|  t  } t| d | d | 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|||||D s]J tt	t
td td td ddf}	tt	t
ddddtd  ddtd d  ddtd  f}
tt	t
ddddddtd   dtd d  f}t fd d||	|
||D sJ d S )!Nr   r   r   r   r   r   r   r    Fc                 S   s   t | |S rI   )r   rM   )AbrC   rC   rD   rR   n   s    z6test_kane_rolling_disc_kdes_callable.<locals>.<lambda>)rq   rr   r   rs   rt   kd_eqs_solverrF   rG   r   r   rT   )rU         ?g      @r`   c                 3   $    | ]\}}t ||  k V  qd S rI   rY   rJ   qdifuir\   rC   rD   rL   v      " z7test_kane_rolling_disc_kdes_callable.<locals>.<genexpr>   rS   r   gzG?r_   r   gUUUUUU?r|   c                 3   r}   rI   rY   r~   r\   rC   rD   rL      r   )rE   r   r	   r   r   tuplekindiffdictitemsrf   mapfloatr   r   )rw   rg   rN   rO   rP   qd	eval_kdesrk   	zero_valsrl   qd_valsrm   rC   r\   rD   $test_kane_rolling_disc_kdes_callableg   s8   
$r   N)F)sympyr   r   r   r   r   r   r   sympy.physics.mechanicsr	   r
   r   r   r   r   rE   ro   rx   r   rC   rC   rC   rD   <module>   s   $  
0%