+
    hR                        ^ RI Ht ^ RIHt ^ RIHt ^ RIHt ^ RIH	t	H
t
 ^ RIHt ^ RIHtHtHtHt ^ RIHtHtHtHtHtHtHtHtHt ^ R	IHt ^ R
IHt ]! R4      w  t t!t"t#t$]! R4      t%]%PM                  RR] ]%PN                  .4      t(](PM                  RR]!](PR                  .4      t*]*PM                  RR]"]*PV                  .4      t,R t-R t.R t/R t0R t1R t2R t3R t4R t5R t6R t7R# )    )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   \        \        P                  \        P                  4      ^8X  g   Q h\        \        P                  \        P                  4      ^ 8X  g   Q h\        \        P                  \        P                  4      ^ 8X  g   Q h\        \        P                  \        P                  4      ^ 8X  g   Q h\        \        P                  \        P                  4      ^8X  g   Q h\        \        P                  \        P                  4      ^ 8X  g   Q h\        \        P                  \        P                  4      ^ 8X  g   Q h\        \        P                  \        P                  4      ^ 8X  g   Q h\        \        P                  \        P                  4      ^8X  g   Q hR# )   N)r   r   xyz     K/usr/lib/python3/dist-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                  
   \        \        P                  \        P                  4      \	        \
        4      8X  g   Q h\        \        P                  \        P                  4      \        \
        4      ) 8X  g   Q h\        \        P                  \        P                  4      ^ 8X  g   Q h\        \        P                  \        P                  4      \        \
        4      8X  g   Q h\        \        P                  \        P                  4      \	        \
        4      8X  g   Q h\        \        P                  \        P                  4      ^ 8X  g   Q h\        \        P                  \        P                  4      ^ 8X  g   Q h\        \        P                  \        P                  4      ^ 8X  g   Q h\        \        P                  \        P                  4      ^8X  g   Q h\        \        \        P                  \        P                  \        P                  ,           4      4      \        ^4      \	        \
        \        ^,          ,           4      ,          8X  g   Q h\        \        \        P                  \        P                  \        P                  ,           4      4      \        \        \        P                  \        P                  ,           \        P                  4      4      8X  g   Q h\        \        P                  \        P                  4      \	        \        4      8X  g   Q h\        \        P                  \        P                  4      ^ 8X  g   Q h\        \        P                  \        P                  4      \        \        4      8X  g   Q h\        \        P                  \        P                  4      \        \        4      \        \        4      ,          8X  g   Q h\        \        P                  \        P                  4      \	        \        4      8X  g   Q h\        \        P                  \        P                  4      \        \        4      ) \	        \        4      ,          8X  g   Q h\        \        P                  \        P                  4      \	        \        4      ) \        \        4      ,          8X  g   Q h\        \        P                  \        P                  4      \        \        4      8X  g   Q h\        \        P                  \        P                  4      \	        \        4      \	        \        4      ,          8X  g   Q hR# r   N)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                     \        \        P                  \        P                  4      ^ 8X  g   Q h\        \        P                  \        P                  4      \        P                  8X  g   Q h\        \        P                  \        P                  4      \        P                  ) 8X  g   Q h\        \        P                  \        P                  4      \        P                  ) 8X  g   Q h\        \        P                  \        P                  4      ^ 8X  g   Q h\        \        P                  \        P                  4      \        P                  8X  g   Q h\        \        P                  \        P                  4      \        P                  8X  g   Q h\        \        P                  \        P                  4      \        P                  ) 8X  g   Q h\        \        P                  \        P                  4      ^ 8X  g   Q hR# r(   )r   r   r    r!   r"   r#   r$   r%   
