U
    ZÊ¹j)T  ã                   @   sÔ   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 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 d dlmZ dd„ Zdd	„ Zd
d„ Zdd„ Zdd„ Zdd„ Zdd„ Z dd„ Z!dd„ Z"dd„ Z#dS )é    )Úsolve)
ÚcosÚexpandÚMatrixÚsinÚsymbolsÚtanÚsqrtÚSÚzerosÚeye)Úsimplify)	ÚdynamicsymbolsÚReferenceFrameÚPointÚ	RigidBodyÚKanesMethodÚinertiaÚParticleÚdotÚfind_dynamicsymbols)Úraisesc                     sº   t dƒ\} }}t dƒ\‰‰ˆ tj¡ˆ g‰tdƒtdƒ ‰}| ˆd¡ tdtdƒ|ƒ‰ˆj 	|| t
ˆƒˆj tˆƒˆj   ¡ ˆj| | ˆj f‰ tt‡ ‡‡‡‡‡fdd„ƒ d S )	Nzl m gúq uÚNÚOr   ÚPc                      s   t ˆˆgˆgˆˆgˆ gd�S )N©ÚbodiesZ	forcelist©r   © ©ÚFr   r   ÚkdÚqÚur   úf/var/www/html/bwave/python/venv/lib/python3.8/site-packages/sympy/physics/mechanics/tests/test_kane.pyÚ<lambda>   s   ÿz*test_invalid_coordinates.<locals>.<lambda>)r   Údiffr   Z_tr   r   Úset_velr   ÚpointZset_posr   Úxr   Úyr   Ú
ValueError)ÚlÚmÚgr   r   r    r%   Útest_invalid_coordinates   s    *r0   c                  C   sf  t dƒ\} }t ddƒ\}}tdƒ\}}}tdƒ}tdƒ}| |||j ¡ || g}	|| |  ||  |j fg}
td||ƒ}|g}t|| g|g|	ƒ}| ||
¡ |j	|ks°t
‚|j|
ks¾t
‚|j}|j}| ¡ | }t|d ƒt| | ||   | ƒk�st
‚t| ¡ |j |j¡ ƒtddƒk�s,t
‚|jd	d
�d tddg| | | | ggƒk�sbt
‚d S )Nr   é   úm c kr   r   Úpar   é   T©ZA_and_B)r   r   r   r   r(   r*   r   r   Úkanes_equationsr   ÚAssertionErrorÚloadsÚmass_matrixÚforcingÚinvr   r   ÚrhsÚmass_matrix_fullÚLUsolveÚforcing_fullr   Ú	linearizer   )r#   r$   ÚqdÚudr.   ÚcÚkr   r   r"   ÚFLr3   ÚBLÚKMÚMMr:   r<   r   r   r%   Útest_one_dof   s0    
,ÿÿ
rI   c                     s|  t dƒ\‰‰‰‰t ddƒ\} }}}tdƒ\}}}}}tdƒ‰ tdƒ}	tdƒ}
|	 ˆ ˆˆ j ¡ |
 ˆ ˆˆ ˆ j ¡ | d ˆd  d| dˆ  g‰|	| ˆ |ˆ  |ˆ  |ˆ  ˆ j f|
| ˆ |ˆ  ˆ j fg}td|	|ƒ}td	|
|ƒ}||g}tˆ ˆˆgˆˆgˆd
�}| ||¡ |j	}|j
}| ¡ | }t|d ƒt| ˆ |ˆ  |ˆ  |ˆ  | ƒk�stt‚t|d ƒt|ˆ |ˆ  d| ˆ  d| ˆ  | ƒk�s¶t‚|j�sÂt‚|jtdƒk�sÖt‚d|_|jttdƒd dgddggƒk�st‚dD ]6}||_t| ¡ |j |j¡ ƒtddƒk�st‚�q| ˆd  t|ƒtˆƒ g‰tt‡ ‡‡‡‡‡fdd„ƒ d S )Núq1 q2 u1 u2r1   úm c1 c2 k1 k2r   ÚP1ÚP2r4   Úpa1Úpa2©Úq_indÚu_indÚkd_eqsr   F)FTé   c                      s   t ˆ ˆˆgˆˆgˆd�S )NrP   r   r   ©r   r"   Úq1Úq2Úu1Úu2r   r%   r&   p   s   
 ÿztest_two_dof.<locals>.<lambda>)r   r   r   r   r(   r*   r   r   r6   r9   r:   r;   r   r7   Úexplicit_kinematicsÚmass_matrix_kinr   r   r
   r   r<   r=   r>   r?   r   r   r   r   r,   )Úq1dÚq2dÚu1dÚu2dr.   Úc1Úc2Úk1Úk2rL   rM   rE   rN   rO   rF   rG   rH   r:   r<   rZ   r   rU   r%   Útest_two_dof9   sZ     0ÿÿÿ<(ÿÿÿ(ÿÿrd   c                  C   s2  t dƒ\} }t ddƒ\}}tdƒ\}}}tdƒ}tdƒ}| || | t| ƒ |j || t| ƒ |j  ¡ || g}	||| |j fg}
