U
    ZÊ¹jÙ3  ã                   @   sÀ   d dl mZmZmZmZmZmZmZ d dlm	Z	 d dl
mZ d dlmZ d dlmZmZmZmZmZmZmZmZmZmZmZ d dlmZ edd„ ƒZd	d
„ Zdd„ Zdd„ Zdd„ Z dd„ Z!dS )é    )ÚsymbolsÚMatrixÚcosÚsinÚatanÚsqrtÚRational)Úsympify)Úsimplify)Úsolve)ÚdynamicsymbolsÚReferenceFrameÚPointÚdotÚcrossÚinertiaÚKanesMethodÚParticleÚ	RigidBodyÚ
LagrangianÚLagrangesMethod)Úslowc            ;         sø  t dƒ\‰} }}}tdƒ \}}}}}}	}
‡fdd„|
D ƒ \}}}}}}}tdƒ}tdƒ \}}}}}}}‡fdd„|D ƒ\}}}}}}tdƒ‰td	ƒ}ˆ d
d|ˆjg¡} |  dd|| jg¡}!|! dd||!jg¡‰ | d|ˆj |ˆj  |	ˆj  ¡‰ˆ  	ˆ¡‰|! 	ˆ¡}"ˆ  
ˆ||!j ||!j  ||!j  ¡ ˆ |¡ ˆ¡‰ˆ ˆ|ˆ j |ˆ j  |ˆ j  ¡ ˆ d| |!j ¡‰ˆ ˆˆˆ ¡ t|	tˆ ˆ¡ˆjƒ gƒ}#t‡‡fdd„ˆ D ƒƒ}$t‡ ‡‡fdd„|!D ƒ‡‡‡fdd„ˆD ƒ ƒ}%t|%|ƒ}&|! 
ˆ|" |&¡¡ ˆ  ˆˆ  	ˆ¡ |!¡t|! 	ˆ¡ˆ  	ˆ¡ƒ ¡ || | j }'|| d  d }(|| d  d })tˆ |(|)|(ƒ}*tdˆˆ ||*ˆfƒ}+|+g},ˆ|'fg}-tˆ|||||g|||g|%|	g|#|||g|$d�}.|. |,|-¡\}/}0|. ¡ }1|1j|#k�sòt‚|1j|$k�st‚|1j|$ ˆ¡ |. ¡ ¡k�s"t‚t|1j |1j! |ƒ}2|& "¡ D ]}3|2|3 |&|3 k�s<t‚�q<t#|1j$|1j% |/ |0 ƒtdddgƒk�s†t‚|	|  t&|ƒ i}4|d|t'|ƒ| | |t&|ƒ| ||  t'|ƒ| |  t&|ƒ |d||  t'|ƒ| |  t'|ƒ i}5|d||  t'|ƒ| |  t&|ƒ ||  t'|ƒ| |  t'|ƒ |di}6|d| t'|ƒ d|   t'd| ƒ|d  d  dt&|ƒ | | d  |d|d|| t'|ƒt'|ƒ | | t'|ƒ|d    || d| t'|ƒ d|   t'd| ƒ|d  d  dt&|ƒ | | d   ||  t'|ƒt&|ƒ | | t&|ƒ|d    i}7|1j(|4|5|6|7gddd�\} }!|d|d|d| d|di}8tddddddddgddddddddgddddddddgt'|ƒ| ddddt'|ƒ t&|ƒ dgt&|ƒ | ddddt&|ƒt'|ƒ dgdt)ddƒdddddd| d gddddddddgdddddd| ddggƒ}9tg ƒ}:|  |8¡|9k�s`t‚|! |8¡|:k�stt‚t*|  |8¡ |dt+dƒ ¡ƒ ,¡ ddik�s¤t‚|.jd d!�}1|1j(|4|5|6|7gddd�\} }!|  |8¡|9k�sàt‚|! |8¡|:k�sôt‚d S )"Nz	t r m g vzq1:7c                    s   g | ]}|  ˆ ¡‘qS © ©Údiff)Ú.0Úqi©Útr   úk/var/www/html/bwave/python/venv/lib/python3.8/site-packages/sympy/physics/mechanics/tests/test_linearize.pyÚ
<listcomp>   s     z4test_linearize_rolling_disc_kane.<locals>.<listcomp>zu:6zu1:7c                    s   g | ]}|  ˆ ¡‘qS r   r   )r   Zuir   r   r   r       s     ÚNÚNOÚAÚAxisÚBÚCÚCOÚPc                    s   g | ]}t ˆ ˆ ¡|ƒ‘qS r   ©r   Úvel©r   Zuv)r!   r(   r   r   r    6   s     c                    s    g | ]}t ˆˆ  ˆ¡ |ƒ‘qS r   )r   Ú
ang_vel_inr+   )r&   r!   Úw_c_n_qdr   r   r    9   s     c                    s    g | ]}t ˆˆ  ˆ¡ |ƒ‘qS r   r)   r+   )r'   r!   Ú	v_co_n_qdr   r   r    :   s     é   é   ÚDisc)Úkd_eqsÚq_dependentÚconfiguration_constraintsÚu_dependentÚvelocity_constraintsr   é   é   T©Úop_pointÚA_and_Br
   é   éþÿÿÿé   é   ÚGJ)Úlinear_solver)-r   r   r   r   Ú	orientnewÚzÚxÚyÚ	locatenewr,   Úset_ang_velÚpos_fromÚdtÚset_velÚv2pt_theoryr   r   r   ÚsubsZset_ang_accr   r   r   r   Úkanes_equationsZto_linearizerÚf_cÚAssertionErrorÚf_vÚf_ar   ZkindiffdictZf_0Zf_1Úkeysr
   Zf_2Zf_3r   r   Ú	linearizer   r	   r   Z	eigenvals);ÚrÚmÚgÚvÚq1Úq2Úq3Zq4Zq5Zq6ÚqÚq1dÚq2dÚq3dZq4dZq5dZq6dÚqdÚuÚu1Úu2Úu3Zu4Zu5Zu6Úu1dÚu2dZu3dZu4dZu5dZu6dr"   r#   r%   Zw_b_n_qdrN   rP   ZkindiffsZqdotsZF_COÚIÚJZI_C_COr1   ZBLZFLÚKMÚfrZfr_starZ
