
    sgR                        d dl mZ d dlmZ d dlmZ d dlmZ d dlm	Z	m
Z
 d dlmZ d dlmZmZmZ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
lmZ  ed      \  Z Z!Z"Z#Z$ ed      Z%e%jM                  dde e%jN                  g      Z(e(jM                  dde!e(jR                  g      Z*e*jM                  dde"e*jV                  g      Z,d Z-d Z.d Z/d Z0d Z1d Z2d Z3d Z4d Z5d Z6d Z7y)    )pi)S)symbols)sqrt)cossin)Integral)DyadicPointReferenceFrameVector)	crossdotexpresstime_derivativekinematic_equationsouterpartial_velocityget_motion_paramsdynamicsymbols)trigsimp)raiseszq1 q2 q3 q4 q5NAAxisBCc                  .   t        t        j                  t        j                        dk(  sJ t        t        j                  t        j                        dk(  sJ t        t        j                  t        j                        dk(  sJ t        t        j                  t        j                        dk(  sJ t        t        j                  t        j                        dk(  sJ t        t        j                  t        j                        dk(  sJ t        t        j                  t        j                        dk(  sJ t        t        j                  t        j                        dk(  sJ t        t        j                  t        j                        dk(  sJ y )N   r   )r   r   xyz     \/var/www/html/venv/lib/python3.12/site-packages/sympy/physics/vector/tests/test_functions.pytest_dotr&      s    qssACC=AqssACC=AqssACC=AqssACC=AqssACC=AqssACC=AqssACC=AqssACC=AqssACC=Ar$   c                  	   t        t        j                  t        j                        t	        t
              k(  sJ t        t        j                  t        j                        t        t
               k(  sJ t        t        j                  t        j                        dk(  sJ t        t        j                  t        j                        t        t
              k(  sJ t        t        j                  t        j                        t	        t
              k(  sJ t        t        j                  t        j                        dk(  sJ t        t        j                  t        j                        dk(  sJ t        t        j                  t        j                        dk(  sJ t        t        j                  t        j                        dk(  sJ t        t        t        j                  t        j                  t        j                  z               t        d      t	        t
        t        dz  z         z  k(  sJ t        t        t        j                  t        j                  t        j                  z               t        t        t        j                  t        j                  z   t        j                              k(  sJ t        t        j                  t        j                        t	        t              k(  sJ t        t        j                  t        j                        dk(  sJ t        t        j                  t        j                        t        t              k(  sJ t        t        j                  t        j                        t        t              t        t              z  k(  sJ t        t        j                  t        j                        t	        t              k(  sJ t        t        j                  t        j                        t        t               t	        t              z  k(  sJ t        t        j                  t        j                        t	        t               t        t              z  k(  sJ t        t        j                  t        j                        t        t              k(  sJ t        t        j                  t        j                        t	        t              t	        t              z  k(  sJ y )Nr   r         )r   r   r    r   r   q1r!   r   r"   r   r   r   r   q3q2r#   r$   r%   test_dot_different_framesr-   %   s   qssACC=CG###qssACC=SWH$$$qssACC=AqssACC=CG###qssACC=CG###qssACC=AqssACC=AqssACC=AqssACC=ACQSS133Y'(DGCRT	N,BBBBCQSS133Y'(HSqssACC5H,IIIIqssACC=CG###qssACC=AqssACC=CG###qssACC=CGCGO+++qssACC=CG###qssACC=SWHSW,,,,qssACC=SWHSW,,,,qssACC=CG###qssACC=CGCGO+++r$   c                     t        t        j                  t        j                        dk(  sJ t        t        j                  t        j                        t        j                  k(  sJ t        t        j                  t        j                        t        j                   k(  sJ t        t        j                  t        j                        t        j                   k(  sJ t        t        j                  t        j                        dk(  sJ t        t        j                  t        j                        t        j                  k(  sJ t        t        j                  t        j                        t        j                  k(  sJ t        t        j                  t        j                        t        j                   k(  sJ t        t        j                  t        j                        dk(  sJ y Nr   )r   r   r    r!   r"   r#   r$   r%   
test_crossr0   >   s   acc?aacc?acc!!!acc?qssd"""acc?qssd"""acc?aacc?acc!!!acc?acc!!!acc?qssd"""acc?ar$   c                     t        t        j                  t        j                        t	        t
              t        j                  z  k(  sJ t        t        j                  t        j                        t        t
              t        j                  z  k(  sJ t        t        j                  t        j                        t	        t
               t        j                  z  t        t
              t        j                  z  z
  k(  sJ t        t        j                  t        j                        t        t
               t        j                  z  k(  sJ t        t        j                  t        j                        t	        t
              t        j                  z  k(  sJ t        t        j                  t        j                        t        t
              t        j                  z  t	        t
              t        j                  z  z
  k(  sJ t        t        j                  t        j                        t        j                  k(  sJ t        t        j                  t        j                        t        j                   k(  sJ t        t        j                  t        j                        dk(  sJ t        t        j                  t        j                        t	        t
              t        j                  z  k(  sJ t        t        j                  t        j                        t        t
              t        j                  z  k(  sJ t        t        j                  t        j                  t        j                  z         t	        t
              t        j                  z  t        t
              t        j                  z  z   k(  sJ t        t        j                  t        j                  z   t        j                        t	        t
               t        j                  z  t        t
              t        j                  z  z
  k(  sJ t        t        j                  t        j                        t	        t              t        j                  z  k(  sJ t        t        j                  t        j                        t	        t               t        j                  z  t        t              t        j                  z  z   k(  sJ t        t        j                  t        j                        t        t               t        j                  z  k(  sJ t        t        j                  t        j                        t	        t               t        j                  z  k(  sJ t        t        j                  t        j                        j                  t              j                         t	        t              t        j                  z  t        t              t        j                  z  z
  k(  sJ t        t        j                  t        j                        t        t              t        j                  z  k(  sJ y r/   )r   r   r    r   r   r*   r"   r!   r   r   r+   r   simplifyr#   r$   r%   test_cross_different_framesr3   L   sx   acc?c"gacck)))acc?c"gacck)))acc?s2whqsslSWQSS[8888acc?s2whqssl***acc?c"gacck)))acc?c"gacckCGACCK7777acc?acc!!!acc?qssd"""acc?aacc?c"gacck)))acc?c"gacck)))accACCi CGACCK#b'!##+$====qssACC SWHQSSL3r7133;$>>>>acc?c"gacck)))acc?s2whqsslSWQSS[8888acc?s2whqssl***acc?s2whqssl***acc?""1%..0CGACCK#b'!##+4MMMMacc?c"gacck)))r$   c                  	   t        d      } | j                  | j                  z   z  t        d      t	        d      z  t              k(  sJ z  t              k(  sJ z  t              k(  sJ z  t              k(  sJ z  t              k(  sJ z  t              k(  sJ z  t              k(  sJ t        t        fd       t        t        fd       t        t        fd       t        t        fd       z  t              k(  sJ z  t              k(  sJ z  t              k(  sJ t        t        fd       t        t        fd       t        t        fd	       t        t        fd
       t        t        fd       t        t        fd       t        t        fd       z  t              k(  sJ z  t              k(  sJ z  t              k(  sJ z  t              k(  sJ z  t              k(  sJ t        t        fd       t        t        fd       t        t        fd       t        t        fd       z  t              k(  sJ z  t              k(  sJ z  t              k(  sJ t        t        fd       t        t        fd       t        t        fd       t        t        fd       t        t        fd       t        t        fd       t        t        fd       t        t        fd       t        t        fd       t        t        fd       t        t        fd       t        t        fd       t        t        fd       t        t        fd       t        t        fd        t        t        fd!       z  t              k(  sJ z  t              k(  sJ z  t              k(  sJ t        t        fd"       t        t        fd#       t        t        fd$       t        t        fd%       y&)'zUTest that the output of dot, cross, outer functions match
    operator behavior.
    r   r   c                  8    t         t        j                        S Nr   r   Zerods   r%   <lambda>z%test_operator_match.<locals>.<lambda>u       c!QVVn r$   c                  8    t        t        j                         S r6   r7   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>v       c!&&!n r$   c                      t         d      S r/   r   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>w       c!Qi r$   c                      t        d       S r/   r@   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>x   rA   r$   c                  8    t         t        j                        S r6   r7   vs   r%   r;   z%test_operator_match.<locals>.<lambda>|   r<   r$   c                  8    t        t        j                         S r6   r7   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>}   r>   r$   c                      t         d      S r/   r@   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>~   rA   r$   c                      t        d       S r/   r@   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rA   r$   c                      t                S r6   r   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>       eAqk r$   c                      t               S r6   rJ   r:   zerods   r%   r;   z%test_operator_match.<locals>.<lambda>       eAuo r$   c                      t               S r6   rJ   rM   s   r%   r;   z%test_operator_match.<locals>.<lambda>       eE1o r$   c                  8    t         t        j                        S r6   r   r   r8   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>       eAqvv. r$   c                  8    t        t        j                         S r6   rS   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>       eAFFA. r$   c                      t         d      S r/   rJ   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                      t        d       S r/   rJ   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                  8    t         t        j                        S r6   rS   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rT   r$   c                  8    t        t        j                         S r6   rS   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rV   r$   c                      t         d      S r/   rJ   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                      t        d       S r/   rJ   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                      t                S r6   r   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                      t               S r6   r^   rM   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rO   r$   c                      t               S r6   r^   rM   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rQ   r$   c                      t               S r6   r^   r:   rE   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                      t               S r6   r^   rb   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                      t               S r6   r^   r:   zerovs   r%   r;   z%test_operator_match.<locals>.<lambda>   rO   r$   c                      t               S r6   r^   re   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rQ   r$   c                      t               S r6   r^   re   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rQ   r$   c                  8    t         t        j                        S r6   r   r   r8   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rT   r$   c                  8    t        t        j                         S r6   rj   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rV   r$   c                      t         d      S r/   r^   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                      t        d       S r/   r^   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                  8    t         t        j                        S r6   rj   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rT   r$   c                  8    t        t        j                         S r6   rj   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rV   r$   c                      t         d      S r/   r^   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                      t        d       S r/   r^   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   N)
r   r    r!   r   r
   r   r   	TypeErrorr   r   )r   r:   rE   rN   rf   s    @@@@r%   test_operator_matchrs   c   s    	sA	acc	A	AA1IE1IE q5C1Iu9Au%%%19E1%%%q5C1Iq5C1Iu9Au%%%19E1%%%
9,-
9,-
9'(
9'(q5C1Iu9Au%%%19E1%%%
9,-
9,-
9'(
9'( 9)*
9-.
9-.q5E!QKq5E!QKu9a'''19eQ'''19eQ'''
9./
9./
9)*
9)*q5E!QKu9a'''19eQ'''
9./
9./
9)*
9)* 9)*
9-.
9-.
9)*
9)*
9-.
9-.
9-.
9./
9./
9)*
9)*q5E!QKu9a'''19eQ'''
9./
9./
9)*
9)*r$   c                  $N   t        t        d      t              t        d      k(  sJ t        t        j                  t              t        j                  u sJ t        t
        j                  t              t        t              t        j                  z  t        t              t        j                  z  z   k(  sJ t        t
        j                  t              t        t              t        t              z  t        j                  z  t        t              t        j                  z  z   t        t              t        t              z  t        j                  z  z
  k(  sJ t        t
        j                  t              t        t               t        t              z  t        j                  z  t        t              t        j                  z  z   t        t              t        t              z  t        j                  z  z   k(  sJ t        t
        j                  t              t        t              t        j                  z  t        t              t        j                  z  z   k(  sJ t        t
        j                  t              t        t               t        j                  z  t        t              t        j                  z  z   k(  sJ t        t
        j                  t              t        j                  k(  sJ t        t
        j                  t
              t
        j                  k(  sJ t        t
        j                  t
              t
        j                  k(  sJ t        t
        j                  t
              t
        j                  k(  sJ t        t
        j                  t              t        j                  k(  sJ t        t
        j                  t              t        t              t        j                  z  t        t              t        j                  z  z
  k(  sJ t        t
        j                  t              t        t              t        j                  z  t        t              t        j                  z  z   k(  sJ t        t
        j                  t              t        t              t        j                  z  t        t              t        j                  z  z   k(  sJ t        t
        j                  t              t        t              t        t              z  t        j                  z  t        t              t        j                  z  z   t        t              t        t              z  t        j                  z  z
  k(  sJ t        t
        j                  t              t        t               t        t              z  t        j                  z  t        t              t        j                  z  z   t        t              t        t              z  t        j                  z  z   k(  sJ t        t        j                  t              t        j                  k(  sJ t        t        j                  t              t        j                  k(  sJ t        t        j                  t              t        j                  k(  sJ t        t        j                  t
              t        t              t
        j                  z  t        t              t
        j                  z  z
  k(  sJ t        t        j                  t
              t        t              t
        j                  z  t        t              t
        j                  z  z   k(  sJ t        t        j                  t
              t
        j                  k(  sJ t        t        j                  t              t        t              t        j                  z  t        t              t        t              z  t        j                  z  z
  t        t              t        t              z  t        j                  z  z   k(  sJ t        t        j                  t              t        t              t        j                  z  t        t              t        t              z  t        j                  z  z   t        t              t        t              z  t        j                  z  z
  k(  sJ t        t        j                  t              t        t              t        j                  z  t        t              t        j                  z  z   k(  sJ t        t        j                  t              t        t              t        t              z  t        t              t        t              z  t        t              z  z
  t        j                  z  t        t              t        t              z  t        j                  z  z
  t        t              t        t              z  t        t              t        t              z  t        t              z  z   t        j                  z  z   k(  sJ t        t        j                  t              t        t              t        t              z  t        t              t        t              z  t        t              z  z   t        j                  z  t        t              t        t              z  t        j                  z  z   t        t              t        t              z  t        t              t        t              z  t        t              z  z
  t        j                  z  z   k(  sJ t        t        j                  t              t        t               t        t              z  t        j                  z  t        t              t        j                  z  z   t        t              t        t              z  t        j                  z  z   k(  sJ t        t
        j                  t              t        t              t        j                  z  t        t              t        j                  z  z   k(  sJ t        t
        j                  t              t        t               t        j                  z  t        t              t        j                  z  z   k(  sJ t        t
        j                  t              t        j                  k(  sJ t        t
        j                  t
              t
        j                  k(  sJ t        t
        j                  t
              t
        j                  k(  sJ t        t
        j                  t
              t
        j                  k(  sJ t        t
        j                  t              t        j                  k(  sJ t        t
        j                  t              t        t              t        j                  z  t        t              t        j                  z  z
  k(  sJ t        t
        j                  t              t        t              t        j                  z  t        t              t        j                  z  z   k(  sJ t        t
        j                  t              t        t              t        j                  z  t        t              t        j                  z  z   k(  sJ t        t
        j                  t              t        t              t        t              z  t        j                  z  t        t              t        j                  z  z   t        t              t        t              z  t        j                  z  z
  k(  sJ t        t
        j                  t              t        t               t        t              z  t        j                  z  t        t              t        j                  z  z   t        t              t        t              z  t        j                  z  z   k(  sJ t        t        j                  t              t        t              t        j                  z  t        t              t        j                  z  z   k(  sJ t        t        j                  t              t        t               t        t              z  t        j                  z  t        t              t        t              z  t        j                  z  z   t        t              t        j                  z  z   k(  sJ t        t        j                  t              t        t              t        t              z  t        j                  z  t        t              t        t              z  t        j                  z  z
  t        t              t        j                  z  z   k(  sJ t        t        j                  t
              t
        j                  k(  sJ t        t        j                  t
              t        t              t
        j                  z  t        t              t
        j                  z  z   k(  sJ t        t        j                  t
              t        t               t
        j                  z  t        t              t
        j                  z  z   k(  sJ t        t        j                  t              t        j                  k(  sJ t        t        j                  t              t        j                  k(  sJ t        t        j                  t              t        j                  k(  sJ t        t        j                  t              t        t              t        j                  z  t        t              t        j                  z  z   k(  sJ t        t        j                  t              t        j                  k(  sJ t        t        j                  t              t        t               t        j                  z  t        t              t        j                  z  z   k(  sJ t        t        j                  t              t        t              t        t              z  t        t              t        t              z  t        t              z  z
  t        j                  z  t        t              t        t              z  t        t              t        t              z  t        t              z  z   t        j                  z  z   t        t              t        t              z  t        j                  z  z
  k(  sJ t        t        j                  t              t        t               t        t              z  t        j                  z  t        t              t        t              z  t        j                  z  z   t        t              t        j                  z  z   k(  sJ t        t        j                  t              t        t              t        t              z  t        t              t        t              z  t        t              z  z   t        j                  z  t        t              t        t              z  t        t              t        t              z  t        t              z  z
  t        j                  z  z   t        t              t        t              z  t        j                  z  z   k(  sJ t        t        j                  t
              t        t              t
        j                  z  t        t              t        t              z  t
        j                  z  z   t        t              t        t              z  t
        j                  z  z
  k(  sJ t        t        j                  t
              t        t              t
        j                  z  t        t              t
        j                  z  z   k(  sJ t        t        j                  t
              t        t              t
        j                  z  t        t              t        t              z  t
        j                  z  z
  t        t              t        t              z  t
        j                  z  z   k(  sJ t        t        j                  t              t        t              t        j                  z  t        t              t        j                  z  z
  k(  sJ t        t        j                  t              t        j                  k(  sJ t        t        j                  t              t        t              t        j                  z  t        t              t        j                  z  z   k(  sJ t        t        j                  t              t        j                  k(  sJ t        t        j                  t              t        j                  k(  sJ t        t        j                  t              t        j                  cxk(  rt        j                  k(  sJ  J t        j                  t        t        t              t
        j                  z  t        t              t
        j                  z  z
  t              j!                         k(  sJ t        j                  t        t        t              t
        j                  z  t        t              t
        j                  z  z   t              j!                         k(  sJ t        j                  t        t        t              t        j                  z  t        t              t        t              z  t        j                  z  z
  t        t              t        t              z  t        j                  z  z   t              j!                         k(  sJ t        j                  t        t        t              t        j                  z  t        t              t        t              z  t        j                  z  z   t        t              t        t              z  t        j                  z  z
  t              j!                         k(  sJ t        j                  t        t        t              t        j                  z  t        t              t        j                  z  z   t              j!                         k(  sJ 	 t
        j                  t        t        t              t        j                  z  t        t              t        j                  z  z   t
              j!                         k(  sJ t
        j                  t        t        t               t        j                  z  t        t              t        j                  z  z   t
              j!                         k(  sJ t
        j                  t        t        t              t        j                  z  t        t              t        j                  z  z
  t
              j!                         k(  sJ t
        j                  t        t        t              t        j                  z  t        t              t        j                  z  z   t
              j!                         k(  sJ t
        j                  t        t        t              t        j                  z  t        t              t        j                  z  z   t
              j!                         k(  sJ t
        j                  t        t        t              t        t              z  t        j                  z  t        t              t        j                  z  z   t        t              t        t              z  t        j                  z  z
  t
              j!                         k(  sJ t
        j                  t        t        t               t        t              z  t        j                  z  t        t              t        j                  z  z   t        t              t        t              z  t        j                  z  z   t
              j!                         k(  sJ t        j                  t        t        t              t        j                  z  t        t              t        j                  z  z   t              j!                         k(  sJ t        j                  t        t        t               t        t              z  t        j                  z  t        t              t        t              z  t        j                  z  z   t        t              t        j                  z  z   t              j!                         k(  sJ t        j                  t        t        t              t        t              z  t        j                  z  t        t              t        t              z  t        j                  z  z
  t        t              t        j                  z  z   t              j!                         k(  sJ t        j                  t        t        t              t
        j                  z  t        t              t
        j                  z  z   t              j!                         k(  sJ t        j                  t        t        t               t
        j                  z  t        t              t
        j                  z  z   t              j!                         k(  sJ t        j                  t        t        t              t        j                  z  t        t              t        j                  z  z   t              j!                         k(  sJ t        j                  t        t        t               t        j                  z  t        t              t        j                  z  z   t              j!                         k(  sJ 	 t        j                  t        t        t              t
        j                  z  t        t              t        t              z  t
        j                  z  z   t        t              t        t              z  t
        j                  z  z
  t              j!                         k(  sJ t        j                  t        t        t              t
        j                  z  t        t              t
        j                  z  z   t              j!                         k(  sJ t        j                  t        t        t              t
        j                  z  t        t              t        t              z  t
        j                  z  z
  t        t              t        t              z  t
        j                  z  z   t              j!                         k(  sJ t        j                  t        t        t              t        j                  z  t        t              t        j                  z  z
  t              j!                         k(  sJ t        j                  t        t        t              t        j                  z  t        t              t        j                  z  z   t              j!                         k(  sJ y r/   )r   r   r   r   r8   r   r    r   r   r+   r   r"   r!   r,   r*   r   r2   r#   r$   r%   test_expressru      s   6!9a F1I---1661'''133?c"gacckCGACCK7777133?c"gc"goacc1CGACCK?BB   133?s2whs2w.qss2SWQSS[@BB   133?c"gacckCGACCK7777133?s2whqsslSWQSS[8888133?acc!!!133?acc!!!133?acc!!!133?acc!!!133?acc!!!133?c"gacckCGACCK7777133?c"gacckCGACCK7777133?c"gacckCGACCK7777133?c"gc"goacc1CGACCK?BB   133?s2whs2w.qss2SWQSS[@BB    133?acc!!!133?acc!!!133?acc!!!133?s2wqss{SWQSS[8999133?s2wqss{SWQSS[8999133?acc!!!133?s2wqss{SWSW_QSS-@@GCGOACC   ! ! !133?s2wqss{SWSW_QSS-@@GCGOACC   ! ! !133?s2wqss{SWQSS[8999133?	RR3r73r7?3r72	2ACC7BB		RR3r73r7?3r72	2ACC7	89 9 9 133?	RR3r73r7?3r72	2ACC7BB		RR3r73r7?3r72	2ACC7	89 9 9 133?BxB/3c"gacckAGCGOACC   ! ! ! 133?s2wqss{SWQSS[8999133?Bx|c"gacck9:::133?acc!!!133?acc!!!133?acc!!!133?acc!!!133?acc!!!133?s2wqss{SWQSS[8999133?s2wqss{SWQSS[8999133?s2wqss{SWQSS[8999133?s2ws2wqss2SWQSS[@GCGOACC   ! ! !133?BxB/3c"gacckAGCGOACC   ! ! ! 133?s2wqss{SWQSS[8999133?BxB/3GCGOACC  "%b'!##+ . / / /133?s2ws2wqss2GCGOACC  "%b'!##+ . / / /133?acc!!!133?s2wqss{SWQSS[8999133?Bx|c"gacck9:::133?acc!!!133?acc!!!133?acc!!!133?s2wqss{SWQSS[8999133?acc!!!133?Bx|c"gacck9:::133?	RR3r73r7?3r72	2ACC7	RR3r73r7?3r72	2ACC7	8BB	   133?	RRs2ws2wqss22SWQSS[@B B B133?	RR3r73r7?3r72	2ACC7	RR3r73r7?3r72	2ACC7	8BB	   133?s2wqss{SWSW_QSS-@@GCGOACC   ! ! !133?s2wqss{SWQSS[8999133?s2wqss{SWSW_QSS-@@GCGOACC   ! ! !133?s2wqss{SWQSS[8999133?acc!!!133?s2wqss{SWQSS[8999133?acc!!!133?acc!!!133?acc*acc***** 33'3r7133;R4q9BBDDDD33'3r7133;R4q9BBDDDD33'3r7133;RR)<<GCGOACC "#%%-XZ0 0 033'3r7133;RR)<<BB!!), , ,33'3r7133;R4q9BBDDDD 33'3r7133;R4q9BBDDDD33'CG8ACC<#b'!##+5:CCEEEE33'3r7133;R4q9BBDDDD33'3r7133;R4q9BBDDDD33'3r7133;R4q9BBDDDD
 33'3r73r7?133.R<GCGOACC "#%%-XZ0 0 0 33'CG8CG+ACC/#b'!##+=GCGOACC "#%%-XZ0 0 033'3r7133;R4q9BBDDDD33'CG8CG+ACC/GCGOACC "%b'!##+.0133;8:> > > 33'3r73r7?133.GCGOACC "%b'!##+.0133;8:> > > 33'3r7133;R4q9BBDDDD33'CG8ACC<#b'!##+5:CCEEEE33'3r7133;R4q9BBDDDD33'CG8ACC<#b'!##+5:CCEEEE 33'3r7133;RR)<<GCGOACC "#%%-XZ0 0 033'3r7133;R4q9BBDDDD33'3r7133;RR)<<GCGOACC "#%%-XZ0 0 033'3r7133;R4q9BBDDDD33'3r7133;R4q9BBDDDDr$   c                  l
   t        d      } t        d      }t        dd      }| j                  dd|| j                  g      | j                  | j                  z  }t        |      | | j                  | j                  z  z  | | j                  | j                  z  z  z   k(  sJ | j                  j                  z  }t        ||       | | j                  j                  z  z  k(  sJ t        |      | | j                  j                  z  z  k(  sJ | j                  j                  z  }t        ||       || j                  j                  z  z  k(  sJ t        |      | | j                  j                  z  z  k(  sJ | j                  j                  z  }t        ||       dk(  sJ t        |      | | j                  j                  z  z  k(  sJ t        d      \  }}}	}
t        dd      \  }}}}t        dd      \  }}}}j                  d	d|
j                  g      || j                  z  }|| j                  z  |	j                  z  z   }|| j                  z  || j                  z  z   |	| j                  z  z   }t        j                        dk(  sJ t        j                        | j                  z  k(  sJ t        j                        |j                  z  k(  sJ t        |      || j                  z  k(  sJ t        |      | t        |      z  |z  | j                  z  |t        |      z  |z  | j                  z  z   || j                  z  z   k(  sJ t        ||       || j                  z  |	|z  j                  z  z
  |j                  z  z   k(  sJ t        |      || j                  z  ||z  | j                  z  z
  |t        |      z  |z  | j                  z  z   |j                  z  z   |	|z  j                  z  z
  k(  sJ t        |      ||z  |z   | j                  z  | |z  |z   | j                  z  z   || j                  z  z   k(  sJ t        |      | | j                  | j                  z  z  t        |      |z  | j                  | j                  z  z  z   || j                  | j                  z  z  z
  t        |      |z  | j                  | j                  z  z  z   k(  sJ t        t        fd
       t        t        fd       y )Nr   qr   r   r   r   zq1 q2 q3 q4r(   r   c                  4    t         j                  d      S )N      ?orderr   r    r   r   s   r%   r;   z&test_time_derivative.<locals>.<lambda>}  s    qssASA r$   c                  4    t         j                  d      S )Nrz   r|   r}   s   r%   r;   z&test_time_derivative.<locals>.<lambda>~  s    qssAR@ r$   )r   r   	orientnewr"   r    r   r!   r   r   r   
ValueError)r   rw   qdr:   d1d2d3r*   r,   r+   q4q1dq2dq3dq4dq1ddq2ddq3ddq4ddv1v2v3r   r   s                         @@r%   test_time_derivativer   V  s    	sAsA	Q	B	C!QSS*A	acc	A1a bSQSS133Y$7CACC!##I%   	
qssB2q!rT133qss7^3332q!rT133qss7^333	
qssB2q!RQSS\1112q!rT133qss7^333	
qssB2q!Q&&&2q!rT133qss7^333#M2NBB'q9Cc3+M1=D$d	C"acc+A	accB	ACC"QSS&B	ACC"QSS&2acc6	!B133"a'''133"eACCi///133"c!##g---2q!SW,,,2q!rT#a&[_QSS%8c!f9S=&"133w&' ' ' '2q!SWr"uQSSy%83qss7%BBBB2q!SWr"uQSSy%8c!f9S=&"133w&')+C&4 4 4 42q!beck133%6CFSL!##& #ACC&( ( ( (1a bD!##acc'Nq6#:qss133w% "$acc!##g,%/14QQSSW1E%F F F F
:AB
:@Ar$   c            	      ~	   t         j                  } t        d      \  }}}t        d      \  }}}t        d      \  }}}	t        d      \  }
}t        d      \  }}}t        dd      \  }}}t        dd      \  }}}|t        j                  z  |t        j
                  z  z   |t        j                  z  z   }|t        j                  z  |t        j
                  z  z   |	t        j                  z  z   }|t        j                  z  |t        j
                  z  z   |t        j                  z  z   }|t        j                  z  |t        j
                  z  z   |t        j                  z  z   }|t        j                  z  |t        j
                  z  z   |t        j                  z  z   }|t        j                  z  |t        j
                  z  z   |t        j                  z  z   }t        t              d	k(  sJ t        t        |
      dd|fk(  sJ t        t        |
      |||fk(  sJ t        t        |      d||| z  fk(  sJ t        t        |||
      d|||| |
z
  z  z   fk(  sJ t        t        |||
      }d||| z  ||
z  z
  |j                  | |
      z   f}||k(  sJ t        t        |||
      }t        || |
| f      t        j                  z  t        || |
| f      t        j
                  z  z   t        || |
| f      t        j                  z  z   }||||z   f}||k(  sJ t        t        |      ||| z  || dz  z  dz  fk(  sJ t        t        ||||
|      |||| z  z   ||z  z
  | |
z  || dz  z  dz  z   ||z  |
z  z   ||
dz  z  dz  z
  | |||z  z
  z  z   |j                  | |
      z   fk(  sJ t        t        ||||
|      |||| z  z   ||z  z
  | |
z  |z   || dz  z  dz  z   ||z  |
z  z   ||
dz  z  dz  z
  | |||z  z
  z  z   fk(  sJ t        t        |t        j                  z  |t        j                  z  |t        j                  z  |
|      }t        || || f      }|t        j                  z  ||z   t        j                  z  |t        ||z   | |
| f      z   t        j                  z  f}||k(  sJ y )Nzs1 s2 s3zS1 S2 S3zS4 S5 S6zt1 t2za b cr   r(   )framer   r   r   )positionr   )velocity)r   r   
timevalue1)acceleration)r   r   r   r   
timevalue2)
r   _tr   r   r    r!   r"   r   subsr	   )ts1s2s3S1S2S3S4S5S6t1t2abcadbdcda2db2dc2dv0v01r   r   v2dv2ddansweranswer_expectedintegral_vectori1s                                  r%   test_get_motion_methodsr     s   A$JBB$JBB$JBBWFBW%GAq!+JBB"7A.MCc	ACC"QSS&2acc6	!B
QSS&2acc6/BqssF
"C	ACC"QSS&2acc6	!B	
133133133	B
QSS&2acc6/BqssF
"Cqss7SWs133w&DQ'9444Q,Ar
:::Q,sB???Q,BQ???QbRHr2AF#$% % %q2rJF"bdRUlRWWQ^;<O_$$$q2rJFq1b!*-acc1HQB
4KACC4OOq1b!*%acc)*OB_ 45O_$$$ QR0QQT	!$% % %QR"#%"EbdRU"CFR1WQYBr)r1uHQJBBJ(GGArN   QR"&)bRIRT	BrE!CFSL2ad719$rE"H"a%xz"rBrEzN   qqur!##v#%acc6bRIF	!aQZ	 BurBwm	hrBwB
+	+QSS02O_$$$r$   c            
        	
 t        d      \  	t        dd      \  } }}}t        d      \  
