o GZŽh‡Rã@sZddlmZddlmZddlmZddlmZddlm Z m Z ddl m Z ddl mZmZmZmZddlmZmZmZmZmZmZmZmZmZdd lmZdd lmZed ƒ\Z Z!Z"Z#Z$ed ƒZ%e% &d de e%j'g¡Z(e( &dde!e(j)g¡Z*e* &dde"e*j+g¡Z,dd„Z-dd„Z.dd„Z/dd„Z0dd„Z1dd„Z2dd„Z3dd „Z4d!d"„Z5d#d$„Z6d%d&„Z7d'S)(é)Ú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)Úraiseszq1 q2 q3 q4 q5ÚNÚAÚAxisÚBÚCcCsÊttjtjƒdks J‚ttjtjƒdksJ‚ttjtjƒdks!J‚ttjtjƒdks,J‚ttjtjƒdks7J‚ttjtjƒdksBJ‚ttjtjƒdksMJ‚ttjtjƒdksXJ‚ttjtjƒdkscJ‚dS)Nér)rrÚxÚyÚz©r!r!úX/var/www/auris/lib/python3.10/site-packages/sympy/physics/vector/tests/test_functions.pyÚtest_dotsr#cCsTttjtjƒttƒks J‚ttjtjƒttƒ ksJ‚ttjtjƒdks&J‚ttjtjƒttƒks3J‚ttjtjƒttƒks@J‚ttjtjƒdksKJ‚ttjtjƒdksVJ‚ttjtjƒdksaJ‚ttjtjƒdkslJ‚t ttjtjtjƒƒt dƒttt dƒks†J‚t ttjtjtjƒƒt ttjtjtjƒƒks J‚ttjt jƒtt ƒks­J‚ttjt jƒdks¸J‚ttjt jƒtt ƒksÅJ‚ttjt jƒttƒtt ƒksÖJ‚ttjt jƒttƒksãJ‚ttjt jƒttƒ tt ƒksõJ‚ttjt jƒttƒ tt ƒksJ‚ttjt jƒttƒksJ‚ttjt jƒttƒtt ƒks(J‚dS)Nrréé)rrrrrÚq1rrr rrrrÚq3Úq2r!r!r!r"Útest_dot_different_frames%s(44"$&(r)cCsÜttjtjƒdks J‚ttjtjƒtjksJ‚ttjtjƒtj ks$J‚ttjtjƒtj ks1J‚ttjtjƒdkssr+cCsÌttjtjƒttƒtjksJ‚ttjtjƒttƒtjks J‚ttjtjƒttƒ tjttƒtjks8J‚ttjtjƒttƒ tjksIJ‚ttjtjƒttƒtjksYJ‚ttjtjƒttƒtjttƒtjkspJ‚ttjtjƒtjks|J‚ttjtjƒtj ks‰J‚ttjtjƒdks”J‚ttjtjƒttƒtjks¤J‚ttjtjƒttƒtjks´J‚ttjtjtjƒttƒtjttƒtjksÎJ‚ttjtjtjƒttƒ tjttƒtjkséJ‚ttjt jƒtt ƒt jksùJ‚ttjt jƒtt ƒ t jtt ƒt jksJ‚ttjt jƒtt ƒ t jks$J‚tt jtjƒtt ƒ t jks6J‚tt jtjƒ  t ¡  ¡tt ƒt jtt ƒt jksSJ‚tt jtjƒtt ƒt jksdJ‚dSr*) r rrrrr&r rrrr'rÚsimplifyr!r!r!r"Útest_cross_different_framesLs&  0" .  46 2$$:&r-cs˜tdƒ}|j|j‰ˆˆB‰tdƒ‰tdƒ‰ˆˆ@tˆˆƒks!J‚ˆˆ@tˆˆƒks,J‚ˆˆ@tˆˆƒks7J‚ˆˆ@tˆˆƒksBJ‚ˆˆ@tˆˆƒksMJ‚ˆˆ@tˆˆƒksXJ‚ˆˆ@tˆˆƒkscJ‚tt‡fdd„ƒtt‡fdd„ƒtt‡fdd„ƒtt‡fdd„ƒˆˆ@tˆˆƒks’J‚ˆˆ@tˆˆƒksJ‚ˆˆ@tˆˆƒks¨J‚tt‡fdd„ƒtt‡fd d„ƒtt‡fd d„ƒtt‡fd d„ƒtt‡fd d„ƒtt‡‡fd d„ƒtt‡‡fdd„ƒˆˆAtˆˆƒksôJ‚ˆˆAtˆˆƒksÿJ‚ˆˆAtˆˆƒks J‚ˆˆAtˆˆƒksJ‚ˆˆAtˆˆƒks#J‚tt‡fdd„ƒtt‡fdd„ƒtt‡fdd„ƒtt‡fdd„ƒˆˆAtˆˆƒksSJ‚ˆˆAtˆˆƒks_J‚ˆˆAtˆˆƒkskJ‚tt‡fdd„ƒtt‡fdd„ƒtt‡fdd„ƒtt‡fdd„ƒtt‡fdd„ƒtt‡‡fdd„ƒtt‡‡fdd„ƒtt‡‡fdd„ƒtt‡‡fdd„ƒtt‡‡fdd„ƒtt‡‡fdd„ƒtt‡‡fdd„ƒtt‡fdd„ƒtt‡fd d„ƒtt‡fd!d„ƒtt‡fd"d„ƒˆˆBt ˆˆƒksJ‚ˆˆBt ˆˆƒksJ‚ˆˆBt ˆˆƒks&J‚tt‡fd#d„ƒtt‡fd$d„ƒtt‡fd%d„ƒtt‡fd&d„ƒd'S)(zUTest that the output of dot, cross, outer functions match operator behavior. rrcó tˆtjƒS©N©rrÚZeror!©Údr!r"Úuó z%test_operator_match..có ttjˆƒSr/r0r!r2r!r"r4vr5có tˆdƒSr*©rr!r2r!r"r4wó có tdˆƒSr*r8r!r2r!r"r4xr9cr.r/r0r!©Úvr!r"r4|r5cr6r/r0r!r;r!r"r4}r5cr7r*r8r!r;r!r"r4~r9cr:r*r8r!r;r!r"r4r9có tˆˆƒSr/©r r!r2r!r"r4‚r9có tˆˆƒSr/r>r!©r3Úzerodr!r"r4ƒr9có tˆˆƒSr/r>r!r@r!r"r4„r9cr.r/©r rr1r!r2r!r"r4Šr5cr6r/rCr!r2r!r"r4‹r5cr7r*r>r!r2r!r"r4Œr9cr:r*r>r!r2r!r"r4r9cr.r/rCr!r;r!r"r4‘r5cr6r/rCr!r;r!r"r4’r5cr7r*r>r!r;r!r"r4“r9cr:r*r>r!r;r!r"r4”r9cr=r/©rr!r2r!r"r4—r9cr?r/rDr!r@r!r"r4˜r9crBr/rDr!r@r!r"r4™r9cr?r/rDr!©r3r<r!r"r4šr9crBr/rDr!rEr!r"r4›r9cr?r/rDr!©r3Úzerovr!r"r4œr9crBr/rDr!rFr!r"r4r9crBr/rDr!rFr!r"r4žr9cr.r/©rrr1r!r2r!r"r4Ÿr5cr6r/rHr!r2r!r"r4 r5cr7r*rDr!r2r!r"r4¡r9cr:r*rDr!r2r!r"r4¢r9cr.r/rHr!r;r!r"r4¦r5cr6r/rHr!r;r!r"r4§r5cr7r*rDr!r;r!r"r4¨r9cr:r*rDr!r;r!r"r4©r9N) r rrr r rrÚ TypeErrorr r)rr!)r3r<rArGr"Útest_operator_matchcsz rJcCsHttdƒtƒtdƒks J‚ttjtƒtjusJ‚ttjtƒtt ƒtjt t ƒtj ks.J‚ttj tƒt t ƒt t ƒtjtt ƒtj t t ƒtt ƒtj ksSJ‚ttj tƒt t ƒ tt ƒtjt t ƒtj tt ƒtt ƒtj ksyJ‚ttjtƒttƒtjt tƒtj ksJ‚ttj tƒt tƒ tjttƒtj ks¦J‚ttj tƒtj ks±J‚ttjtƒtjks¼J‚ttj tƒtj ksÇJ‚ttj tƒtj ksÒJ‚ttjtƒtjksÝJ‚ttj tƒtt ƒtj t t ƒtj ksóJ‚ttj tƒt t ƒtj tt ƒtj ks J‚ttjtƒtt ƒtjt t ƒtj ks!J‚ttj tƒt t ƒt t ƒtjtt ƒtj t t ƒtt ƒtj ksGJ‚ttj tƒt t ƒ tt ƒtjt t ƒtj tt ƒtt ƒtj ksnJ‚ttjtƒtjkszJ‚ttj tƒtj ks†J‚ttj tƒtj ks’J‚ttjtƒttƒtjt tƒtj ks©J‚ttj tƒt tƒtjttƒtj ksÀJ‚ttj tƒtj ksÌJ‚ttjtƒttƒtjt tƒtt ƒtj t tƒt t ƒtj ksòJ‚ttj tƒt tƒtjttƒtt ƒtj t t ƒttƒtj ksJ‚ttj tƒt t ƒtj tt ƒtj ks/J‚ttjtƒttƒtt ƒt tƒt t ƒt t ƒtjt tƒtt ƒtj t t ƒttƒt tƒt t ƒtt ƒtj ksqJ‚ttj tƒt tƒtt ƒt t ƒt t ƒttƒtjttƒtt ƒtj t tƒt t ƒt t ƒttƒtt ƒtj ks³J‚ttj tƒt t ƒ tt ƒtjt t ƒtj tt ƒtt ƒtj ksÚJ‚ttjtƒttƒtjt tƒtj ksñJ‚ttj tƒt tƒ tjttƒtj ks J‚ttj tƒtj ksJ‚ttjtƒtjks!J‚ttj tƒtj ks-J‚ttj tƒtj ks9J‚ttjtƒtjksEJ‚ttj tƒtt ƒtj t t ƒtj ks\J‚ttj tƒt t ƒtj tt ƒtj kssJ‚ttjtƒtt ƒtjt t ƒtj ksŠJ‚ttj tƒt t ƒt t ƒtjtt ƒtj t t ƒtt ƒtj ks°J‚ttj tƒt t ƒ tt ƒtjt t ƒtj tt ƒtt ƒtj ks×J‚ttjtƒttƒtjt tƒtj ksîJ‚ttj tƒt tƒ tt ƒtjttƒtt ƒtj t t ƒtj ksJ‚ttj tƒt tƒt t ƒtjt t ƒttƒtj tt ƒtj ks;J‚ttjtƒtjksGJ‚ttj tƒtt ƒtj t t ƒtj ks^J‚ttj tƒt t ƒ tj tt ƒtj ksvJ‚ttjtƒtjks‚J‚ttj tƒtj ksŽJ‚ttj tƒtj ksšJ‚ttjtƒtt ƒtjt t ƒtj ks±J‚ttj tƒtj ks½J‚ttj tƒt t ƒ tjtt ƒtj ksÕJ‚ttjtƒttƒtt ƒt tƒt t ƒt t ƒtjt tƒtt ƒt t ƒt t ƒttƒtj t t ƒtt ƒtj ksJ‚ttj tƒt tƒ tt ƒtjttƒtt ƒtj t t ƒtj ks>J‚ttj tƒt t ƒttƒt tƒt t ƒtt ƒtjt tƒt t ƒt t ƒttƒtt ƒtj tt ƒtt ƒtj ks€J‚ttjtƒtt ƒtjt t ƒt t ƒtj t t ƒtt ƒtj ks¦J‚ttj tƒtt ƒtj t t ƒtj ks½J‚ttj tƒt t ƒtjt t ƒtt ƒtj tt ƒtt ƒtj ksãJ‚ttjtƒtt ƒtjt t ƒtj ksúJ‚ttj tƒtj ksJ‚ttj tƒt t ƒtjtt ƒtj ksJ‚ttjtƒtjks)J‚ttj tƒtj ks5J‚ttj tƒtj krHtj ksKJ‚J‚tjtttƒtjt tƒtj tƒ ¡ksdJ‚tj tt tƒtjttƒtj tƒ ¡ks}J‚tjtttƒtjt tƒtt ƒtj t tƒt t ƒtj tƒ ¡ks¥J‚tj tt tƒtjttƒtt ƒtj t t ƒttƒtj tƒ ¡ksÍJ‚tj tt t ƒtj tt ƒtj tƒ ¡ksæJ‚ tjtttƒtjt tƒtj tƒ ¡ksJ‚tj tt tƒ tjttƒtj tƒ ¡ksJ‚tj ttt ƒtj t t ƒtj tƒ ¡ks3J‚tj tt t ƒtj tt ƒtj tƒ ¡ksLJ‚tjttt ƒtjt t ƒtj tƒ ¡kseJ‚tj tt t ƒt t ƒtjtt ƒtj t t ƒtt ƒtj tƒ ¡ksJ‚tj tt t ƒ tt ƒtjt t ƒtj tt ƒtt ƒtj tƒ ¡ks¶J‚tjtttƒtjt tƒtj tƒ ¡ksÏJ‚tj tt tƒ tt ƒtjttƒtt ƒtj t t ƒtj tƒ ¡ksøJ‚tj tt tƒt t ƒtjt t ƒttƒtj tt ƒtj tƒ ¡ks J‚tj ttt ƒtj t t ƒtj tƒ ¡ks9J‚tj tt t ƒ tj tt ƒtj tƒ ¡ksSJ‚tjttt ƒtjt t ƒtj tƒ ¡kslJ‚tj tt t ƒ tjtt ƒtj tƒ ¡ks†J‚ tjttt ƒtjt t ƒt t ƒtj t t ƒtt ƒtj tƒ ¡ks¯J‚tj ttt ƒtj t t ƒtj tƒ ¡ksÈJ‚tj tt t ƒtjt t ƒtt ƒtj tt ƒtt ƒtj tƒ ¡ksðJ‚tjttt ƒtjt t ƒtj tƒ ¡k s J‚tj tt t ƒtjtt ƒtj tƒ ¡k s"J‚dSr*)rr rrr1rrrrr'rr rr(r&rr,r!r!r!r"Ú test_express¬sŽ,, ÿ. ÿ,.,.., ÿ. ÿ.., ÿ, ÿ. ,ÿ,þ ÿ ,ÿ,þ ÿ. ÿ.0..., ÿ. ÿ. ÿ  ÿÿ  ÿ.0.0 ,,ÿþ ÿ : ÿ ,,ÿþ ÿ, ÿ., ÿ..,22(ÿÿ ÿ(ÿÿ ÿ224222(ÿÿ ÿ*ÿÿ ÿ2ÿ ÿÿ ÿÿ ÿÿ ÿ2424( ÿÿ ÿ2(ÿÿ ÿ26rKcsÈtdƒ}tdƒ}tddƒ}| dd||jg¡‰|j|jB}t|ˆƒ| |j|jB| |j|jBks6J‚|jˆjB}t||ƒ| |jˆjBksLJ‚t|ˆƒ| |jˆjBks\J‚|jˆjB}t||ƒ||jˆjBksqJ‚t|ˆƒ| |jˆjBksJ‚|jˆjB}t||ƒdksJ‚t|ˆƒ| |jˆjBks J‚tdƒ\}}} } tddƒ\} } } }tddƒ\}}}}ˆ d d| ˆjg¡‰||j}||j| ˆj}||j||j| |j}tˆjˆƒdksìJ‚tˆjˆƒ| ˆjksúJ‚tˆjˆƒ|ˆjksJ‚t|ˆƒ| |jksJ‚t|ˆƒ| t|ƒ||j|t|ƒ||j| |jks9J‚t||ƒ| |j| |ˆj| ˆjksRJ‚t|ˆƒ| |j|||j|t|ƒ||j| ˆj| |ˆjks}J‚t|ˆƒ||| |j| || |j| |jksJ‚t|ˆƒ| |j|jBt|ƒ||j|jB||j|jBt|ƒ||j|jBksÎJ‚t t ‡‡fd d „ƒt t ‡‡fd d „ƒdS) NrÚqrrrrz q1 q2 q3 q4r$rcótˆjˆddS)Nçà?©Úorder©rrr!©rrr!r"r4}óz&test_time_derivative..crM)NéÿÿÿÿrOrQr!rRr!r"r4~rS) r rÚ orientnewr rrrrrrÚ ValueError)rrLZqdr3Zd1Zd2Zd3r&r(r'Úq4Úq1dÚq2dÚq3dZq4dZq1ddZq2ddZq3ddZq4ddÚv1Úv2Zv3r!rRr"Útest_time_derivativeVsn   ÿ        ÿ ÿ2ÿÿ  ÿÿ ÿÿÿ ÿr]cCsütj}tdƒ\}}}tdƒ\}}}tdƒ\}}} tdƒ\} } tdƒ\} } }tddƒ\}}}tddƒ\}}}|tj|tj|tj}|tj|tj| tj}|tj|tj|tj}| tj| tj|tj}|tj|tj|tj}|tj|tj|tj}ttdd ks˜J‚tt|d d d |fks¥J‚tt|d |||fks²J‚tt|d d |||fksÁJ‚tt||| d d ||||| fksÖJ‚tt||| d }d ||||| | || ¡f}||ksõJ‚tt||| d }t | || |fƒtjt | || |fƒtjt ||| |fƒtj}||||f}||ks,J‚tt|d|||||ddfksBJ‚tt|||| | d|||||| | | ||dd|| | || dd|||| | || ¡fksƒJ‚tt|||| | d|||||| | | |||dd|| | || dd|||| fksÀJ‚tt| tj|tj|tj| | d}t | || |fƒ}| tj||tj|t |||| |fƒtjf}||ksüJ‚dS)Nzs1 s2 s3zS1 S2 S3zS4 S5 S6zt1 t2za b crr$)Úframe©rrr)Úpositionr)Úvelocity)rar`Ú timevalue1)Ú acceleration)rcrar`rbZ timevalue2) rZ_trrrrr rÚsubsr)ÚtÚs1Ús2Zs3ZS1ZS2ZS3ZS4ZS5ZS6Út1Út2ÚaÚbÚcÚadZbdÚcdZa2dZb2dZc2dZv0Zv01r[r\Zv2dZv2ddZanswerZanswer_expectedZintegral_vectorÚi1r!r!r"Útest_get_motion_methodss† ÿ" *ÿ  ÿÿ$ÿÿ þÿ þÿ ÿÿþÿ þ ÿÿrpc sVtdƒ\‰‰‰‰tddƒ\}}}}tdƒ\‰‰‰tˆˆˆgˆˆˆgddƒ}|tˆˆˆgˆˆˆgddƒks6J‚tˆˆˆgˆˆˆˆgdƒ}|dˆˆd ˆˆd ˆˆ|dˆˆd ˆˆd ˆˆ|dˆˆd ˆˆd ˆˆ|d ˆˆd ˆˆd ˆˆ|gks•J‚tt‡‡‡‡‡‡fd d „ƒtt‡‡‡‡‡‡‡fd d „ƒtt‡‡‡‡‡‡‡fd d „ƒtt‡‡‡‡‡fdd „ƒtt‡‡‡‡‡fdd „ƒtt‡‡‡‡fdd „ƒtt‡‡‡‡‡‡‡fdd „ƒtt‡‡‡‡‡‡‡fdd „ƒtt‡‡‡‡‡‡fdd „ƒtgd¢ˆˆˆgdƒtjtjtjgks)J‚dS)Nz q0 q1 q2 q3rzu1 u2 u3Úbodyi9Z313Ú quaterniongà¿rNcstˆˆˆgˆˆˆgdƒS©Nrr©rr!©Úq0r&r(Úu1Úu2Úu3r!r"r4Åsztest_kin_eqs..cstˆˆˆgˆˆˆˆgddƒS)NrrZ123rtr!©rvr&r(r'rwrxryr!r"r4ÆscótˆˆˆgˆˆˆˆgdƒS)NZfoortr!rzr!r"r4ÇócstˆˆˆˆˆgdƒSrsrtr!©rvr&r(r'rwr!r"r4ÈscstˆgˆˆˆˆgdƒSrsrtr!r}r!r"r4ÉscstˆˆˆgˆdƒSrsrtr!)rvrwrxryr!r"r4Êócr{)Nrqrtr!rzr!r"r4Ër|cr{)NÚspacertr!rzr!r"r4Ìr|cstˆˆˆgˆˆˆgddƒS)NrqZ222rtr!rur!r"r4Ír|r_r)rrrrVrIrr1)Zq0drXrYrZZkeZkdsr!rzr"Ú test_kin_eqsºs*"(&&& ý0r€c sþtdƒ\}}}‰}}tdƒ\}}tdƒ}tdƒ‰ˆ dd|ˆjg¡}| dd||jg¡} |  dd|| jg¡} |  ˆˆ| j|| j|| j¡td ƒ} |   ˆ|| j||j| jA¡|   d || j¡‰ˆ  | ˆ| ¡ˆ  ˆ¡|   ˆ¡|   ˆ¡g‰ˆ||||g‰tˆˆˆƒ| | j|| jd | jt|ƒ| jt|ƒ| jgd d d | jt|ƒ| jt|ƒ| jg| j| j| jd d ggksÃJ‚td ƒ} td ƒ} || j|| j}t|f||f| ƒ| j| jggksçJ‚tt‡‡‡fdd„ƒtt‡‡‡fdd„ƒdS)Nzq1 q2 q3 u1 u2 u3zu4, u5ÚrrÚYrÚLÚRrÚDmcrrrcstˆ ˆ¡ˆˆƒSr/)rÚvelr!)r…rÚu_listr!r"r4ïr~z'test_partial_velocity..cs tˆˆˆƒSr/)rr!)rrwÚvel_listr!r"r4ðr5)rrr rUr rrZ set_ang_velr Zset_velZ locatenewZ v2pt_theoryr†Z ang_vel_inrrrrrI)r&r(r'rxryZu4Zu5rr‚rƒr„rrrr<r!)r…rrwr‡rˆr"Útest_partial_velocityÑs4 &" 4&þÿ$r‰cCs€tdƒ}tddd}tddd}tddd \}}td dd }|jdus%J‚|js*J‚|js/J‚||||ks9J‚|js>J‚dS) NÚf1Úf2T)ÚrealÚf3)Zpositivezf4,f5F)Z commutativeÚf6)Úinteger)rZis_realZ is_positiveÚ is_integer)rŠr‹rZf4Zf5rŽr!r!r"Útest_dynamicsymbolsòs     r‘N)8Zsympy.core.numbersrZsympy.core.singletonrZsympy.core.symbolrZ(sympy.functions.elementary.miscellaneousrZ(sympy.functions.elementary.trigonometricrrZsympy.integrals.integralsrZsympy.physics.vectorr r r r Zsympy.physics.vector.functionsr rrrrrrrrZsympy.simplifyrZsympy.testing.pytestrr&r(r'rWZq5rrUr rrrrrr#r)r+r-rJrKr]rpr€r‰r‘r!r!r!r"Ús6    ,  I++9 !