U
    0ewP                     @   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   K/tmp/pip-unpacked-wheel-_6tpq7m6/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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"   FLr2   BLKMMMr9   r;   r   r   r%   test_one_dof   s0    
,
rH   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 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 )NrO   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   r5   r8   r9   r:   r   r6   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_dof:   sZ     0<((rc   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   r0   zm l gr   r   r2   r   r3   )r   r   r   r   r(   r   r)   r   r*   r   r   r5   r8   r9   r:   r   r   r6   r;   r<   r=   r>   r   )r#   r$   r@   rA   r-   r,   r.   r   r   r"   rD   r2   rE   rF   rG   r9   r;   r   r   r%   	test_pendt   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*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 u3r0   r m gr   YAxisLRCr   DmcrS   r3   c                    s    g | ]}t   |qS r   r   
ang_vel_in.0Zuvr   rj   Zw_R_N_qdr   r%   
<listcomp>   s     z%test_rolling_disc.<locals>.<listcomp>BodyDrO            Tr4   )(r   r   r   	orientnewzr)   r*   rn   set_ang_velr   r(   	locatenewv2pt_theoryr   r   r   r5   r8   r9   r:   Zkindiffdictsubsr   r   r   r   r   r6   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"   	ForceListrs   BodyListrF   rG   r9   r;   ZkddAZ	A_uprightr~   r   rq   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 )Nre   r0   zu4, u5, f1, f2zu4, u5rf   r   rg   rh   ri   rj   rk   rl   rS   r3   c                    s    g | ]}t   |qS r   rm   ro   rq   r   r%   rr      s     ztest_aux.<locals>.<listcomp>rs   rO   r   )rP   rQ   rR   u_auxiliary)r   r   r   rx   ry   r)   r*   rn   rz   r   r(   r{   r|   Za2pt_theoryr   r   r   r5   r}   r   r   r   r6   )$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   rs   r   rF   frfrstarZKM2Zfr2Zfrstar2r   rq   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 q2r0   zu1 u2r   r   rh   r   rk   Aor   CartPendulum)rw   rw   )r   r   r   Zorientry   rz   r   r{   r)   r*   r(   r|   r   r   r   r   r5   r<   r6   ) 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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   r0   r1   r   r   r2   r   )r   r7   )r7   c                      s
     dS )Nz	bad input)Z_form_frr   rF   r   r%   r&   \      z#test_input_format.<locals>.<lambda>r   rI   rJ   rK   rL   rM   rN   rO   r3   )r   r   r   r   r(   r)   r   r   r5   r   r6   r   r+   r8   r9   r:   r   ) r#   r$   r@   rA   r-   rB   rC   r   r   r"   rD   r2   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_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_cmr0   r3   zP Q RzU V WF)Zq_dependentrR   Zconfiguration_constraintsZvelocity_constraintsZu_dependentr   rY   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   r0   )r
   )rp   Zaxr   r   r%   rr     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rp   r)   r   r   r%   rr     s     c                 S   s   g | ]}|  qS r   r   r   r   r   r%   rr     s     c                 S   s   g | ]}|  qS r   r   r   r   r   r%   rr     s     c                 S   s   g | ]}|  qS r   r   r   r   r   r%   rr     s     g?r   rw   )&r   r   r(   r   rx   r{   r)   r*   ry   rn   rz   r   Zvelr   Tr'   r   	Exceptionr   strr   r   r}   r
   r   Z
masscenterZmassframer5   sumr>   r<   rZ   Zforcing_kinrY   r6   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   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   r/   rH   rc   rd   r   r   r   r   r   r   r   r   r%   <module>   s   0( :J48=