t        
g	gdd      }|t        
g	gdd      k(  sJ t        
g	gd      }|dz  
z  d	z  z  z
  d		z  z  z   |z   dz  z  d	z  z  z   d		z  
z  z
  |z   dz  z  d	z  z  z
  d	z  
z  z   |z   d	z  
z  d	z  z  z   d		z  z  z   | z   gk(  sJ t        t        
fd
       t        t        	
fd       t        t        	
fd       t        t        	
fd       t        t        	
fd       t        t        
fd       t        t        	
fd       t        t        	
fd       t        t        
fd       t        g dgd      t
        j                  t
        j                  t
        j                  gk(  sJ y )Nzq0 q1 q2 q3r   zu1 u2 u3bodyi9  313
quaterniong      ry   c                  *    t        g gd      S Nr   r   q0r*   r,   u1u2u3s   r%   r;   ztest_kin_eqs.<locals>.<lambda>  s    2BB<"b"|\ r$   c                  .    t        g gdd      S )Nr   123r   r   r*   r,   r+   r   r   r   s   r%   r;   ztest_kin_eqs.<locals>.<lambda>  s$    2BB<"b"bAQS_afg r$   c                  ,    t        g gd      S )Nfoor   r   s   r%   r;   ztest_kin_eqs.<locals>.<lambda>  s!    2BB<"b"bAQSXY r$   c                  &    t         gd      S r   r   r   r*   r,   r+   r   s   r%   r;   ztest_kin_eqs.<locals>.<lambda>  s    1"r2r26FU r$   c                  (    t        g gd      S r   r   r   s   r%   r;   ztest_kin_eqs.<locals>.<lambda>  s    12$RR8H,W r$   c                  $    t        g d      S r   r   )r   r   r   r   s   r%   r;   ztest_kin_eqs.<locals>.<lambda>  s    12r2,LQ r$   c                  ,    t        g gd      S )Nr   r   r   s   r%   r;   ztest_kin_eqs.<locals>.<lambda>  s!    2BB<"b"bAQSYZ r$   c                  ,    t        g gd      S )Nspacer   r   s   r%   r;   ztest_kin_eqs.<locals>.<lambda>  s!    2BB<"b"bAQSZ[ r$   c                  ,    t        g gdd      S )Nr   222r   r   s   r%   r;   ztest_kin_eqs.<locals>.<lambda>  s     2BB<"b"vW\] r$   r   r   )r   r   r   r   rr   r   r8   )q0dr   r   r   kekdsr   r*   r,   r+   r   r   r   s         @@@@@@@r%   test_kin_eqsr     s   #M2NBB'q9Cc3
+JBB	bBZ"RVS	AB$bBZ"RVUKKKK
r2rlRR,<l
KC4"9r>C"HrM1C"HrMACG2INS2X]*S2X]:S@2INS2X]*S2X]:S@"HrMC"HrM)C"HrM9C?A A A A :\\]
:ggh
:YYZ
9UV
9WX
9QR
:ZZ[
:[[\
:]]^y2r2,@QVVQVVUVU[U[D\\\\r$   c            
         t        d      \  } }}}}t        d      \  }}t        d      }t        d      j                  dd| j                  g      }|j                  dd||j
                  g      }	|	j                  dd||	j                  g      }
