o
    GZh=                     @   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groundZdisk)mass         r   	int_frameZzxyZcontact_point)frameq_indu_indu_depkdesfnhbodiesloads)r	   Z_tr   r   r   r   Z
masscenterZset_velr   Zorient_body_fixedZorient_axisyZ
ang_vel_inZset_ang_velxzZ	locatenewZset_posr
   Zpos_fromdotdiffZvelr   )tq1q2Zq3Zq4Zq5u1u2u3Zu4Zu5grmr   Zdiscr   Zg_w_dcpr   Zv0r   r   r    r-   W/var/www/auris/lib/python3.10/site-packages/sympy/physics/mechanics/tests/test_kane5.py_create_rolling_disc   sV   
&r/   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)Zcsec                    s   t jdd  | ||D  S )Nc                 s   s    | ]}t |V  qd S N)r   ).0matr-   r-   r.   	<genexpr><   s    zE_verify_rolling_disc_numerically.<locals>.<lambda>.<locals>.<genexpr>)r   LUsolve)qup)eval_sysr-   r.   <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 r2   absr3   r   epsr-   r.   r5   I       z3_verify_rolling_disc_numerically.<locals>.<genexpr>)g(\@gQѿgffffff @r?   r@   )g      пgףp=
?)
g7?gWw`?gF-g[!?gz?gs4?g4;eg^] gw{Dÿg-}¿c                 3   rA   r2   rB   rD   rE   r-   r.   r5   T   rG   r   r   r   r   r   
   r   )
r	   r   r   Zmass_matrix_fullZforcing_fullZ_Arsr   extendallr   )kaneZall_zeror7   r8   r9   Z	solve_sysZsolve_u_depp_valsq_valsu_valsexpectedr-   )rF   r:   r.    _verify_rolling_disc_numerically7   s8   .rR   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   FZLU)u_dependentvelocity_constraintsr   	forcelistexplicit_kinematicsZconstraint_solver)r/   r   Zkanes_equationsrR   )propsrM   r-   r-   r.   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 r2   )r   r6   )Abr-   r-   r.   r;   n   s    z6test_kane_rolling_disc_kdes_callable.<locals>.<lambda>)rS   rT   r   rU   rV   Zkd_eqs_solverr0   r1   r   r   r=   )r>         ?g      @rI   c                 3   $    | ]\}}t ||  k V  qd S r2   rB   r3   ZqdiZfuirE   r-   r.   r5   v      " z7test_kane_rolling_disc_kdes_callable.<locals>.<genexpr>   r<   r   gzG?rH   r   gUUUUUU?r[   c                 3   r\   r2   rB   r]   rE   r-   r.   r5      r^   )r/   r   r	   r   r   tupleZkindiffdictitemsrL   mapfloatr   r   )rW   rM   r7   r8   r9   ZqdZ	eval_kdesrN   Z	zero_valsrO   Zqd_valsrP   r-   rE   r.   $test_kane_rolling_disc_kdes_callableg   s8   
$rf   N)F)Zsympyr   r   r   r   r   r   r   Zsympy.physics.mechanicsr	   r
   r   r   r   r   r/   rR   rX   rf   r-   r-   r-   r.   <module>   s   $  
0%