o
    g)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bodies	forcelistr    Fr   r   kdqur    ^/var/www/visachat/venv/lib/python3.10/site-packages/sympy/physics/mechanics/tests/test_kane.py<lambda>   s    z*test_invalid_coordinates.<locals>.<lambda>)r   diffr   _tr   r   set_velr   pointset_posr   xr   yr   
ValueError)lmgr   r    r!   r&   test_invalid_coordinates   s   * r3   c                  C   s`  t d\} }t dd\}}td\}}}td}td}||||j  || g}	|| |  ||  |j fg}
td||}|g}t|| g|g|	}|||
 |j	|ksXJ |j
|
ks_J |j}|j}| | }t|d t| | ||   | ksJ t| |j|j tddksJ |jd	d
d tddg| | | | ggksJ d S )Nr      m c kr   r   par      TA_and_B)r   r   r   r   r*   r-   r   r   kanes_equationsr   loadsmass_matrixforcinginvr   r   rhsmass_matrix_fullLUsolveforcing_fullr   	linearizer   )r$   r%   qdudr1   ckr   r   r#   FLr6   BLKMMMr=   r?   r    r    r&   test_one_dof   s0   
*8rL   c                     sp  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sJ t|d t| |  d|   d|   | ksJ |jsJ |jtdksJ d|_|jttdd dgddggksJ dD ]}||_t| |j|j tddksJ q| d  t|t gtt fdd d S )Nq1 q2 u1 u2r4   m c1 c2 k1 k2r   P1P2r7   pa1pa2q_indu_indkd_eqsr   F)FT   c                      s   t  ggdS )NrS   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   r:   r<   r=   r>   r   explicit_kinematicsmass_matrix_kinr   r   r
   r   r?   r@   rA   rB   r   r   r   r   r/   )q1dq2du1du2dr1   c1c2k1k2rO   rP   rH   rQ   rR   rI   rJ   rK   r=   r?   r]   r    rX   r&   test_two_dof9   sZ    0:(
& rg   c                  C   s.  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sJ t| |j|j tddksJ d S )	Nr   r4   zm l gr   r   r6   r   r7   )r   r   r   r   r*   r   r-   r   r.   r   r   r:   r<   r=   r>   r   r   r?   r@   rA   rB   r   )r$   r%   rD   rE   r1   r0   r2   r   r   r#   rH   r6   rI   rJ   rK   r=   r?   r    r    r&   	test_pends   s,   6
&rh   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J t| |j|j tddks*J |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sfJ d S )Nq1 q2 q3 u1 u2 u3r4   r m gr   YAxisLRCr   DmcrW   r7   c                        g | ]}t   |qS r    r   
ang_vel_in.0uvr   rn   w_R_N_qdr    r&   
<listcomp>        z%test_rolling_disc.<locals>.<listcomp>BodyDrS            Tr8   )'r   r   r   	orientnewzr-   r.   rs   set_ang_velr   r*   	locatenewv2pt_theoryr   r   r   r:   r<   r=   r>   kindiffdictsubsr   r   r   r   r   r?   r@   rA   rB   r   rC   sympysympifyr	   	eigenvalsr
   Zero) rY   rZ   q3r[   r\   u3r_   r`   q3dra   rb   u3drr1   r2   rk   rm   ro   rp   Ir#   	ForceListr{   BodyListrJ   rK   r=   r?   kddA	A_uprightr   r    rw   r&   test_rolling_disc   sX   
&2
*$

