U
    9%ewP                     @   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 d dl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S )    )solve)
cosexpandMatrixsinsymbolstansqrtSzeroseye)simplify)dynamicsymbolsReferenceFramePoint	RigidBodyKanesMethodinertiaParticledot)raises)USE_SYMENGINEc                     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/Darija-Ai-API/env/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st
|j|
kst
|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st|jst|jtdkst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&   q   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_dof:   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_pendt   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st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st| |#  tdddddgkst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st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st
 j|d dd t	dgkst
 j|d d	d t	dgkst
 |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st
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st
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_formatB  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 nF tk
r } z&tr dt|kr W Y d S |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sHt#|!}"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st#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   zMatrix is rank deficient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   strr   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   N)$r   r   Zsympy.core.backendr   r   r   r   r   r   r	   r
   r   r   Zsympy.simplify.simplifyr   Zsympy.physics.mechanicsr   r   r   r   r   r   r   r   Zsympy.testing.pytestr   r   r0   rI   rd   re   r   r   r   r   r   r   r   r   r%   <module>   s   0( :J48=