t	d||ƒ}|g}t
|| g|g|	ƒ}| ||
¡ |j}|j}| ¡ | }| ¡  t|d ƒt| | t| ƒ ƒk�st‚t| ¡ |j |j¡ ƒtddƒk�s.t‚d S )	Nr   r1   zm l gr   r   r3   r   r4   )r   r   r   r   r(   r   r*   r   r+   r   r   r6   r9   r:   r;   r   r   r7   r<   r=   r>   r?   r   )r#   r$   rA   rB   r.   r-   r/   r   r   r"   rE   r3   rF   rG   rH   r:   r<   r   r   r%   Ú	test_pends   s,    6
(ÿÿre   c                      sÐ  t dƒ\} }}}}}t ddƒ\}}}}	}
}tdƒ\}}}tdƒ‰ ˆ  dd| ˆ jg¡}| dd||jg¡}| dd||jg¡‰ˆ ˆ ¡‰ˆ ˆ ||j ||j  ||j  ¡ t	d	ƒ}| 
ˆ d
¡ | d||j ¡}| |ˆ ˆ¡ t||d |d  |d |d  |d |d  ƒ}‡ ‡‡fdd„|D ƒ}|| | |j fg}td|ˆ|||fƒ}|g}tˆ | ||g|||g|d�}| ||¡ |j}|j}| ¡ | }| ¡ }| |¡}| ¡  | ¡ td| | | |d | t|ƒ  d| t|ƒ  d|  d| | d |d| |t|ƒ   gƒ ¡ k�s*t‚t| ¡ |j |j¡ ƒt ddƒk�sTt‚|j!dd�d
 }| |d|d|di¡ | d
|d
|d
|d
|d
i¡}d
d l"}| #| |dt$dƒ i¡¡ %¡ t&j'dik�sÌt‚d S )Núq1 q2 q3 u1 u2 u3r1   úr m gr   ÚYÚAxisÚLÚRÚCr   ÚDmcrT   r4   c                    s    g | ]}t ˆ ˆ ¡ˆ |ƒ‘qS r   ©r   Ú
ang_vel_in©Ú.0Zuv©r   rk   Zw_R_N_qdr   r%   Ú
<listcomp>¯   s     z%test_rolling_disc.<locals>.<listcomp>ÚBodyDrP   é   é   éþÿÿÿé   Tr5   )(r   r   r   Ú	orientnewÚzr*   r+   ro   Úset_ang_velr   r(   Ú	locatenewÚv2pt_theoryr   r   r   r6   r9   r:   r;   ZkindiffdictÚsubsr   r   r   r   r   r7   r<   r=   r>   r?   r   r@   ÚsympyZsympifyr	   Z	eigenvalsr
   ZZero) rV   rW   Úq3rX   rY   Úu3r\   r]   Úq3dr^   r_   Úu3dÚrr.   r/   rh   rj   rl   rm   ÚIr"   Ú	ForceListrt   ÚBodyListrG   rH   r:   r<   ZkddÚAZ	A_uprightr   r   rr   r%   Útest_rolling_disc‹   sV    
