gpadZddlZddlmZmZmZmZmZmZddl m Z m Z m Z m Z mZmZmZmZmZmZmZddlmZdZGddZGd d ZGd d ZdS) zz#_simplify_loads..sF     t}dk&:&:&<&<==   r)loadss r_simplify_loadsr!s#     rc DeZdZdZeejdededfifgdZ eejdedfedededfgdZ eejdd edfedd fgd Z ej d d Z dZdZdZdZdZdZdZdZdZdZdZd S)TestLinearPathwayc>tttsJdSN) issubclassr r selfs rtest_is_pathway_base_subclassz/TestLinearPathway.test_is_pathway_base_subclass&s-5555555rz args, kwargspApBc|\}}t|i|}t|tsJt|dsJt|jdksJ|jd|usJ|jd|usJt|jdt sJ|jdjdksJt|jdt sJ|jdjdksJdS)N attachmentsrr*r+)r isinstancehasattrlenr-r name)argskwargspointApointBinstances rtest_valid_constructorz(TestLinearPathway.test_valid_constructor)s $1&11(M22222x/////8'((A----#A&&0000#A&&0000(.q1599999#A&+t3333(.q1599999#A&+t333333rr-pZc|tjt5t|}ddddS#1swxYwYdSr%)pytestraises ValueErrorr r-_s r)test_invalid_attachments_incorrect_numberz;TestLinearPathway.test_invalid_attachments_incorrect_number=s]: & & , ,{+A , , , , , , , , , , , , , , , , , , 155Nc|tjt5t|}ddddS#1swxYwYdSr%)r<r= TypeErrorr r?s r"test_invalid_attachments_not_pointz4TestLinearPathway.test_invalid_attachments_not_pointIs]9 % % , ,{+A , , , , , , , , , , , , , , , , , ,rBTautousectd|_td|_td|_t |j|j|_td|_td|_ td|_ tdd|_ tdd|_ tdd|_ td|_dS) NNr*r+q1q2q3r/F)rrIr r*r+r pathwayrrJrKrLq1dq2dq3drrMr's r_linear_pathway_fixturez)TestLinearPathway._linear_pathway_fixtureUs$$++++$TWdg66  && && &&!$**!$**!$**rct|j|j}tjt 5d|_dddn #1swxYwYtjt5d|jd<dddn #1swxYwYtjt5d|jd<ddddS#1swxYwYdSNrr/)r r*r+r<r=AttributeErrorr-rDr(r8s rtest_properties_are_immutablez/TestLinearPathway.test_properties_are_immutablecsu $'22 ]> * * ( (#'H  ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ]9 % % + +&*H  # + + + + + + + + + + + + + + + ]9 % % + +&*H  # + + + + + + + + + + + + + + + + + +s5AA A , BB B' B??CCcht|j|j}d}t||ksJdS)NzLinearPathway(pA, pB))r r*r+reprr(rNexpecteds r test_reprzTestLinearPathway.test_reprls711*G}}((((((rc|j|jd|jjz|jjdksJdSNr.)r+set_posr*rIxrNlengthr's rtest_static_pathway_lengthz,TestLinearPathway.test_static_pathway_lengthqs> 468,,,|"a''''''rc|j|jd|jjz|jjdksJdSNr.r)r+r_r*rIr`rNextension_velocityr's r&test_static_pathway_extension_velocityz8TestLinearPathway.test_static_pathway_extension_velocityus> 468,,,|.!333333rc|j|jd|jjz|j|j |jjzf|j|j|jjzfg}|j|j|ksJdSr^)r+r_r*rIr`rMrNto_loadsr(r[s rtest_static_pathway_to_loadsz.TestLinearPathway.test_static_pathway_to_loadsys 468,,, Whtvx' ( WdfTVXo & |$$TV,,888888rc|j|jd|jz|jjzdt |jdzz}|jj|ksJdSr^) r+r_r*rJrIr`rrNraris rtest_2D_pathway_lengthz(TestLinearPathway.test_2D_pathway_lengths] 47468!3444T$'1*%%%|"h......rc|j|jd|jz|jjzdt |jdzz|jz|jz }|jj |ksJdSr^) r+r_r*rJrIr`rrOrNreris r"test_2D_pathway_extension_velocityz4TestLinearPathway.test_2D_pathway_extension_velocitysk 47468!3444T$'1*%%%dh.tw6|.(::::::rc|j|jd|jz|jjz|j|j |jt|jdzz z|jjzf|j|j|jt|jdzz z|jjzfg}|j |j|ksJdSr^) r+r_r*rJrIr`rMrrNrhris rtest_2D_pathway_to_loadsz*TestLinearPathway.test_2D_pathway_to_loadss 47468!3444 Wh$twz*:*: :;DFHD E WdfdgTWaZ(8(889$&(B C |$$TV,,888888rch|j|j|j|jjz|j|jjzz d|jz|jj zzt|jdz|jdzzd|jdzzz}t|j j |z dksJdSNr.r)r+r_r*rJrIr`rKyrLzrrrNraris rtest_3D_pathway_lengthz(TestLinearPathway.test_3D_pathway_lengths  G GDFH twtvx/ /!DG)DFH2D D    TWaZ/!DGQJ,>?? +h6771<<<<<tttsJdSr%)r&r r r's rr)z4TestObstacleSetPathway.test_is_pathway_base_subclasss,k:::::::rznum_attachments, attachmentsc,g|]}t|Srr rr3s rrz!TestObstacleSetPathway.<<<t<<BBBtBBBr)rr*r+rc,g|]}t|Srrrs rrz!TestObstacleSetPathway.HHHtHHHr)rr*r+pCrc,g|]}t|Srrrs rrz!TestObstacleSetPathway.sNNNtNNNr)rr*r+rpDrct|}t|tsJt|dsJt|j|ksJ|jD]}t|t sJdS)Nr-)r r0r1r2r-r )num_attachmentsr-r8 attachments rr9z-TestObstacleSetPathway.test_valid_constructors&{3($677777x/////8'((O;;;;". 1 1Jj%00 0 0 0 0 1 1rr-rrc|tjt5t|}ddddS#1swxYwYdSr%)r<r=r>r r?s r5test_invalid_constructor_attachments_incorrect_numberzLTestObstacleSetPathway.test_invalid_constructor_attachments_incorrect_numbers ]: & & 1 1"K0A 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1rBNr*c|tjt5t|}ddddS#1swxYwYdSr%r<r=rDrr?s r.test_invalid_constructor_attachments_not_pointzETestObstacleSetPathway.test_invalid_constructor_attachments_not_points]9 % % . .-A . . . . . . . . . . . . . . . . . .rBcJttdtdtd}tjt5d|_dddn #1swxYwYtjt 5d|jd<dddn #1swxYwYtjt 5d|jd<dddn #1swxYwYtjt 5d|jd<ddddS#1swxYwYdS)Nrr*rrr/)r r r<r=rUr-rDr(rNs rrWz4TestObstacleSetPathway.test_properties_are_immutables$U4[[%++uT{{KK ]> * * ' '"&G  ' ' ' ' ' ' ' ' ' ' ' ' ' ' ' ]9 % % * *%)G  " * * * * * * * * * * * * * * * ]9 % % * *%)G  " * * * * * * * * * * * * * * * ]9 % % + +&*G  # + + + + + + + + + + + + + + + + + +sHA&&A*-A* B!!B%(B% CC #C  DDDzattachments, expectedc,g|]}t|Srrrs rrz!TestObstacleSetPathway.rrzObstacleSetPathway(pO, pA, pI)c,g|]}t|Srrrs rrz!TestObstacleSetPathway.rrz"ObstacleSetPathway(pO, pA, pB, pI)c,g|]}t|Srrrs rrz!TestObstacleSetPathway.rrz&ObstacleSetPathway(pO, pA, pB, pC, pI)cBt|}t||ksJdSr%)r rY)r-r[rNs rr\z TestObstacleSetPathway.test_reprs+&%k2G}}((((((rTrFcHtd|_td|_td|_td|_td|_td|_tdd|_ td|_ dS) NrIrrr*r+qr/rM) rrIr rrr*r+rrqdrrMr's r_obstacle_set_pathway_fixturez4TestObstacleSetPathway._obstacle_set_pathway_fixture sv$$++++++++$$ a((rc|j|j|jj|j|j|jj|j|j|jjt|j|j|j|j}|j ddtdzzksJdS)Nr/r.) r*r_rrIr`r+rtrrur rarrs rrbz1TestObstacleSetPathway.test_static_pathway_lengths *** *** ***$TWdgtwHH~Qa[000000rch|j|j|jj|j|j|jj|j|j|jjt|j|j|j|j}|j dksJdSNr) r*r_rrIr`r+rtrrur rers rrfz=TestObstacleSetPathway.test_static_pathway_extension_velocitys *** *** ***$TWdgtwHH)Q......rc \|j|j|jj|j|j|jj|j|j|jjt|j|j|j|j}t|j|j |jjzt|j|j |jjzt|j|j tdzdz |jj|jjz zt|j|j tdzdz |jj|jjz zt|j|j tdzdz |jj|jjz zt|j|j tdzdz |jj|jjz zg}| |j |ksJdSr^)r*r_rrIr`r+rtrrur r rMrrhrZs rrjz3TestObstacleSetPathway.test_static_pathway_to_loads&s *** *** ***$TWdgtwHH $'DF7TVX- . . $'46DFH, - - $'46DGG+a/468dfh3FG H H $'46DGG+a/468dfh3FG H H $'46DGG+a/468dfh3FG H H $'46DGG+a/468dfh3FG H H  ''8333333rc |j|j|jj|jjz |j|jt|j|jjzt|jdz|jjzz |j |jt|j|jjzt|jdz |jjzzt|j|j|j|j }dtdztddt|jzzz}|j |z dksJdSNr/r.r)r*r_rrIr`rtr+rrrrr rrarrZs rrlz-TestObstacleSetPathway.test_2D_pathway_length5s4 468dfh#6!7888  GS[[468+s46{{Q$&(.JJ     GS[[468+s46{{Q$&(.JJ   %TWdgtwHHtAww;a!CKK-&7!8!88)3355::::::rc@|j|j|jj|jjz |j|jt|j|jjzt|jdz|jjzz |j |jt|j|jjzt|jdz |jjzzt|j|j|j|j }tdt|jz|j z dtt|jdzzz }|j|z dksJdSr)r*r_rrIr`rtr+rrrrr rrrerrZs rrnz9TestObstacleSetPathway.test_2D_pathway_extension_velocityAsI 468dfh#6!7888  GS[[468+s46{{Q$&(.JJ     GS[[468+s46{{Q$&(.JJ   %TWdgtwHH1ggDF +dg56!d3tv;;QR?>S>S:ST*X5??AAQFFFFFFrc 4|j|j|jj|jjz |j|jt|j|jjzt|jdz|jjzz |j |jt|j|jjzt|jdz |jjzzt|j|j|j|j }tddz |jj|jjzz}tdt|jzdz dz |jjztdt|jzdtt|jdzzz |jjzz}t|jtdz z|jjzt|jtdz z|jjzz }t|j|j|zt|j|j |zt|j|j|zt|j|j |zt|j|j|zt|j |j |zg}t!||j|ksJdS)Nr/r.rs)r*r_rrIr`rtr+rrrrr rrr rMr!rh)r(rNpO_pA_force_vecpA_pB_force_vecpB_pI_force_vecr[s rrpz/TestObstacleSetPathway.test_2D_pathway_to_loadsMsw 468dfh#6!7888  GS[[468+s46{{Q$&(.JJ     GS[[468+s46{{Q$&(.JJ   %TWdgtwHHq''A+DFH)<=1s46{{?Q&'' '! +dfh 61ggDF #q4DF a+@+@'@ADFHL M dfr!tm,,tvx7#dfr!tm:L:Ltvx:WW $'46O3 4 4 $'DF7_4 5 5 $'46O3 4 4 $'DF7_4 5 5 $'46O3 4 4 $'DF7_4 5 5  w//7788HDDDDDDr)r}r~rr)rr<rrr9r rrrWr\rrrbrfrjrlrnrprrrrrs;;; [& <<);<<< = BB)ABBB C HH)GHHH I NN)MNNN O  11\1 [ %++teeDkk2311 \ 1 [ 55;;d , U4[[$d , U4[[%%++t , ..\. + + + [=<);<<<0  CB)ABBB4  IH)GHHH8  "))#\$)V^D!!!"!111/// 4 4 4 ; ; ; G G GEEEEErrc eZdZdZejddZdZej de dfe de de d fgd Z e ej dd e dfe dd fgd Z d Zej dededededjgdZdZdZdZe dZej dddedz fdeddz ed dz dfdezdz fdeddeddz dfedz fgdZej dddeddezfdd efd defdeddz ed dz dfd!ezdz fdeddeddz dfedz fdededdzed eddzdfeded!dezdzzfdeddededdzdfededdezdzzfgd"Zej d#d$dededdzed eddzdffdeddededdzdffgd%Zej d#d$d&d'deddz ed dz dffdeddeddz dffdededdzed dz dffdeddeddz dffgd(Zej d)d*deddz ed dz dfdeddz eddz dfd+eddz z ed dz dffdeddeddz dfdeddz edd dfed dz edddz dfffd,Zej d)d*d-d.deddz ed dz dfd ed dz ed dz dfdeddz zeddz dffdeddeddz dfdeddz edd dfed dz edddz dffdeddz eddz dfdeddz ed dz dfed dz eddz dz dffd/deddz ed dz dfd0ezed1d2edzzzz dded1d2edzzzz fd0edzezded1d2edzzzzz d0edzezded1d2edzzzzz d3ed1d2edzzzz fd!eddzzezded1d2edzzzzz d!edzezded1d2edzzzzz dfffd4Z d5Z!d6Z"d7Z#d S)8TestWrappingPathwayc>tttsJdSr%)r&rr r's rr)z1TestWrappingPathway.test_is_pathway_base_subclassis/;7777777rTrFctd|_td|_tdd|_td|_t d|_|jj|_ t|j|j|_ t|j|j|j |_ t|j|j|j |_td|_dS) Nr*r+rT)positiverrIrM)r r*r+rrrrrIruaxrspherercylinderrrNrMr's r_wrapping_pathway_fixturez-TestWrappingPathway._wrapping_pathway_fixturels++++d+++++$$&($TVTW55 ($'BB &twGG rc4t|j|j|j}t |tsJt |dsJt |jdksJt |jdtsJ|jd|jksJt |jdtsJ|jd|jksJt |dsJt |j tsJ|j |jksJdS)Nr-r.rr/geometry) rr*r+rr0r1r2r-r rrrVs rr9z*TestWrappingPathway.test_valid_constructorys"47DGT]CC(O44444x/////8'((A----(.q1599999#A&$'1111(.q1599999#A&$'1111x,,,,,(+-ABBBBB DM111111rr-r*r+r:ctjt5tg||jR}ddddS#1swxYwYdSr%)r<r=rDrr)r(r-r@s rrzITestWrappingPathway.test_invalid_constructor_attachments_incorrect_numbers]9 % % = =< * * ) )$(DL ! ) ) ) ) ) ) ) ) ) ) ) ) ) ) ) ) ) )s 488cPd|jd}t|j|ksJdS)Nz!WrappingPathway(pA, pB, geometry=))rrYrNris rr\zTestWrappingPathway.test_reprsC +  + + + DL!!X------rcPtdt||DS)Nc3&K|] \}}||zV dSr%rrmagunits r z9TestWrappingPathway._expand_pos_to_vec..s*?? d3t8??????r)sumzip)posframes r_expand_pos_to_vecz&TestWrappingPathway._expand_pos_to_vecs'??s3??????rzpA_vec, pB_vec, factorr/rrrr/rr.rrrsr/c|||j}|||j}|j|j|j|z|j|j|j|zt|j|j|j}||jz}t|j |z dksJdSr) rrIr*r_rrr+rrrrar(pA_vecpB_vecfactorrNr[s r$test_static_pathway_on_sphere_lengthz8TestWrappingPathway.test_static_pathway_on_sphere_lengths((88((88 /// ///!$'47DK@@$&=122a777777rrrrrc|||j}|||j}|j|j|j|z|j|j|j|zt|j|j|j}|t|jdzz}t|j |z dksJdSrd) rrIr*r_rrr+rrrrrars r&test_static_pathway_on_cylinder_lengthz:TestWrappingPathway.test_static_pathway_on_cylinder_lengths*((88((88 /// ///!$'47DMBB$tvqy//)122a777777rzpA_vec, pB_vec)rrcl|||j}|||j}|j|j|j|z|j|j|j|zt|j|j|j}|j dksJdSr) rrIr*r_rrr+rrrer(rrrNs r0test_static_pathway_on_sphere_extension_velocityzDTestWrappingPathway.test_static_pathway_on_sphere_extension_velocitys((88((88 /// ///!$'47DK@@)Q......r)rr)rrcl|||j}|||j}|j|j|j|z|j|j|j|zt|j|j|j}|j dksJdSr) rrIr*r_rrr+rrrers r2test_static_pathway_on_cylinder_extension_velocityzFTestWrappingPathway.test_static_pathway_on_cylinder_extension_velocitys((88((88 /// ///!$'47DMBB)Q......rzApA_vec, pB_vec, pA_vec_expected, pB_vec_expected, pO_vec_expected)rrrr)rrrrc |||j}|||j}|j|j|j|z|j|j|j|zt|j|j|j}tdt||jD}tdt||jD}tdt||jD}t|j|j |jdzt|jdzz z|zt|j|j |jdzt|jdzz z|zt|j|j |jdzt|jdzz z|zg}||j |ksJdS)Nc3&K|] \}}||zV dSr%rrs rrzMTestWrappingPathway.test_static_pathway_on_sphere_to_loads..7;  $#tCH      rc3&K|] \}}||zV dSr%rrs rrzMTestWrappingPathway.test_static_pathway_on_sphere_to_loads..:rrc3&K|] \}}||zV dSr%rrs rrzMTestWrappingPathway.test_static_pathway_on_sphere_to_loads..=rrrr)rrIr*r_rrr+rrrrr rMrrh)r(rrpA_vec_expectedpB_vec_expectedpO_vec_expectedrNr[s r&test_static_pathway_on_sphere_to_loadsz:TestWrappingPathway.test_static_pathway_on_sphere_to_loadss8((88((88 /// ///!$'47DK@@  (+OTV(D(D       (+OTV(D(D       (+OTV(D(D      $'464619T$&!)__#<=oM N N $'464619T$&!)__#<=oM N N $'464619T$&!)__#<=oM N N  ''8333333r)rrrr)rr)rrrrrr)rr.r)r)rr/r/)rrr/)rrr)rrrc|||j}|||j}|j|j|j|z|j|j|j|zt|j|j|j}|j |||jz}|j |||jz}|j |||jz} t|j|t|j|t|j| g} t| |j | ksJdSr%) rrIr*r_rrr+rrrMr r!rh) r(rrrrrrNpA_force_expectedpB_force_expectedpO_force_expectedr[s r(test_static_pathway_on_cylinder_to_loadsz>      r)$r}r~rr)r<rrr9rrr rrrrrrrr`rrrr\rrrrrrrrrrrrrrrrrrgs* 888V^D!!!  "!  2 2 2 [ U4[[O U4[[%%++uuT{{ 3 === [ 55;;  U4[[$  ..\.222 [ F3KK N3   N3   N3   !  <<<222 )))...@@\@ [  2a4 ( aTT!WWHQJ2AbDF ; !QaA61 = 888 [  88Aq>>"#4 5  B ' B ' aTT!WWHQJ2AbDF ; !QaA61 =a!Q'$$q''((1a..)@!DQ((1a..+a//00  !Qa!Q!7;Q((1a..+a//00  (88)(8 [ " a!Q/$$q''((1a..1H!L M !Qa!Q)?C D /// [ " # # aTT!WWHQJ2 3 !QaA6 7 a!Q/$$q''!Q? @ !QaA6 7   //  / [K EaTT!WWHQJ*aDDGGAIq)dd1ggai$$q''!Q/  !QaA.aXXa^^OQ/$q''!XXa^^a/3   (44)(4< [K E E FaTT!WWHQJ*$q''!dd1ggXaZ+TT!WWQYQ 1-  !QaA.aXXa^^OQ/$q''!XXa^^a/3  aDDGGAIq)aTT!WWHQJ*$q''!TT!WWQY]A.   EaTT!WWHQJ*BttBBEM***Aqb2b!e8m1D1D/DEttAwwJrM1TT"r"a%x-%8%8#89ttAwwJrM1TT"r"a%x-%8%8#89ttBBEM*** ttAww{OB&$$rBr1uH}*=*=(=>dd1ggIbL!DDbQh$7$7"78 5) ,,ZEE[,,ZE6===III!!!!!rr)__doc__r<sympyrrrrrrsympy.physics.mechanicsr r r r r rrrrrrsympy.simplify.simplifyrr!r#rrrrrr sBB                           -,,,,,X9X9X9X9X9X9X9X9veEeEeEeEeEeEeEeEPL!L!L!L!L!L!L!L!L!L!r