04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g dkscJ | |#  tg dksrJ d S )Nri   r4   zu4, u5, f1, f2zu4, u5rj   r   rk   rl   rm   rn   ro   rp   rW   r7   c                    rq   r    rr   rt   rw   r    r&   ry      rz   ztest_aux.<locals>.<listcomp>r{   rS   r   )rT   rU   rV   u_auxiliary)r   r   r   r   r   )r   r   r   r   r   r-   r.   rs   r   r   r*   r   r   a2pt_theoryr   r   r   r:   r   r   r   r   )$rY   rZ   r   r[   r\   r   r_   r`   r   ra   rb   r   u4u5f1f2u4du5dr   r1   r2   rk   rm   ro   rp   r   r#   r   r{   r   rJ   frfrstarKM2fr2frstar2r    rw   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sJ d S )Nr2   zk lsza mA mCr"   zIx Iy Izzq1 q2r4   zu1 u2r   r   rl   r   ro   Aor   CartPendulum)r   r   )r   r   r   orientr   r   r   r   r-   r.   r*   r   r   r   r   r   r:   r@   ) gravityrG   lsamAmCr"   IxIyIzrY   rZ   r_   r`   r[   r\   ra   rb   r   r   r   ro   r   r   r   kindiffsbodyList	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sYJ  j|d dd t	dgksiJ  j|d d	d t	dgksyJ  |d t	dgksJ  |g d t	dgksJ t
t fd
d t|| g|g|	||
d   d t	| | ||   gksJ 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sqJ t|d t|| ||  d| |  d| |  | ksJ d S )Nr   r4   r5   r   r   r6   r   )r   r;   )r;   c                      s
     dS )Nz	bad input)_form_frr    rJ   r    r&   r'   [  s   
 z#test_input_format.<locals>.<lambda>r   rM   rN   rO   rP   rQ   rR   rS   r7   )r   r   r   r   r*   r-   r   r   r:   r   r   r/   r<   r=   r>   r   ) r$   r%   rD   rE   r1   rF   rG   r   r   r#   rH   r6   rI   rY   rZ   r[   r\   r_   r`   ra   rb   rc   rd   re   rf   rO   rP   rQ   rR   rK   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y } z|d }~ww td}t|gdd dD R  }|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 sJ ||j!  | t|ksJ |!}"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sJ d S )"NNEDNED_or   z
lambda_0:4T)realB
QuaternionzB_x:zB_cmr4   r7   zP Q RzU V WF)q_dependentrV   configuration_constraintsvelocity_constraintsu_dependentr   r]   M_Bc                 S   s2   g | ]}t d | |d |d krdnd qS )J_B_r   r4   )r
   )ru   axr    r    r&   ry     s    *z,test_implicit_kinematics.<locals>.<listcomp>)xxyyzzxyyzxzJ_B_xyJ_B_yzRBr2   T_zF_zc                 S      g | ]}|  qS r    	count_opsru   r-   r    r    r&   ry         c                 S   r   r    r   r   r    r    r&   ry     r   c                 S   r   r    r   r   r    r    r&   ry     r   c                 S   r   r    r   r   r    r    r&   ry     r   g?r   r   )#r   r   r*   r   r   r   r-   r.   r   rs   r   r   velr   Tr(   r   	Exceptionr   r   r   r
   r   
masscentermassframer:   sumrB   r@   r^   forcing_kinr]   r$   r   )&r   r   q_attr   q_posr   rT   q_depkinematic_eqs	B_ang_velr   Qrn   B_ang_vel_kdB_cm_velUVWB_ref_vel_kdrU   	q_att_vecconfig_consrJ   er   J_Br   rigid_bodies
force_listn_ops_implicitmass_matrix_kin_implicitforcing_kin_implicitn_ops_explicitforcing_kin_explicitqdot_candidatequat_dot_textbooklambda_0_sol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g dg dg dg }t|| | t|t| t|t| t|   || |	   gdg|| |  t|t| t|t| t|   || |   gg}t|j||||||	hsJ t|j| tddksJ t|j| tddksJ d S )Nzg l m cz	q1:4 u1:4r   r   zxyr   r   Por   r   r7   )r4   r   r   )r   r   r   )r   r   r4   r   r4   )r   r   r   orient_body_fixedrs   r   r-   r.   r   r   r*   r   r   r   r:   r   r   r   r   r=   issubsetr   r<   r   )r2   r0   r1   rF   rY   rZ   r   r[   r\   r   r   r   N_w_Akdesr   r   r   kaneexpected_mdexpected_fdr    r    r&   test_issue_24887  s0   
.&@F "r  N)$r   r   r   r   r   r   r   r   r	   r
   r   r   sympy.simplify.simplifyr   sympy.physics.mechanicsr   r   r   r   r   r   r   r   r   sympy.testing.pytestr   r3   rL   rg   rh   r   r   r   r   r   r  r    r    r    r&   <module>   s     0, :J48= 