&2
*ÿÿ ÿÿÿ
0r‰   c            $         sô  t dƒ\} }}}}}t ddƒ\}}}}	}
}t dƒ\}}}}t ddƒ\}}tdƒ\}}}tdƒ‰ ˆ  dd| ˆ jg¡}| d	d||jg¡}| d
d||jg¡‰ˆ ˆ ¡‰ˆ ˆ ||j ||j  ||j  ¡ t	dƒ}| 
ˆ ||j ||j|jA   ¡ | d||j ¡}| |ˆ ˆ¡ | |ˆ ˆ¡ t||d |d  |d |d  |d |d  ƒ}‡ ‡‡fdd„|D ƒ}|| | |j f|||j ||j|jA   fg}td|ˆ|||fƒ}|g}tˆ | ||g|||||g|d�}| ||¡\}} | |d|di¡ |d|di¡}|  |d|di¡ |d|di¡} tˆ | ||g|||g|||gd�}!|! ||¡\}"}#|" |d|di¡ |d|di¡}"|# |d|di¡ |d|di¡}#|  ¡  |# ¡  ||"  ¡ tdddddgƒk�sÌt‚| |#  ¡ tdddddgƒk�sðt‚d S )Nrf   r1   zu4, u5, f1, f2zu4, u5rg   r   rh   ri   rj   rk   rl   rm   rT   r4   c                    s    g | ]}t ˆ ˆ ¡ˆ |ƒ‘qS r   rn   rp   rr   r   r%   rs   ð   s     ztest_aux.<locals>.<listcomp>rt   rP   r   )rQ   rR   rS   Úu_auxiliary)r   r   r   ry   rz   r*   r+   ro   r{   r   r(   r|   r}   Za2pt_theoryr   r   r   r6   r~   r   r   r   r7   )$rV   rW   r€   rX   rY   r�   r\   r]   r‚   r^   r_   rƒ   Zu4Zu5Úf1Úf2Zu4dZu5dr„   r.   r/   rh   rj   rl   rm   r…   r"   r†   rt   r‡   rG   ÚfrÚfrstarZKM2Zfr2Zfrstar2r   rr   r%   Útest_auxÕ   sJ    
&"22ÿ  ÿ  $r�   c                
   C   sÌ  t dƒ} t dƒ\}}t dƒ\}}}tdƒ}t dƒ\}}}	tdƒ\}
}tddƒ\}}tdƒ\}}tddƒ\}}td	ƒ}td
ƒ}| |d| |jg¡ | || |j ¡ tdƒ}| d|
|j ¡}| d||j	 ¡}| 
|d¡ | 
|||j ¡ | |||¡ td||ƒ}td|||t||||	ƒ|fƒ}|| || g}||g}||j	 |  | f||j	 |  | f||j | |
|  f||j| fg}t||
|g||g|ƒ}| ||¡\}}|j}|d |	k�sÈt‚d S )Nr/   zk lsza mA mCr!   zIx Iy Izzq1 q2r1   zu1 u2r   rˆ   ri   r   rl   ÚAor   ÚCartÚPendulum)rx   rx   )r   r   r   Zorientrz   r{   r   r|   r*   r+   r(   r}   r   r   r   r   r6   r=   r7   ) ZgravityrD   ZlsÚaZmAZmCr!   ZIxZIyZIzrV   rW   r\   r]   rX   rY   r^   r_   r   rˆ   r   rl   r�   r‘   r’   ZkindiffsZbodyListZ	forceListÚkmr�   rŽ   Úmmr   r   r%   Útest_parallel_axis	  s@    ýr–   c                      s.  t dƒ\} }t ddƒ\}}tdƒ\}}}tdƒ}tdƒ}| |||j ¡ || g}	|| |  ||  |j fg}
td||ƒ}|g}t|| g|g|	ƒ‰ ˆ  |¡d t	dgƒks²t
‚ˆ j|d d�d t	dgƒksÒt
‚ˆ j|d d	�d t	dgƒksòt
‚ˆ  |¡d t	dgƒk�st
‚ˆ  |g ¡d t	dgƒk�s0t
‚tt‡ fd
d„ƒ t|| g|g|	||
d�‰ ˆ  ¡ d t	| | ||   gƒk�s„t
‚t dƒ\}}}}t ddƒ\}}}}tdƒ\}}}}}tdƒ}tdƒ}tdƒ}| |||j ¡ | ||| |j ¡ || || g}	|| | ||  ||  ||  |j f|| | ||  |j ff}
td||ƒ}td||ƒ}||f}t|||g||g|	d�‰ ˆ  ||
¡ ˆ j}ˆ j}| ¡ | }t|d ƒt| | ||  ||  ||  | ƒk�sèt
‚t|d ƒt|| ||  d| |  d| |  | ƒk�s*t
‚d S )Nr   r1   r2   r   r   r3   r   )r   r8   )r8   c                      s
   ˆ   d¡S )Nz	bad input)Z_form_frr   ©rG   r   r%   r&   [  ó    z#test_input_format.<locals>.<lambda>r   rJ   rK   rL   rM   rN   rO   rP   r4   )r   r   r   r   r(   r*   r   r   r6   r   r7   r   r,   r9   r:   r;   r   ) r#   r$   rA   rB   r.   rC   rD   r   r   r"   rE   r3   rF   rV   rW   rX   rY   r\   r]   r^   r_   r`   ra   rb   rc   rL   rM   rN   rO   rH   r:   r<   r   r—   r%   Útest_input_formatA  sf    
   *0ÿÿÿ<(ÿÿÿr™   c            &      C   sä  t dƒ} tdƒ}| | d¡ tddd�}|  dd|¡}td	ƒ}| d
|d |j |d |j  |d |j  ¡}|dd … | }|d g}g }| 	| ¡}	tdƒ\}
}}| 
| |
|j ||j  ||j  ¡ | 	| ¡|	  ¡ }|||j@ ||j@ ||j@ g7 }| | ¡}tdƒ\}}}| | ||j ||j  ||j  ¡ | | ¡| }|||j@ ||j@ ||j@ g7 }||||
||g}t|ƒ}|j| d d g}||j| ¡  d g }z t| |||||g g g dd�
}W n( tk
�rö } z|‚W 5 d }~X Y nX tdƒ}t|fdd„ dD ƒžŽ }| tdƒdtdƒdi¡}td|||||fƒ}|g}|j|jtdƒ | j f|jtdƒ|j f|jtdƒ|j fg}| ||¡ tdd„ |jD ƒdd„ |jD ƒ ƒ}|j}|j}d|_ tdd„ |jD ƒdd„ |jD ƒ ƒ} |j}!||  dk �s
t!‚||j" ¡  | t|ƒk�s*t!‚|!}"td|
 | | g|
