diff --git a/.github/workflows/publish_package.yml b/.github/workflows/publish_package.yml index 1803ec3..2215726 100644 --- a/.github/workflows/publish_package.yml +++ b/.github/workflows/publish_package.yml @@ -15,8 +15,8 @@ on: jobs: deploy: - runs-on: ubuntu-latest - + runs-on: ubuntu-24.04 + env: working-directory: src/ diff --git a/examples/g_codes/g_code_logger_joint_positions.txt b/examples/g_codes/g_code_logger_joint_positions.txt index 167c9e1..48bfb35 100644 --- a/examples/g_codes/g_code_logger_joint_positions.txt +++ b/examples/g_codes/g_code_logger_joint_positions.txt @@ -1,83 +1,83 @@ %prefix -G1 RA1=0.244972669564758 RA2=0.118754015429859 RA3=-1.880942356805656 RA4=-3.14159265358 RA5=-1.142014751574451 RA6=0.244950201609223 -G1 RA1=0.244972669581823 RA2=0.118762307626707 RA3=-1.88094235680345 RA5=-1.14205892045918 RA6=0.244950202696079 -G1 RA2=0.118762307626713 RA5=-1.142058920459209 -G1 RA1=0.244985826248174 RA2=0.118749563611398 RA3=-1.880941312822245 RA5=-1.14203957928935 RA6=0.245008690304835 -G1 RA1=0.240780998868506 RA2=0.114215522144274 RA3=-1.879052769285107 RA4=-3.107674178952319 RA5=-1.141004309783504 RA6=0.226826211771184 -G1 RA1=0.236519401589246 RA2=0.109855519412232 RA3=-1.877157643136318 RA4=-3.073339493454699 RA5=-1.140532478545434 RA6=0.208544886685298 -G1 RA1=0.23225725131636 RA2=0.105654516578447 RA3=-1.875280060915837 RA4=-3.038930821538312 RA5=-1.140557924106639 RA6=0.190413989910428 -G1 RA1=0.227999527126608 RA2=0.101614954681469 RA3=-1.873423375719587 RA4=-3.004472573860054 RA5=-1.141100927653939 RA6=0.172458646788741 -G1 RA1=0.223750807025074 RA2=0.097731085338848 RA3=-1.871590409366141 RA4=-2.969982685912362 RA5=-1.142136627807536 RA6=0.154699222604455 -G1 RA1=0.219515701837699 RA2=0.093999767213729 RA3=-1.869783996480549 RA4=-2.935479594829957 RA5=-1.143654145254105 RA6=0.137155919539452 -G1 RA1=0.215298833843626 RA2=0.090417828726886 RA3=-1.868006769836642 RA4=-2.900980858167137 RA5=-1.145641963296637 RA6=0.119848025360505 -G1 RA1=0.21110482890575 RA2=0.086982039513516 RA3=-1.866261205054489 RA4=-2.866503068251733 RA5=-1.148087936471582 RA6=0.102793830633689 -G1 RA1=0.2069383096201 RA2=0.083689117219161 RA3=-1.864549615165003 RA4=-2.832061786184815 RA5=-1.150979345373504 RA6=0.086010567193094 -G1 RA1=0.202803888472961 RA2=0.080535734460962 RA3=-1.862874145153311 RA4=-2.797671488630156 RA5=-1.154302953376878 RA6=0.069514359229441 -G1 RA1=0.198691391920509 RA2=0.077511869608475 RA3=-1.861257173399351 RA4=-2.763291649230559 RA5=-1.158033009216723 RA6=0.053247965859043 -G1 RA1=0.194635321140016 RA2=0.074627001201723 RA3=-1.859661056811521 RA4=-2.729042566349626 RA5=-1.162178950990157 RA6=0.03737000220363 -G1 RA1=0.19062514227199 RA2=0.071871597536917 RA3=-1.858106173311185 RA4=-2.694881349859513 RA5=-1.166715067140832 RA6=0.021820900325552 -G1 RA1=0.186665306360183 RA2=0.069242130780751 RA3=-1.856594105551902 RA4=-2.660817658793678 RA5=-1.171626511752639 RA6=0.006611848418719 -G1 RA1=0.182760265062198 RA2=0.066735149019084 RA3=-1.855126090551275 RA4=-2.626860167881602 RA5=-1.176898317271221 RA6=-0.008246894270427 -G1 RA1=0.17891441873092 RA2=0.0643471942491 RA3=-1.853703174789525 RA4=-2.593016396801203 RA5=-1.182515334646528 RA6=-0.022746227168291 -G1 RA1=0.175132109700711 RA2=0.062074807635145 RA3=-1.85232621286303 RA4=-2.55929273286171 RA5=-1.188462283054435 RA6=-0.036878183929693 -G1 RA1=0.171417615912396 RA2=0.0599145352637 RA3=-1.850995865421367 RA4=-2.525694462237112 RA5=-1.194723797707115 RA6=-0.050635899548773 -G1 RA1=0.167775144647136 RA2=0.057862933715066 RA3=-1.849712597569668 RA4=-2.492225807558672 RA5=-1.201284474660063 RA6=-0.064013571604597 -G1 RA1=0.164208673225008 RA2=0.055914148539015 RA3=-1.848476677492925 RA4=-2.458880439085779 RA5=-1.208118510851359 RA6=-0.07701110143516 -G1 RA1=0.160722548406805 RA2=0.054069576131664 RA3=-1.847288211797064 RA4=-2.425679228917029 RA5=-1.215231403642337 RA6=-0.089615480154451 -G1 RA1=0.157320588022897 RA2=0.052323483228994 RA3=-1.846147000772995 RA4=-2.392614420764915 RA5=-1.222597445280085 RA6=-0.101828275521261 -G1 RA1=0.154006655249908 RA2=0.050672500417644 RA3=-1.845052752534138 RA4=-2.35968643335395 RA5=-1.230201434978425 RA6=-0.113647491141755 -G1 RA1=0.150784517155279 RA2=0.049113315291234 RA3=-1.844004941537835 RA4=-2.32689489750983 RA5=-1.238028331436282 RA6=-0.125071911479421 -G1 RA1=0.147664753951742 RA2=0.047651785207668 RA3=-1.842975576591826 RA4=-2.294277221864861 RA5=-1.246074005531303 RA6=-0.136046981957487 -G1 RA1=0.1446365746618 RA2=0.046266637001868 RA3=-1.842018099502119 RA4=-2.261753387651963 RA5=-1.254301959176775 RA6=-0.146682460903184 -G1 RA1=0.14171083577528 RA2=0.044963381641114 RA3=-1.841104735131218 RA4=-2.229360305485133 RA5=-1.262708515301841 RA6=-0.156924229501958 -G1 RA1=0.138890894081832 RA2=0.043739105004444 RA3=-1.840233790400904 RA4=-2.197095220617566 RA5=-1.271279610804619 RA6=-0.166773380269478 -G1 RA1=0.136179952508046 RA2=0.042590746457799 RA3=-1.839403744150921 RA4=-2.1649545635832 RA5=-1.28000120803616 RA6=-0.17623194125155 -G1 RA1=0.133581087925027 RA2=0.041515304726239 RA3=-1.838612886316188 RA4=-2.132934285384296 RA5=-1.288859508582757 RA6=-0.185302443458903 -G1 RA1=0.131097247748339 RA2=0.040509844165957 RA3=-1.837859315673382 RA4=-2.101029911858678 RA5=-1.29784096409378 RA6=-0.193987871931425 -G1 RA1=0.128731246596549 RA2=0.039571498395726 RA3=-1.837140941871221 RA4=-2.069236593595976 RA5=-1.306932282641979 RA6=-0.202291622940974 -G1 RA1=0.126485763180944 RA2=0.038697473785084 RA3=-1.836455487652834 RA4=-2.03754915378086 RA5=-1.316120433177686 RA6=-0.210217464095334 -G1 RA1=0.124363337396717 RA2=0.037885052759673 RA3=-1.835800491400746 RA4=-2.005962133726194 RA5=-1.325392648266055 RA6=-0.217769497694046 -G1 RA1=0.12236636764789 RA2=0.037131597010625 RA3=-1.835173309756006 RA4=-1.974469836328773 RA5=-1.33473642537916 RA6=-0.224952126990982 -G1 RA1=0.120497108366274 RA2=0.036434550540726 RA3=-1.834571120523836 RA4=-1.943066367207931 RA5=-1.344139526897249 RA6=-0.231770025739231 -G1 RA1=0.118757667738134 RA2=0.035791442591774 RA3=-1.83399092569932 RA4=-1.911745673650442 RA5=-1.353589979052745 RA6=-0.238228110825684 -G1 RA1=0.1171500056194 RA2=0.035199890418375 RA3=-1.83342955475628 RA4=-1.880501581394811 RA5=-1.36307606998874 RA6=-0.244331518041832 -G1 RA1=0.115675931645827 RA2=0.034657601958651 RA3=-1.832883668035448 RA4=-1.849327829397941 RA5=-1.372586347147862 RA6=-0.250085580773161 -G1 RA1=0.11433710351826 RA2=0.034162378369036 RA3=-1.832349760334042 RA4=-1.818218102561102 RA5=-1.382109614154101 RA6=-0.255495811685468 -G1 RA1=0.113135025462166 RA2=0.033712116412388 RA3=-1.831824164717897 RA4=-1.787166062546956 RA5=-1.39163492735181 RA6=-0.26056788726893 -G1 RA1=0.112071046859696 RA2=0.033304810761704 RA3=-1.8313030564126 RA4=-1.756165376934149 RA5=-1.401151592200256 RA6=-0.265307634915443 -G1 RA1=0.111146361041346 RA2=0.032938556161328 RA3=-1.830782456901525 RA4=-1.72520974666599 RA5=-1.410649159642041 RA6=-0.269721022604466 -G1 RA1=0.110362004231801 RA2=0.032611549476694 RA3=-1.830258238187188 RA4=-1.694292931888085 RA5=-1.420117422613845 RA6=-0.273814151050061 -G1 RA1=0.109718854652801 RA2=0.032322091629478 RA3=-1.829726127175504 RA4=-1.663408776504252 RA5=-1.429546412822054 RA6=-0.277593247905438 -G1 RA1=0.109218059271148 RA2=0.032077935814785 RA3=-1.829152997138278 RA4=-1.632581029813354 RA5=-1.438924990097934 RA6=-0.281019314671494 -G1 RA1=0.108859010180371 RA2=0.031858944378457 RA3=-1.828592222758533 RA4=-1.60174309585529 RA5=-1.448246141315815 RA6=-0.284191176866223 -G1 RA1=0.108642888047821 RA2=0.031672736136395 RA3=-1.828010409794779 RA4=-1.570919882574621 RA5=-1.457499418167078 RA6=-0.287068787806445 -G1 RA1=0.108569914700305 RA2=0.031518343856847 RA3=-1.827402256131846 RA4=-1.540106052480069 RA5=-1.466675921753609 RA6=-0.289658444047592 -G1 RA1=0.108569914700252 RA2=0.031518343856765 RA3=-1.827402256131418 RA4=-1.540106052458461 RA5=-1.466675921760156 RA6=-0.289658444049387 -G1 RA1=0.108568540175154 RA2=0.031511627156124 RA3=-1.827424519726627 RA4=-1.540090341074667 RA5=-1.466685348266437 RA6=-0.28969347052374 -G1 RA1=0.108028272666835 RA2=0.041069133039265 RA3=-1.817662042922318 RA4=-1.540216940214787 RA5=-1.467168819307615 RA6=-0.289508873283653 -G1 RA1=0.107485200802372 RA2=0.050708318542459 RA3=-1.80771111515635 RA4=-1.540413885654317 RA5=-1.467690184531448 RA6=-0.289174038248974 -G1 RA1=0.106949197068518 RA2=0.060319020245121 RA3=-1.797732372432674 RA4=-1.540606224907371 RA5=-1.468193368718576 RA6=-0.288788268433424 -G1 RA1=0.106418501333358 RA2=0.06992457048386 RA3=-1.787695378830874 RA4=-1.540803131915765 RA5=-1.468689666559673 RA6=-0.288339078899164 -G1 RA1=0.105893073693712 RA2=0.079523900628348 RA3=-1.77759909821255 RA4=-1.540994685397871 RA5=-1.469173256481098 RA6=-0.28782923014055 -G1 RA1=0.105372748820846 RA2=0.089122272991963 RA3=-1.767442609887745 RA4=-1.541200333796917 RA5=-1.469655998704715 RA6=-0.287253433381094 -G1 RA1=0.104857494017235 RA2=0.098718649784203 RA3=-1.757224898137365 RA4=-1.541410386261905 RA5=-1.470132235923586 RA6=-0.286614374406774 -G1 RA1=0.104347237002117 RA2=0.108314064737063 RA3=-1.746944970200047 RA4=-1.541624771203184 RA5=-1.470602069618769 RA6=-0.285912102478056 -G1 RA1=0.103841905729699 RA2=0.11790958062087 RA3=-1.736601776246177 RA4=-1.54184342878806 RA5=-1.471065606503362 RA6=-0.285146632641029 +G1 RA1=0.244972671629607 RA2=0.118754008886935 RA3=-1.880942366021997 RA4=-3.14159265358 RA5=-1.142014743628623 RA6=0.244950210727371 +G1 RA1=0.244972671646672 RA2=0.118762301083612 RA3=-1.880942366019792 RA5=-1.14205891251267 RA6=0.244950211814227 +G1 RA2=0.118762301083617 RA5=-1.1420589125127 +G1 RA1=0.244985827134098 RA2=0.118749559968514 RA3=-1.880941317903654 RA5=-1.142039575010788 RA6=0.245008694242169 +G1 RA1=0.240780998868023 RA2=0.114215522112477 RA3=-1.879052769328737 RA4=-3.107674178948048 RA5=-1.141004309748286 RA6=0.226826211766237 +G1 RA1=0.236519401589919 RA2=0.10985551941202 RA3=-1.877157643136604 RA4=-3.073339493462164 RA5=-1.140532478545409 RA6=0.208544886693561 +G1 RA1=0.232257251315727 RA2=0.105654516578389 RA3=-1.875280060915931 RA4=-3.03893082153109 RA5=-1.140557924106361 RA6=0.190413989902467 +G1 RA1=0.227999527125855 RA2=0.101614954681381 RA3=-1.873423375719791 RA4=-3.004472573853905 RA5=-1.141100927653653 RA6=0.172458646781669 +G1 RA1=0.223750807024426 RA2=0.097731085338761 RA3=-1.871590409366353 RA4=-2.969982685907054 RA5=-1.142136627807264 RA6=0.154699222598359 +G1 RA1=0.219515701837101 RA2=0.093999767213895 RA3=-1.869783996480821 RA4=-2.935479594832102 RA5=-1.143654145254216 RA6=0.137155919541414 +G1 RA1=0.215298833843839 RA2=0.09041782872692 RA3=-1.868006769836549 RA4=-2.900980858168908 RA5=-1.145641963296742 RA6=0.119848025362533 +G1 RA1=0.211104828903996 RA2=0.086982039513541 RA3=-1.866261205055481 RA4=-2.866503068246785 RA5=-1.148087936471213 RA6=0.102793830627032 +G1 RA1=0.206938309621494 RA2=0.083689117219164 RA3=-1.864549615163943 RA4=-2.832061786186431 RA5=-1.150979345373791 RA6=0.086010567196149 +G1 RA1=0.202803888473148 RA2=0.080535734461005 RA3=-1.862874145153192 RA4=-2.79767148863175 RA5=-1.154302953376996 RA6=0.069514359231259 +G1 RA1=0.198691391920677 RA2=0.0775118696083 RA3=-1.861257173399183 RA4=-2.763291649228593 RA5=-1.158033009216501 RA6=0.053247965857065 +G1 RA1=0.194635321139827 RA2=0.074627001201663 RA3=-1.859661056811678 RA4=-2.729042566347977 RA5=-1.162178950989964 RA6=0.037370002201744 +G1 RA1=0.190625142271831 RA2=0.071871597536862 RA3=-1.858106173311327 RA4=-2.694881349858107 RA5=-1.166715067140661 RA6=0.021820900323947 +G1 RA1=0.18666530636005 RA2=0.069242130780703 RA3=-1.856594105552031 RA4=-2.660817658792484 RA5=-1.171626511752488 RA6=0.006611848417355 +G1 RA1=0.182760265062097 RA2=0.066735149019311 RA3=-1.85512609055145 RA4=-2.626860167883902 RA5=-1.176898317271526 RA6=-0.008246894268096 +G1 RA1=0.178914418731142 RA2=0.064347194249463 RA3=-1.853703174789356 RA4=-2.593016396806419 RA5=-1.182515334647233 RA6=-0.022746227162644 +G1 RA1=0.175132109701151 RA2=0.062074807635056 RA3=-1.85232621286242 RA4=-2.559292732862757 RA5=-1.18846228305457 RA6=-0.036878183928197 +G1 RA1=0.17141761591249 RA2=0.059914535263743 RA3=-1.850995865421248 RA4=-2.525694462238014 RA5=-1.194723797707242 RA6=-0.050635899547746 +G1 RA1=0.167775144647246 RA2=0.057862933715404 RA3=-1.849712597569628 RA4=-2.492225807562713 RA5=-1.201284474660651 RA6=-0.064013571600312 +G1 RA1=0.164208673224545 RA2=0.055914148538789 RA3=-1.848476677493702 RA4=-2.458880439082615 RA5=-1.208118510850893 RA6=-0.077011101439078 +G1 RA1=0.160722548406547 RA2=0.054069576131529 RA3=-1.847288211797458 RA4=-2.425679228914382 RA5=-1.215231403641953 RA6=-0.089615480157462 +G1 RA1=0.157320588022691 RA2=0.052323483228883 RA3=-1.846147000773326 RA4=-2.392614420762752 RA5=-1.22259744527977 RA6=-0.101828275523721 +G1 RA1=0.154006655249745 RA2=0.050672500417551 RA3=-1.845052752534417 RA4=-2.359686433352192 RA5=-1.230201434978168 RA6=-0.113647491143754 +G1 RA1=0.150784517155683 RA2=0.049113315290794 RA3=-1.844004941536791 RA4=-2.326894897508505 RA5=-1.238028331435967 RA6=-0.125071911480152 +G1 RA1=0.147664753951652 RA2=0.047651785207632 RA3=-1.842975576591973 RA4=-2.294277221863866 RA5=-1.246074005531225 RA6=-0.136046981958614 +G1 RA1=0.14463657466173 RA2=0.046266637001837 RA3=-1.842018099502244 RA4=-2.261753387651154 RA5=-1.254301959176711 RA6=-0.1466824609041 +G1 RA1=0.141710835775225 RA2=0.044963381641088 RA3=-1.841104735131321 RA4=-2.229360305484479 RA5=-1.262708515301791 RA6=-0.156924229502698 +G1 RA1=0.13889089408179 RA2=0.043739105004423 RA3=-1.840233790400989 RA4=-2.19709522061704 RA5=-1.271279610804581 RA6=-0.166773380270073 +G1 RA1=0.136179952508013 RA2=0.042590746457782 RA3=-1.839403744150991 RA4=-2.16495456358278 RA5=-1.280001208036131 RA6=-0.176231941252026 +G1 RA1=0.133581087925003 RA2=0.041515304726226 RA3=-1.838612886316245 RA4=-2.132934285383961 RA5=-1.288859508582735 RA6=-0.185302443459281 +G1 RA1=0.13109724774832 RA2=0.040509844165947 RA3=-1.837859315673427 RA4=-2.101029911858414 RA5=-1.297840964093764 RA6=-0.193987871931724 +G1 RA1=0.128731246596534 RA2=0.039571498395719 RA3=-1.837140941871256 RA4=-2.069236593595768 RA5=-1.306932282641968 RA6=-0.202291622941209 +G1 RA1=0.126485763180934 RA2=0.038697473785079 RA3=-1.836455487652862 RA4=-2.037549153780697 RA5=-1.316120433177677 RA6=-0.210217464095519 +G1 RA1=0.124363337396709 RA2=0.037885052759668 RA3=-1.835800491400768 RA4=-2.005962133726067 RA5=-1.325392648266049 RA6=-0.21776949769419 +G1 RA1=0.122366367647985 RA2=0.037131597010887 RA3=-1.835173309756073 RA4=-1.97446983633195 RA5=-1.334736425379359 RA6=-0.224952126987744 +G1 RA1=0.120497108366084 RA2=0.036434550540704 RA3=-1.834571120524214 RA4=-1.943066367203592 RA5=-1.344139526897104 RA6=-0.23177002574384 +G1 RA1=0.118757667738362 RA2=0.035791442591959 RA3=-1.833990925698351 RA4=-1.911745673653427 RA5=-1.353589979052741 RA6=-0.238228110821959 +G1 RA1=0.117150005619621 RA2=0.035199890418704 RA3=-1.833429554755883 RA4=-1.880501581400423 RA5=-1.363076069988943 RA6=-0.244331518035863 +G1 RA1=0.115675931646126 RA2=0.034657601959028 RA3=-1.832883668034694 RA4=-1.849327829405449 RA5=-1.372586347148052 RA6=-0.25008558076507 +G1 RA1=0.114337103518178 RA2=0.034162378368928 RA3=-1.832349760334581 RA4=-1.818218102560501 RA5=-1.382109614154201 RA6=-0.255495811686499 +G1 RA1=0.113135025462039 RA2=0.033712116412137 RA3=-1.831824164717966 RA4=-1.787166062543168 RA5=-1.391634927351709 RA6=-0.260567887272826 +G1 RA1=0.112071046859694 RA2=0.033304810761449 RA3=-1.831303056412252 RA4=-1.756165376934822 RA5=-1.401151592200168 RA6=-0.26530763491464 +G1 RA1=0.11114636104129 RA2=0.032938556161525 RA3=-1.830782456902304 RA4=-1.725209746663015 RA5=-1.410649159642181 RA6=-0.269721022607821 +G1 RA1=0.110362004231955 RA2=0.032611549477066 RA3=-1.830258238187648 RA4=-1.694292931892433 RA5=-1.420117422613984 RA6=-0.273814151045913 +G1 RA1=0.109718854652807 RA2=0.032322091629511 RA3=-1.829726127176531 RA4=-1.663408776504478 RA5=-1.429546412822316 RA6=-0.277593247905852 +G1 RA1=0.10921805927115 RA2=0.03207793581478 RA3=-1.829152997138272 RA4=-1.632581029813425 RA5=-1.438924990097923 RA6=-0.281019314671423 +G1 RA1=0.108859010180372 RA2=0.031858944378457 RA3=-1.828592222758524 RA4=-1.601743095855346 RA5=-1.448246141315807 RA6=-0.284191176866167 +G1 RA1=0.108642888047821 RA2=0.031672736136395 RA3=-1.828010409794772 RA4=-1.570919882574665 RA5=-1.457499418167072 RA6=-0.287068787806401 +G1 RA1=0.108569914700305 RA2=0.031518343856847 RA3=-1.827402256131841 RA4=-1.540106052480104 RA5=-1.466675921753604 RA6=-0.289658444047558 +G1 RA1=0.108569914700253 RA2=0.031518343856766 RA3=-1.827402256131413 RA4=-1.540106052458496 RA5=-1.466675921760151 RA6=-0.289658444049354 +G1 RA1=0.108568540175155 RA2=0.031511627156125 RA3=-1.82742451972662 RA4=-1.540090341074701 RA5=-1.466685348266439 RA6=-0.289693470523708 +G1 RA1=0.108028272666835 RA2=0.04106913303927 RA3=-1.817662042922312 RA4=-1.540216940214799 RA5=-1.46716881930763 RA6=-0.289508873283644 +G1 RA1=0.107485200802372 RA2=0.050708318542461 RA3=-1.807711115156348 RA4=-1.540413885654322 RA5=-1.467690184531453 RA6=-0.28917403824897 +G1 RA1=0.106949197068518 RA2=0.060319020245122 RA3=-1.797732372432672 RA4=-1.540606224907373 RA5=-1.468193368718579 RA6=-0.288788268433422 +G1 RA1=0.106418501333358 RA2=0.069924570483861 RA3=-1.787695378830873 RA4=-1.540803131915767 RA5=-1.468689666559675 RA6=-0.288339078899163 +G1 RA1=0.105893073693711 RA2=0.079523900628348 RA3=-1.77759909821255 RA4=-1.540994685397872 RA5=-1.4691732564811 RA6=-0.287829230140549 +G1 RA1=0.105372748820846 RA2=0.089122272991963 RA3=-1.767442609887744 RA4=-1.541200333796917 RA5=-1.469655998704715 RA6=-0.287253433381093 +G1 RA1=0.104857494017235 RA2=0.098718649784203 RA3=-1.757224898137366 RA4=-1.541410386261905 RA5=-1.470132235923586 RA6=-0.286614374406774 +G1 RA1=0.104347237002117 RA2=0.108314064737062 RA3=-1.746944970200047 RA4=-1.541624771203184 RA5=-1.47060206961877 RA6=-0.285912102478055 +G1 RA1=0.103841905729699 RA2=0.117909580620869 RA3=-1.736601776246177 RA4=-1.54184342878806 RA5=-1.471065606503362 RA6=-0.28514663264103 G1 RA1=0.103341429527501 RA2=0.127506277308229 RA3=-1.726194226985831 RA4=-1.542066302943551 RA5=-1.471522951817845 RA6=-0.284317957284165 -G1 RA1=0.102845739066685 RA2=0.137105252724296 RA3=-1.715721192133479 RA4=-1.542293341281892 RA5=-1.471974209316052 RA6=-0.283426045593692 -G1 RA1=0.1023547663335 RA2=0.146707623924052 RA3=-1.705181498680907 RA4=-1.542524495076072 RA5=-1.472419481294021 RA6=-0.282470842906996 -G1 RA1=0.10186848348858 RA2=0.156312495820588 RA3=-1.694573929058221 RA4=-1.54275089309111 RA5=-1.472853183949914 RA6=-0.281454777084991 -G1 RA1=0.10138674923626 RA2=0.165925003444792 RA3=-1.683897242575657 RA4=-1.542989883318442 RA5=-1.473286568676662 RA6=-0.28037282657133 -G1 RA1=0.100909535231421 RA2=0.175544396302914 RA3=-1.673150081415612 RA4=-1.543232865924634 RA5=-1.473714265912927 RA6=-0.279227260464609 -G1 RA1=0.100436779443256 RA2=0.185171865528247 RA3=-1.662331103851648 RA4=-1.543479798551362 RA5=-1.474136365256741 RA6=-0.278017934691448 +G1 RA1=0.102845739066685 RA2=0.137105252724296 RA3=-1.71572119213348 RA4=-1.542293341281891 RA5=-1.471974209316052 RA6=-0.283426045593692 +G1 RA1=0.1023547663335 RA2=0.146707623924052 RA3=-1.705181498680907 RA4=-1.542524495076071 RA5=-1.472419481294021 RA6=-0.282470842906996 +G1 RA1=0.10186848348858 RA2=0.156312495820589 RA3=-1.69457392905822 RA4=-1.54275089309111 RA5=-1.472853183949913 RA6=-0.281454777084991 +G1 RA1=0.101386749236259 RA2=0.165925003444792 RA3=-1.683897242575657 RA4=-1.542989883318442 RA5=-1.473286568676662 RA6=-0.28037282657133 +G1 RA1=0.100909535231421 RA2=0.175544396302913 RA3=-1.673150081415612 RA4=-1.543232865924634 RA5=-1.473714265912928 RA6=-0.279227260464609 +G1 RA1=0.100436779443256 RA2=0.185171865528246 RA3=-1.662331103851649 RA4=-1.543479798551362 RA5=-1.474136365256741 RA6=-0.278017934691448 G1 RA1=0.09996841998588 RA2=0.194808642164244 RA3=-1.651438893430105 RA4=-1.543730650358661 RA5=-1.474552961806866 RA6=-0.276744664221622 G1 RA1=0.099504396112556 RA2=0.204455984434026 RA3=-1.640471977929671 RA4=-1.543985394136976 RA5=-1.474964149365797 RA6=-0.275407235208748 -G1 RA1=0.099044648192832 RA2=0.214115179284648 RA3=-1.629428826821296 RA4=-1.544244006296868 RA5=-1.475370020448979 RA6=-0.274005404016239 +G1 RA1=0.099044648192832 RA2=0.214115179284648 RA3=-1.629428826821296 RA4=-1.544244006296868 RA5=-1.475370020448978 RA6=-0.27400540401624 G1 RA1=0.098589151522017 RA2=0.2237856118428 RA3=-1.618307848424801 RA4=-1.544498491786568 RA5=-1.47576524032245 RA6=-0.272541233386644 G1 RA1=0.098137782791145 RA2=0.233472402918571 RA3=-1.607107413807405 RA4=-1.544764527234336 RA5=-1.47616053037783 RA6=-0.27100984287837 -G1 RA1=0.097689468783562 RA2=0.243198435934657 RA3=-1.595791087333867 RA4=-1.545043875598899 RA5=-1.476560280984482 RA6=-0.269394851831479 -G1 RA1=0.097246236805528 RA2=0.252919508890732 RA3=-1.584424747912498 RA4=-1.54531786718198 RA5=-1.476945873889738 RA6=-0.267731530044848 -G1 RA1=0.096807002942147 RA2=0.26265931151495 RA3=-1.572973609346818 RA4=-1.545595622289252 RA5=-1.477326557793045 RA6=-0.266002150142296 +G1 RA1=0.097689468783562 RA2=0.243198435934657 RA3=-1.595791087333867 RA4=-1.545043875598899 RA5=-1.476560280984482 RA6=-0.26939485183148 +G1 RA1=0.097246236805528 RA2=0.252919508890732 RA3=-1.584424747912498 RA4=-1.54531786718198 RA5=-1.476945873889737 RA6=-0.267731530044848 +G1 RA1=0.096807002942147 RA2=0.26265931151495 RA3=-1.572973609346819 RA4=-1.545595622289252 RA5=-1.477326557793045 RA6=-0.266002150142296 G1 RA1=0.096371737758398 RA2=0.272417687849702 RA3=-1.561435556487595 RA4=-1.545870182608532 RA5=-1.477697507814504 RA6=-0.264208278419541 G1 RA1=0.095940332203346 RA2=0.282199633741992 RA3=-1.549808568474359 RA4=-1.546155320025398 RA5=-1.478068506392115 RA6=-0.262345301562131 G1 RA1=0.095512761892187 RA2=0.292005070030603 RA3=-1.538090487140829 RA4=-1.546444275273107 RA5=-1.478434890082817 RA6=-0.260414695926936 @@ -124,55 +124,55 @@ G1 RA1=0.098600832493184 RA2=0.402879270357151 RA3=-1.411172699006026 RA4=-1.900 G1 RA1=0.099848747801863 RA2=0.396097627843388 RA3=-1.420585711016674 RA4=-1.922448160452515 RA5=-1.389457023715924 RA6=-0.194245972126746 G1 RA1=0.101158865668183 RA2=0.389362496190781 RA3=-1.429946215175516 RA4=-1.94415441858444 RA5=-1.383027259362364 RA6=-0.192174269159149 G1 RA1=0.102531181848894 RA2=0.382674302043998 RA3=-1.439256005424227 RA4=-1.965879581443216 RA5=-1.376548917032503 RA6=-0.189882403213848 -G1 RA1=0.103965667266765 RA2=0.376033108120881 RA3=-1.448517453264673 RA4=-1.987626471871406 RA5=-1.370028174483788 RA6=-0.187367566921975 -G1 RA1=0.105462313884601 RA2=0.369440597673444 RA3=-1.457732847455228 RA4=-2.009403683030275 RA5=-1.363476409207624 RA6=-0.184624633786305 -G1 RA1=0.107020927547641 RA2=0.362893799195274 RA3=-1.466904332361578 RA4=-2.031202461569144 RA5=-1.356889518998552 RA6=-0.181655739028273 -G1 RA1=0.108641436891666 RA2=0.356394503207393 RA3=-1.476034122371759 RA4=-2.053031582168358 RA5=-1.350279070530467 RA6=-0.17845590293899 -G1 RA1=0.110323687565094 RA2=0.349943037405806 RA3=-1.485124186212568 RA4=-2.074893967111956 RA5=-1.343651496202024 RA6=-0.175022572757914 -G1 RA1=0.112067493603458 RA2=0.343539751258269 RA3=-1.494176461493046 RA4=-2.096792520874317 RA5=-1.337013269830652 RA6=-0.171353303975243 -G1 RA1=0.113872707104124 RA2=0.337186753346648 RA3=-1.503192805893077 RA4=-2.118736488049544 RA5=-1.330376896329933 RA6=-0.167442976843243 -G1 RA1=0.115736880316188 RA2=0.330906175495245 RA3=-1.512141310019063 RA4=-2.140715849031007 RA5=-1.323756947635581 RA6=-0.163292222130421 -G1 RA1=0.117663804712052 RA2=0.324649519345008 RA3=-1.521091970737827 RA4=-2.162739310072831 RA5=-1.317126070283214 RA6=-0.158902587232352 -G1 RA1=0.11965121359002 RA2=0.318442523368851 RA3=-1.530012470644697 RA4=-2.184810091852885 RA5=-1.310510726064256 RA6=-0.154268499400517 -G1 RA1=0.121698755104553 RA2=0.312286243402493 RA3=-1.538903651544442 RA4=-2.206931148553958 RA5=-1.303918089232179 RA6=-0.149387640375843 -G1 RA1=0.123806048755947 RA2=0.306181327298643 RA3=-1.547766985443652 RA4=-2.229105153191446 RA5=-1.297355048350899 RA6=-0.144258161291809 -G1 RA1=0.125972768178539 RA2=0.30013028443441 RA3=-1.556603891436543 RA4=-2.251341445428072 RA5=-1.290835454033222 RA6=-0.138875264033609 -G1 RA1=0.128198293280725 RA2=0.294130172338627 RA3=-1.565415634782676 RA4=-2.273628788140144 RA5=-1.284352426751846 RA6=-0.133243583807346 -G1 RA1=0.130482231234914 RA2=0.28818353457239 RA3=-1.574203678152843 RA4=-2.295976499269321 RA5=-1.277919995752838 RA6=-0.127358623633562 -G1 RA1=0.132824071599154 RA2=0.282291226392574 RA3=-1.582969156508394 RA4=-2.318386870163302 RA5=-1.271545358127976 RA6=-0.121219061538874 -G1 RA1=0.135223270107187 RA2=0.276454087519355 RA3=-1.591713227498356 RA4=-2.340862018752697 RA5=-1.265235726894381 RA6=-0.114823797106685 -G1 RA1=0.137679249781586 RA2=0.270672992877671 RA3=-1.60043698908874 RA4=-2.363403907810866 RA5=-1.258998374637894 RA6=-0.108171912818975 -G1 RA1=0.140191401031324 RA2=0.264948852550678 RA3=-1.609141479029287 RA4=-2.38601432978862 RA5=-1.252840631410382 RA6=-0.101262685687997 -G1 RA1=0.142759178032051 RA2=0.259284534924607 RA3=-1.617827676030038 RA4=-2.408701617555792 RA5=-1.246777843321205 RA6=-0.094092355416759 -G1 RA1=0.145381712856759 RA2=0.253677154588005 RA3=-1.626496418647463 RA4=-2.431453446175728 RA5=-1.240801402824738 RA6=-0.086667193992559 -G1 RA1=0.148058393497644 RA2=0.248129606977261 RA3=-1.635148738121797 RA4=-2.454277965691159 RA5=-1.234926811909792 RA6=-0.078983874183845 -G1 RA1=0.150788483689853 RA2=0.242642983652518 RA3=-1.643785359053817 RA4=-2.47717616100658 RA5=-1.229161639051167 RA6=-0.071042533104624 -G1 RA1=0.153571213045953 RA2=0.23721835489659 RA3=-1.652407033339011 RA4=-2.500148727557186 RA5=-1.223513433805063 RA6=-0.06284362212148 -G1 RA1=0.156405779635841 RA2=0.231856818762062 RA3=-1.661014460528192 RA4=-2.523196090313107 RA5=-1.217989770073582 RA6=-0.054387870858486 -G1 RA1=0.159291451991414 RA2=0.226561634004206 RA3=-1.669608287284865 RA4=-2.546325122040433 RA5=-1.212607765712697 RA6=-0.045672918470827 -G1 RA1=0.162227161359256 RA2=0.22132966612394 RA3=-1.678189027885677 RA4=-2.569521850036695 RA5=-1.20735581654071 RA6=-0.036706976857949 -G1 RA1=0.16521211233872 RA2=0.216164196852158 RA3=-1.686757389802739 RA4=-2.592792830697419 RA5=-1.202251142025975 RA6=-0.027488243504254 -G1 RA1=0.168245383787703 RA2=0.211066481280197 RA3=-1.695313783614662 RA4=-2.616137284516419 RA5=-1.197301388629045 RA6=-0.01801864695956 -G1 RA1=0.171326021183609 RA2=0.206037747780008 RA3=-1.703858651467181 RA4=-2.639554044705407 RA5=-1.192514126213187 RA6=-0.008300512784204 -G1 RA1=0.174453040630138 RA2=0.201079247113586 RA3=-1.712392387951004 RA4=-2.663041580422885 RA5=-1.187896891314355 RA6=0.001663474515697 -G1 RA1=0.177625429368545 RA2=0.196192252412297 RA3=-1.720915339562303 RA4=-2.686597984249867 RA5=-1.18345717583282 RA6=0.011870260978821 -G1 RA1=0.180842146281267 RA2=0.191378058393333 RA3=-1.729427805344115 RA4=-2.710220960313246 RA5=-1.179202414256382 RA6=0.022316413089257 -G1 RA1=0.184102209068552 RA2=0.186640239987988 RA3=-1.737930037570523 RA4=-2.733913321697537 RA5=-1.175151000330846 RA6=0.033000992242736 -G1 RA1=0.187404346169959 RA2=0.181975582356605 RA3=-1.746422173987121 RA4=-2.757660551569229 RA5=-1.171287937632117 RA6=0.043913839992952 -G1 RA1=0.190747518962934 RA2=0.17738768687617 RA3=-1.754904515361997 RA4=-2.781465015934094 RA5=-1.16763159425254 RA6=0.055053355351022 -G1 RA1=0.194130583000473 RA2=0.172877972555033 RA3=-1.763377106205746 RA4=-2.805322808407681 RA5=-1.164189089333309 RA6=0.066414555950692 -G1 RA1=0.197552363978873 RA2=0.168447831152181 RA3=-1.771840016773175 RA4=-2.829229566025274 RA5=-1.160967358743941 RA6=0.07799201009571 -G1 RA1=0.201011662805709 RA2=0.164098670954574 RA3=-1.780293274327276 RA4=-2.853180499563758 RA5=-1.157973192057508 RA6=0.089779878001211 -G1 RA1=0.20450725635668 RA2=0.159831916709491 RA3=-1.78873686271658 RA4=-2.877170394844711 RA5=-1.155213217287971 RA6=0.101771914590538 -G1 RA1=0.208037962404171 RA2=0.155651559948272 RA3=-1.797170722834794 RA4=-2.901197597379075 RA5=-1.152707063376714 RA6=0.113963604622272 -G1 RA1=0.211602379820335 RA2=0.151553913986252 RA3=-1.805594691262075 RA4=-2.925247634784235 RA5=-1.150434332934732 RA6=0.126343422210488 -G1 RA1=0.215199281841838 RA2=0.14754299919648 RA3=-1.814008751049016 RA4=-2.949318480875973 RA5=-1.14841449454113 RA6=0.138906277691901 -G1 RA1=0.218827360892978 RA2=0.143620343070928 RA3=-1.822412652128848 RA4=-2.973403304558324 RA5=-1.146653423325526 RA6=0.15164440752236 -G1 RA1=0.222485285195027 RA2=0.139787443709348 RA3=-1.830806166966813 RA4=-2.997494868747124 RA5=-1.145156701198826 RA6=0.164549661579007 -G1 RA1=0.22617170368186 RA2=0.136045811521258 RA3=-1.839189027684956 RA4=-3.021585570041116 RA5=-1.143929653744741 RA6=0.177613550599841 -G1 RA1=0.229885246929045 RA2=0.132396969119591 RA3=-1.847560925711253 RA4=-3.045667457988086 RA5=-1.142977336361827 RA6=0.190827266673997 -G1 RA1=0.233624528069824 RA2=0.128842450758277 RA3=-1.855921512081748 RA4=-3.069732256699095 RA5=-1.142304520581658 RA6=0.204181706004522 -G1 RA1=0.237388143768705 RA2=0.125383801759181 RA3=-1.864270397753163 RA4=-3.093771388976514 RA5=-1.141915681192551 RA6=0.217667494194972 -G1 RA1=0.241174675159616 RA2=0.122022577955033 RA3=-1.872607153890842 RA4=-3.117776002543748 RA5=-1.14181498432681 RA6=0.231275013568937 -G1 RA1=0.244982688983104 RA2=0.118763069457143 RA3=-1.88093131215734 RA4=-3.14159265358 RA5=-1.142020787949005 RA6=0.244994436285743 +G1 RA1=0.103965667267105 RA2=0.376033108121906 RA3=-1.448517453264122 RA4=-1.987626471873229 RA5=-1.370028174489813 RA6=-0.187367566920808 +G1 RA1=0.105462313884214 RA2=0.369440597672547 RA3=-1.45773284745557 RA4=-2.009403683028436 RA5=-1.36347640920134 RA6=-0.184624633787392 +G1 RA1=0.10702092754721 RA2=0.362893799195137 RA3=-1.466904332361178 RA4=-2.031202461568367 RA5=-1.356889518996654 RA6=-0.181655739028742 +G1 RA1=0.108641436891327 RA2=0.356394503205651 RA3=-1.476034122372923 RA4=-2.053031582166157 RA5=-1.350279070519768 RA6=-0.178455902939934 +G1 RA1=0.110323687565104 RA2=0.349943037406388 RA3=-1.485124186212068 RA4=-2.074893967112669 RA5=-1.343651496206197 RA6=-0.175022572757692 +G1 RA1=0.112067493602832 RA2=0.343539751257964 RA3=-1.494176461492634 RA4=-2.096792520873199 RA5=-1.337013269826501 RA6=-0.171353303975756 +G1 RA1=0.113872707103987 RA2=0.337186753347578 RA3=-1.503192805892237 RA4=-2.11873648804952 RA5=-1.330376896334258 RA6=-0.167442976843834 +G1 RA1=0.115736880316021 RA2=0.330906175496276 RA3=-1.512141310018285 RA4=-2.140715849029821 RA5=-1.323756947640743 RA6=-0.163292222132295 +G1 RA1=0.117663804711759 RA2=0.324649519344759 RA3=-1.521091970737895 RA4=-2.162739310072927 RA5=-1.317126070280785 RA6=-0.158902587231986 +G1 RA1=0.119651213589707 RA2=0.318442523368893 RA3=-1.530012470644528 RA4=-2.184810091852714 RA5=-1.310510726063638 RA6=-0.154268499400622 +G1 RA1=0.12169875510455 RA2=0.312286243402498 RA3=-1.538903651544442 RA4=-2.20693114855394 RA5=-1.303918089232211 RA6=-0.149387640375868 +G1 RA1=0.123806048755946 RA2=0.306181327298647 RA3=-1.547766985443649 RA4=-2.229105153191436 RA5=-1.297355048350918 RA6=-0.144258161291822 +G1 RA1=0.12597276817884 RA2=0.300130284435584 RA3=-1.556603891435895 RA4=-2.251341445424931 RA5=-1.290835454040922 RA6=-0.138875264037887 +G1 RA1=0.128198293280476 RA2=0.294130172339317 RA3=-1.565415634782331 RA4=-2.273628788137908 RA5=-1.284352426755115 RA6=-0.133243583810259 +G1 RA1=0.13048223123477 RA2=0.288183534572194 RA3=-1.574203678152856 RA4=-2.295976499269985 RA5=-1.27791999575196 RA6=-0.127358623632669 +G1 RA1=0.132824071598529 RA2=0.282291226392707 RA3=-1.582969156508268 RA4=-2.318386870163227 RA5=-1.27154535812703 RA6=-0.121219061538946 +G1 RA1=0.135223270107088 RA2=0.276454087518863 RA3=-1.59171322749866 RA4=-2.340862018754954 RA5=-1.26523572689112 RA6=-0.114823797103893 +G1 RA1=0.137679249781675 RA2=0.270672992876874 RA3=-1.600436989089161 RA4=-2.363403907814926 RA5=-1.258998374633183 RA6=-0.108171912813989 +G1 RA1=0.140191401030921 RA2=0.264948852550813 RA3=-1.609141479029286 RA4=-2.38601432978866 RA5=-1.252840631409588 RA6=-0.101262685688035 +G1 RA1=0.142759178032061 RA2=0.259284534924589 RA3=-1.617827676030047 RA4=-2.408701617555883 RA5=-1.246777843321128 RA6=-0.094092355416649 +G1 RA1=0.145381712856766 RA2=0.253677154587994 RA3=-1.626496418647467 RA4=-2.431453446175798 RA5=-1.240801402824687 RA6=-0.086667193992475 +G1 RA1=0.148058393497589 RA2=0.248129606976906 RA3=-1.635148738122049 RA4=-2.454277965693944 RA5=-1.2349268119073 RA6=-0.078983874180598 +G1 RA1=0.150788483690144 RA2=0.242642983652551 RA3=-1.643785359053653 RA4=-2.477176161005996 RA5=-1.229161639052079 RA6=-0.071042533105198 +G1 RA1=0.15357121304585 RA2=0.23721835489635 RA3=-1.652407033339224 RA4=-2.500148727559556 RA5=-1.223513433803159 RA6=-0.062843622118782 +G1 RA1=0.156405779635462 RA2=0.231856818762609 RA3=-1.661014460527854 RA4=-2.523196090308936 RA5=-1.217989770076334 RA6=-0.054387870863327 +G1 RA1=0.159291451992466 RA2=0.226561634003972 RA3=-1.669608287284007 RA4=-2.546325122045427 RA5=-1.212607765711904 RA6=-0.045672918464419 +G1 RA1=0.162227161359727 RA2=0.22132966612381 RA3=-1.678189027885525 RA4=-2.569521850037632 RA5=-1.207355816540706 RA6=-0.036706976856681 +G1 RA1=0.165212112337996 RA2=0.216164196852539 RA3=-1.686757389802758 RA4=-2.592792830694881 RA5=-1.202251142026728 RA6=-0.027488243507452 +G1 RA1=0.168245383788127 RA2=0.21106648127998 RA3=-1.695313783614745 RA4=-2.616137284517607 RA5=-1.197301388628543 RA6=-0.018018646958132 +G1 RA1=0.171326021183719 RA2=0.206037747779927 RA3=-1.703858651467194 RA4=-2.639554044706387 RA5=-1.192514126212791 RA6=-0.008300512783055 +G1 RA1=0.174453040630229 RA2=0.201079247113523 RA3=-1.712392387951014 RA4=-2.663041580423687 RA5=-1.187896891314049 RA6=0.001663474516636 +G1 RA1=0.17762542936862 RA2=0.196192252412247 RA3=-1.720915339562311 RA4=-2.686597984250525 RA5=-1.183457175832581 RA6=0.011870260979591 +G1 RA1=0.180842146281329 RA2=0.191378058393294 RA3=-1.729427805344122 RA4=-2.710220960313788 RA5=-1.179202414256195 RA6=0.02231641308989 +G1 RA1=0.184102209068603 RA2=0.186640239987957 RA3=-1.737930037570528 RA4=-2.733913321697984 RA5=-1.175151000330698 RA6=0.033000992243256 +G1 RA1=0.187404346170002 RA2=0.18197558235658 RA3=-1.746422173987127 RA4=-2.757660551569597 RA5=-1.171287937631999 RA6=0.043913839993381 +G1 RA1=0.190747518962913 RA2=0.177387686875832 RA3=-1.754904515362262 RA4=-2.781465015940816 RA5=-1.167631594249908 RA6=0.055053355358445 +G1 RA1=0.194130582999785 RA2=0.172877972555547 RA3=-1.763377106205381 RA4=-2.805322808400054 RA5=-1.164189089336092 RA6=0.066414555942141 +G1 RA1=0.197552363978131 RA2=0.168447831152559 RA3=-1.771840016773066 RA4=-2.829229566019 RA5=-1.160967358745724 RA6=0.077992010088434 +G1 RA1=0.201011662805037 RA2=0.164098670954572 RA3=-1.780293274327397 RA4=-2.853180499565049 RA5=-1.157973192056635 RA6=0.089779878002304 +G1 RA1=0.204507256356797 RA2=0.159831916709433 RA3=-1.788736862716602 RA4=-2.877170394845694 RA5=-1.155213217287707 RA6=0.101771914591676 +G1 RA1=0.208037962404269 RA2=0.155651559948227 RA3=-1.797170722834811 RA4=-2.901197597379888 RA5=-1.152707063376503 RA6=0.113963604623213 +G1 RA1=0.211602379820416 RA2=0.151553913986215 RA3=-1.805594691262092 RA4=-2.925247634784906 RA5=-1.150434332934561 RA6=0.126343422211266 +G1 RA1=0.215199281841905 RA2=0.14754299919645 RA3=-1.81400875104903 RA4=-2.949318480876527 RA5=-1.148414494540992 RA6=0.138906277692543 +G1 RA1=0.218827360893034 RA2=0.143620343070903 RA3=-1.822412652128862 RA4=-2.973403304558781 RA5=-1.146653423325415 RA6=0.151644407522889 +G1 RA1=0.222485285195074 RA2=0.139787443709328 RA3=-1.830806166966826 RA4=-2.997494868747501 RA5=-1.145156701198736 RA6=0.164549661579443 +G1 RA1=0.226171703681898 RA2=0.136045811521242 RA3=-1.839189027684967 RA4=-3.021585570041425 RA5=-1.143929653744668 RA6=0.177613550600199 +G1 RA1=0.229885246929077 RA2=0.132396969119577 RA3=-1.847560925711263 RA4=-3.04566745798834 RA5=-1.142977336361768 RA6=0.190827266674291 +G1 RA1=0.23362452806985 RA2=0.128842450758266 RA3=-1.855921512081758 RA4=-3.069732256699301 RA5=-1.142304520581611 RA6=0.204181706004762 +G1 RA1=0.237388143768726 RA2=0.125383801759173 RA3=-1.86427039775317 RA4=-3.093771388976683 RA5=-1.141915681192513 RA6=0.217667494195168 +G1 RA1=0.241174675159633 RA2=0.122022577955026 RA3=-1.872607153890848 RA4=-3.117776002543884 RA5=-1.14181498432678 RA6=0.231275013569096 +G1 RA1=0.244982688983118 RA2=0.118763069457137 RA3=-1.880931312157347 RA4=-3.14159265358 RA5=-1.14202078794898 RA6=0.244994436285872 %postfix diff --git a/examples/g_codes/g_code_logger_robot_view.txt b/examples/g_codes/g_code_logger_robot_view.txt index 2910b88..1701589 100644 --- a/examples/g_codes/g_code_logger_robot_view.txt +++ b/examples/g_codes/g_code_logger_robot_view.txt @@ -1,82 +1,82 @@ %prefix -G1 X4.000012844955127 Y-6.000009525250678 Z1.499996011167368 A-1.570621383874621 B0.000114933736584 C0.000022466438922 -G1 X4.000003657177785 Y-6.000011822100228 Z1.499982732536868 A-1.570634106107133 B0.000165828821332 C0.000022463583131 -G1 X4.00000365717778 Y-6.000011822100229 Z1.499982732536858 A-1.570634106107143 B0.00016582882137 C0.000022463583131 -G1 X3.999998610754486 Y-5.999985125769192 Z1.500004684047642 A-1.570626079837758 B0.000133686625617 C-0.000022866156544 -G1 X3.999988486570375 Y-5.999961899206292 Z1.510195648463692 A-1.538852230095917 B0.000036320780687 C-0.000066107649057 -G1 X3.999989879154124 Y-5.999960762636877 Z1.520402626184503 A-1.506799077564654 B0.000041249913336 C-0.0000666454645 -G1 X3.999994613194489 Y-5.999959236250381 Z1.530614324975195 A-1.474739376020603 B0.000029561353463 C-0.000067210094153 -G1 X3.999996356579825 Y-5.999958843937966 Z1.540821441589196 A-1.442684275517028 B0.000034876337555 C-0.000066591369049 -G1 X3.999998163758146 Y-5.999958663370414 Z1.551028523026677 A-1.410628888219525 B0.000040221355939 C-0.000065566541092 -G1 X3.999999982012223 Y-5.999958703843768 Z1.56123551608593 A-1.378573198730758 B0.000045511308234 C-0.000064140250538 -G1 X4.000001793145238 Y-5.999958961933331 Z1.571442388052141 A-1.346517206842352 B0.000050691262878 C-0.000062324153821 -G1 X4.000003580215775 Y-5.99995943073889 Z1.581649108957969 A-1.31446091588563 B0.000055709667731 C-0.000060135481714 -G1 X4.000005327499469 Y-5.99996010029966 Z1.59185565193413 A-1.282404332368349 B0.000060518983449 C-0.000057596465364 -G1 X4.000007020677876 Y-5.999960957856004 Z1.602061993668175 A-1.250347465733831 B0.000065076361467 C-0.000054733721747 -G1 X4.000004110046022 Y-5.999981247309078 Z1.612252640389724 A-1.218243114838756 B0.000056528504482 C-0.000022094088294 -G1 X4.000005002552006 Y-5.999982012204597 Z1.622457458545497 A-1.186186208148577 B0.000058736823427 C-0.000019685902875 -G1 X4.000005914805569 Y-5.99998278275514 Z1.63266220850277 A-1.15412932931885 B0.000060822779112 C-0.000017226539994 -G1 X4.000006763231719 Y-5.999983617858271 Z1.642866803597786 A-1.122072322174675 B0.000062683651846 C-0.000014655944835 -G1 X4.000007543961723 Y-5.999984505544689 Z1.653071241978781 A-1.090015193859148 B0.000064308797579 C-0.000011996014926 -G1 X4.000008254871764 Y-5.999985433368074 Z1.663275524748931 A-1.057957952489724 B0.000065691440451 C-0.000009269036307 -G1 X4.000008894924667 Y-5.999986388986267 Z1.67347965532208 A-1.025900606192892 B0.000066827930135 C-0.00000649702628 -G1 X4.000009464094425 Y-5.999987360369831 Z1.683683639260582 A-0.993843162981551 B0.000067717553403 C-0.000003701430781 -G1 X4.000009963251718 Y-5.999988336046748 Z1.693887484000249 A-0.961785630497261 B0.00006836223177 C-0.000000902783792 -G1 X4.000012238102864 Y-5.999988007897881 Z1.704096432376584 A-0.9297195341113 B0.000056729950294 C-0.000003839466042 -G1 X4.000012559373492 Y-5.999988965230391 Z1.714300159633 A-0.897661567508178 B0.000056943753098 C-0.00000138658602 -G1 X4.000012835762649 Y-5.999989897273643 Z1.724503793963718 A-0.865603562480944 B0.000056967100845 C0.000001018048366 -G1 X4.000013050754112 Y-5.999990802085456 Z1.734707329891143 A-0.83354550766693 B0.000056790097594 C0.000003360787695 -G1 X4.000013207947788 Y-5.9999916730106 Z1.744910779222428 A-0.801487409286018 B0.000056423989271 C0.000005629081757 -G1 X4.000022702263883 Y-5.999982918149923 Z1.755134631169553 A-0.769461328712057 B0.000078820541469 C-0.000003153159002 -G1 X4.000023059200992 Y-5.999984341668369 Z1.765337911159957 A-0.737402131298066 B0.000078156685813 C0.000000140706694 -G1 X4.000023138218195 Y-5.999985753617028 Z1.775540894544785 A-0.705342702472538 B0.000077035242798 C0.000003279041882 -G1 X4.000023145916846 Y-5.999987090405659 Z1.785743792072294 A-0.673283268160023 B0.000075701030001 C0.000006250188919 -G1 X4.00002309068071 Y-5.99998834846385 Z1.795946622420037 A-0.641223838114606 B0.000074178520666 C0.000009048064172 -G1 X4.000022978313814 Y-5.999989526003385 Z1.806149401357308 A-0.609164419441687 B0.000072489530723 C0.00001166838541 -G1 X4.000022814397483 Y-5.999990622449203 Z1.816352143476806 A-0.577105018547127 B0.00007065521231 C0.000014108901774 -G1 X4.000022604252604 Y-5.999991638295189 Z1.826554862082827 A-0.545045641290313 B0.000068695917963 C0.000016369208575 -G1 X4.00002235291795 Y-5.999992574943711 Z1.836757569142929 A-0.512986293136039 B0.000066631106486 C0.000018450550619 -G1 X4.000022065093475 Y-5.999993434572566 Z1.846960275189264 A-0.480926979180854 B0.000064479185376 C0.000020355645896 -G1 X4.000021745142002 Y-5.99999421997449 Z1.857162989348458 A-0.448867704281203 B0.000062257477525 C0.000022088488063 -G1 X4.000021397066456 Y-5.999994934423894 Z1.867365719321675 A-0.41680847308991 B0.000059982158831 C0.000023654167004 -G1 X4.000021024522862 Y-5.999995581535518 Z1.877568471474845 A-0.384749290069669 B0.00005766820156 C0.000025058690254 -G1 X4.000020630806213 Y-5.999996165149728 Z1.887771250869122 A-0.352690159482865 B0.000055329341317 C0.000026308806706 -G1 X4.000020218875414 Y-5.99999668921533 Z1.897974061382498 A-0.320631085403442 B0.000052978093104 C0.000027411833973 -G1 X4.000019791374453 Y-5.99999715769362 Z1.908176905836415 A-0.288572071657049 B0.000050625732354 C0.000028375496484 -G1 X4.000019350630347 Y-5.999997574480483 Z1.9183797860972 A-0.256513121723161 B0.000048282266565 C0.000029207758403 -G1 X4.000018898690167 Y-5.999997943337304 Z1.928582703235056 A-0.224454238714808 B0.000045956501988 C0.000029916673301 -G1 X4.000018437335274 Y-5.999998267841222 Z1.938785657664062 A-0.192395425308481 B0.00004365607819 C0.000030510237901 -G1 X4.000017968101186 Y-5.999998551351794 Z1.948988649280736 A-0.160336683603924 B0.000041387459801 C0.000030996252507 -G1 X4.000017492290128 Y-5.999998796988876 Z1.959191677601879 A-0.128278015125791 B0.000039156057416 C0.000031382200013 -G1 X4.000028854410797 Y-5.999997400216755 Z1.969416349225056 A-0.096246081576313 B0.000059612352245 C0.000034922136685 -G1 X4.000028404927842 Y-5.999997962583903 Z1.979618856162187 A-0.064186703846719 B0.000056613289027 C0.000035722479838 -G1 X4.000027629288924 Y-5.999998465917391 Z1.989821171585395 A-0.032127257800692 B0.000053467188334 C0.000036237778934 -G1 X4.000026833228332 Y-5.999998914990695 Z2.000023528966181 A-0.000067933908393 B0.000050387544316 C0.000036625823697 -G1 X4.00002683322833 Y-5.99999891499081 Z2.000023528974101 A-0.000067933883369 B0.000050387544254 C0.000036625823789 -G1 X4.000016296116242 Y-6.000002745784036 Z2.000004646186802 A-0.000051949842288 B0.000032450544819 C0.000039830272632 -G1 X4.01015418657969 Y-5.999997861320239 Z1.999963004926101 A-0.000012399494473 B0.000000268888513 C-0.000001180989124 -G1 X4.020379409037495 Y-5.999998603316927 Z1.999970674123935 A-0.000017867028866 B0.000004969794515 C0.000011264271748 -G1 X4.030582758962658 Y-5.999998638193916 Z1.999969616281574 A-0.000018478009538 B0.000005182485909 C0.000012021399144 -G1 X4.040785971791061 Y-5.999998670131919 Z1.999968467009717 A-0.000019083170124 B0.000005379990043 C0.00001274982938 -G1 X4.050989666981597 Y-5.999998806232655 Z1.999972983313602 A-0.000011742531232 B0.000001709831024 C0.0000050173702 -G1 X4.06119278016927 Y-5.999998843101428 Z1.999971916041582 A-0.000012187966022 B0.000001800256728 C0.000005517179853 -G1 X4.071395852275462 Y-5.999998882563413 Z1.999970807229289 A-0.000012657848934 B0.000001905999737 C0.000006040951088 -G1 X4.081598867511149 Y-5.999998923930195 Z1.999969647101227 A-0.000013147683192 B0.000002022223277 C0.000006581547755 -G1 X4.091801822642847 Y-5.999998967379432 Z1.999968432753668 A-0.000013658032022 B0.000002149237332 C0.000007139378158 -G1 X4.102004714257792 Y-5.99999901310313 Z1.999967161141823 A-0.000014189496253 B0.000002287393848 C0.00000771486537 -G1 X4.112207538681496 Y-5.999999061311332 Z1.999965829039191 A-0.000014742687981 B0.000002437059139 C0.000008308397046 -G1 X4.1224102919528 Y-5.999999112227483 Z1.999964433018848 A-0.000015318232426 B0.000002598618593 C0.000008920328595 -G1 X4.132613439616955 Y-5.999999260460828 Z1.99996838407335 A-0.000008850805764 B-0.00000089511651 C0.00000168950071 -G1 X4.142816033741938 Y-5.999999320205514 Z1.999967042230502 A-0.000009267239043 B-0.000000848836814 C0.000002067154538 -G1 X4.153018557016288 Y-5.999999384121793 Z1.999965639902076 A-0.000009707246137 B-0.000000789270329 C0.00000246389898 +G1 X4.000012837708117 Y-6.000009522674565 Z1.499996008031959 A-1.570621382597067 B0.000114928621295 C0.00002245938577 +G1 X4.000003649930882 Y-6.00001181952411 Z1.499982729401721 A-1.570634104829836 B0.000165823705101 C0.000022456530043 +G1 X4.000003649930875 Y-6.000011819524111 Z1.49998272940171 A-1.570634104829846 B0.000165823705139 C0.000022456530044 +G1 X3.999998606844738 Y-5.999985124864064 Z1.500004682375699 A-1.570626079149392 B0.000133683870265 C-0.000022869207861 +G1 X3.999988486540103 Y-5.999961899213769 Z1.510195648450068 A-1.538852230087035 B0.000036320758651 C-0.000066107647436 +G1 X3.999989879153927 Y-5.999960762637462 Z1.520402626184299 A-1.506799077571304 B0.00004124991231 C-0.000066645469058 +G1 X3.99999461319445 Y-5.999959236249842 Z1.530614324975323 A-1.474739376014114 B0.000029561353988 C-0.000067210089767 +G1 X3.999996356579849 Y-5.999958843937958 Z1.540821441589231 A-1.442684275511498 B0.000034876337765 C-0.000066591365242 +G1 X3.999998163758169 Y-5.999958663370406 Z1.551028523026715 A-1.41062888821474 B0.000040221355911 C-0.000065566537824 +G1 X3.99999998201247 Y-5.999958703845528 Z1.56123551608515 A-1.378573198732802 B0.000045511308711 C-0.00006414025222 +G1 X4.000001793145227 Y-5.999958961933332 Z1.571442388052124 A-1.346517206843955 B0.000050691263023 C-0.000062324154892 +G1 X4.00000358021605 Y-5.999959430741288 Z1.581649108956754 A-1.314460915881318 B0.000055709667646 C-0.000060135478968 +G1 X4.000005327499345 Y-5.999960100297145 Z1.591855651935605 A-1.282404332369626 B0.000060518983251 C-0.000057596466222 +G1 X4.000007020677867 Y-5.999960957856002 Z1.602061993668157 A-1.250347465735278 B0.000065076361777 C-0.000054733722669 +G1 X4.000004110045986 Y-5.999981247308261 Z1.612252640390434 A-1.218243114836879 B0.000056528503618 C-0.000022094087053 +G1 X4.000005002552024 Y-5.9999820122046 Z1.622457458545516 A-1.186186208147077 B0.000058736822933 C-0.000019685901974 +G1 X4.000005914805586 Y-5.999982782755144 Z1.632662208502787 A-1.154129329317572 B0.000060822778642 C-0.000017226539248 +G1 X4.000006763231733 Y-5.999983617858275 Z1.642866803597803 A-1.122072322173589 B0.000062683651405 C-0.00001465594422 +G1 X4.000007543961704 Y-5.999984505545419 Z1.653071241977877 A-1.090015193861342 B0.000064308798928 C-0.000011996016163 +G1 X4.000008254871698 Y-5.999985433368765 Z1.663275524747946 A-1.057957952494569 B0.000065691443152 C-0.000009269038886 +G1 X4.000008894924663 Y-5.999986388985572 Z1.673479655323016 A-1.025900606193736 B0.000066827930164 C-0.000006497026697 +G1 X4.000009464094414 Y-5.999987360369825 Z1.683683639260568 A-0.993843162982369 B0.000067717553849 C-0.000003701431178 +G1 X4.000009963251632 Y-5.999988336047354 Z1.69388748399917 A-0.961785630501034 B0.000068362234358 C-0.000000902785526 +G1 X4.000012238102793 Y-5.999988007898231 Z1.704096432376241 A-0.929719534108477 B0.000056729948612 C-0.000003839464678 +G1 X4.000012559373533 Y-5.999988965230412 Z1.714300159633038 A-0.897661567505782 B0.000056943751578 C-0.000001386585021 +G1 X4.000012835762684 Y-5.999989897273663 Z1.72450379396375 A-0.865603562478988 B0.00005696709955 C0.000001018049135 +G1 X4.00001305075414 Y-5.999990802085472 Z1.734707329891168 A-0.833545507665342 B0.000056790096501 C0.000003360788279 +G1 X4.000013207947914 Y-5.999991673009516 Z1.744910779224973 A-0.801487409284592 B0.000056423987259 C0.000005629082049 +G1 X4.000022702263902 Y-5.999982918149933 Z1.755134631169571 A-0.769461328711149 B0.000078820540856 C-0.000003153158676 +G1 X4.000023059201005 Y-5.999984341668378 Z1.765337911159971 A-0.737402131297328 B0.000078156685295 C0.00000014070694 +G1 X4.000023138218205 Y-5.999985753617036 Z1.775540894544795 A-0.705342702471943 B0.000077035242368 C0.000003279042067 +G1 X4.000023145916854 Y-5.999987090405664 Z1.785743792072304 A-0.673283268159544 B0.000075701029646 C0.000006250189056 +G1 X4.000023090680716 Y-5.999988348463855 Z1.795946622420044 A-0.641223838114222 B0.000074178520374 C0.000009048064272 +G1 X4.000022978313819 Y-5.999989526003388 Z1.806149401357314 A-0.609164419441381 B0.000072489530486 C0.000011668385482 +G1 X4.000022814397488 Y-5.999990622449206 Z1.816352143476808 A-0.577105018546885 B0.000070655212119 C0.000014108901825 +G1 X4.00002260425261 Y-5.999991638295192 Z1.826554862082832 A-0.545045641290122 B0.000068695917809 C0.00001636920861 +G1 X4.000022352917956 Y-5.999992574943711 Z1.836757569142932 A-0.512986293135889 B0.000066631106363 C0.000018450550643 +G1 X4.000022065093478 Y-5.999993434572568 Z1.846960275189266 A-0.480926979180737 B0.000064479185279 C0.000020355645911 +G1 X4.000021745141869 Y-5.999994219974642 Z1.857162989347185 A-0.44886770428421 B0.000062257480482 C0.00002208848787 +G1 X4.000021397066726 Y-5.999994934423825 Z1.867365719322178 A-0.416808473085871 B0.000059982155412 C0.000023654167268 +G1 X4.000021024522993 Y-5.999995581535308 Z1.877568471475311 A-0.38474929007241 B0.000057668203895 C0.000025058689959 +G1 X4.000020630806035 Y-5.99999616514976 Z1.887771250867819 A-0.352690159488136 B0.000055329346293 C0.000026308806695 +G1 X4.000020218875202 Y-5.999996689215303 Z1.897974061381172 A-0.320631085410483 B0.000052978099637 C0.00002741183409 +G1 X4.000019791374275 Y-5.999997157693762 Z1.908176905835914 A-0.288572071656502 B0.000050625731887 C0.000028375496613 +G1 X4.000019350630497 Y-5.999997574480505 Z1.918379786098466 A-0.256513121719564 B0.000048282263 C0.00002920775807 +G1 X4.000018898690032 Y-5.999997943337363 Z1.928582703235816 A-0.224454238715405 B0.000045956502081 C0.000029916673233 +G1 X4.000018437335445 Y-5.999998267841183 Z1.938785657663339 A-0.192395425305721 B0.000043656076207 C0.000030510237761 +G1 X4.000017968100932 Y-5.999998551351651 Z1.948988649278294 A-0.160336683608116 B0.000041387464286 C0.000030996253381 +G1 X4.000017492289829 Y-5.999998796988894 Z1.959191677600197 A-0.128278015126051 B0.000039156058067 C0.000031382200386 +G1 X4.000028854410791 Y-5.999997400216754 Z1.969416349225053 A-0.096246081576383 B0.000059612352297 C0.000034922136688 +G1 X4.000028404927841 Y-5.999997962583903 Z1.979618856162186 A-0.064186703846773 B0.000056613289067 C0.000035722479844 +G1 X4.000027629288923 Y-5.999998465917389 Z1.989821171585394 A-0.032127257800735 B0.000053467188366 C0.000036237778939 +G1 X4.000026833228332 Y-5.999998914990694 Z2.000023528966179 A-0.000067933908427 B0.000050387544341 C0.000036625823702 +G1 X4.000026833228331 Y-5.999998914990809 Z2.000023528974099 A-0.000067933883403 B0.000050387544279 C0.000036625823794 +G1 X4.000016296116242 Y-6.000002745784034 Z2.000004646186802 A-0.000051949842319 B0.000032450544842 C0.000039830272644 +G1 X4.01015418657969 Y-5.999997861320238 Z1.999963004926101 A-0.000012399494481 B0.000000268888519 C-0.000001180989106 +G1 X4.020379409037495 Y-5.999998603316927 Z1.999970674123935 A-0.000017867028868 B0.000004969794518 C0.000011264271754 +G1 X4.030582758962659 Y-5.999998638193918 Z1.999969616281574 A-0.00001847800954 B0.00000518248591 C0.000012021399147 +G1 X4.040785971791061 Y-5.999998670131918 Z1.999968467009717 A-0.000019083170124 B0.000005379990044 C0.000012749829383 +G1 X4.050989666981595 Y-5.999998806232655 Z1.999972983313603 A-0.000011742531233 B0.000001709831025 C0.000005017370202 +G1 X4.061192780169269 Y-5.999998843101428 Z1.999971916041582 A-0.000012187966022 B0.000001800256728 C0.000005517179853 +G1 X4.071395852275462 Y-5.999998882563412 Z1.999970807229288 A-0.000012657848935 B0.000001905999738 C0.000006040951089 +G1 X4.081598867511148 Y-5.999998923930195 Z1.999969647101228 A-0.000013147683193 B0.000002022223277 C0.000006581547755 +G1 X4.091801822642847 Y-5.999998967379433 Z1.999968432753669 A-0.000013658032022 B0.000002149237333 C0.000007139378158 +G1 X4.102004714257792 Y-5.99999901310313 Z1.999967161141824 A-0.000014189496253 B0.000002287393847 C0.00000771486537 +G1 X4.112207538681496 Y-5.999999061311332 Z1.999965829039191 A-0.000014742687981 B0.000002437059139 C0.000008308397045 +G1 X4.122410291952799 Y-5.999999112227483 Z1.999964433018848 A-0.000015318232425 B0.000002598618593 C0.000008920328594 +G1 X4.132613439616956 Y-5.999999260460828 Z1.99996838407335 A-0.000008850805764 B-0.00000089511651 C0.00000168950071 +G1 X4.142816033741938 Y-5.999999320205514 Z1.999967042230501 A-0.000009267239043 B-0.000000848836814 C0.000002067154538 +G1 X4.153018557016288 Y-5.999999384121793 Z1.999965639902076 A-0.000009707246137 B-0.000000789270329 C0.000002463898981 G1 X4.163220989821128 Y-5.999999451990268 Z1.999964165701255 A-0.000010166378144 B-0.000000721388998 C0.000002871905242 G1 X4.17342332622918 Y-5.99999952415827 Z1.999962615238013 A-0.000010645124369 B-0.00000064496343 C0.000003291007805 -G1 X4.183625559931456 Y-5.999999601000051 Z1.999960983872389 A-0.000011143991142 B-0.000000559736557 C0.00000372098369 -G1 X4.193827684103554 Y-5.999999682923274 Z1.999959266651011 A-0.000011663487252 B-0.000000465438764 C0.000004161524793 -G1 X4.204030097121247 Y-5.999999858985638 Z1.999962646975741 A-0.000005828386522 B-0.000003866802956 C-0.000002716358496 -G1 X4.21423197077958 Y-5.999999955583481 Z1.999960943179217 A-0.000006185729437 B-0.000003899400776 C-0.000002525868266 -G1 X4.224456409340792 Y-5.999999896078609 Z1.999968967952185 A-0.00001203718608 B0.000002171146685 C0.000008301719821 -G1 X4.234658990120318 Y-5.99999995591762 Z1.999967476871904 A-0.000012630280361 B0.000002400672653 C0.00000886854854 +G1 X4.183625559931457 Y-5.999999601000051 Z1.999960983872388 A-0.000011143991142 B-0.000000559736557 C0.00000372098369 +G1 X4.193827684103555 Y-5.999999682923274 Z1.99995926665101 A-0.000011663487251 B-0.000000465438763 C0.000004161524792 +G1 X4.204030097121248 Y-5.999999858985638 Z1.999962646975742 A-0.000005828386522 B-0.000003866802956 C-0.000002716358497 +G1 X4.214231970779581 Y-5.99999995558348 Z1.999960943179216 A-0.000006185729437 B-0.000003899400776 C-0.000002525868266 +G1 X4.224456409340793 Y-5.999999896078609 Z1.999968967952184 A-0.00001203718608 B0.000002171146685 C0.000008301719821 +G1 X4.234658990120318 Y-5.999999955917618 Z1.999967476871906 A-0.000012630280362 B0.000002400672653 C0.00000886854854 G1 X4.244861360069499 Y-6.00000002128733 Z1.999965847845121 A-0.000013218503837 B0.000002610890963 C0.000009400602948 G1 X4.255063950628784 Y-6.000000172763877 Z1.999968894138925 A-0.000008222273273 B-0.000000382722148 C0.000003372514823 G1 X4.26526607775213 Y-6.000000253019249 Z1.999967269144059 A-0.0000086641674 B-0.000000288961886 C0.000003678116452 @@ -124,55 +124,55 @@ G1 X4.385771139647217 Y-5.99999905950892 Z1.885718686285768 A-0.359070001039915 G1 X4.37862566746799 Y-5.999999239359194 Z1.878575623420393 A-0.381508627910937 B0.000040054364602 C0.000044414876996 G1 X4.371479954671878 Y-5.999999431381315 Z1.871432394842012 A-0.40394717005529 B0.00003897695144 C0.000042338716403 G1 X4.364334388572661 Y-5.999999616753325 Z1.864289174830689 A-0.426385726148056 B0.00003787704759 C0.000040355663818 -G1 X4.357188967340779 Y-5.999999796548581 Z1.857145964956739 A-0.448824295515386 B0.000036759240684 C0.000038466542202 -G1 X4.35004304378435 Y-6.000000319416833 Z1.849998593992702 A-0.471267616038698 B0.000041025870393 C0.000041599883501 -G1 X4.342897914211015 Y-6.000000499726516 Z1.842855574297361 A-0.493706077837314 B0.000039826575061 C0.000039702704196 -G1 X4.335752876597561 Y-6.000000679320262 Z1.835712541660664 A-0.51614453697483 B0.000038595013213 C0.000037888233694 -G1 X4.328607955237695 Y-6.000000857505631 Z1.828569510104863 A-0.538583001374528 B0.000037348640959 C0.000036166194765 -G1 X4.321463144863755 Y-6.000001035481784 Z1.821426479124417 A-0.561021469543748 B0.000036090007256 C0.000034536093486 -G1 X4.314317599910251 Y-6.000001712356386 Z1.814279014077329 A-0.583465222303096 B0.000041366868967 C0.000038146825672 -G1 X4.307196025265165 Y-6.000002920596995 Z1.807141547399122 A-0.60589930939947 B0.000044977233489 C0.000049131002024 -G1 X4.300051135456721 Y-6.000003248837773 Z1.799998400952004 A-0.628337133038596 B0.000043467023203 C0.000047607026849 -G1 X4.292905942536678 Y-6.000003618110883 Z1.792855054881251 A-0.650774817307283 B0.000041726178012 C0.000046063458957 -G1 X4.28576087213842 Y-6.000004000521675 Z1.785711710799453 A-0.673212481093358 B0.0000399825376 C0.000044645336355 -G1 X4.278615930104836 Y-6.000004396705665 Z1.778568374272113 A-0.695650127048988 B0.000038247296381 C0.000043355816066 -G1 X4.271470034459126 Y-6.000005481450503 Z1.77142048978621 A-0.718093457703561 B0.000044348538435 C0.000047302408945 -G1 X4.264325398254782 Y-6.000005900508677 Z1.764277388694734 A-0.740530895476595 B0.000042549183062 C0.000046034145396 -G1 X4.25718083082112 Y-6.000006341243243 Z1.757134265891515 A-0.762968293809396 B0.000040738673767 C0.000044887148447 -G1 X4.25003638054121 Y-6.000006798694189 Z1.749991151907848 A-0.785405676157836 B0.000038958685832 C0.000043873331303 -G1 X4.242892046306608 Y-6.00000727352073 Z1.742848049585978 A-0.807843044316992 B0.000037217759611 C0.0000429921538 -G1 X4.235747826137762 Y-6.000007766307033 Z1.73570496149763 A-0.830280400057677 B0.000035524038164 C0.000042242370021 -G1 X4.228603718197956 Y-6.000008277411228 Z1.728561890488406 A-0.852717745500301 B0.00003388590398 C0.000041622296057 -G1 X4.221458375672367 Y-6.00000964658578 Z1.721414344889373 A-0.87516095715403 B0.000041469708366 C0.000045841954791 -G1 X4.214314552684643 Y-6.000010170762089 Z1.71427154423393 A-0.897598082665447 B0.000039816917026 C0.000045216722614 -G1 X4.207170788234116 Y-6.000010719540065 Z1.707128735324239 A-0.920035183241193 B0.000038204700282 C0.000044713143226 -G1 X4.200027129564338 Y-6.000011284041521 Z1.699985951128936 A-0.942472288368143 B0.000036679140771 C0.000044334370773 -G1 X4.192883575786285 Y-6.000011863170671 Z1.692843195978738 A-0.964909402888987 B0.000035249976658 C0.000044075938364 -G1 X4.185740124992105 Y-6.00001245562804 Z1.685700473828807 A-0.987346531719914 B0.000033926141684 C0.000043932431033 -G1 X4.178595093590285 Y-6.000014056835079 Z1.678553133008962 A-1.009789883710001 B0.000043668172667 C0.000048349148374 -G1 X4.171451916447578 Y-6.000014630955501 Z1.671410729282713 A-1.032226811335004 B0.000042372116763 C0.000048131996099 -G1 X4.164308786890729 Y-6.000015221979771 Z1.664268333106014 A-1.054663746274468 B0.000041163905657 C0.000048015936715 -G1 X4.157165750678327 Y-6.000015816550663 Z1.657125978418991 A-1.077100723775737 B0.000040090240764 C0.000047994767045 -G1 X4.150022805330861 Y-6.000016411544908 Z1.649983669338283 A-1.099537751462024 B0.000039158619279 C0.000048059242649 -G1 X4.142879947208287 Y-6.000017003637923 Z1.642841409307919 A-1.121974836896658 B0.000038375089804 C0.000048199285451 -G1 X4.135737172258128 Y-6.000017589173686 Z1.63569920145305 A-1.144411987930382 B0.000037744691175 C0.00004840408064 -G1 X4.128594476019194 Y-6.000018164178781 Z1.628557048526606 A-1.166849212665033 B0.000037271360078 C0.000048662150083 -G1 X4.121449818139173 Y-6.000019766463552 Z1.621410501960344 A-1.189292376381861 B0.000049538829076 C0.000052382181104 -G1 X4.114307344358033 Y-6.000020249784907 Z1.614268696616366 A-1.211729496554771 B0.000049103706999 C0.000052420478794 -G1 X4.107164890530806 Y-6.000020721437151 Z1.60712691660131 A-1.234166696409222 B0.000048794107062 C0.000052489053156 -G1 X4.100022493205531 Y-6.000021164340907 Z1.599985188989634 A-1.25660401174315 B0.00004864435012 C0.000052571399586 -G1 X4.092880146220079 Y-6.000021573589812 Z1.592843514047223 A-1.279041451565105 B0.000048652308423 C0.000052654028455 -G1 X4.085737842446928 Y-6.000021944406505 Z1.585701890984688 A-1.301479024413154 B0.000048813788261 C0.000052723437126 -G1 X4.078595574395249 Y-6.000022272049907 Z1.578560318238133 A-1.323916738586081 B0.000049122936083 C0.000052766229587 -G1 X4.071450803451843 Y-6.000023597666067 Z1.57141421274733 A-1.346360242041138 B0.000064544396863 C0.000055151745819 -G1 X4.064308672911911 Y-6.000023753199628 Z1.564272958857038 A-1.368797950264616 B0.000064762696077 C0.000054802994212 -G1 X4.057166513570095 Y-6.000023866240801 Z1.55713171307142 A-1.391235809672935 B0.000065070914356 C0.000054408089688 -G1 X4.050024357259361 Y-6.000023920247234 Z1.549990494212229 A-1.413673849724625 B0.000065490008252 C0.000053950688472 -G1 X4.042882195881306 Y-6.000023911864369 Z1.542849297148255 A-1.436112076221437 B0.000066007236759 C0.000053420268353 -G1 X4.035740020929894 Y-6.000023838355941 Z1.535708115803595 A-1.458550493861277 B0.000066608251356 C0.000052807444284 -G1 X4.028597824018949 Y-6.000023697539981 Z1.528566943442551 A-1.480989106361025 B0.000067277583358 C0.000052104074355 -G1 X4.021455596981767 Y-6.000023487859022 Z1.52142577274849 A-1.503427916381268 B0.000067998831331 C0.000051303383512 -G1 X4.014313331946768 Y-6.000023208401351 Z1.514284595907873 A-1.52586692552355 B0.000068754849032 C0.000050400030566 -G1 X4.007171021443357 Y-6.000022858986227 Z1.50714340471288 A-1.548306134168594 B0.000069527996715 C0.000049390268237 -G1 X4.000016659961092 Y-5.999987280088333 Z1.499997825493375 A-1.570622370243438 B0.000118857411282 C-0.00001174894119 +G1 X4.357188967339718 Y-5.999999796547765 Z1.85714596495545 A-0.448824295516068 B0.000036759244591 C0.000038466547826 +G1 X4.35004304378565 Y-6.00000031941774 Z1.849998593994057 A-0.471267616038025 B0.000041025866254 C0.000041599877639 +G1 X4.342897914211812 Y-6.000000499727469 Z1.842855574298345 A-0.493706077836852 B0.000039826573308 C0.000039702702145 +G1 X4.335752876599268 Y-6.000000679321191 Z1.835712541662248 A-0.516144536974554 B0.000038595006851 C0.000037888224096 +G1 X4.328607955237044 Y-6.000000857505544 Z1.828569510104536 A-0.538583001374495 B0.000037348643265 C0.000036166198373 +G1 X4.321463144865192 Y-6.000001035483192 Z1.821426479125724 A-0.561021469543305 B0.000036090004052 C0.000034536089376 +G1 X4.314317599910006 Y-6.000001712356486 Z1.814279014077041 A-0.583465222302362 B0.000041366870917 C0.000038146829413 +G1 X4.3071960252648 Y-6.000002920596956 Z1.807141547398836 A-0.605899309397525 B0.000044977235234 C0.000049131006791 +G1 X4.300051135457353 Y-6.000003248838545 Z1.79999840095226 A-0.62833713303905 B0.000043467021898 C0.000047607024428 +G1 X4.2929059425371 Y-6.000003618111579 Z1.792855054881449 A-0.650774817307199 B0.000041726177453 C0.000046063458175 +G1 X4.285760872138416 Y-6.000004000521676 Z1.785711710799448 A-0.673212481093337 B0.000039982537607 C0.000044645336388 +G1 X4.278615930104834 Y-6.000004396705664 Z1.778568374272113 A-0.695650127048976 B0.000038247296385 C0.000043355816085 +G1 X4.271470034457919 Y-6.000005481448945 Z1.771420489785893 A-0.718093457699572 B0.000044348541133 C0.000047302416917 +G1 X4.264325398254631 Y-6.000005900508683 Z1.764277388694523 A-0.74053089547408 B0.000042549183715 C0.000046034148868 +G1 X4.257180830821291 Y-6.00000634124373 Z1.757134265891707 A-0.762968293810114 B0.000040738673546 C0.000044887147247 +G1 X4.250036380541918 Y-6.000006798695586 Z1.749991151907791 A-0.78540567615788 B0.000038958685133 C0.000043873330043 +G1 X4.242892046307086 Y-6.00000727352152 Z1.74284804958592 A-0.807843044319479 B0.00003721775872 C0.000042992150106 +G1 X4.235747826138277 Y-6.00000776630783 Z1.735704961497606 A-0.830280400061975 B0.000035524037165 C0.00004224236447 +G1 X4.228603718198459 Y-6.00000827741214 Z1.728561890488142 A-0.852717745500439 B0.000033885903486 C0.000041622295175 +G1 X4.221458375672367 Y-6.000009646585781 Z1.721414344889375 A-0.875160957154121 B0.000041469708357 C0.000045841954695 +G1 X4.214314552684645 Y-6.000010170762089 Z1.714271544233931 A-0.897598082665516 B0.000039816917021 C0.000045216722545 +G1 X4.207170788234474 Y-6.000010719540866 Z1.70712873532401 A-0.920035183243995 B0.000038204699779 C0.000044713139939 +G1 X4.20002712956404 Y-6.000011284040724 Z1.699985951129233 A-0.942472288367509 B0.000036679141123 C0.000044334371883 +G1 X4.192883575786606 Y-6.000011863171475 Z1.692843195978462 A-0.964909402891326 B0.000035249976261 C0.000044075935704 +G1 X4.185740124992035 Y-6.000012455627841 Z1.685700473828984 A-0.987346531715889 B0.000033926142135 C0.000043932434888 +G1 X4.178595093590209 Y-6.000014056833867 Z1.678553133009987 A-1.009789883714536 B0.000043668173577 C0.000048349145174 +G1 X4.171451916447341 Y-6.00001463095469 Z1.671410729283046 A-1.032226811335834 B0.000042372116989 C0.000048131995781 +G1 X4.164308786891084 Y-6.000015221980719 Z1.66426833310565 A-1.054663746272146 B0.000041163905522 C0.000048015938317 +G1 X4.157165750678098 Y-6.00001581655005 Z1.657125978419094 A-1.077100723776829 B0.000040090240679 C0.000047994766365 +G1 X4.150022805330866 Y-6.00001641154491 Z1.649983669338288 A-1.099537751462925 B0.000039158619223 C0.000048059241881 +G1 X4.14287994720829 Y-6.000017003637924 Z1.642841409307924 A-1.121974836897393 B0.000038375089751 C0.000048199284832 +G1 X4.135737172258132 Y-6.000017589173687 Z1.635699201453056 A-1.144411987930981 B0.000037744691125 C0.000048404080139 +G1 X4.128594476019197 Y-6.000018164178782 Z1.628557048526607 A-1.166849212665524 B0.000037271360032 C0.000048662149676 +G1 X4.121449818139174 Y-6.000019766463552 Z1.621410501960346 A-1.189292376382263 B0.000049538829031 C0.000052382180773 +G1 X4.114307344358033 Y-6.000020249784908 Z1.614268696616367 A-1.211729496555101 B0.000049103706957 C0.000052420478523 +G1 X4.107164890531352 Y-6.000020721438858 Z1.607126916600923 A-1.234166696415237 B0.000048794105767 C0.000052489047638 +G1 X4.100022493205399 Y-6.000021164340441 Z1.59998518898991 A-1.256604011736374 B0.000048644351639 C0.000052571405162 +G1 X4.092880146220052 Y-6.000021573589808 Z1.592843514047203 A-1.27904145155956 B0.000048652309445 C0.000052654032928 +G1 X4.08573784244747 Y-6.000021944408221 Z1.585701890984426 A-1.301479024414277 B0.000048813787589 C0.00005272343564 +G1 X4.07859557439525 Y-6.000022272049909 Z1.578560318238137 A-1.323916738586941 B0.000049122935885 C0.000052766228899 +G1 X4.071450803451847 Y-6.000023597666067 Z1.571414212747334 A-1.346360242041847 B0.000064544396685 C0.000055151745254 +G1 X4.064308672911913 Y-6.000023753199627 Z1.564272958857042 A-1.368797950265198 B0.000064762695917 C0.000054802993749 +G1 X4.057166513570098 Y-6.000023866240802 Z1.557131713071423 A-1.391235809673414 B0.000065070914213 C0.00005440808931 +G1 X4.050024357259365 Y-6.000023920247234 Z1.549990494212231 A-1.413673849725018 B0.000065490008124 C0.000053950688163 +G1 X4.042882195881308 Y-6.00002391186437 Z1.542849297148257 A-1.436112076221759 B0.000066007236646 C0.000053420268101 +G1 X4.035740020929898 Y-6.00002383835594 Z1.535708115803598 A-1.45855049386154 B0.000066608251256 C0.000052807444079 +G1 X4.02859782401895 Y-6.000023697539981 Z1.528566943442553 A-1.48098910636124 B0.00006727758327 C0.000052104074189 +G1 X4.021455596981769 Y-6.000023487859023 Z1.52142577274849 A-1.503427916381441 B0.000067998831255 C0.000051303383378 +G1 X4.014313331946771 Y-6.000023208401349 Z1.514284595907875 A-1.525866925523693 B0.000068754848966 C0.000050400030459 +G1 X4.007171021443359 Y-6.000022858986227 Z1.507143404712881 A-1.548306134168708 B0.000069527996659 C0.000049390268151 +G1 X4.000016659961085 Y-5.999987280088306 Z1.499997825493376 A-1.570622370243432 B0.000118857411259 C-0.000011748941306 %postfix diff --git a/src/pybullet_industrial/g_code_processor.py b/src/pybullet_industrial/g_code_processor.py index 7e46759..b8586a0 100644 --- a/src/pybullet_industrial/g_code_processor.py +++ b/src/pybullet_industrial/g_code_processor.py @@ -338,7 +338,7 @@ def __build_precise_path(self, g_com: int): previous_postion = position interpolation_steps = total_distance/self.interpolation_precision - interpolation_steps = int(np.ceil(interpolation_steps)) + interpolation_steps = int(np.ceil(interpolation_steps)) + 1 return path diff --git a/src/pybullet_industrial/interpolation.py b/src/pybullet_industrial/interpolation.py index 6f2c93e..28d063a 100644 --- a/src/pybullet_industrial/interpolation.py +++ b/src/pybullet_industrial/interpolation.py @@ -1,10 +1,34 @@ import numpy as np import scipy.interpolate as sci -import pybullet as p +from scipy.spatial.transform import Rotation as R, RotationSpline from pybullet_industrial.toolpath import ToolPath +def slerp(start_quat, end_quat, t_vals): + """Performs Spherical Linear Interpolation (SLERP) between two quaternions + + Args: + start_quat (np.array): The starting quaternion + end_quat (np.array): The ending quaternion + t_vals (np.array): Array of interpolation parameters (0 <= t <= 1) + + Returns: + np.array: The array of interpolated quaternions + """ + start_rot = R.from_quat(start_quat) + end_rot = R.from_quat(end_quat) + + # Create the spline (SLERP) object from two quaternions + slerp_rotations = R.from_quat([start_rot.as_quat(), end_rot.as_quat()]) + slerp_spline = RotationSpline([0, 1], slerp_rotations) + + # Perform SLERP interpolation at the time values + interpolated_rots = slerp_spline(t_vals) + + return interpolated_rots.as_quat() + + def build_circular_path(center: np.array, radius: float, min_angle: float, max_angle: float, step_num: int, clockwise: bool = True): """Function which builds a circular path @@ -14,7 +38,7 @@ def build_circular_path(center: np.array, radius: float, radius (float): the radius of the circle min_angle (float): minimum angle of the circle path max_angle (float): maximum angle of the circle path - step_num (int): the number of steps between min_angle and max_angle + step_num (int): the number of steps including min_angle and max_angle clockwise (bool): boolean value indicating if the interpolation is performed clockwise or anticlockwise @@ -25,9 +49,12 @@ def build_circular_path(center: np.array, radius: float, circular_path = np.zeros((2, step_num)) for j in range(step_num): if clockwise: - path_angle = min_angle-j*(max_angle-min_angle)/step_num + path_angle = min_angle - j * \ + (max_angle - min_angle) / (step_num - 1) else: - path_angle = min_angle+j*(max_angle-min_angle)/step_num + path_angle = min_angle + j * \ + (max_angle - min_angle) / (step_num - 1) + new_position = center + radius * \ np.array([np.cos(path_angle), np.sin(path_angle)]) circular_path[:, j] = new_position @@ -37,12 +64,12 @@ def build_circular_path(center: np.array, radius: float, def linear_interpolation(start_point: np.array, end_point: np.array, samples: int, start_orientation: np.array = None, end_orientation: np.array = None): - """Performs a linear interpolation betwenn two points in 3D space + """Performs a linear interpolation between two points in 3D space Args: start_point (np.array): The start point of the interpolation end_point (np.array): The end point of the interpolation - samples (int): The number of samples used to interpolate + samples (int): The number of samples including start and end start_orientation (np.array): Start orientation as quaternion end_orientation (np.array): End orientation as quaternion @@ -52,18 +79,17 @@ def linear_interpolation(start_point: np.array, end_point: np.array, samples: in positions = np.linspace(start_point, end_point, num=samples) final_path = ToolPath(positions=positions.transpose()) - if start_orientation is not None and end_orientation is not None: - start_orientation = p.getEulerFromQuaternion(start_orientation) - end_orientation = p.getEulerFromQuaternion(end_orientation) - orientations = np.linspace(start_orientation, - end_orientation, num=samples) - final_orientations = [] + if start_orientation is not None: + if end_orientation is None: + end_orientation = start_orientation + + # Time values for SLERP interpolation + t_vals = np.linspace(0, 1, samples) - for orientation in orientations: - orientation_quat = p.getQuaternionFromEuler(orientation) - final_orientations.append(orientation_quat) + # Interpolate orientations using SLERP + final_orientations = slerp(start_orientation, end_orientation, t_vals) - final_path.orientations = np.array(final_orientations).transpose() + final_path.orientations = final_orientations.transpose() return final_path @@ -76,7 +102,7 @@ def planar_circular_interpolation(start_point: np.array, end_point: np.array, start_point (np.array): The start point of the interpolation end_point (np.array): The end point of the interpolation radius (float): The radius of the circle - samples (int): The number of samples used to interpolate + samples (int): The number of including start and end clockwise (bool): boolean value indicating if the interpolation is performed clockwise or anticlockwise @@ -115,15 +141,15 @@ def circular_interpolation(start_point: np.array, end_point: np.array, radius: float, samples: int, axis: int = 2, clockwise: bool = True, start_orientation: np.array = None, end_orientation: np.array = None): - """AI is creating summary for circular_interpolation + """Performs a circular interpolation between two points in 3D space Args: start_point (np.array): The start point of the interpolation end_point (np.array): The end point of the interpolation radius (float): The radius of the circle used for the interpolation - samples (int): The number of samples used to interpolate + samples (int): The number of samples including start and end axis (int, optional): The axis around which the circle is interpolated. - Defaults to 2 which corresponds to the z-axis (0=x,1=y). + Defaults to 2 which corresponds to the z-axis (0=x, 1=y). clockwise (bool, optional): The direction of circular travel. Defaults to True. start_orientation (np.array): Start orientation as quaternion end_orientation (np.array): End orientation as quaternion @@ -131,9 +157,10 @@ def circular_interpolation(start_point: np.array, end_point: np.array, Returns: ToolPath: A ToolPath object of the interpolated path """ - all_axis = [0, 1, 2] all_axis.remove(axis) + + # Interpolate positions in the x-y plane planar_start_point = np.array( [start_point[all_axis[0]], start_point[all_axis[1]]]) planar_end_point = np.array( @@ -146,20 +173,20 @@ def circular_interpolation(start_point: np.array, end_point: np.array, for i in range(2): positions[all_axis[i]] = planar_path[i] positions[axis] = np.linspace(start_point[axis], end_point[axis], samples) + final_path = ToolPath(positions=positions) - if start_orientation is not None and end_orientation is not None: - start_orientation = p.getEulerFromQuaternion(start_orientation) - end_orientation = p.getEulerFromQuaternion(end_orientation) - orientations = np.linspace(start_orientation, - end_orientation, num=samples) - final_orientations = [] + if start_orientation is not None: + if end_orientation is None: + end_orientation = start_orientation + + # Time values for SLERP interpolation + t_vals = np.linspace(0, 1, samples) - for orientation in orientations: - orientation_quat = p.getQuaternionFromEuler(orientation) - final_orientations.append(orientation_quat) + # Interpolate orientations using SLERP + final_orientations = slerp(start_orientation, end_orientation, t_vals) - final_path.orientations = np.array(final_orientations).transpose() + final_path.orientations = final_orientations.transpose() return final_path diff --git a/tests/test_agv_robots.py b/tests/test_agv_robots.py index a7da8a7..82f1baf 100644 --- a/tests/test_agv_robots.py +++ b/tests/test_agv_robots.py @@ -146,7 +146,7 @@ def test_standard_position_controller(self): distance, angle, target_angle_error = self.robot._calculate_position_error() # check if the position error is correct - self.assertAlmostEqual(distance, 0, delta=0.05) + self.assertAlmostEqual(distance, 0, delta=0.06) diff --git a/tests/test_grippers.py b/tests/test_grippers.py index 5a2687f..55f4f01 100644 --- a/tests/test_grippers.py +++ b/tests/test_grippers.py @@ -84,12 +84,10 @@ def test_grippers(self): within_precision = True path = pi.linear_interpolation(np.array(safepoint1), - np.array(grippoint1_2), 10) - path.orientations = np.transpose([start_orientation_gr] * len(path.orientations[0])) + np.array(grippoint1_2), 10, start_orientation_gr) move_along_path(gripper, path, start_orientation_gr) - path = pi.linear_interpolation( - np.array(grippoint1_2), np.array(grippoint1), 10) - path.orientations = np.transpose([start_orientation_gr] * len(path.orientations[0])) + path = pi.circular_interpolation( + start_point=np.array(grippoint1_2), end_point=np.array(grippoint1), samples=10, axis=0, radius=10, start_orientation=start_orientation_gr) move_along_path(gripper, path) for _ in range(25): p.stepSimulation() diff --git a/tests/test_interpolation.py b/tests/test_interpolation.py new file mode 100644 index 0000000..94029e5 --- /dev/null +++ b/tests/test_interpolation.py @@ -0,0 +1,285 @@ +import unittest +import numpy as np +import pybullet as p +from pybullet_industrial.toolpath import ToolPath +from pybullet_industrial.interpolation import ( + linear_interpolation, + circular_interpolation, +) + + +class TestInterpolation(unittest.TestCase): + + def test_linear_interpolation(self): + """ + Test linear interpolation function with different orientation cases. + + This test covers two scenarios: + 1. Interpolation with both start and end orientations. + 2. Interpolation with only the start orientation + (end orientation is None). + + It verifies that the interpolated positions and orientations match the + expected values. + + Test cases: + - Case 1: Both start and end orientations are provided. + - Case 2: Only the start orientation is provided + (end orientation is None). + + Assertions: + - Checks if the interpolated positions and orientations are almost + equal to the expected values. + """ + + # Case 1: Test with both start and end orientations + start_point = np.array([0.0, 0.0, 0.0]) + end_point = np.array([10.0, 10.0, 10.0]) + samples = 5 + start_orientation = p.getQuaternionFromEuler([0, 0, 0]) + end_orientation = p.getQuaternionFromEuler( + [0, np.pi / 2, 0] + ) # 90 degrees rotation + + result_with_both_orientations = linear_interpolation( + start_point, end_point, samples, start_orientation, end_orientation + ) + + # Expected positions for both orientations case + expected_positions = np.array( + [ + [0.0, 2.5, 5.0, 7.5, 10.0], + [0.0, 2.5, 5.0, 7.5, 10.0], + [0.0, 2.5, 5.0, 7.5, 10.0], + ] + ) + + # Expected orientations + expected_orientations_both = np.array( + [ + [0.0, 0.0, 0.0, 1.0], + [0.0, 0.19509032, 0.0, 0.98078528], + [0.0, 0.38268343, 0.0, 0.92387953], + [0.0, 0.55557023, 0.0, 0.83146961], + [0.0, 0.70710678, 0.0, 0.70710678], + ] + ) + + expected_toolpath_with_both = ToolPath( + positions=expected_positions, + orientations=expected_orientations_both.transpose(), + ) + + # Check positions and orientations for both orientations case + np.testing.assert_array_almost_equal( + result_with_both_orientations.positions, + expected_toolpath_with_both.positions, + decimal=6, + ) + np.testing.assert_array_almost_equal( + result_with_both_orientations.orientations, + expected_toolpath_with_both.orientations, + decimal=6, + ) + + # Case 2: Test with only start orientation (end_orientation is None) + result_with_only_start_orientation = linear_interpolation( + start_point, end_point, samples, start_orientation, + end_orientation=None + ) + + # Expected orientations for only start orientation case + expected_orientations_start_only = np.tile( + start_orientation, (samples, 1) + ).transpose() + + expected_toolpath_with_start_only = ToolPath( + positions=expected_positions, + orientations=expected_orientations_start_only + ) + + # Check positions and orientations for only start orientation case + np.testing.assert_array_almost_equal( + result_with_only_start_orientation.positions, + expected_toolpath_with_start_only.positions, + decimal=6, + ) + np.testing.assert_array_almost_equal( + result_with_only_start_orientation.orientations, + expected_toolpath_with_start_only.orientations, + decimal=6, + ), + + def test_circular_interpolation(self): + """ + Test the circular_interpolation function for generating circular paths + with specified orientations. + + This test covers two cases: + 1. Circular interpolation with both start and + end orientations specified. + 2. Circular interpolation with only the start orientation specified + (end orientation is None). + + The test verifies that the generated positions and orientations match + the expected values. + + Assertions: + - Positions and orientations for both start and end orientations. + - Positions and orientations for only the start orientation. + """ + + # Case 1: Test with both start and end orientations + start_point = np.array([1.0, 0.0, 0.0]) + + end_point = np.array([0.0, 1.0, 0.0]) + radius = 1.0 + samples = 5 + axis = 2 # Z-axis + clockwise = True + start_orientation = p.getQuaternionFromEuler( + [0, 0, 0]) # Neutral orientation + end_orientation = p.getQuaternionFromEuler( + [0, 0, np.pi / 2] + ) # 90 degrees rotation around Z-axis + + result_with_both_orientations = circular_interpolation( + start_point, + end_point, + radius, + samples, + axis, + clockwise, + start_orientation, + end_orientation, + ) + + # Expected positions for both orientations case + expected_positions = np.array( + [ + [1.0, 0.61731657, 0.29289322, 0.07612047, 0.0], + [0.0, 0.07612047, 0.29289322, 0.61731657, 1.0], + [0.0, 0.0, 0.0, 0.0, 0.0], + ] + ) + + # Generate expected orientations + expected_orientations_both = np.array( + [ + [0.0, 0.0, 0.0, 1.0], + [0.0, 0.0, 0.19509032, 0.98078528], + [0.0, 0.0, 0.38268343, 0.92387953], + [0.0, 0.0, 0.55557023, 0.83146961], + [0.0, 0.0, 0.70710678, 0.70710678], + ] + ) + + expected_toolpath_with_both = ToolPath( + positions=expected_positions, + orientations=expected_orientations_both.transpose(), + ) + + # Check positions and orientations for both orientations case + np.testing.assert_array_almost_equal( + result_with_both_orientations.positions, + expected_toolpath_with_both.positions, + decimal=6, + ) + np.testing.assert_array_almost_equal( + result_with_both_orientations.orientations, + expected_toolpath_with_both.orientations, + decimal=6, + ) + + # Case 2: Test with only start orientation (end_orientation is None) + result_with_only_start_orientation = circular_interpolation( + start_point, + end_point, + radius, + samples, + axis, + clockwise, + start_orientation, + end_orientation=None, + ) + + # Expected orientations for only start orientation case + expected_orientations_start_only = np.tile( + start_orientation, (samples, 1) + ).transpose() + + expected_toolpath_with_start_only = ToolPath( + positions=expected_positions, + orientations=expected_orientations_start_only + ) + + # Check positions and orientations for only start orientation case + np.testing.assert_array_almost_equal( + result_with_only_start_orientation.positions, + expected_toolpath_with_start_only.positions, + decimal=6, + ) + np.testing.assert_array_almost_equal( + result_with_only_start_orientation.orientations, + expected_toolpath_with_start_only.orientations, + decimal=6, + ) + + def test_zero_sample_size(self): + """ + Test interpolation functions with only start and end point & zero + samples in between. + + This test checks the linear and circular interpolation functions with + a sample size of 2.It verifies that the generated paths match the + expected positions and orientations. + """ + start_point = np.array([1.0, 0.0, 0.0]) + end_point = np.array([0.0, 1.0, 0.0]) + radius = 1.0 + samples = 2 + axis = 2 # Z-axis + clockwise = True + start_orientation = p.getQuaternionFromEuler( + [0, 0, 0]) # Neutral orientation + end_orientation = p.getQuaternionFromEuler( + [0, 0, np.pi / 2] + ) # 90 degrees rotation around Z-axis + + linear_path = linear_interpolation( + start_point, end_point, samples, start_orientation, end_orientation + ) + circular_path = circular_interpolation( + start_point, + end_point, + radius, + samples, + axis, + clockwise, + start_orientation, + end_orientation, + ) + + expected_position = np.array([[1, 0], [0, 1], [0, 0]]) + expected_orientation = np.array( + [[0, 0], [0, 0], [0, 0.70710678], [1, 0.70710678]] + ) + + expected_toolpath = ToolPath(expected_position, expected_orientation) + + np.testing.assert_array_almost_equal( + linear_path.positions, expected_toolpath.positions, decimal=6 + ) + np.testing.assert_array_almost_equal( + circular_path.positions, expected_toolpath.positions, decimal=6 + ) + np.testing.assert_array_almost_equal( + linear_path.orientations, expected_toolpath.orientations, decimal=6 + ) + np.testing.assert_array_almost_equal( + linear_path.orientations, expected_toolpath.orientations, decimal=6 + ) + + +if __name__ == "__main__": + unittest.main()