#!/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)]
