o
    *ήc5                     @   s   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l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S )    )	cosexpandMatrixsinsymbolstansqrtSzeros)simplify)dynamicsymbolsReferenceFramePoint	RigidBodyKanesMethodinertiaParticledot)raisesc                  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 )Nq u   m c kNPpar      TA_and_B)r   r   r   r   set_velxr   r   kanes_equationsbodiesloadsmass_matrixforcinginvr   r   rhsmass_matrix_fullLUsolveforcing_fullr
   	linearizer   )quqdudmckr   r   kdFLr   BLKMMMr$   r&    r7   N/tmp/pip-target-vg8gfxp4/lib/python/sympy/physics/mechanics/tests/test_kane.pytest_one_dof
   s0   
*8r9   c                     s  t d\t dd\} }}}td\}}}}}td td}	td}
|	  j  |
   j  |  | 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 t| |j|j tddksJ | d  t|t gtt fdd d S )Nq1 q2 u1 u2r   m c1 c2 k1 k2r   P1P2pa1pa2q_indu_indkd_eqsr   r      c                      s   t  ggdS )Nr@   )r   r7   r   r2   q1q2u1u2r7   r8   <lambda>S   s   
 ztest_two_dof.<locals>.<lambda>)r   r   r   r   r   r   r   r   r    r#   r$   r%   r   r   r&   r'   r(   r)   r
   r   r   r   
ValueError)q1dq2du1du2dr/   c1c2k1k2r<   r=   r3   r>   r?   r4   r5   r6   r$   r&   r7   rE   r8   test_two_dof*   sN   0:( rT   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   r   zm l gr   r   r   r   r   )r   r   r   r   r   r   r   r   yr   r   r    r#   r$   r%   r   r   r&   r'   r(   r)   r
   )r+   r,   r-   r.   r/   lgr   r   r2   r3   r   r4   r5   r6   r$   r&   r7   r7   r8   	test_pendV   s,   6
&rX   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 u3r   r m gr   YAxisLRCr   DmcrD   r   c                        g | ]}t   |qS r7   r   
ang_vel_in.0uvr   r^   w_R_N_qdr7   r8   
<listcomp>        z%test_rolling_disc.<locals>.<listcomp>BodyDr@            Tr   )'r   r   r   	orientnewzr   rU   rc   set_ang_velr   r   	locatenewv2pt_theoryr   r   r   r    r#   r$   r%   kindiffdictsubsr   r   r   r   r   r&   r'   r(   r)   r
   r*   sympysympifyr   	eigenvalsr	   Zero) rF   rG   q3rH   rI   u3rL   rM   q3drN   rO   u3drr/   rW   r[   r]   r_   r`   Ir2   	ForceListrk   BodyListr5   r6   r$   r&   kddA	A_uprightrw   r7   rg   r8   test_rolling_discn   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 )NrY   r   zu4, u5, f1, f2zu4, u5rZ   r   r[   r\   r]   r^   r_   r`   rD   r   c                    ra   r7   rb   rd   rg   r7   r8   ri      rj   ztest_aux.<locals>.<listcomp>rk   r@   r   )rA   rB   rC   u_auxiliary)r   r   r   r   r   )r   r   r   rp   rq   r   rU   rc   rr   r   r   rs   rt   a2pt_theoryr   r   r   r    rv   r   r   r   )$rF   rG   r{   rH   rI   r|   rL   rM   r}   rN   rO   r~   u4u5f1f2u4du5dr   r/   rW   r[   r]   r_   r`   r   r2   r   rk   r   r5   frfrstarKM2fr2frstar2r7   rg   r8   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 )NrW   zk lsza mA mCFzIx Iy Izzq1 q2r   zu1 u2r   r   r\   Or_   Aor   CartPendulum)ro   ro   )r   r   r   orientrq   rr   r   rs   r   rU   r   rt   r   r   r   r   r    r'   ) gravityr1   lsamAmCr   IxIyIzrF   rG   rL   rM   rH   rI   rN   rO   r   r   r   r_   r   r   r   kindiffsbodyList	forceListkmr   r   mmr7   r7   r8   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   r   r   r   r   r   r   )r!   r"   )r"   c                      s
     dS )Nz	bad input)_form_frr7   r5   r7   r8   rJ   >  s   
 z#test_input_format.<locals>.<lambda>)r!   	forcelistr:   r;   r<   r=   r>   r?   r@   r   )r   r   r   r   r   r   r   r   r    r   r   rK   r#   r$   r%   r   ) r+   r,   r-   r.   r/   r0   r1   r   r   r2   r3   r   r4   rF   rG   rH   rI   rL   rM   rN   rO   rP   rQ   rR   rS   r<   r=   r>   r?   r6   r$   r&   r7   r   r8   test_input_format$  sf   
  (0<(r   N)sympy.core.backendr   r   r   r   r   r   r   r	   r
   sympy.simplify.simplifyr   sympy.physics.mechanicsr   r   r   r   r   r   r   r   sympy.testing.pytestr   r9   rT   rX   r   r   r   r   r7   r7   r7   r8   <module>   s   , ( ,J48