U
    L?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-   Z/var/www/html/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 st	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	|rd,}d,}	|||	|t
d-d.kst	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#@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   s     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   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 st	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   gz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
%