a
    rh=                     @   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-   d/var/www/html/assistant/venv/lib/python3.9/site-packages/sympy/physics/mechanics/tests/test_kane5.py_create_rolling_disc   sV    
&r/   Fc                    sb  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 sJ d}g d}	|	|||	|d ddf  tg d}
t fdd|||	||
 D s4J |r^d}d}	|||	|t	ddks^J 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><       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   s   | ]}t | k V  qd S r2   absr3   r   epsr-   r.   r4   I   r5   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   s   | ]}t | k V  qd S r2   rA   rC   rD   r-   r.   r4   T   r5   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-   )rE   r:   r.    _verify_rolling_disc_numerically7   s4    .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 sJ 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 szJ d S )!Nr   r   r   r   r   r   r   r   Fc                 S   s   t | |S r2   )r   r6   )Abr-   r-   r.   r;   n   r5   z6test_kane_rolling_disc_kdes_callable.<locals>.<lambda>)rQ   rR   r   rS   rT   Zkd_eqs_solverr0   r1   r   r   r=   )r>         ?g      @rG   c                 3   s"   | ]\}}t ||  k V  qd S r2   rA   r3   ZqdiZfuirD   r-   r.   r4   v   r5   z7test_kane_rolling_disc_kdes_callable.<locals>.<genexpr>   r<   r   gzG?rF   r   gUUUUUU?rY   c                 3   s"   | ]\}}t ||  k V  qd S r2   rA   rZ   rD   r-   r.   r4      r5   )r/   r   r	   r   r   tupleZkindiffdictitemsrJ   mapfloatr   r   )rU   rK   r7   r8   r9   ZqdZ	eval_kdesrL   Z	zero_valsrM   Zqd_valsrN   r-   rD   r.   $test_kane_rolling_disc_kdes_callableg   s8    
$rb   N)F)Zsympyr   r   r   r   r   r   r   Zsympy.physics.mechanicsr	   r
   r   r   r   r   r/   rP   rV   rb   r-   r-   r-   r.   <module>   s
   $ 0
%