o
    GZh)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   V/var/www/auris/lib/python3.10/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   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Z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udr.   ckr   r   r"   FLr3   BLKMMMr9   r;   r   r   r%   test_one_dof   s0   
*8rH   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 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 )NrO   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   r8   r9   r:   r   explicit_kinematicsmass_matrix_kinr   r   r
   r   r;   r<   r=   r>   r   r   r   r   r,   )q1dq2du1du2dr.   c1c2k1k2rK   rL   rD   rM   rN   rE   rF   rG   r9   r;   rY   r   rT   r%   test_two_dof9   sZ    0:(
& rc   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   r1   zm l gr   r   r3   r   r4   )r   r   r   r   r(   r   r*   r   r+   r   r   r6   r8   r9   r:   r   r   r;   r<   r=   r>   r   )r#   r$   r@   rA   r.   r-   r/   r   r   r"   rD   r3   rE   rF   rG   r9   r;   r   r   r%   	test_pends   s,   6
&rd   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 u3r1   r m gr   YAxisLRCr   DmcrS   r4   c                        g | ]}t   |qS r   r   
ang_vel_in.0Zuvr   rj   Zw_R_N_qdr   r%   
<listcomp>        z%test_rolling_disc.<locals>.<listcomp>BodyDrO            Tr5   )'r   r   r   	orientnewzr*   r+   ro   set_ang_velr   r(   	locatenewv2pt_theoryr   r   r   r6   r8   r9   r:   Zkindiffdictsubsr   r   r   r   r   r;   r<   r=   r>   r   r?   sympyZsympifyr	   Z	eigenvalsr
   ZZero) rU   rV   q3rW   rX   u3r[   r\   q3dr]   r^   u3drr.   r/   rg   ri   rk   rl   Ir"   	ForceListru   BodyListrF   rG   r9   r;   ZkddAZ	A_uprightr   r   rr   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 )Nre   r1   zu4, u5, f1, f2zu4, u5rf   r   rg   rh   ri   rj   rk   rl   rS   r4   c                    rm   r   rn   rp   rr   r   r%   rs      rt   ztest_aux.<locals>.<listcomp>ru   rO   r   )rP   rQ   rR   u_auxiliary)r   r   r   r   r   )r   r   r   rz   r{   r*   r+   ro   r|   r   r(   r}   r~   Za2pt_theoryr   r   r   r6   r   r   r   r   )$rU   rV   r   rW   rX   r   r[   r\   r   r]   r^   r   Zu4Zu5f1f2Zu4dZu5dr   r.   r/   rg   ri   rk   rl   r   r"   r   ru   r   rF   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sJ d S )Nr/   zk lsza mA mCr!   zIx Iy Izzq1 q2r1   zu1 u2r   r   rh   r   rk   Aor   CartPendulum)ry   ry   )r   r   r   Zorientr{   r|   r   r}   r*   r+   r(   r~   r   r   r   r   r6   r<   ) ZgravityrC   ZlsaZmAZmCr!   ZIxZIyZIzrU   rV   r[   r\   rW   rX   r]   r^   r   r   r   rk   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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   r1   r2   r   r   r3   r   )r   r7   )r7   c                      s
     dS )Nz	bad input)Z_form_frr   rF   r   r%   r&   [  s   
 z#test_input_format.<locals>.<lambda>r   rI   rJ   rK   rL   rM   rN   rO   r4   )r   r   r   r   r(   r*   r   r   r6   r   r   r,   r8   r9   r:   r   ) r#   r$   r@   rA   r.   rB   rC   r   r   r"   rD   r3   rE   rU   rV   rW   rX   r[   r\   r]   r^   r_   r`   ra   rb   rK   rL   rM   rN   rG   r9   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BZ
QuaternionzB_x:zB_cmr1   r4   zP Q RzU V WF)Zq_dependentrR   Zconfiguration_constraintsZvelocity_constraintsZu_dependentr   rY   M_Bc                 S   s2   g | ]}t d | |d |d krdnd qS )ZJ_B_r   r1   )r
   )rq   axr   r   r%   rs     s    *z,test_implicit_kinematics.<locals>.<listcomp>)xxyyzzZxyZyzxzZJ_B_xyZJ_B_yzRBr/   ZT_zZF_zc                 S      g | ]}|  qS r   Z	count_opsrq   r*   r   r   r%   rs         c                 S   r   r   r   r   r   r   r%   rs     r   c                 S   r   r   r   r   r   r   r%   rs     r   c                 S   r   r   r   r   r   r   r%   rs     r   g?r   ry   )#r   r   r(   r   rz   r}   r*   r+   r{   ro   r|   r   Zvelr   Tr'   r   	Exceptionr   r   r   r
   r   Z
masscenterZmassframer6   sumr>   r<   rZ   Zforcing_kinrY   r#   r   )&r   r   Zq_attr   Zq_posr   rP   Zq_depZkinematic_eqsZ	B_ang_velr   Qrj   ZB_ang_vel_kdZB_cm_velUVWZB_ref_vel_kdrQ   Z	q_att_vecZconfig_consrF   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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   Zzxyr   r   Por   r   r4   )r1   r   r   )r   r   r   )r   r   r1   ry   r1   )r   r   r   Zorient_body_fixedro   r   r*   r+   r{   r   r(   r}   r   r   r6   r   r   r   r   r9   issubsetr   r8   r   )r/   r-   r.   rB   rU   rV   r   rW   rX   r   r   r   ZN_w_AZkdesr   r   r   ZkaneZexpected_mdZexpected_fdr   r   r%   test_issue_24887  s0   
.&@F "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   rH   rc   rd   r   r   r   r   r   r   r   r   r   r%   <module>   s     0, :J48= 