U
    ZÊ¹j=  ã                   @   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-   úg/var/www/html/bwave/python/venv/lib/python3.8/site-packages/sympy/physics/mechanics/tests/test_kane5.pyÚ_create_rolling_disc   s^    ÿ
ÿÿ 
ÿ  ÿ
 
ÿø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}dddg}	|	 |||	|ƒd d…df ¡ tddddddddddg
ƒ}
t‡ fdd„|||	|ƒ|
 D ƒƒsèt	‚d}ddd g}	|	 |||	|ƒd d…df ¡ td!d"d#d$d%d&d'd(d)d*g
ƒ}
t‡ fd+d„|||	|ƒ|
 D ƒƒ�sXt	‚|�r‚d,}d,}	|||	|ƒt
d-d.ƒk�s‚t	‚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   )Ú.0Z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¸ž#@g¤p=
×£Ð?gq=
×£p@)g333333Ó?gš™™™™™¹?g…ëQ¸…ÿ?çffffffÖ¿ç)\�Âõ(@gš™™™™™É¿gÍÌÌÌÌÌô?g333333Ã?r   r   gæ¾3ÔŽ4À?g5ðk•¤Ë?g8{‡™ô?g¼a@¢wÔ?güHyR¹?gÉZ—û� í¿g	‘§ÿ‘’¿gy¸øëU@gÑã?ÆU„¿gÛtßÃu#¥?c                 3   s   | ]}t |ƒˆ k V  qd S r2   ©Úabs©r3   r   ©Úepsr-   r.   r4   I   s     z3_verify_rolling_disc_numerically.<locals>.<genexpr>)gÃõ(\�Â@gìQ¸…ëÑ¿gffffff @r>   r?   g      Ð¿gš™™™™™Àç×£p=
×ã?góÃÆ7³‰š?güWwç˜`å?g¡F-¥îŠÀg[Ã!«Ø?gz¨ãÚ?gâæ»s4÷?gñ4;e¢·†¿g^÷]¶ Àgw´{D€¹Ã¿g-î¿ú}³Â¿c                 3   s   | ]}t |ƒˆ k V  qd S r2   r@   rB   rC   r-   r.   r4   T   s     ©r   r   r   r   r   é
   r   )r	   r   r   Zmass_matrix_fullZforcing_fullZ_Arsr   ÚextendÚallÚAssertionErrorr   )ÚkaneZall_zeror6   r7   r8   Z	solve_sysZsolve_u_depÚp_valsÚq_valsÚu_valsÚexpectedr-   )rD   r9   r.   Ú _verify_rolling_disc_numerically7   s`    ÿ.
      üÿ
      üÿrP   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_equationsrP   )ÚpropsrK   r-   r-   r.   Útest_kane_rolling_disc_lu\   s       ürV   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ºt	‚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 ƒƒ�szt	‚d S )!Nr   r   r   r   r   r   r   r   Fc                 S   s   t |  |¡ƒS r2   )r   r5   )ÚAÚbr-   r-   r.   r:   n   ó    z6test_kane_rolling_disc_kdes_callable.<locals>.<lambda>)rQ   rR   r   rS   rT   Zkd_eqs_solverr0   r1   r   r   r<   )r=   ç      Ð?g      @rF   c                 3   s"   | ]\}}t || ƒˆ k V  qd S r2   r@   ©r3   ZqdiZfuirC   r-   r.   r4   v   s     z7test_kane_rolling_disc_kdes_callable.<locals>.<genexpr>é   r;   r   gáz®GáÚ?rE   r   gUUUUUUÕ?rZ   éþÿÿÿéýÿÿÿc                 3   s"   | ]\}}t || ƒˆ k V  qd S r2   r@   r[   rC   r-   r.   r4      s     )r/   r   r	   r   r   ÚtupleZkindiffdictÚitemsrI   rJ   ÚmapÚfloatr   r   )rU   rK   r6   r7   r8   ZqdZ	eval_kdesrL   Z	zero_valsrM   Zqd_valsrN   r-   rC   r.   Ú$test_kane_rolling_disc_kdes_callableg   sB         û
ÿ$þÿÿrc   N)F)Zsympyr   r   r   r   r   r   r   Zsympy.physics.mechanicsr	   r
   r   r   r   r   r/   rP   rV   rc   r-   r-   r-   r.   Ú<module>   s
   $ 0
%