
    hR                        S SK Jr  S SKJr  S SKJr  S SKJr  S SKJ	r	J
r
  S SKJr  S SKJrJrJrJr  S SKJrJrJrJrJrJrJrJrJr  S S	KJr  S S
KJr  \" S5      u  r r!r"r#r$\" S5      r%\%RM                  SS\ \%RN                  /5      r(\(RM                  SS\!\(RR                  /5      r*\*RM                  SS\"\*RV                  /5      r,S r-S r.S r/S r0S r1S r2S r3S r4S r5S r6S r7g)    )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                  @   [        [        R                  [        R                  5      S:X  d   e[        [        R                  [        R                  5      S:X  d   e[        [        R                  [        R                  5      S:X  d   e[        [        R                  [        R                  5      S:X  d   e[        [        R                  [        R                  5      S:X  d   e[        [        R                  [        R                  5      S:X  d   e[        [        R                  [        R                  5      S:X  d   e[        [        R                  [        R                  5      S:X  d   e[        [        R                  [        R                  5      S:X  d   eg )N   r   )r   r   xyz     c/var/www/fran/franai/venv/lib/python3.13/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                   
   [        [        R                  [        R                  5      [	        [
        5      :X  d   e[        [        R                  [        R                  5      [        [
        5      * :X  d   e[        [        R                  [        R                  5      S:X  d   e[        [        R                  [        R                  5      [        [
        5      :X  d   e[        [        R                  [        R                  5      [	        [
        5      :X  d   e[        [        R                  [        R                  5      S:X  d   e[        [        R                  [        R                  5      S:X  d   e[        [        R                  [        R                  5      S:X  d   e[        [        R                  [        R                  5      S:X  d   e[        [        [        R                  [        R                  [        R                  -   5      5      [        S5      [	        [
        [        S-  -   5      -  :X  d   e[        [        [        R                  [        R                  [        R                  -   5      5      [        [        [        R                  [        R                  -   [        R                  5      5      :X  d   e[        [        R                  [        R                  5      [	        [        5      :X  d   e[        [        R                  [        R                  5      S:X  d   e[        [        R                  [        R                  5      [        [        5      :X  d   e[        [        R                  [        R                  5      [        [        5      [        [        5      -  :X  d   e[        [        R                  [        R                  5      [	        [        5      :X  d   e[        [        R                  [        R                  5      [        [        5      * [	        [        5      -  :X  d   e[        [        R                  [        R                  5      [	        [        5      * [        [        5      -  :X  d   e[        [        R                  [        R                  5      [        [        5      :X  d   e[        [        R                  [        R                  5      [	        [        5      [	        [        5      -  :X  d   eg )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                     [        [        R                  [        R                  5      S:X  d   e[        [        R                  [        R                  5      [        R                  :X  d   e[        [        R                  [        R                  5      [        R                  * :X  d   e[        [        R                  [        R                  5      [        R                  * :X  d   e[        [        R                  [        R                  5      S:X  d   e[        [        R                  [        R                  5      [        R                  :X  d   e[        [        R                  [        R                  5      [        R                  :X  d   e[        [        R                  [        R                  5      [        R                  * :X  d   e[        [        R                  [        R                  5      S:X  d   eg 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                     [        [        R                  [        R                  5      [	        [
        5      [        R                  -  :X  d   e[        [        R                  [        R                  5      [        [
        5      [        R                  -  :X  d   e[        [        R                  [        R                  5      [	        [
        5      * [        R                  -  [        [
        5      [        R                  -  -
  :X  d   e[        [        R                  [        R                  5      [        [
        5      * [        R                  -  :X  d   e[        [        R                  [        R                  5      [	        [
        5      [        R                  -  :X  d   e[        [        R                  [        R                  5      [        [
        5      [        R                  -  [	        [
        5      [        R                  -  -
  :X  d   e[        [        R                  [        R                  5      [        R                  :X  d   e[        [        R                  [        R                  5      [        R                  * :X  d   e[        [        R                  [        R                  5      S:X  d   e[        [        R                  [        R                  5      [	        [
        5      [        R                  -  :X  d   e[        [        R                  [        R                  5      [        [
        5      [        R                  -  :X  d   e[        [        R                  [        R                  [        R                  -   5      [	        [
        5      [        R                  -  [        [
        5      [        R                  -  -   :X  d   e[        [        R                  [        R                  -   [        R                  5      [	        [
        5      * [        R                  -  [        [
        5      [        R                  -  -
  :X  d   e[        [        R                  [        R                  5      [	        [        5      [        R                  -  :X  d   e[        [        R                  [        R                  5      [	        [        5      * [        R                  -  [        [        5      [        R                  -  -   :X  d   e[        [        R                  [        R                  5      [        [        5      * [        R                  -  :X  d   e[        [        R                  [        R                  5      [	        [        5      * [        R                  -  :X  d   e[        [        R                  [        R                  5      R                  [        5      R                  5       [	        [        5      [        R                  -  [        [        5      [        R                  -  -
  :X  d   e[        [        R                  [        R                  5      [        [        5      [        R                  -  :X  d   eg 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                  	  ^^^^ [        S5      n U R                  U R                  -   mTT-  m[        S5      m[	        S5      mTT-  [        TT5      :X  d   eTT-  [        TT5      :X  d   eTT-  [        TT5      :X  d   eTT-  [        TT5      :X  d   eTT-  [        TT5      :X  d   eTT-  [        TT5      :X  d   eTT-  [        TT5      :X  d   e[        [        U4S j5        [        [        U4S j5        [        [        U4S j5        [        [        U4S j5        TT-  [        TT5      :X  d   eTT-  [        TT5      :X  d   eTT-  [        TT5      :X  d   e[        [        U4S j5        [        [        U4S j5        [        [        U4S	 j5        [        [        U4S
 j5        [        [        U4S j5        [        [        UU4S j5        [        [        UU4S j5        TT-  [        TT5      :X  d   eTT-  [        TT5      :X  d   eTT-  [        TT5      :X  d   eTT-  [        TT5      :X  d   eTT-  [        TT5      :X  d   e[        [        U4S j5        [        [        U4S j5        [        [        U4S j5        [        [        U4S j5        TT-  [        TT5      :X  d   eTT-  [        TT5      :X  d   eTT-  [        TT5      :X  d   e[        [        U4S j5        [        [        U4S j5        [        [        U4S j5        [        [        U4S j5        [        [        U4S j5        [        [        UU4S j5        [        [        UU4S j5        [        [        UU4S j5        [        [        UU4S j5        [        [        UU4S j5        [        [        UU4S j5        [        [        UU4S j5        [        [        U4S j5        [        [        U4S j5        [        [        U4S  j5        [        [        U4S! j5        TT-  [        TT5      :X  d   eTT-  [        TT5      :X  d   eTT-  [        TT5      :X  d   e[        [        U4S" j5        [        [        U4S# j5        [        [        U4S$ j5        [        [        U4S% j5        g&)'zMTest that the output of dot, cross, outer functions match
operator behavior.
r   r   c                  8   > [        T [        R                  5      $ Nr   r   Zerods   r%   <lambda>%test_operator_match.<locals>.<lambda>u       c!QVVnr$   c                  8   > [        [        R                  T 5      $ r6   r7   r9   s   r%   r;   r<   v       c!&&!nr$   c                     > [        T S5      $ r/   r   r9   s   r%   r;   r<   w       c!Qir$   c                     > [        ST 5      $ r/   rA   r9   s   r%   r;   r<   x   rB   r$   c                  8   > [        T [        R                  5      $ r6   r7   vs   r%   r;   r<   |   r=   r$   c                  8   > [        [        R                  T 5      $ r6   r7   rE   s   r%   r;   r<   }   r?   r$   c                     > [        T S5      $ r/   rA   rE   s   r%   r;   r<   ~   rB   r$   c                     > [        ST 5      $ r/   rA   rE   s   r%   r;   r<      rB   r$   c                     > [        T T 5      $ r6   r   r9   s   r%   r;   r<          eAqkr$   c                     > [        T T5      $ r6   rK   r:   zerods   r%   r;   r<          eAuor$   c                     > [        TT 5      $ r6   rK   rN   s   r%   r;   r<          eE1or$   c                  8   > [        T [        R                  5      $ r6   r   r   r8   r9   s   r%   r;   r<          eAqvv.r$   c                  8   > [        [        R                  T 5      $ r6   rT   r9   s   r%   r;   r<          eAFFA.r$   c                     > [        T S5      $ r/   rK   r9   s   r%   r;   r<      rL   r$   c                     > [        ST 5      $ r/   rK   r9   s   r%   r;   r<      rL   r$   c                  8   > [        T [        R                  5      $ r6   rT   rE   s   r%   r;   r<      rU   r$   c                  8   > [        [        R                  T 5      $ r6   rT   rE   s   r%   r;   r<      rW   r$   c                     > [        T S5      $ r/   rK   rE   s   r%   r;   r<      rL   r$   c                     > [        ST 5      $ r/   rK   rE   s   r%   r;   r<      rL   r$   c                     > [        T T 5      $ r6   r   r9   s   r%   r;   r<      rL   r$   c                     > [        T T5      $ r6   r_   rN   s   r%   r;   r<      rP   r$   c                     > [        TT 5      $ r6   r_   rN   s   r%   r;   r<      rR   r$   c                     > [        T T5      $ r6   r_   r:   rF   s   r%   r;   r<      rL   r$   c                     > [        TT 5      $ r6   r_   rc   s   r%   r;   r<      rL   r$   c                     > [        T T5      $ r6   r_   r:   zerovs   r%   r;   r<      rP   r$   c                     > [        TT 5      $ r6   r_   rf   s   r%   r;   r<      rR   r$   c                     > [        TT 5      $ r6   r_   rf   s   r%   r;   r<      rR   r$   c                  8   > [        T [        R                  5      $ r6   r   r   r8   r9   s   r%   r;   r<      rU   r$   c                  8   > [        [        R                  T 5      $ r6   rk   r9   s   r%   r;   r<      rW   r$   c                     > [        T S5      $ r/   r_   r9   s   r%   r;   r<      rL   r$   c                     > [        ST 5      $ r/   r_   r9   s   r%   r;   r<      rL   r$   c                  8   > [        T [        R                  5      $ r6   rk   rE   s   r%   r;   r<      rU   r$   c                  8   > [        [        R                  T 5      $ r6   rk   rE   s   r%   r;   r<      rW   r$   c                     > [        T S5      $ r/   r_   rE   s   r%   r;   r<      rL   r$   c                     > [        ST 5      $ r/   r_   rE   s   r%   r;   r<      rL   r$   N)
r   r    r!   r   r
   r   r   	TypeErrorr   r   )r   r:   rF   rO   rg   s    @@@@r%   test_operator_matchrt   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   [        [        S5      [        5      [        S5      :X  d   e[        [        R                  [        5      [        R                  L d   e[        [
        R                  [        5      [        [        5      [        R                  -  [        [        5      [        R                  -  -   :X  d   e[        [
        R                  [        5      [        [        5      [        [        5      -  [        R                  -  [        [        5      [        R                  -  -   [        [        5      [        [        5      -  [        R                  -  -
  :X  d   e[        [
        R                  [        5      [        [        5      * [        [        5      -  [        R                  -  [        [        5      [        R                  -  -   [        [        5      [        [        5      -  [        R                  -  -   :X  d   e[        [
        R                  [        5      [        [        5      [        R                  -  [        [        5      [        R                  -  -   :X  d   e[        [
        R                  [        5      [        [        5      * [        R                  -  [        [        5      [        R                  -  -   :X  d   e[        [
        R                  [        5      [        R                  :X  d   e[        [
        R                  [
        5      [
        R                  :X  d   e[        [
        R                  [
        5      [
        R                  :X  d   e[        [
        R                  [
        5      [
        R                  :X  d   e[        [
        R                  [        5      [        R                  :X  d   e[        [
        R                  [        5      [        [        5      [        R                  -  [        [        5      [        R                  -  -
  :X  d   e[        [
        R                  [        5      [        [        5      [        R                  -  [        [        5      [        R                  -  -   :X  d   e[        [
        R                  [        5      [        [        5      [        R                  -  [        [        5      [        R                  -  -   :X  d   e[        [
        R                  [        5      [        [        5      [        [        5      -  [        R                  -  [        [        5      [        R                  -  -   [        [        5      [        [        5      -  [        R                  -  -
  :X  d   e[        [
        R                  [        5      [        [        5      * [        [        5      -  [        R                  -  [        [        5      [        R                  -  -   [        [        5      [        [        5      -  [        R                  -  -   :X  d   e[        [        R                  [        5      [        R                  :X  d   e[        [        R                  [        5      [        R                  :X  d   e[        [        R                  [        5      [        R                  :X  d   e[        [        R                  [
        5      [        [        5      [
        R                  -  [        [        5      [
        R                  -  -
  :X  d   e[        [        R                  [
        5      [        [        5      [
        R                  -  [        [        5      [
        R                  -  -   :X  d   e[        [        R                  [
        5      [
        R                  :X  d   e[        [        R                  [        5      [        [        5      [        R                  -  [        [        5      [        [        5      -  [        R                  -  -
  [        [        5      [        [        5      -  [        R                  -  -   :X  d   e[        [        R                  [        5      [        [        5      [        R                  -  [        [        5      [        [        5      -  [        R                  -  -   [        [        5      [        [        5      -  [        R                  -  -
  :X  d   e[        [        R                  [        5      [        [        5      [        R                  -  [        [        5      [        R                  -  -   :X  d   e[        [        R                  [        5      [        [        5      [        [        5      -  [        [        5      [        [        5      -  [        [        5      -  -
  [        R                  -  [        [        5      [        [        5      -  [        R                  -  -
  [        [        5      [        [        5      -  [        [        5      [        [        5      -  [        [        5      -  -   [        R                  -  -   :X  d   e[        [        R                  [        5      [        [        5      [        [        5      -  [        [        5      [        [        5      -  [        [        5      -  -   [        R                  -  [        [        5      [        [        5      -  [        R                  -  -   [        [        5      [        [        5      -  [        [        5      [        [        5      -  [        [        5      -  -
  [        R                  -  -   :X  d   e[        [        R                  [        5      [        [        5      * [        [        5      -  [        R                  -  [        [        5      [        R                  -  -   [        [        5      [        [        5      -  [        R                  -  -   :X  d   e[        [
        R                  [        5      [        [        5      [        R                  -  [        [        5      [        R                  -  -   :X  d   e[        [
        R                  [        5      [        [        5      * [        R                  -  [        [        5      [        R                  -  -   :X  d   e[        [
        R                  [        5      [        R                  :X  d   e[        [
        R                  [
        5      [
        R                  :X  d   e[        [
        R                  [
        5      [
        R                  :X  d   e[        [
        R                  [
        5      [
        R                  :X  d   e[        [
        R                  [        5      [        R                  :X  d   e[        [
        R                  [        5      [        [        5      [        R                  -  [        [        5      [        R                  -  -
  :X  d   e[        [
        R                  [        5      [        [        5      [        R                  -  [        [        5      [        R                  -  -   :X  d   e[        [
        R                  [        5      [        [        5      [        R                  -  [        [        5      [        R                  -  -   :X  d   e[        [
        R                  [        5      [        [        5      [        [        5      -  [        R                  -  [        [        5      [        R                  -  -   [        [        5      [        [        5      -  [        R                  -  -
  :X  d   e[        [
        R                  [        5      [        [        5      * [        [        5      -  [        R                  -  [        [        5      [        R                  -  -   [        [        5      [        [        5      -  [        R                  -  -   :X  d   e[        [        R                  [        5      [        [        5      [        R                  -  [        [        5      [        R                  -  -   :X  d   e[        [        R                  [        5      [        [        5      * [        [        5      -  [        R                  -  [        [        5      [        [        5      -  [        R                  -  -   [        [        5      [        R                  -  -   :X  d   e[        [        R                  [        5      [        [        5      [        [        5      -  [        R                  -  [        [        5      [        [        5      -  [        R                  -  -
  [        [        5      [        R                  -  -   :X  d   e[        [        R                  [
        5      [
        R                  :X  d   e[        [        R                  [
        5      [        [        5      [
        R                  -  [        [        5      [
        R                  -  -   :X  d   e[        [        R                  [
        5      [        [        5      * [
        R                  -  [        [        5      [
        R                  -  -   :X  d   e[        [        R                  [        5      [        R                  :X  d   e[        [        R                  [        5      [        R                  :X  d   e[        [        R                  [        5      [        R                  :X  d   e[        [        R                  [        5      [        [        5      [        R                  -  [        [        5      [        R                  -  -   :X  d   e[        [        R                  [        5      [        R                  :X  d   e[        [        R                  [        5      [        [        5      * [        R                  -  [        [        5      [        R                  -  -   :X  d   e[        [        R                  [        5      [        [        5      [        [        5      -  [        [        5      [        [        5      -  [        [        5      -  -
  [        R                  -  [        [        5      [        [        5      -  [        [        5      [        [        5      -  [        [        5      -  -   [        R                  -  -   [        [        5      [        [        5      -  [        R                  -  -
  :X  d   e[        [        R                  [        5      [        [        5      * [        [        5      -  [        R                  -  [        [        5      [        [        5      -  [        R                  -  -   [        [        5      [        R                  -  -   :X  d   e[        [        R                  [        5      [        [        5      [        [        5      -  [        [        5      [        [        5      -  [        [        5      -  -   [        R                  -  [        [        5      [        [        5      -  [        [        5      [        [        5      -  [        [        5      -  -
  [        R                  -  -   [        [        5      [        [        5      -  [        R                  -  -   :X  d   e[        [        R                  [
        5      [        [        5      [
        R                  -  [        [        5      [        [        5      -  [
        R                  -  -   [        [        5      [        [        5      -  [
        R                  -  -
  :X  d   e[        [        R                  [
        5      [        [        5      [
        R                  -  [        [        5      [
        R                  -  -   :X  d   e[        [        R                  [
        5      [        [        5      [
        R                  -  [        [        5      [        [        5      -  [
        R                  -  -
  [        [        5      [        [        5      -  [
        R                  -  -   :X  d   e[        [        R                  [        5      [        [        5      [        R                  -  [        [        5      [        R                  -  -
  :X  d   e[        [        R                  [        5      [        R                  :X  d   e[        [        R                  [        5      [        [        5      [        R                  -  [        [        5      [        R                  -  -   :X  d   e[        [        R                  [        5      [        R                  :X  d   e[        [        R                  [        5      [        R                  :X  d   e[        [        R                  [        5      [        R                  s=:X  a  [        R                  :X  d   e   e[        R                  [        [        [        5      [
        R                  -  [        [        5      [
        R                  -  -
  [        5      R!                  5       :X  d   e[        R                  [        [        [        5      [
        R                  -  [        [        5      [
        R                  -  -   [        5      R!                  5       :X  d   e[        R                  [        [        [        5      [        R                  -  [        [        5      [        [        5      -  [        R                  -  -
  [        [        5      [        [        5      -  [        R                  -  -   [        5      R!                  5       :X  d   e[        R                  [        [        [        5      [        R                  -  [        [        5      [        [        5      -  [        R                  -  -   [        [        5      [        [        5      -  [        R                  -  -
  [        5      R!                  5       :X  d   e[        R                  [        [        [        5      [        R                  -  [        [        5      [        R                  -  -   [        5      R!                  5       :X  d   e [
        R                  [        [        [        5      [        R                  -  [        [        5      [        R                  -  -   [
        5      R!                  5       :X  d   e[
        R                  [        [        [        5      * [        R                  -  [        [        5      [        R                  -  -   [
        5      R!                  5       :X  d   e[
        R                  [        [        [        5      [        R                  -  [        [        5      [        R                  -  -
  [
        5      R!                  5       :X  d   e[
        R                  [        [        [        5      [        R                  -  [        [        5      [        R                  -  -   [
        5      R!                  5       :X  d   e[
        R                  [        [        [        5      [        R                  -  [        [        5      [        R                  -  -   [
        5      R!                  5       :X  d   e[
        R                  [        [        [        5      [        [        5      -  [        R                  -  [        [        5      [        R                  -  -   [        [        5      [        [        5      -  [        R                  -  -
  [
        5      R!                  5       :X  d   e[
        R                  [        [        [        5      * [        [        5      -  [        R                  -  [        [        5      [        R                  -  -   [        [        5      [        [        5      -  [        R                  -  -   [
        5      R!                  5       :X  d   e[        R                  [        [        [        5      [        R                  -  [        [        5      [        R                  -  -   [        5      R!                  5       :X  d   e[        R                  [        [        [        5      * [        [        5      -  [        R                  -  [        [        5      [        [        5      -  [        R                  -  -   [        [        5      [        R                  -  -   [        5      R!                  5       :X  d   e[        R                  [        [        [        5      [        [        5      -  [        R                  -  [        [        5      [        [        5      -  [        R                  -  -
  [        [        5      [        R                  -  -   [        5      R!                  5       :X  d   e[        R                  [        [        [        5      [
        R                  -  [        [        5      [
        R                  -  -   [        5      R!                  5       :X  d   e[        R                  [        [        [        5      * [
        R                  -  [        [        5      [
        R                  -  -   [        5      R!                  5       :X  d   e[        R                  [        [        [        5      [        R                  -  [        [        5      [        R                  -  -   [        5      R!                  5       :X  d   e[        R                  [        [        [        5      * [        R                  -  [        [        5      [        R                  -  -   [        5      R!                  5       :X  d   e [        R                  [        [        [        5      [
        R                  -  [        [        5      [        [        5      -  [
        R                  -  -   [        [        5      [        [        5      -  [
        R                  -  -
  [        5      R!                  5       :X  d   e[        R                  [        [        [        5      [
        R                  -  [        [        5      [
        R                  -  -   [        5      R!                  5       :X  d   e[        R                  [        [        [        5      [
        R                  -  [        [        5      [        [        5      -  [
        R                  -  -
  [        [        5      [        [        5      -  [
        R                  -  -   [        5      R!                  5       :X  d   e[        R                  [        [        [        5      [        R                  -  [        [        5      [        R                  -  -
  [        5      R!                  5       :X  d   e[        R                  [        [        [        5      [        R                  -  [        [        5      [        R                  -  -   [        5      R!                  5       :X  d   eg r/   )r   r   r   r   r8   r   r    r   r   r+   r   r"   r!   r,   r*   r   r2   r#   r$   r%   test_expressrv      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                  `
  ^^ [        S5      n [        S5      n[        SS5      nU R                  SSXR                  /5      mU R                  U R                  -  n[        UT5      U* U R                  U R                  -  -  U* U R                  U R                  -  -  -   :X  d   eU R                  TR                  -  n[        X@5      U* U R                  TR                  -  -  :X  d   e[        UT5      U* U R                  TR                  -  -  :X  d   eU R                  TR                  -  n[        XP5      X R                  TR                  -  -  :X  d   e[        UT5      U* U R                  TR                  -  -  :X  d   eU R                  TR                  -  n[        X`5      S:X  d   e[        UT5      U* U R                  TR                  -  -  :X  d   e[        S5      u  pxp[        SS5      u  pp[        SS5      u  nnnnTR                  S	SU
TR                  /5      mXpR                  -  nXR                  -  U	TR                  -  -   nXpR                  -  XR                  -  -   XR                  -  -   n[        TR                  T5      S:X  d   e[        TR                  T5      U* TR                  -  :X  d   e[        TR                  T5      UTR                  -  :X  d   e[        UT5      XR                  -  :X  d   e[        UT5      U* [        U5      -  U-  U R                  -  U[        U5      -  U-  U R                  -  -   XR                  -  -   :X  d   e[        UU 5      XR                  -  X-  TR                  -  -
  UTR                  -  -   :X  d   e[        UT5      XR                  -  X-  U R                  -  -
  U[        U5      -  U-  U R                  -  -   UTR                  -  -   X-  TR                  -  -
  :X  d   e[        UT5      X-  U-   U R                  -  U* U-  U-   U R                  -  -   XR                  -  -   :X  d   e[        UT5      U* U R                  U R                  -  -  [        U5      U-  U R                  U R                  -  -  -   X R                  U R                  -  -  -
  [        U5      U-  U R                  U R                  -  -  -   :X  d   e[        [        UU4S
 j5        [        [        UU4S j5        g )Nr   qr   r   r   r   zq1 q2 q3 q4r(   r   c                  .   > [        T R                  TSS9$ )N      ?orderr   r    r   r   s   r%   r;   &test_time_derivative.<locals>.<lambda>}  s    qssASAr$   c                  .   > [        T R                  TSS9$ )Nr{   r}   r~   s   r%   r;   r   ~  s    qssAR@r$   )r   r   	orientnewr"   r    r   r!   r   r   r   
ValueError)r   rx   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!SS*A	acc	A1a bSQSS133Y$7CACC!##I%   	
qssB2!rT133qss7^3332q!rT133qss7^333	
qssB2!RQSS\1112q!rT133qss7^333	
qssB2!Q&&&2q!rT133qss7^333#M2NBB'q9Cc+M1=D$d	C"acc+A	ccB	CC"QSS&B	CC"SS&2cc6	!B133"a'''133"eACCi///133"c!##g---2q!SW,,,2q!rT#a&[_QSS%8c!f9S=&"33w&' ' ' '2q!SWruQSSy%83qss7%BBBB2q!SWruQSSy%8c!f9S=&"133w&')+&4 4 4 42q!beck133%6CFSL!##& #CC&( ( ( (1a bD!##acc'Nq6#:qss133w% "$cc!##g,%/14QQSSW1E%F F F F
:AB
:@Ar$   c            	      *	   [         R                  n [        S5      u  pn[        S5      u  pEn[        S5      u  pxn	[        S5      u  p[        S5      u  pn[        SS5      u  nnn[        SS5      u  nnnU[        R                  -  U[        R
                  -  -   U[        R                  -  -   nU[        R                  -  U[        R
                  -  -   U	[        R                  -  -   nU[        R                  -  U[        R
                  -  -   U[        R                  -  -   nU[        R                  -  U[        R
                  -  -   U[        R                  -  -   nU[        R                  -  U[        R
                  -  -   U[        R                  -  -   nU[        R                  -  U[        R
                  -  -   U[        R                  -  -   n[        [        S9S	:X  d   e[        [        US
9SSU4:X  d   e[        [        US
9UUU4:X  d   e[        [        US9SUUU -  4:X  d   e[        [        UUU
S9SUUUX
-
  -  -   4:X  d   e[        [        UUU
S9nSUUU -  UU
-  -
  UR                  X
5      -   4nUU:X  d   e[        [        UUU
S9n[        XX45      [        R                  -  [        XX45      [        R
                  -  -   [        XX45      [        R                  -  -   nUUUU-   4nUU:X  d   e[        [        US9UUU -  UU S-  -  S-  4:X  d   e[        [        UUUXS9UUUU -  -   UU-  -
  U* U
-  UU S-  -  S-  -   UU-  U
-  -   UU
S-  -  S-  -
  U UUU-  -
  -  -   UR                  X
5      -   4:X  d   e[        [        UUUXS9UUUU -  -   UU-  -
  U* U
-  U-   UU S-  -  S-  -   UU-  U
-  -   UU
S-  -  S-  -
  U UUU-  -
  -  -   4:X  d   e[        [        U[        R                  -  U[        R                  -  U[        R                  -  XS9n[        XX45      nU[        R                  -  UU-   [        R                  -  U[        UU-   X
U 45      -   [        R                  -  4nUU:X  d   eg )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$JBB$JBB$JBBWFBW%GA!+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AF#$% % %q2rJF"bdRUlRWWQ^;<O_$$$q2rJFqb*-acc1HQB
4KACC4OOqb*%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(GGAN   QR"&)bIRT	BrE!CFSL2ad719$rE"H"a%xz"rBrEzN   qqur!##v#%acc6bIF	!Z	 BurBwm	hrBw
+	+QSS02O_$$$r$   c            
      .  ^^^^	^
^^ [        S5      u  mmmm	[        SS5      u  pp#[        S5      u  m
mm[        T
TT/TTT	/SS5      nU[        T
TT/TTT	/SS5      :X  d   e[        T
TT/TTTT	/S5      nUST-  T
-  S	T-  T-  -
  S	T	-  T-  -   U-   ST-  T-  S	T-  T-  -   S	T	-  T
-  -
  U-   ST-  T-  S	T-  T-  -
  S	T-  T
-  -   U-   S	T-  T
-  S	T-  T-  -   S	T	-  T-  -   U -   /:X  d   e[        [        UUUU
UU4S
 j5        [        [        UUUU	U
UU4S j5        [        [        UUUU	U
UU4S j5        [        [        UUUU	U
4S j5        [        [        UUUU	U
4S j5        [        [        UU
UU4S j5        [        [        UUUU	U
UU4S j5        [        [        UUUU	U
UU4S j5        [        [        UUUU
UU4S j5        [        / SQTTT/S5      [
        R                  [
        R                  [
        R                  /:X  d   eg )Nzq0 q1 q2 q3r   zu1 u2 u3bodyi9  313
quaterniong      rz   c                  *   > [        TTT/T TT/S5      $ Nr   r   q0r*   r,   u1u2u3s   r%   r;   test_kin_eqs.<locals>.<lambda>  s    2BB<"b"|\r$   c                  .   > [        TTT/T TTT/SS5      $ )Nr   123r   r   r*   r,   r+   r   r   r   s   r%   r;   r     s"    2BB<"b"bAQS_afgr$   c                  ,   > [        TTT/T TTT/S5      $ )Nfoor   r   s   r%   r;   r     s    2BB<"b"bAQSXYr$   c                  &   > [        TT TTT/S5      $ r   r   r   r*   r,   r+   r   s   r%   r;   r     s    1"r2r26FUr$   c                  (   > [        T/T TTT/S5      $ r   r   r   s   r%   r;   r     s    12$RR8H,Wr$   c                  $   > [        TTT/T S5      $ r   r   )r   r   r   r   s   r%   r;   r     s    12r2,LQr$   c                  ,   > [        TTT/T TTT/S5      $ )Nr   r   r   s   r%   r;   r     s    2BB<"b"bAQSYZr$   c                  ,   > [        TTT/T TTT/S5      $ )Nspacer   r   s   r%   r;   r     s    2BB<"b"bAQSZ[r$   c                  ,   > [        TTT/T TT/SS5      $ )Nr   222r   r   s   r%   r;   r     s    2BB<"b"vW\]r$   r   r   )r   r   r   r   rs   r   r8   )q0dr   r   r   kekdsr   r*   r,   r+   r   r   r   s         @@@@@@@r%   test_kin_eqsr     s   #M2NBB'q9Cc
+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UV
9WWX
9QR
:ZZ[
:[[\
:]]^y2r2,@QVVQVVUVU[U[D\\\\r$   c            
        ^^^^^ [        S5      u  pnmp4[        S5      u  pV[        S5      n[        S5      mTR                  SSU TR                  /5      nUR                  SSXR
                  /5      n	U	R                  SSX)R                  /5      n
U
R                  TTU	R
                  -  X9R                  -  -   XIR                  -  -   5        [        S	5      nUR                  TXYR
                  -  XhR                  U	R
                  -  -  -   5        UR                  S
XyR                  -  5      mTR                  UTU
5        TR                  T5      UR                  T5      U
R                  T5      /mTX4XV/m[        TTT5      U* U	R                  -  XyR
                  -  SU	R
                  [        U5      U	R                  -  [!        U5      U	R                  -  -
  /SSSU	R
                  [        U5      U	R                  -  [!        U5      U	R                  -  -
  /U	R
                  U	R                  U	R                  SS//:X  d   e[        S5      n[        S5      nX\R
                  -  XmR                  -  -   n[        U4XV4U5      UR
                  UR                  //:X  d   e[#        [$        UUU4S j5        [#        [$        UUU4S j5        g )Nzq1 q2 q3 u1 u2 u3zu4, u5rr   Yr   LRr   Dmcr   r   r   c                  <   > [        T R                  T5      TT5      $ r6   )r   vel)r   r   u_lists   r%   r;   'test_partial_velocity.<locals>.<lambda>  s    .swwqz61Er$   c                     > [        TTT 5      $ r6   )r   )r   r   vel_lists   r%   r;   r     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   rs   )r*   r,   r+   r   r   u4u5r   r   r   r   r   r   r   rF   r   r   r   r   r   s                  @@@@@r%   test_partial_velocityr     sF   +,?@BBBH%FBAsA	C"acc+A	C"cc+A	C"cc+AMM!R!##XSS(2834c
AIIaccB##),,-
++eQW
%COOAq!
AEE!Hall1o6H""!FXvq1c!##gquac"gacckCGACCK&?@1acc3r7133;R45SS!##qssAq!## $ # 	sAsA
SS28AQEB8Q/QSS!##J<???
9EF
9?@r$   c                     [        S5      n [        SSS9n[        SSS9n[        SSS	9u  p4[        S
SS9nU R                  b   eUR                  (       d   eUR                  (       d   eX4-  XC-  :w  d   eUR                  (       d   eg )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:::::>>>5BE>>===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   rt   rv   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$   