d|| g|| d|
g|||
 dggƒ| d }#|#d |"d < |#d |"d< |#d |"d< |#d! |"d< t#|d |d ƒd }$t||" ƒ |d |$i¡}%|%|k�sàt!‚d S )"NÚNEDÚNED_or   z
lambda_0:4T)ÚrealÚBZ
QuaternionzB_x:zÚB_cmr1   r4   zP Q RzU V WF)Zq_dependentrS   Zconfiguration_constraintsZvelocity_constraintsZu_dependentrŠ   rZ   ÚM_Bc                 S   s2   g | ]*}t d |› �ƒ|d |d kr(dnd ‘qS )ZJ_B_r   r1   éÿÿÿÿ)r
   )rq   Zaxr   r   r%   rs   Ì  s   ÿz,test_implicit_kinematics.<locals>.<listcomp>)ZxxÚyyÚzzZxyZyzÚxzZJ_B_xyZJ_B_yzÚRBr/   ZT_zZF_zc                 S   s   g | ]}|  ¡ ‘qS r   ©Z	count_ops©rq   r*   r   r   r%   rs   ß  s     c                 S   s   g | ]}|  ¡ ‘qS r   r¥   r¦   r   r   r%   rs   à  s     c                 S   s   g | ]}|  ¡ ‘qS r   r¥   r¦   r   r   r%   rs   è  s     c                 S   s   g | ]}|  ¡ ‘qS r   r¥   r¦   r   r   r%   rs   é  s     gš™™™™™©?r    rx   )$r   r   r(   r   ry   r|   r*   r+   rz   ro   r{   r   Zvelr   ÚTr'   r   Ú	Exceptionr   r   r~   r
   r   Z
masscenterZmassÚframer6   Úsumr?   r=   r[   Zforcing_kinrZ   r7   r#   r   )&rš   r›   Zq_attr�   Zq_posrž   rQ   Zq_depZkinematic_eqsZ	B_ang_velr   ÚQrk   ZB_ang_vel_kdZB_cm_velÚUÚVÚWZB_ref_vel_kdrR   Z	q_att_vecZconfig_consrG   ÚerŸ   ZJ_Br¤   Zrigid_bodiesZ
force_listZn_ops_implicitZmass_matrix_kin_implicitZforcing_kin_implicitZn_ops_explicitZforcing_kin_explicitZqdot_candidateZquat_dot_textbookZlambda_0_solZlhs_candidater   r   r%   Útest_implicit_kinematics~  s´    2

&ý
&ýù
	ÿ
ûÿÿÿÿ	 üûû
r°   c               	   C   s  t dƒ\} }}}tdƒ\}}}}}}	tdƒ}
tdƒ}| |
|||fd¡ | |
¡}| |j¡| | |j¡| | |j¡|	 g}t	dƒ}| 
|
d¡ | d| |j ¡}| 
|d¡ td	||ƒ}t|
|||g|||	g||g|| |  |
j fgd
�}| ¡  ||d  tdddgdddgdddggƒ }t|| | t|ƒt|ƒ t|ƒt|ƒ t|ƒ   || |	   gdg|| |  t|ƒt|ƒ t|ƒt|ƒ t|ƒ   || |   ggƒ}t|jƒ ||||||	h¡�sØt‚t|j| ƒtddƒk�söt‚t|j| ƒtddƒk�st‚d S )Nzg l m cz	q1:4 u1:4r   rˆ   Zzxyr   r   ÚPor   r   r4   r1   rx   )r   r   r   Zorient_body_fixedro   r   r*   r+   rz   r   r(   r|   r   r   r6   r   r   r   r   r:   Úissubsetr7   r   r9   r   )r/   r-   r.   rC   rV   rW   r€   rX   rY   r�   r   rˆ   ZN_w_AZkdesr   r±   r   ZkaneZexpected_mdZexpected_fdr   r   r%   Útest_issue_24887  s2    
.ÿ,@ Bþ"r³   N)$r   r   r   r   r   r   r   r   r	   r
   r   r   Zsympy.simplify.simplifyr   Zsympy.physics.mechanicsr   r   r   r   r   r   r   r   r   Zsympy.testing.pytestr   r0   rI   rd   re   r‰   r�   r–   r™   r°   r³   r   r   r   r%   Ú<module>   s   0, :J48= 