U
    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dotZdiffZvelr   )tZq1Zq2Zq3Zq4Zq5u1u2u3Zu4Zu5grmr   Zdiscr   Zg_w_dcpr   Zv0r   r   r    r*   L/tmp/pip-unpacked-wheel-6t8vlncq/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   ).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   s   | ]}t | k V  qd S r/   absr0   r   epsr*   r+   r2   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 r/   r>   r@   rA   r*   r+   r2   T   s     r   r   r   r   r   
   r   )r	   r   r   Zmass_matrix_fullZforcing_fullZ_Arsr   extendallAssertionErrorr   )kaneZall_zeror4   r5   r6   Z	solve_sysZsolve_u_depp_valsq_valsu_valsexpectedr*   )rB   r7   r+    _verify_rolling_disc_numerically7   s`    .
      
      rN   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_equationsrN   )propsrI   r*   r*   r+   test_kane_rolling_disc_lu\   s       rT   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 r/   )r   r3   )Abr*   r*   r+   r8   n       z6test_kane_rolling_disc_kdes_callable.<locals>.<lambda>)rO   rP   r   rQ   rR   Zkd_eqs_solverr-   r.   r   r   r:   )r;         ?g      @rD   c                 3   s"   | ]\}}t ||  k V  qd S r/   r>   r0   ZqdiZfuirA   r*   r+   r2   v   s     z7test_kane_rolling_disc_kdes_callable.<locals>.<genexpr>   r9   r   gzG?rC   r   gUUUUUU?rX   c                 3   s"   | ]\}}t ||  k V  qd S r/   r>   rY   rA   r*   r+   r2      s     )r,   r   r	   r   r   tupleZkindiffdictitemsrG   rH   mapfloatr   r   )rS   rI   r4   r5   r6   ZqdZ	eval_kdesrJ   Z	zero_valsrK   Zqd_valsrL   r*   rA   r+   $test_kane_rolling_disc_kdes_callableg   sB         
$ra   N)F)Zsympyr   r   r   r   r   r   r   Zsympy.physics.mechanicsr	   r
   r   r   r   r   r,   rN   rT   ra   r*   r*   r*   r+   <module>   s
   $ 0
%