blob: 88026293662aef1a1de18d27ee1a558c9bf29a67 [file] [log] [blame]
#!/usr/bin/python3
# Path points from Parker.
# Points in (theta0, theta1) of the boundry of the safe region.
points_bounary = (
[1.8577014383575772,
-1.7353804562372057], [1.8322288438826315, -1.761574570216062],
[1.7840339881582052,
-1.8122752826851365], [1.7367354460218392, -1.863184376466228
], [1.69054633425053, -1.9140647421906793],
[1.6456590387358871, -1.9646939485526782
], [1.6022412524081968, -2.0148691793459164
], [1.5604334435784202, -2.0644108218872432
], [1.5203477477575325, -2.1131646494909866
], [1.4820681545944325, -2.1610026706370666
], [1.4456517763321752, -2.207822815007094
], [1.4111309389855604, -2.253547685496041
], [1.3785158286698027, -2.298122627340264],
[1.3477974448369014, -2.3415133576238922
], [1.318950649522341, -2.3837033700108243
], [1.2919371475531436, -2.4246912899477153
], [1.266708279449626, -2.464488312575651
], [1.2432075513427807, -2.5031158147261996
], [1.2213728618107609, -2.5406031969824228
], [1.2011384131259828, -2.5769859833310096
], [1.1824363142639513, -2.61230418457078
], [1.165197896140849, -2.64660091671642],
[1.149354767212547, -2.6799212560849437
], [1.134839641107821, -2.712311307404836
], [1.121586968578607, -2.7438174590386177
], [1.1095334047223224, -2.774485799309971
], [1.09861813993779, -2.8043616692157007
], [1.088783119985477, -2.8334893289039758
], [1.079973177230731, -2.861911717797479
], [1.0721360919151535, -2.8896702908486835
], [1.065222599283597, -2.916804915950962],
[1.0591863556764607, -2.943353819884641
], [1.0539838743128325, -2.969353572295195
], [1.049574439440948, -2.9948390990606946
], [1.0459200058000553, -3.01984371800932
], [1.0429850888932353, -3.044399191310831
], [1.0407366503803421, -3.0685357900111128
], [1.039143981929867, -3.092282367132034
], [1.0381785900853944, -3.1156664365461078
], [1.0378140840766794, -3.1387142554815886],
[1.038026068010855,
-3.1614509090414518], [1.0387920384931424, -3.1839003955494407], [
1.0400912884293725,
-3.2060857118856374
], [1.0419048175385754, -3.2280289382581637
], [1.0442152499395827, -3.2497513220895704
], [1.047006759060362, -3.2712733608874625
], [1.050265000044113, -3.2926148841283163
], [1.0513266200838918, -3.2986722862692828
], [0.6882849734317291, -3.2986722862692828
], [0.6882849734317291, -2.40847516476558
], [0.8062364039734171, -2.2816832357742984],
[0.9846964491122989,
-2.0749837238115147], [1.080856081125841, -1.9535526052833936], [
1.1403399741223543,
-1.8700303125904767
], [1.1796460643255697,
-1.8073338252603661], [1.206509908068604, -1.7574623871869075], [
1.225108933139178,
-1.7160975113819317
], [1.237917343016644, -1.6806816402603253
], [1.2465009152225068, -1.6495957958330445
], [1.251901644035212, -1.6217619064422375
], [1.2548410275662123, -1.5964327471863136
], [1.2558349967266738, -1.5730732293972975
], [1.2552624664807475, -1.551289614657788
], [1.2534080548485127, -1.5307854126408047],
[1.2504896957865235,
-1.5113328783804112], [1.2466770509718135, -1.492754008779478], [
1.2421041193998992,
-1.4749075280685116
], [1.236878076935188, -1.457679763958034
], [1.231085601347444, -1.4409781183381307
], [1.2247974811461413, -1.4247263085140511
], [1.2180720288351026, -1.408860841660136
], [1.2109576458572935, -1.3933283641188086
], [1.2034947755974974, -1.378083641634472
], [1.1957174082841977, -1.363088001457586
], [1.1876542532510088, -1.3483081171759035],
[1.179329661157153,
-1.3337150510329991], [1.1707643560768641, -1.3192834919003447], [
1.1385726725405734,
-1.3512941162901886
], [1.1061744838535637,
-1.3828092440478137], [1.0736355415504857, -1.4137655512448706], [
1.0410203155384732,
-1.444102884084807
], [1.0083912519665894,
-1.4737649313813326], [0.975808099297045, -1.5026998012630925], [
0.9433273192311136,
-1.5308604887780404
], [0.941597772428203, -1.50959779639341
], [0.9392183822389457, -1.488861961175901
], [0.9362399962318921, -1.4685999567644576
], [0.9327074201772598, -1.448764371832554
], [0.9286602163651203, -1.4293126388801052
], [0.9241333769591611, -1.410206381002334],
[0.9191578939466147,
-1.3914108561164176], [0.9137612431353617, -1.3728944819981919], [
0.9079677963791273,
-1.3546284285619337
], [0.9017991735984072,
-1.3365862662768213], [0.8952745440670551, -1.3187436615861219], [
0.888410884742945,
-1.3010781117818295
], [0.8812232020507231,
-1.2835687130679965], [0.8737247224089426, -1.2661959565824437], [
0.8659270558807739,
-1.2489415479874506
], [0.8578403365760008, -1.2317882469234374
], [0.8810761292150717, -1.2118184809610988
], [0.9041762360363244, -1.1914283310662528
], [0.9271196683064211, -1.1706361926784383
], [0.949885054142765, -1.1494613527499984
], [0.9724507572661958, -1.1279238910069835],
[0.9947950004998649,
-1.1060445714951375], [1.0168959923240788, -1.0838447258650978], [
1.0387320546908576,
-1.0613461300367468
], [1.0602817502430675,
-1.0385708760262022], [1.0815240070740941, -1.0155412408147642], [
1.102438239206001,
-0.9922795541836177
], [1.123004461055328,
-0.9688080674301042], [1.1432033942926907, -0.9451488248218312], [
1.1630165656794818,
-0.9213235395370803
], [1.1824263946752058, -0.8973534756890716
], [1.2014162698440272, -0.8732593378443982
], [1.2199706133398625, -0.8490611692304734
], [1.2380749330068292, -0.8247782595916947
], [1.2557158618869284, -0.800429063408306
], [1.2728811851714203, -0.776031128944234],
[1.2895598548592355,
-0.7516010383486005], [1.3057419925890068, -0.7271543588072293], [
1.3214188812865908,
-0.702705604531139
], [1.3365829464142562,
-0.6782682091832445], [1.351227727719687, -0.6538545081853449], [
1.365347842462401, -0.6294757302167044
], [1.3789389411433586,
-0.6051419971134862], [1.391997656782351, -0.5808623313043475], [
1.4045215487801634,
-0.5566446698699925
], [1.4165090423718034, -0.5324958842910054
], [1.4279593646268474, -0.5084218049460658
], [1.4388724778869602, -0.4844272494383845], [
1.449249011452494, -0.460516053858597
], [1.4590901922431447, -0.4366911061340692
], [1.468397775065033, -0.4129543806643518
], [1.4771739730209745, -0.38930697349737264
], [1.485421388504271, -0.36574913735813025],
[1.4931429451209484,
-0.3422803158986589], [1.5003418207921726, -0.3188991765927855], [
1.5070213821985838,
-0.2956036417497373
], [1.513185120641734,
-0.2723909171654731], [1.5188365893149296, -0.24925751796822435], [
1.5239793418959948,
-0.22619929124396096
], [1.5286168722972349,
-0.2032114350471563], [1.5327525553319497, -0.1802885134112242], [
1.5363895879810432,
-0.15742446697009113
], [1.5395309308654115, -0.1346126187862511
], [1.5421792494481048, -0.11184567494963411
], [1.5443368544016174, -0.0891157194637005
], [1.5460056404769755, -0.06641420286771664
], [1.5471870230984175, -0.043731923953761714],
[1.547881871775246,
-0.021059003819238205], [1.5480904392645911, 0.0016151486553661097], [
1.547812285227133, 0.024301881005710235
], [1.5470461928818935, 0.047013352405288866
], [1.5457900768723736,
0.06976260156314103], [1.5440408801865422, 0.09256362934244797], [
1.5417944575035705,
0.11543149864415554
], [1.5390454417383017, 0.13838245474060726
], [1.5357870897759938, 0.16143407007284788
], [1.5320111023738603, 0.18460541860588778
], [1.5277074118667517, 0.20791728625864334
], [1.5228639295308124, 0.23139242581719505
], [1.5174662420569858, 0.25505586728497265],
[1.5114972433117844,
0.27893529808946027], [1.5049366830391921, 0.30306153234932315], [
1.4977606078174102,
0.32746909510770755
], [1.4899406605544634,
0.35219695697813347], [1.4814431917184283, 0.37728946847450484], [
1.4722281161523716,
0.40279756372788583
], [1.4622474200998372,
0.4287803341537376], [1.4514431778273647, 0.45530712040457033], [
1.4397448652396179,
0.48246034696312606
], [1.427065639662639, 0.5103394485717625
], [1.4132970536897833, 0.5390664502570618
], [1.3983013135749314, 0.5687941401756967
], [1.3818995257862978, 0.5997184788509978
], [1.3638530549860057, 0.6320982830132342
], [1.3438323057823602, 0.6662881915757862],
[1.3213606855701236,
0.7027978462604986], [1.2957042956404132, 0.7424084023365868], [
1.2656247456808565,
0.786433646353686
], [1.2287034601894442,
0.8374338794107618], [1.1786904071656892, 0.902017503822168], [
1.0497616572686415,
1.0497616572686426
], [0.9097522267255194,
1.1864251300690412], [0.8484431816837388, 1.2397127624624213], [
0.8000049977037023,
1.2791960970308718
], [0.7581818853313602, 1.3114777786351866
], [0.7205493452982701, 1.3391121846078957
], [0.6882849734317291, 1.3617105524217008
], [0.6882849734317291, 2.356194490192345
], [1.3376784164442164, 2.356194490192345
], [1.3516056511178856, 2.3360528189227487],
[1.3708660530144583,
2.308792458458483], [1.3913233553973305, 2.2804469909122105], [
1.4131687110627962,
2.2508089759388485
], [1.436656614785,
2.219604401551449], [1.4621380338543308, 2.18645769238238], [
1.4901198983671067,
2.1508288797419346
], [1.5213822452925796, 2.111889875368928
], [1.5572408030205178, 2.0682471032649676
], [1.6002650085871786, 2.0171829478543404
], [1.657096323745671, 1.9516778011159774
], [1.7977393629734166, 1.7977393629734166
], [1.8577014383575772, 1.7364646168980775
], [1.8577014383575772, -1.7353804562372057])
# Test data set in terms of (theta0, theta1) of a motion from Parker.
# This is an example box lift profile.
path_with_accelerations = [
(1.3583511559969876, 0.99753029519739866, 0.63708920330895369,
-0.77079007974101643, -0.21483375398380378, -0.17756921128311187),
(1.4037744780290744, 0.94141413786797179, 0.62102298265172207,
-0.78379235452915641, -0.23084099683729808, -0.18290283743090688),
(1.4401396868545593, 0.89467165253531666, 0.606891738771278,
-0.79478451004733031, -0.24685749210031543, -0.18849894149927623),
(1.4710837705397868, 0.85345617877170399, 0.59376006111610313,
-0.80464215016577489, -0.26311393698286972, -0.19415687958799988),
(1.4982967960378542, 0.81597755723156562, 0.58119389263733834,
-0.81376511301545618, -0.27984413029527577, -0.19986598102433009),
(1.5227269571172215, 0.78122893136122973, 0.56894801707769227,
-0.82237348805962973, -0.29704891442706877, -0.20550961547441812),
(1.5449685847030388, 0.74857685672553398, 0.55686625320351735,
-0.83060217676278458, -0.31482379304256025, -0.21106978462024978),
(1.5654226738527495, 0.71759174238125301, 0.54484048864732038,
-0.83853970802255351, -0.33321287621660045, -0.21650513557965106),
(1.5843743655705165, 0.68796601612512265, 0.53279106733541426,
-0.84624681894089859, -0.35226023809265267, -0.22178093106252236),
(1.6020345343012865, 0.65947020123078393, 0.52065635488289785,
-0.85376633812774205, -0.37188463022164508, -0.22678852570730737),
(1.6185639244576269, 0.63192742228662813, 0.50838674358661662,
-0.86112886314731996, -0.39227952043050979, -0.23159139751917215),
(1.6340880277208036, 0.60519768777296334, 0.49594100637973321,
-0.868356216187261, -0.41330592927121557, -0.23605025316897368),
(1.6487067026409843, 0.57916772325998889, 0.48328397846038601,
-0.87546364639743945, -0.43479093110952094, -0.24001893904573279),
(1.6625006419641934, 0.55374413183503834, 0.47038503527185527,
-0.88246128447218319, -0.45715591769925784, -0.2436817242766752),
(1.6755358637715485, 0.52884863988931285, 0.45721707379330323,
-0.88935513009814537, -0.48005035022840359, -0.24679412703629519),
(1.6878669166029825, 0.50441469954774187, 0.44375581873319114,
-0.89614774079971626, -0.50380737957408728, -0.24947659419788909),
(1.6995392207123023, 0.48038500205371104, 0.42997935653089725,
-0.90283871923908732, -0.52783372117172589, -0.25138272577381626),
(1.7105908129416818, 0.45670961972392227, 0.41586782149756141,
-0.90942506840468851, -0.55267961927927023, -0.25273346631273114),
(1.7210536699519405, 0.43334459201815007, 0.40140320353386438,
-0.91590145113584731, -0.57770009966870806, -0.25318354738409404),
(1.7309547270317198, 0.41025083198931545, 0.38656923494570777,
-0.92226038979969771, -0.60317645612010684, -0.25282447346857195),
(1.7403166729890138, 0.38739326814551689, 0.37135135004406289,
-0.92849242044318914, -0.62907036438628838, -0.25159784841893079),
(1.7491585775728877, 0.36474016215239319, 0.35573669259875446,
-0.93458622156486959, -0.6551663269414254, -0.24938020059543903),
(1.7574963917487576, 0.34226255982896742, 0.33971416817223166,
-0.94052872574050006, -0.68164592193873064, -0.24620769226541636),
(1.7653433501187576, 0.31993384454025714, 0.32327453032164366,
-0.94630522456833177, -0.70757451300020135, -0.24172061458903144),
(1.7727102970907476, 0.29772937021195395, 0.30641049133414705,
-0.95189947515500117, -0.73378453575677771, -0.23620138680600353),
(1.7796059529520747, 0.27562615695138953, 0.28911685939500692,
-0.95729381154041093, -0.75990897245769828, -0.22950448838605128),
(1.7860371320859008, 0.25360263640618674, 0.27139068895919466,
-0.9624692690918778, -0.78523056852177775, -0.22141489322699751),
(1.7920089227108962, 0.23163843702152076, 0.25323144050682866,
-0.96740572540110403, -0.81025448009679157, -0.21209581939563796),
(1.7975248354162883, 0.20971420159976159, 0.2346411508301583,
-0.97208205946674009, -0.83408589705170777, -0.20133249883457041),
(1.8025869261905516, 0.187811431247703, 0.21562459548465329,
-0.97647633551565383, -0.85752536967991499, -0.18935884543741083),
(1.8071958984562269, 0.16591235107247332, 0.19618945367704119,
-0.98056600913243175, -0.8793458743889202, -0.17593847769244828),
(1.8113511877225346, 0.14399979396679621, 0.17634645105214913,
-0.98432816133711831, -0.89983233183814704, -0.16120962647016274),
(1.8150510317784807, 0.12205709958524935, 0.15610948821796866,
-0.98773975706575867, -0.91852354093170219, -0.14517104001739675),
(1.8182925288197413, 0.10006802621144681, 0.13549574094137548,
-0.99077792879471627, -0.93602333307674279, -0.12800867856143744),
(1.821071685494891, 0.078016673692446373, 0.11452572935697461,
-0.99342028231522084, -0.95112385045254688, -0.10965064890920556),
(1.8233834565427998, 0.055887416001137141, 0.093223341590826445,
-0.99564522224667962, -0.96368362602021329, -0.090231797498176183),
(1.8252217774528923, 0.03366484230234558, 0.071615815013953282,
-0.99743229095507402, -0.97395276760288163, -0.069931055757802438),
(1.8265795913984428, 0.011333705660752967, 0.049733665943173709,
-0.99876251555204698, -0.98177893706111263, -0.048889055531294821),
(1.8274488715589303, -0.011121121248675203, 0.02761056315188902,
-0.99961875572762016, -0.98689335559342106, -0.027260179511112551),
(1.8278206398523622, -0.033714683874300592, 0.0052831412013899706,
-0.99998604411213976, -0.98885489268650051, -0.0052254487261787384),
(1.8276849830361703, -0.056461977241467148, -0.017209244112191453,
-0.99985190999321838, -0.98812791978941406, 0.017006330622021722),
(1.8270310671011307, -0.079377971247817633, -0.039824819694542858,
-0.99920667718760625, -0.98450390283226485, 0.039237693528591036),
(1.8258471508733223, -0.10247762977900209, -0.062519753866286318,
-0.99804372668560926, -0.97719363903338485, 0.061212532653782022),
(1.8241205997516765, -0.12577592348843702, -0.085248569812080663,
-0.99635971483445407, -0.9676884196650537, 0.08279433361404552),
(1.8218379005412455, -0.14928783595476772, -0.10796459505798382,
-0.99415473957224865, -0.95496997071328249, 0.103708042997466),
(1.8189846783932371, -0.17302836280361969, -0.13062045461685196,
-0.99143244693508337, -0.93935720014890423, 0.12375848445745616),
(1.8155457169306066, -0.19701250325490266, -0.15316858191907523,
-0.98820007362522466, -0.92129127811992539, 0.14279680293047756),
(1.8115049827211411, -0.22125524343262917, -0.17556174489393769,
-0.98446842190585071, -0.90059657633599033, 0.16060369204333666),
(1.8068456553568204, -0.24577153065112339, -0.19775357131740673,
-0.98025176614541798, -0.87754964758424281, 0.17703366424976713),
(1.8015501645066898, -0.27057623777063011, -0.21969906367786421,
-0.97556769186923664, -0.85256620853260556, 0.19199796793136439),
(1.795600235427889, -0.29568411659857541, -0.24135508622397667,
-0.97043687190554384, -0.82563764988942245, 0.20534143268497296),
(1.7889769445422168, -0.32110973920321939, -0.26268082100774776,
-0.96488278369690872, -0.79733324597202848, 0.2170659935022537),
(1.7816607868090406, -0.34686742590848585, -0.2836381738799823,
-0.95893137727265387, -0.76782873106500538, 0.22711183686998365),
(1.7736317567432636, -0.37297115865829411, -0.30419213280652752,
-0.95261070030659223, -0.73738784843863214, 0.23546528518662449),
(1.7648694450316516, -0.39943447838330554, -0.32431106313855179,
-0.94595049253433039, -0.70610753433104578, 0.2420821140887684),
(1.7553531527820141, -0.42627036498226889, -0.34396694344403017,
-0.93898175797923322, -0.67467492193224388, 0.24714550708779132),
(1.7450620254853675, -0.45349109855564618, -0.36313553561943973,
-0.93173632684916963, -0.64280482438985487, 0.25052642309609846),
(1.7339752087663065, -0.48110810061485765, -0.38179649614837224,
-0.92424641493966653, -0.61125522193929116, 0.2525024014306243),
(1.7220720279237476, -0.50913175415228962, -0.39993342129901005,
-0.91654419343972093, -0.58016017720123902, 0.25315181203742848),
(1.7093321931028456, -0.53757120171300954, -0.41753384214477446,
-0.90866137293483673, -0.54964898595174738, 0.25256545404213349),
(1.6957360316664474, -0.56643412097840984, -0.43458916639025197,
-0.9006288116955985, -0.51972307699036913, 0.25078642241782151),
(1.6812647489267576, -0.59572647787451594, -0.45109457862962754,
-0.89247615157546845, -0.49088885529812037, 0.24811508685641395),
(1.6659007178300929, -0.62545225787260295, -0.46704890024189227,
-0.88423148823305242, -0.46296888429931843, 0.24453849915066983),
(1.6496277974361373, -0.65561317697342414, -0.48245442463761296,
-0.87592107415428122, -0.43630053035835431, 0.24031239163872659),
(1.6324316790779632, -0.68620837486997288, -0.49731672574028513,
-0.86756906024763358, -0.41095471910587472, 0.23557116495526012),
(1.6143002579172752, -0.7172340939700278, -0.51164445121259294,
-0.85919727393850864, -0.38675403181117768, 0.23030820687574571),
(1.5952240262184549, -0.748683349319481, -0.52544910775441078,
-0.85082503204836046, -0.36408233991880601, 0.22484810685703166),
(1.5751964830679697, -0.780545595975073, -0.53874483689504815,
-0.84246899095392713, -0.34276938388236106, 0.21919491474821423),
(1.5542145534957212, -0.81280640198469889, -0.55154819264623711,
-0.83414302801658013, -0.32291038254364612, 0.21351295685970414),
(1.5322790080695103, -0.84544713677503402, -0.56387791937564813,
-0.82585815491559456, -0.30444498192901781, 0.20786805829508584),
(1.509394872119298, -0.87844468632349793, -0.57575473429345714,
-0.81762245929198296, -0.28737482236853346, 0.20236376870812761),
(1.4855718119204786, -0.91177120788178356, -0.58720111439137757,
-0.8094410733694728, -0.27173204177162957, 0.1971250636942595),
(1.4608244835702884, -0.94539393807497152, -0.59824108887263983,
-0.80131616705547515, -0.25741396251510884, 0.19217806498346204),
(1.4351728290976418, -0.97927506876108006, -0.60890003734400222,
-0.79324696313473042, -0.24447483076597848, 0.18765975242966434),
(1.4086423037364302, -1.0133717049339268, -0.61920449072176709,
-0.78522977444184905, -0.2328103488745005, 0.18358578028961331),
(1.3812640184459468, -1.047635918033063, -0.62918193604047223,
-0.77725805969469564, -0.22240924347971955, 0.18003762666126619),
(1.3530747828369902, -1.0820149061676485, -0.63886062143319422,
-0.76932249829443622, -0.21321450222713362, 0.17705731568712871),
(1.324117035769417, -1.1164512699023377, -0.64826936036845306,
-0.76141108240389876, -0.20523063810290823, 0.17473421668463424),
(1.294438654066159, -1.1508834084076087, -0.6574373330314689,
-0.75350922564788103, -0.19832793065660315, 0.17304104866571926),
(1.2640926339871807, -1.1852460360561055, -0.66639388399919663,
-0.74559988691553936, -0.1925723524639939, 0.17211494490833781),
(1.2331366451651338, -1.2194708141674491, -0.67516831336894412,
-0.73766370970960415, -0.18786875722059337, 0.17195217715824379),
(1.2016324623545263, -1.2534870868845998, -0.68378966242702388,
-0.72967917440333774, -0.18419866624510314, 0.17261421082264014),
(1.1696452862242701, -1.2872227045046678, -0.6922864921531301,
-0.72162276348679166, -0.18150423850780073, 0.17412530594411615),
(1.1372429700984137, -1.3206049124326309, -0.70068665547523046,
-0.71346913797229905, -0.17977253485932801, 0.17655152952198189),
(1.104495174566031, -1.3535612797241663, -0.70901706362016093,
-0.70519132403585671, -0.17898758721428376, 0.17995840096834861),
(1.0714724758096574, -1.3860206383273435, -0.71730344800943324,
-0.69676090839955884, -0.17907637813730631, 0.18435585053172451),
(1.0382454559924987, -1.4179140029119093, -0.72557011960647111,
-0.68814824095848082, -0.18003314612431659, 0.18982321746977993),
(1.0048838048727911, -1.4491754417344611, -0.73383972725116353,
-0.67932264404179699, -0.18184420373071258, 0.19643734513631667),
(0.97145546090878643, -1.4797428713062153, -0.74213301743428883,
-0.67025262732336799, -0.18446052990619061, 0.20424250843873848)
]