test_crossr.   >   s   acc?aacc?acc!!!acc?qssd"""acc?qssd"""acc?aacc?acc!!!acc?acc!!!acc?qssd"""acc?ar$   c                     \        \        P                  \        P                  4      \	        \
        4      \        P                  ,          8X  g   Q h\        \        P                  \        P                  4      \        \
        4      \        P                  ,          8X  g   Q h\        \        P                  \        P                  4      \	        \
        4      ) \        P                  ,          \        \
        4      \        P                  ,          ,
          8X  g   Q h\        \        P                  \        P                  4      \        \
        4      ) \        P                  ,          8X  g   Q h\        \        P                  \        P                  4      \	        \
        4      \        P                  ,          8X  g   Q h\        \        P                  \        P                  4      \        \
        4      \        P                  ,          \	        \
        4      \        P                  ,          ,
          8X  g   Q h\        \        P                  \        P                  4      \        P                  8X  g   Q h\        \        P                  \        P                  4      \        P                  ) 8X  g   Q h\        \        P                  \        P                  4      ^ 8X  g   Q h\        \        P                  \        P                  4      \	        \
        4      \        P                  ,          8X  g   Q h\        \        P                  \        P                  4      \        \
        4      \        P                  ,          8X  g   Q h\        \        P                  \        P                  \        P                  ,           4      \	        \
        4      \        P                  ,          \        \
        4      \        P                  ,          ,           8X  g   Q h\        \        P                  \        P                  ,           \        P                  4      \	        \
        4      ) \        P                  ,          \        \
        4      \        P                  ,          ,
          8X  g   Q h\        \        P                  \        P                  4      \	        \        4      \        P                  ,          8X  g   Q h\        \        P                  \        P                  4      \	        \        4      ) \        P                  ,          \        \        4      \        P                  ,          ,           8X  g   Q h\        \        P                  \        P                  4      \        \        4      ) \        P                  ,          8X  g   Q h\        \        P                  \        P                  4      \	        \        4      ) \        P                  ,          8X  g   Q h\        \        P                  \        P                  4      P                  \        4      P                  4       \	        \        4      \        P                  ,          \        \        4      \        P                  ,          ,
          8X  g   Q h\        \        P                  \        P                  4      \        \        4      \        P                  ,          8X  g   Q hR# r(   )r   r   r    r   r   r)   r"   r!   r   r   r*   r   simplifyr#   r$   r%   test_cross_different_framesr1   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                 j
  aaaa \        R4      p V P                  V P                  ,           oSS,          o\        ^ 4      o\	        ^ 4      oSS,          \        SS4      8X  g   Q hSS,          \        SS4      8X  g   Q hSS,          \        SS4      8X  g   Q hSS,          \        SS4      8X  g   Q hSS,          \        SS4      8X  g   Q hSS,          \        SS4      8X  g   Q hSS,          \        SS4      8X  g   Q h\        \        V3R l4       \        \        V3R l4       \        \        V3R l4       \        \        V3R l4       SS,          \        SS4      8X  g   Q hSS,          \        SS4      8X  g   Q hSS,          \        SS4      8X  g   Q h\        \        V3R l4       \        \        V3R l4       \        \        V3R l4       \        \        V3R	 l4       \        \        V3R
 l4       \        \        VV3R l4       \        \        VV3R l4       SS,          \        SS4      8X  g   Q hSS,          \        SS4      8X  g   Q hSS,          \        SS4      8X  g   Q hSS,          \        SS4      8X  g   Q hSS,          \        SS4      8X  g   Q h\        \        V3R l4       \        \        V3R l4       \        \        V3R l4       \        \        V3R l4       SS,          \        SS4      8X  g   Q hSS,          \        SS4      8X  g   Q hSS,          \        SS4      8X  g   Q h\        \        V3R l4       \        \        V3R l4       \        \        V3R l4       \        \        V3R l4       \        \        V3R l4       \        \        VV3R l4       \        \        VV3R l4       \        \        VV3R l4       \        \        VV3R l4       \        \        VV3R l4       \        \        VV3R l4       \        \        VV3R l4       \        \        V3R l4       \        \        V3R l4       \        \        V3R l4       \        \        V3R  l4       SS,          \        SS4      8X  g   Q hSS,          \        SS4      8X  g   Q hSS,          \        SS4      8X  g   Q h\        \        V3R! l4       \        \        V3R" l4       \        \        V3R# l4       \        \        V3R$ l4       R%# )&zMTest that the output of dot, cross, outer functions match
operator behavior.
r   c                  8   < \        S \        P                  4      # Nr   r   Zerods   r%   <lambda>%test_operator_match.<locals>.<lambda>u       c!QVVnr$   c                  8   < \        \        P                  S 4      # r4   r5   r7   s   r%   r9   r:   v       c!&&!nr$   c                     < \        S ^ 4      # r   r   r7   s   r%   r9   r:   w       c!Qir$   c                     < \        ^ S 4      # r?   r@   r7   s   r%   r9   r:   x   rA   r$   c                  8   < \        S \        P                  4      # r4   r5   vs   r%   r9   r:   |   r;   r$   c                  8   < \        \        P                  S 4      # r4   r5   rD   s   r%   r9   r:   }   r=   r$   c                     < \        S ^ 4      # r?   r@   rD   s   r%   r9   r:   ~   rA   r$   c                     < \        ^ S 4      # r?   r@   rD   s   r%   r9   r:      rA   r$   c                     < \        S S 4      # r4   r   r7   s   r%   r9   r:          eAqkr$   c                     < \        S S4      # r4   rJ   r8   zerods   r%   r9   r:          eAuor$   c                     < \        SS 4      # r4   rJ   rM   s   r%   r9   r:          eE1or$   c                  8   < \        S \        P                  4      # r4   r   r   r6   r7   s   r%   r9   r:          eAqvv.r$   c                  8   < \        \        P                  S 4      # r4   rS   r7   s   r%   r9   r:          eAFFA.r$   c                     < \        S ^ 4      # r?   rJ   r7   s   r%   r9   r:      rK   r$   c                     < \        ^ S 4      # r?   rJ   r7   s   r%   r9   r:      rK   r$   c                  8   < \        S \        P                  4      # r4   rS   rD   s   r%   r9   r:      rT   r$   c                  8   < \        \        P                  S 4      # r4   rS   rD   s   r%   r9   r:      rV   r$   c                     < \        S ^ 4      # r?   rJ   rD   s   r%   r9   r:      rK   r$   c                     < \        ^ S 4      # r?   rJ   rD   s   r%   r9   r:      rK   r$   c                     < \        S S 4      # r4   r   r7   s   r%   r9   r:      rK   r$   c                     < \        S S4      # r4   r^   rM   s   r%   r9   r:      rO   r$   c                     < \        SS 4      # r4   r^   rM   s   r%   r9   r:      rQ   r$   c                     < \        S S4      # r4   r^   r8   rE   s   r%   r9   r:      rK   r$   c                     < \        SS 4      # r4   r^   rb   s   r%   r9   r:      rK   r$   c                     < \        S S4      # r4   r^   r8   zerovs   r%   r9   r:      rO   r$   c                     < \        SS 4      # r4   r^   re   s   r%   r9   r:      rQ   r$   c                     < \        SS 4      # r4   r^   re   s   r%   r9   r:      rQ   r$   c                  8   < \        S \        P                  4      # r4   r   r   r6   r7   s   r%   r9   r:      rT   r$   c                  8   < \        \        P                  S 4      # r4   rj   r7   s   r%   r9   r:      rV   r$   c                     < \        S ^ 4      # r?   r^   r7   s   r%   r9   r:      rK   r$   c                     < \        ^ S 4      # r?   r^   r7   s   r%   r9   r:      rK   r$   c                  8   < \        S \        P                  4      # r4   rj   rD   s   r%   r9   r:      rT   r$   c                  8   < \        \        P                  S 4      # r4   rj   rD   s   r%   r9   r:      rV   r$   c                     < \        S ^ 4      # r?   r^   rD   s   r%   r9   r:      rK   r$   c                     < \        ^ S 4      # r?   r^   rD   s   r%   r9   r:      rK   r$   N)
r   r    r!   r   r
   r   r   	TypeErrorr   r   )r   r8   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                  Y   \        \        ^ 4      \        4      \        ^ 4      8X  g   Q h\        \        P                  \        4      \        P                  J g   Q h\        \
        P                  \        4      \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,           8X  g   Q h\        \
        P                  \        4      \        \        4      \        \        4      ,          \        P                  ,          \        \        4      \        P                  ,          ,           \        \        4      \        \        4      ,          \        P                  ,          ,
          8X  g   Q h\        \
        P                  \        4      \        \        4      ) \        \        4      ,          \        P                  ,          \        \        4      \        P                  ,          ,           \        \        4      \        \        4      ,          \        P                  ,          ,           8X  g   Q h\        \
        P                  \        4      \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,           8X  g   Q h\        \
        P                  \        4      \        \        4      ) \        P                  ,          \        \        4      \        P                  ,          ,           8X  g   Q h\        \
        P                  \        4      \        P                  8X  g   Q h\        \
        P                  \
        4      \
        P                  8X  g   Q h\        \
        P                  \
        4      \
        P                  8X  g   Q h\        \
        P                  \
        4      \
        P                  8X  g   Q h\        \
        P                  \        4      \        P                  8X  g   Q h\        \
        P                  \        4      \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,
          8X  g   Q h\        \
        P                  \        4      \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,           8X  g   Q h\        \
        P                  \        4      \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,           8X  g   Q h\        \
        P                  \        4      \        \        4      \        \        4      ,          \        P                  ,          \        \        4      \        P                  ,          ,           \        \        4      \        \        4      ,          \        P                  ,          ,
          8X  g   Q h\        \
        P                  \        4      \        \        4      ) \        \        4      ,          \        P                  ,          \        \        4      \        P                  ,          ,           \        \        4      \        \        4      ,          \        P                  ,          ,           8X  g   Q h\        \        P                  \        4      \        P                  8X  g   Q h\        \        P                  \        4      \        P                  8X  g   Q h\        \        P                  \        4      \        P                  8X  g   Q h\        \        P                  \
        4      \        \        4      \
        P                  ,          \        \        4      \
        P                  ,          ,
          8X  g   Q h\        \        P                  \
        4      \        \        4      \
        P                  ,          \        \        4      \
        P                  ,          ,           8X  g   Q h\        \        P                  \
        4      \
        P                  8X  g   Q h\        \        P                  \        4      \        \        4      \        P                  ,          \        \        4      \        \        4      ,          \        P                  ,          ,
          \        \        4      \        \        4      ,          \        P                  ,          ,           8X  g   Q h\        \        P                  \        4      \        \        4      \        P                  ,          \        \        4      \        \        4      ,          \        P                  ,          ,           \        \        4      \        \        4      ,          \        P                  ,          ,
          8X  g   Q h\        \        P                  \        4      \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,           8X  g   Q h\        \        P                  \        4      \        \        4      \        \        4      ,          \        \        4      \        \        4      ,          \        \        4      ,          ,
          \        P                  ,          \        \        4      \        \        4      ,          \        P                  ,          ,
          \        \        4      \        \        4      ,          \        \        4      \        \        4      ,          \        \        4      ,          ,           \        P                  ,          ,           8X  g   Q h\        \        P                  \        4      \        \        4      \        \        4      ,          \        \        4      \        \        4      ,          \        \        4      ,          ,           \        P                  ,          \        \        4      \        \        4      ,          \        P                  ,          ,           \        \        4      \        \        4      ,          \        \        4      \        \        4      ,          \        \        4      ,          ,
          \        P                  ,          ,           8X  g   Q h\        \        P                  \        4      \        \        4      ) \        \        4      ,          \        P                  ,          \        \        4      \        P                  ,          ,           \        \        4      \        \        4      ,          \        P                  ,          ,           8X  g   Q h\        \
        P                  \        4      \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,           8X  g   Q h\        \
        P                  \        4      \        \        4      ) \        P                  ,          \        \        4      \        P                  ,          ,           8X  g   Q h\        \
        P                  \        4      \        P                  8X  g   Q h\        \
        P                  \
        4      \
        P                  8X  g   Q h\        \
        P                  \
        4      \
        P                  8X  g   Q h\        \
        P                  \
        4      \
        P                  8X  g   Q h\        \
        P                  \        4      \        P                  8X  g   Q h\        \
        P                  \        4      \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,
          8X  g   Q h\        \
        P                  \        4      \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,           8X  g   Q h\        \
        P                  \        4      \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,           8X  g   Q h\        \
        P                  \        4      \        \        4      \        \        4      ,          \        P                  ,          \        \        4      \        P                  ,          ,           \        \        4      \        \        4      ,          \        P                  ,          ,
          8X  g   Q h\        \
        P                  \        4      \        \        4      ) \        \        4      ,          \        P                  ,          \        \        4      \        P                  ,          ,           \        \        4      \        \        4      ,          \        P                  ,          ,           8X  g   Q h\        \        P                  \        4      \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,           8X  g   Q h\        \        P                  \        4      \        \        4      ) \        \        4      ,          \        P                  ,          \        \        4      \        \        4      ,          \        P                  ,          ,           \        \        4      \        P                  ,          ,           8X  g   Q h\        \        P                  \        4      \        \        4      \        \        4      ,          \        P                  ,          \        \        4      \        \        4      ,          \        P                  ,          ,
          \        \        4      \        P                  ,          ,           8X  g   Q h\        \        P                  \
        4      \
        P                  8X  g   Q h\        \        P                  \
        4      \        \        4      \
        P                  ,          \        \        4      \
        P                  ,          ,           8X  g   Q h\        \        P                  \
        4      \        \        4      ) \
        P                  ,          \        \        4      \
        P                  ,          ,           8X  g   Q h\        \        P                  \        4      \        P                  8X  g   Q h\        \        P                  \        4      \        P                  8X  g   Q h\        \        P                  \        4      \        P                  8X  g   Q h\        \        P                  \        4      \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,           8X  g   Q h\        \        P                  \        4      \        P                  8X  g   Q h\        \        P                  \        4      \        \        4      ) \        P                  ,          \        \        4      \        P                  ,          ,           8X  g   Q h\        \        P                  \        4      \        \        4      \        \        4      ,          \        \        4      \        \        4      ,          \        \        4      ,          ,
          \        P                  ,          \        \        4      \        \        4      ,          \        \        4      \        \        4      ,          \        \        4      ,          ,           \        P                  ,          ,           \        \        4      \        \        4      ,          \        P                  ,          ,
          8X  g   Q h\        \        P                  \        4      \        \        4      ) \        \        4      ,          \        P                  ,          \        \        4      \        \        4      ,          \        P                  ,          ,           \        \        4      \        P                  ,          ,           8X  g   Q h\        \        P                  \        4      \        \        4      \        \        4      ,          \        \        4      \        \        4      ,          \        \        4      ,          ,           \        P                  ,          \        \        4      \        \        4      ,          \        \        4      \        \        4      ,          \        \        4      ,          ,
          \        P                  ,          ,           \        \        4      \        \        4      ,          \        P                  ,          ,           8X  g   Q h\        \        P                  \
        4      \        \        4      \
        P                  ,          \        \        4      \        \        4      ,          \
        P                  ,          ,           \        \        4      \        \        4      ,          \
        P                  ,          ,
          8X  g   Q h\        \        P                  \
        4      \        \        4      \
        P                  ,          \        \        4      \
        P                  ,          ,           8X  g   Q h\        \        P                  \
        4      \        \        4      \
        P                  ,          \        \        4      \        \        4      ,          \
        P                  ,          ,
          \        \        4      \        \        4      ,          \
        P                  ,          ,           8X  g   Q h\        \        P                  \        4      \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,
          8X  g   Q h\        \        P                  \        4      \        P                  8X  g   Q h\        \        P                  \        4      \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,           8X  g   Q h\        \        P                  \        4      \        P                  8X  g   Q h\        \        P                  \        4      \        P                  8X  g   Q h\        \        P                  \        4      \        P                  u;8X  d   \        P                  8X  g   Q h Q h\        P                  \        \        \        4      \
        P                  ,          \        \        4      \
        P                  ,          ,
          \        4      P!                  4       8X  g   Q h\        P                  \        \        \        4      \
        P                  ,          \        \        4      \
        P                  ,          ,           \        4      P!                  4       8X  g   Q h\        P                  \        \        \        4      \        P                  ,          \        \        4      \        \        4      ,          \        P                  ,          ,
          \        \        4      \        \        4      ,          \        P                  ,          ,           \        4      P!                  4       8X  g   Q h\        P                  \        \        \        4      \        P                  ,          \        \        4      \        \        4      ,          \        P                  ,          ,           \        \        4      \        \        4      ,          \        P                  ,          ,
          \        4      P!                  4       8X  g   Q h\        P                  \        \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,           \        4      P!                  4       8X  g   Q h \
        P                  \        \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,           \
        4      P!                  4       8X  g   Q h\
        P                  \        \        \        4      ) \        P                  ,          \        \        4      \        P                  ,          ,           \
        4      P!                  4       8X  g   Q h\
        P                  \        \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,
          \
        4      P!                  4       8X  g   Q h\
        P                  \        \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,           \
        4      P!                  4       8X  g   Q h\
        P                  \        \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,           \
        4      P!                  4       8X  g   Q h\
        P                  \        \        \        4      \        \        4      ,          \        P                  ,          \        \        4      \        P                  ,          ,           \        \        4      \        \        4      ,          \        P                  ,          ,
          \
        4      P!                  4       8X  g   Q h\
        P                  \        \        \        4      ) \        \        4      ,          \        P                  ,          \        \        4      \        P                  ,          ,           \        \        4      \        \        4      ,          \        P                  ,          ,           \
        4      P!                  4       8X  g   Q h\        P                  \        \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,           \        4      P!                  4       8X  g   Q h\        P                  \        \        \        4      ) \        \        4      ,          \        P                  ,          \        \        4      \        \        4      ,          \        P                  ,          ,           \        \        4      \        P                  ,          ,           \        4      P!                  4       8X  g   Q h\        P                  \        \        \        4      \        \        4      ,          \        P                  ,          \        \        4      \        \        4      ,          \        P                  ,          ,
          \        \        4      \        P                  ,          ,           \        4      P!                  4       8X  g   Q h\        P                  \        \        \        4      \
        P                  ,          \        \        4      \
        P                  ,          ,           \        4      P!                  4       8X  g   Q h\        P                  \        \        \        4      ) \
        P                  ,          \        \        4      \
        P                  ,          ,           \        4      P!                  4       8X  g   Q h\        P                  \        \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,           \        4      P!                  4       8X  g   Q h\        P                  \        \        \        4      ) \        P                  ,          \        \        4      \        P                  ,          ,           \        4      P!                  4       8X  g   Q h \        P                  \        \        \        4      \
        P                  ,          \        \        4      \        \        4      ,          \
        P                  ,          ,           \        \        4      \        \        4      ,          \
        P                  ,          ,
          \        4      P!                  4       8X  g   Q h\        P                  \        \        \        4      \
        P                  ,          \        \        4      \
        P                  ,          ,           \        4      P!                  4       8X  g   Q h\        P                  \        \        \        4      \
        P                  ,          \        \        4      \        \        4      ,          \
        P                  ,          ,
          \        \        4      \        \        4      ,          \
        P                  ,          ,           \        4      P!                  4       8X  g   Q h\        P                  \        \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,
          \        4      P!                  4       8X  g   Q h\        P                  \        \        \        4      \        P                  ,          \        \        4      \        P                  ,          ,           \        4      P!                  4       8X  g   Q hR# r(   )r   r   r   r   r6   r   r    r   r   r*   r   r"   r!   r+   r)   r   r0   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                  
  aa \        R 4      p \        R4      p\        R^4      pV P                  RRWP                  .4      oV P                  V P                  ,          p\        VS4      V) V P                  V P                  ,          ,          V) V P                  V P                  ,          ,          ,           8X  g   Q hV P                  SP                  ,          p\        W@4      V) V P                  SP                  ,          ,          8X  g   Q h\        VS4      V) V P                  SP                  ,          ,          8X  g   Q hV P                  SP                  ,          p\        WP4      W P                  SP                  ,          ,          8X  g   Q h\        VS4      V) V P                  SP                  ,          ,          8X  g   Q hV P                  SP                  ,          p\        W`4      ^ 8X  g   Q h\        VS4      V) V P                  SP                  ,          ,          8X  g   Q h\        R4      w  rxr\        R^4      w  rr\        R^4      w  ppppSP                  RRV
SP                  .4      oWpP                  ,          pWP                  ,          V	SP                  ,          ,           pWpP                  ,          WP                  ,          ,           WP                  ,          ,           p\        SP                  S4      ^ 8X  g   Q h\        SP                  S4      V) SP                  ,          8X  g   Q h\        SP                  S4      VSP                  ,          8X  g   Q h\        VS4      WP                  ,          8X  g   Q h\        VS4      V) \        V4      ,          V,          V P                  ,          V\        V4      ,          V,          V P                  ,          ,           WP                  ,          ,           8X  g   Q h\        VV 4      WP                  ,          W,          SP                  ,          ,
          VSP                  ,          ,           8X  g   Q h\        VS4      WP                  ,          W,          V P                  ,          ,
          V\        V4      ,          V,          V P                  ,          ,           VSP                  ,          ,           W,          SP                  ,          ,
          8X  g   Q h\        VS4      W,          V,           V P                  ,          V) V,          V,           V P                  ,          ,           WP                  ,          ,           8X  g   Q h\        VS4      V) V P                  V P                  ,          ,          \        V4      V,          V P                  V P                  ,          ,          ,           W P                  V P                  ,          ,          ,
          \        V4      V,          V P                  V P                  ,          ,          ,           8X  g   Q h\        \        VV3R l4       \        \        VV3R l4       R# )	r   qr   r   zq1 q2 q3 q4r   c                  4   < \        S P                  SR R7      # )      ?orderr   r    r   r   s   r%   r9   &test_time_derivative.<locals>.<lambda>}  s    qssASAr$   c                  4   < \        S P                  SRR7      # )r   rz   r|   r}   s   r%   r9   r~   ~  s    qssAR@r$   N)r   r   	orientnewr"   r    r   r!   r   r   r   
ValueError)r   rw   qdr8   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            	         \         P                  p \        R 4      w  rp\        R4      w  rEp\        R4      w  rxp	\        R4      w  r\        R4      w  rp\        R^4      w  ppp\        R^4      w  pppV\        P                  ,          V\        P
                  ,          ,           V\        P                  ,          ,           pV\        P                  ,          V\        P
                  ,          ,           V	\        P                  ,          ,           pV\        P                  ,          V\        P
                  ,          ,           V\        P                  ,          ,           pV\        P                  ,          V\        P
                  ,          ,           V\        P                  ,          ,           pV\        P                  ,          V\        P
                  ,          ,           V\        P                  ,          ,           pV\        P                  ,          V\        P
                  ,          ,           V\        P                  ,          ,           p\        \        R7      R8X  g   Q h\        \        VR7      ^ ^ V38X  g   Q h\        \        VR7      VVV38X  g   Q h\        \        VR7      ^ VVV ,          38X  g   Q h\        \        VVV
R7      ^ VVVW
,
          ,          ,           38X  g   Q h\        \        VVV
R7      p^ VVV ,          VV
,          ,
          VP                  W
4      ,           3pVV8X  g   Q h\        \        VVV
R7      p\        WW34      \        P                  ,          \        WW34      \        P
                  ,          ,           \        WW34      \        P                  ,          ,           pVVVV,           3pVV8X  g   Q h\        \        VR	7      VVV ,          VV ^,          ,          ^,          38X  g   Q h\        \        VVVWR
7      VVVV ,          ,           VV,          ,
          V) V
,          VV ^,          ,          ^,          ,           VV,          V
,          ,           VV
^,          ,          ^,          ,
          V VVV,          ,
          ,          ,           VP                  W
4      ,           38X  g   Q h\        \        VVVWR
7      VVVV ,          ,           VV,          ,
          V) V
,          V,           VV ^,          ,          ^,          ,           VV,          V
,          ,           VV
^,          ,          ^,          ,
          V VVV,          ,
          ,          ,           38X  g   Q h\        \        V\        P                  ,          V\        P                  ,          V\        P                  ,          WR
7      p\        WW34      pV\        P                  ,          VV,           \        P                  ,          V\        VV,           W
V 34      ,           \        P                  ,          3pVV8X  g   Q hR# )zs1 s2 s3zS1 S2 S3zS4 S5 S6zt1 t2za b c)frame)position)velocity)r   r   
timevalue1)acceleration)r   r   r   r   
timevalue2Nr   r   r   )
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            
      V  aaaa	a
aa \        R 4      w  oooo	\        R ^4      w  rr#\        R4      w  o
oo\        S
SS.SSS	.RR4      pV\        S
SS.SSS	.RR4      8X  g   Q h\        S
SS.SSSS	.R4      pVRS,          S
,          RS,          S,          ,
          RS	,          S,          ,           V,           RS,          S,          RS,          S,          ,           RS	,          S
,          ,
          V,           RS,          S,          RS,          S,          ,
          RS,          S
,          ,           V,           RS,          S
,          RS,          S,          ,           RS	,          S,          ,           V ,           .8X  g   Q h\        \        VVVV
VV3R l4       \        \        VVVV	V
VV3R l4       \        \        VVVV	V
VV3R	 l4       \        \        VVVV	V
3R
 l4       \        \        VVVV	V
3R l4       \        \        VV
VV3R l4       \        \        VVVV	V
VV3R l4       \        \        VVVV	V
VV3R l4       \        \        VVVV
VV3R l4       \        . ROSSS.R4      \
        P                  \
        P                  \
        P                  .8X  g   Q hR# )zq0 q1 q2 q3zu1 u2 u3bodyi9  313
quaternionry   c                  *   < \        SSS.S SS.R 4      # r   r   q0r)   r+   u1u2u3s   r%   r9   test_kin_eqs.<locals>.<lambda>  s    2BB<"b"|\r$   c                  .   < \        SSS.S SSS.R R4      # )r   123r   r   r)   r+   r*   r   r   r   s   r%   r9   r     s"    2BB<"b"bAQS_afgr$   c                  ,   < \        SSS.S SSS.R 4      # )foor   r   s   r%   r9   r     s    2BB<"b"bAQSXYr$   c                  &   < \        SS SSS.R 4      # r   r   r   r)   r+   r*   r   s   r%   r9   r     s    1"r2r26FUr$   c                  (   < \        S.S SSS.R 4      # r   r   r   s   r%   r9   r     s    12$RR8H,Wr$   c                  $   < \        SSS.S R 4      # r   r   )r   r   r   r   s   r%   r9   r     s    12r2,LQr$   c                  ,   < \        SSS.S SSS.R 4      # )r   r   r   s   r%   r9   r     s    2BB<"b"bAQSYZr$   c                  ,   < \        SSS.S SSS.R 4      # )spacer   r   s   r%   r9   r     s    2BB<"b"bAQSZ[r$   c                  ,   < \        SSS.S SS.R R4      # )r   222r   r   s   r%   r9   r     s    2BB<"b"vW\]r$   r   Ng      r   )r   r   r   r   rr   r   r6   )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            
      `  aaaaa \        R 4      w  rpor4\        R4      w  rV\        R4      p\        R4      oSP                  RRV SP                  .4      pVP                  RRWP
                  .4      p	V	P                  RRW)P                  .4      p
V
P                  SSV	P
                  ,          W9P                  ,          ,           WIP                  ,          ,           4       \        R4      pVP                  SWYP
                  ,          WhP                  V	P
                  ,          ,          ,           4       VP                  R	WyP                  ,          4      oSP                  VSV
4       SP                  S4      VP                  S4      V
P                  S4      .oSW4WV.o\        SSS4      V) V	P                  ,          WyP
                  ,          ^ V	P
                  \        V4      V	P                  ,          \!        V4      V	P                  ,          ,
          .^ ^ ^ V	P
                  \        V4      V	P                  ,          \!        V4      V	P                  ,          ,
          .V	P
                  V	P                  V	P                  ^ ^ ..8X  g   Q h\        R
4      p\        R4      pW\P
                  ,          WmP                  ,          ,           p\        V3WV3V4      VP
                  VP                  ..8X  g   Q h\#        \$        VVV3R l4       \#        \$        VVV3R l4       R# )zq1 q2 q3 u1 u2 u3zu4, u5rr   Yr   LRr   Dmcr   r   c                  <   < \        S P                  S4      SS4      # r4   )r   vel)r   r   u_lists   r%   r9   'test_partial_velocity.<locals>.<lambda>  s    .swwqz61Er$   c                     < \        SSS 4      # r4   )r   )r   r   vel_lists   r%   r9   r     s    .xQ?r$   N)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   +,?@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                  J   \        R 4      p \        RRR7      p\        RRR7      p\        RRR7      w  r4\        R	RR
7      pV P                  e   Q hVP                  '       g   Q hVP                  '       g   Q hW4,          WC,          8w  g   Q hVP                  '       g   Q hR# )f1f2T)realf3)positivezf4,f5F)commutativef6)integerN)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,   r.   r1   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$   