linearizerÚsolr   Úq_opÚu_opZqd_opÚud_opZupright_nominalZA_solZB_solr   )r&   r'   r!   r(   r   r.   r-   r   Ú test_linearize_rolling_disc_kane   sÐ    "&

&&ÿ
.
  þ ,  
   û   ýH   * J ,û&& ù0 ÿ
rn   c                  C   s6  t dƒ} t dƒ}t ddƒ}tdƒ\}}}d}tdƒ}tdƒ}| |d¡ | d	d
| |jg¡}	|	 |||j ¡ | d||	j	 ¡}
|
 
|||	¡ td|
|ƒ}t|| gƒ}|| |j	 }t|| g|g|d�}| |g|
|fg¡\}}|jddd�\}	}}|	tddgdt| ƒ | dggƒk�s t‚|tg ƒk�s2t‚d S )NrX   ra   r<   úL, m, tçš™™™™™#@r!   úN*r   r#   Úaxisr(   ÚpP)Úq_indÚu_indr2   T)r;   r
   çš™™™™™#À)r   r   r   r   rJ   rB   rC   rG   rF   rD   rK   r   r   r   rM   rS   r   rO   )rX   ra   r\   ÚLrU   r   rV   r!   ÚpNr#   r(   rs   ÚkdeÚRrh   ri   Úfrstarr%   Úinp_vecr   r   r   Ú$test_linearize_pendulum_kane_minimal�   s(    
*r}   c                   C   s²  t dƒ\} }t ddd�\}}t dƒ\}}t ddd�\}}tdƒ\}}	}
d}tdƒ}tdƒ}| |d	¡ t||  ƒ}| d
d||jg¡}| d| |j	 ||j
  ¡}td||	ƒ}t|| || gƒ}t|||gƒ}| || |¡ |¡ |¡¡ t| |¡ ¡ | gƒ}t| |¡ |¡ |j	¡gƒ}| ¡  | |
¡}| ¡  |	| |j	 }t||g|g| g|g||||d�	}| |g||fg¡\}}| ||d	i}|d	|d	i}|d	|d	i}|j|||gddd�\}}}| ¡ td	dgd| d	ggƒk�sìt‚|tg ƒk�sþt‚|j|||gdddd�\}}}| ¡ td	dgd| d	ggƒk�sBt‚|tg ƒk�sTt‚|j|||gdddd„ d�\}}}| ¡ td	dgd| d	ggƒk�sœt‚|tg ƒk�s®t‚d S )Núq1:3r<   ©Úlevelzu1:3ro   rp   r!   rq   r   r#   rr   ÚP1rs   )rt   ru   r3   r5   r4   r6   Zacceleration_constraintsr2   Tr9   rv   r@   )r:   r;   r
   rA   c                 S   s
   |   |¡S ©N©ZLUsolve©r#   Úbr   r   r   Ú<lambda>ý   ó    z9test_linearize_pendulum_kane_nonminimal.<locals>.<lambda>)r   r   r   r   rJ   r   rB   rC   rF   rD   rE   r   r   r   rH   rI   rL   Z	magnituder*   Zexpressr   r
   r   r   rM   rS   ÚexpandrO   ) rX   rY   r\   r]   ra   rb   rd   re   rw   rU   r   rV   r!   rx   Útheta1r#   r(   rs   ry   Zdq_dictrN   rP   rQ   rz   rh   ri   r{   rk   rl   rm   r%   r|   r   r   r   Ú'test_linearize_pendulum_kane_nonminimal²   sj    ÿ
   þÿ& ÿ&ý&rŠ   c                  C   s€  t dƒ} t ddƒ}tdƒ\}}}d}tdƒ}tdƒ}| |d¡ | dd	| |jg¡}| |||j ¡ | d
