a
    kh)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   U/var/www/auris/lib/python3.9/site-packages/sympy/physics/mechanics/tests/test_kane.py<lambda>   s   z*test_invalid_coordinates.<locals>.<lambda>)r   diffr   Z_tr   r   set_velr   ZpointZset_posr   xr   yr   
ValueError)lmgr   r   r    r%   test_invalid_coordinates   s    *r/   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sJ |j
|
ksJ |j}|j}| | }t|d t| | ||   | ksJ t| |j|j tddks,J |jd	d
d tddg| | | | ggksbJ 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"   FLr2   BLKMMMr8   r:   r   r   r%   test_one_dof   s0    
,
rG   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stJ 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sJ dD ]6}||_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 u2r0   m c1 c2 k1 k2r   P1P2r3   pa1pa2q_indu_indkd_eqsr   F)FT   c                      s   t  ggdS )NrN   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   r5   r7   r8   r9   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k2rJ   rK   rC   rL   rM   rD   rE   rF   r8   r:   rX   r   rS   r%   test_two_dof9   sZ     0<((rb   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J t| |j|j tddks.J d S )	Nr   r0   zm l gr   r   r2   r   r3   )r   r   r   r   r(   r   r)   r   r*   r   r   r5   r7   r8   r9   r   r   r:   r;   r<   r=   r   )r#   r$   r?   r@   r-   r,   r.   r   r   r"   rC   r2   rD   rE   rF   r8   r:   r   r   r%   	test_pends   s,    6
(rc   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sTJ |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sJ d S )Nq1 q2 q3 u1 u2 u3r0   r m gr   YAxisLRCr   DmcrR   r3   c                    s    g | ]}t   |qS r   r   
ang_vel_in.0Zuvr   ri   Zw_R_N_qdr   r%   
<listcomp>       z%test_rolling_disc.<locals>.<listcomp>BodyDrN            Tr4   )'r   r   r   	orientnewzr)   r*   rm   set_ang_velr   r(   	locatenewv2pt_theoryr   r   r   r5   r7   r8   r9   Zkindiffdictsubsr   r   r   r   r   r:   r;   r<   r=   r   r>   sympyZsympifyr	   Z	eigenvalsr
   ZZero) rT   rU   q3rV   rW   u3rZ   r[   q3dr\   r]   u3drr-   r.   rf   rh   rj   rk   Ir"   	ForceListrs   BodyListrE   rF   r8   r:   ZkddAZ	A_uprightr~   r   rp   r%   test_rolling_disc   sT    
&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g dksJ | |#  tg dksJ d S )Nrd   r0   zu4, u5, f1, f2zu4, u5re   r   rf   rg   rh   ri   rj   rk   rR   r3   c                    s    g | ]}t   |qS r   rl   rn   rp   r   r%   rq      rr   ztest_aux.<locals>.<listcomp>rs   rN   r   )rO   rP   rQ   u_auxiliary)r   r   r   r   r   )r   r   r   rx   ry   r)   r*   rm   rz   r   r(   r{   r|   Za2pt_theoryr   r   r   r5   r}   r   r   r   )$rT   rU   r   rV   rW   r   rZ   r[   r   r\   r]   r   Zu4Zu5f1f2Zu4dZu5dr   r-   r.   rf   rh   rj   rk   r   r"   r   rs   r   rE   frfrstarZKM2Zfr2Zfrstar2r   rp   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 q2r0   zu1 u2r   r   rg   r   rj   Aor   CartPendulum)rw   rw   )r   r   r   Zorientry   rz   r   r{   r)   r*   r(   r|   r   r   r   r   r5   r;   ) ZgravityrB   ZlsaZmAZmCr!   ZIxZIyZIzrT   rU   rZ   r[   rV   rW   r\   r]   r   r   r   rj   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sJ  j|d dd t	dgksJ  j|d d	d t	dgksJ  |d t	dgksJ  |g d t	dgks0J 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sJ t|d t|| ||  d| |  d| |  | ks*J d S )Nr   r0   r1   r   r   r2   r   )r   r6   )r6   c                      s
     dS )Nz	bad input)Z_form_frr   rE   r   r%   r&   [  rr   z#test_input_format.<locals>.<lambda>r   rH   rI   rJ   rK   rL   rM   rN   r3   )r   r   r   r   r(   r)   r   r   r5   r   r   r+   r7   r8   r9   r   ) r#   r$   r?   r@   r-   rA   rB   r   r   r"   rC   r2   rD   rT   rU   rV   rW   rZ   r[   r\   r]   r^   r_   r`   ra   rJ   rK   rL   rM   rF   r8   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|W Y d }~n
d }~0 0 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 sJ ||j!  | t|ks.J |!}"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_cmr0   r3   zP Q RzU V WF)Zq_dependentrQ   Zconfiguration_constraintsZvelocity_constraintsZu_dependentr   rX   M_Bc                 S   s2   g | ]*}t d | |d |d kr(dnd qS )ZJ_B_r   r0   )r
   )ro   axr   r   r%   rq     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   s   g | ]}|  qS r   Z	count_opsro   r)   r   r   r%   rq     rr   c                 S   s   g | ]}|  qS r   r   r   r   r   r%   rq     rr   c                 S   s   g | ]}|  qS r   r   r   r   r   r%   rq     rr   c                 S   s   g | ]}|  qS r   r   r   r   r   r%   rq     rr   g?r   rw   )#r   r   r(   r   rx   r{   r)   r*   ry   rm   rz   r   Zvelr   Tr'   r   	Exceptionr   r   r}   r
   r   Z
masscenterZmassframer5   sumr=   r;   rY   Zforcing_kinrX   r#   r   )&r   r   Zq_attr   Zq_posr   rO   Zq_depZkinematic_eqsZ	B_ang_velr   Qri   ZB_ang_vel_kdZB_cm_velUVWZB_ref_vel_kdrP   Z	q_att_vecZconfig_consrE   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   r3   )r0   r   r   )r   r   r   )r   r   r0   rw   r0   )r   r   r   Zorient_body_fixedrm   r   r)   r*   ry   r   r(   r{   r   r   r5   r   r   r   r   r8   issubsetr   r7   r   )r.   r,   r-   rA   rT   rU   r   rV   rW   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   r/   rG   rb   rc   r   r   r   r   r   r   r   r   r   r%   <module>   s   0, :J48= 