|
j                  |	j
                  z  ||	j                  z  z   ||	j                  z  z          t        d	      }|j                  ||	j
                  z  ||j                  |	j
                  z  z  z          |j                  d
||	j                  z        j                  ||
       j                        |j                        |
j                        g||||gt              | |	j                  z  ||	j
                  z  d|	j
                  t        |      |	j                  z  t!        |      |	j                  z  z
  gddd|	j
                  t        |      |	j                  z  t!        |      |	j                  z  z
  g|	j
                  |	j                  |	j                  ddggk(  sJ t        d      }t        d      }||j
                  z  ||j                  z  z   }t        |f||f|      |j
                  |j                  ggk(  sJ t#        t$        fd       t#        t$        fd       y )Nzq1 q2 q3 u1 u2 u3zu4, u5rr   Yr   LRr   Dmcr   r   r   c                  <    t         j                              S r6   )r   vel)r   r   u_lists   r%   r;   z'test_partial_velocity.<locals>.<lambda>  s    .swwqz61E r$   c                      t               S r6   )r   )r   r   vel_lists   r%   r;   z'test_partial_velocity.<locals>.<lambda>  s    .xQ? r$   )r   r   r   r   r"   r    r!   set_ang_velr   set_vel	locatenewv2pt_theoryr   
ang_vel_inr   r   r   r   rr   )r*   r,   r+   r   r   u4u5r   r   r   r   r   r   r   rE   r   r   r   r   r   s                  @@@@@r%   test_partial_velocityr     sf   +,?@BBBH%FBAsA	C"acc+A	C"acc+A	C"acc+AMM!R!##XQSS(2834c
AIIaaccB!##),,-
++eQW
%COOAq!
AEE!Hall1o6H"b"b!FXvq1c!##gquac"gacckCGACCK&?@1acc3r7133;R45SS!##qssAq!## $ # 	sAsA
QSS28AQEB8Q/QSS!##J<???
9EF
9?@r$   c                     t        d      } t        dd      }t        dd      }t        dd	      \  }}t        d
d      }| j                  J |j                  sJ |j                  sJ ||z  ||z  k7  sJ |j                  sJ y )Nf1f2T)realf3)positivezf4,f5F)commutativef6)integer)r   is_realis_positive
is_integer)r   r   r   f4f5r   s         r%   test_dynamicsymbolsr     s    		B	4	(B	t	,BG7FB	d	+B:::::>>>b5BrE>>===r$   N)8sympy.core.numbersr   sympy.core.singletonr   sympy.core.symbolr   (sympy.functions.elementary.miscellaneousr   (sympy.functions.elementary.trigonometricr   r   sympy.integrals.integralsr	   sympy.physics.vectorr
   r   r   r   sympy.physics.vector.functionsr   r   r   r   r   r   r   r   r   sympy.simplifyr   sympy.testing.pytestr   r*   r,   r+   r   q5r   r   r"   r   r    r   r!   r   r&   r-   r0   r3   rs   ru   r   r   r   r   r   r#   r$   r%   <module>r     s    ! " % 9 ? . F FO O O
 $ '-. BB3KKVb!##Y'KKVb!##Y'KKVb!##Y',2 *.F+RgET(BV6%r].ABr$   