||j	 ¡}	|	 
|||¡ td|	|ƒ}
t||
ƒ}t|| g|	|| |j	 fg|d�}| ¡  |j| g|gdd�\}}}t|ƒtddgdt| ƒ | dggƒk�st‚|tg ƒk�s t‚|j| g|gddd�\}}}t|ƒtddgdt| ƒ | dggƒk�sjt‚|tg ƒk�s|t‚d S )NrX   r<   ro   rp   r!   rq   r   r#   rr   r(   rs   )Ú	forcelistÚframeT)r;   rv   r@   )r;   rA   )r   r   r   r   rJ   rB   rC   rG   rF   rD   rK   r   r   r   Úform_lagranges_equationsrS   r
   r   r   rO   )rX   r\   rw   rU   r   rV   r!   rx   r#   r(   rs   ÚLagÚLMr%   r|   r   r   r   Ú(test_linearize_pendulum_lagrange_minimal  s*    

"..r�   c                  C   s   t dƒ\} }t ddd�\}}tdƒ\}}}d}tdƒ}tdƒ}	|	 |d¡ t||  ƒ}
| d	d
|
|jg¡}|	 d| |j	 ||j
  ¡}| || |	¡ |¡¡ td||ƒ}t| d |d  |d  gƒ}t||ƒ}t|| |g|||| |j	 fg|d�}| ¡  | ||d|d|d| |¡d| |¡di}|j|d�}| |¡ |j|g|g| g|g|dd�\}}}t|ƒtddgd| dggƒk�sˆt‚|tg ƒk�sšt‚|j|g|g| g|g|ddd„ d�\}}}t|ƒtddgd| dggƒk�sêt‚|tg ƒk�süt‚d S )Nr~   r<   r   ro   rp   r!   rq   r   r#   rr   r�   rs   r/   )Z
hol_coneqsr‹   rŒ   )r:   T)r:   r;   rv   c                 S   s
   |   |¡S r‚   rƒ   r„   r   r   r   r†   L  s    z=test_linearize_pendulum_lagrange_nonminimal.<locals>.<lambda>)r:   r;   rA   )r   r   r   r   rJ   r   rB   rC   rF   rD   rE   rH   rI   r   r   r   r   r�   r   Zsolve_multipliersÚupdaterS   r
   rO   )rX   rY   r\   r]   rw   rU   r   rV   r!   rx   r‰   r#   r(   rs   rN   rŽ   r�   r:   Zlam_opr%   r|   r   r   r   Ú+test_linearize_pendulum_lagrange_nonminimal)  s>    
&(
 ÿ& ÿ&r’   c                  C   sò  t dƒ \} }}}t ddƒ \}}}}tdƒ\}}	}
tdƒ}| dd| |jg¡}| dd||jg¡}| dd||jg¡}td	ƒ}| |d
¡ | 	d||j ¡}| 
|||¡ t||	d |d  |	d |d  |	d |d  ƒ}td|||	||fƒ}|	 |
 | t|ƒ |_t||ƒ}t||ƒ}| ¡  | d
|d
|d
|d
|d
| ¡ d
| ¡ d
| ¡ d
i}|j|||dd�d
 }td
d
d
dd
d
gd
d
d
d
dd
gd
d
d
d
d
dgd
d
d
d
d| d
gd
d|
 d|  d
d| d d
d
gd
d
d
d
d
d
ggƒ}||k�sît‚d S )Nzq1 q2 q3r<   zr m gr!   ÚYr$   rw   rz   r&   r   ÚDmcr0   r/   ÚBodyDT)rt   Zqd_indr:   r;   iúÿÿÿéüÿÿÿr7   r8   )r   r   r   rB   rC   rD   rE   r   rJ   rF   rK   r   r   r   Zpotential_energyr   r   r�   r   rS   r   rO   )rX   rY   rZ   r[   r\   r]   r^   r_   rT   rU   rV   r!   r“   rw   rz   r&   r”   rf   r•   rŽ   Úlr:   r#   rj   r   r   r   Ú$test_linearize_rolling_disc_lagrangeQ  sL    2

        þ"ûr˜   N)"Zsympyr   r   r   r   r   r   r   Zsympy.core.sympifyr	   Zsympy.simplify.simplifyr
   Zsympy.solvers.solversr   Zsympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   r   Zsympy.testing.pytestr   rn   r}   rŠ   r�   r’   r˜   r   r   r   r   Ú<module>   s   $4
 %Q&(