diff --git a/choreo.chor b/choreo.chor index c4e8fdc..93c2062 100644 --- a/choreo.chor +++ b/choreo.chor @@ -2773,8 +2773,8 @@ "controlIntervalCount": 9 }, { - "x": 2.349189281463623, - "y": 5.567087650299072, + "x": 2.4236247539520264, + "y": 5.564249038696289, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -2791,22 +2791,22 @@ "controlIntervalCount": 14 }, { - "x": 2.1126484870910645, - "y": 6.068657398223877, - "heading": 0.9505466984245466, + "x": 2.5229711532592773, + "y": 6.060981750488281, + "heading": 0.8353565172264098, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 8 }, { - "x": 2.583202600479126, - "y": 6.5326762199401855, - "heading": 0.9505466984245466, + "x": 2.891972541809082, + "y": 6.472559928894043, + "heading": 0.8097835525950351, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 17 }, { "x": 1.3126474618911743, @@ -2824,16 +2824,16 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 5 + "controlIntervalCount": 7 }, { - "x": 2.4599039554595947, - "y": 4.502241134643555, + "x": 2.5655484199523926, + "y": 4.357899188995361, "heading": -0.7298997067825479, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 16 }, { "x": 1.3126474618911743, @@ -2849,824 +2849,960 @@ { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": -1.5437737592748072e-37, - "angularVelocity": -7.89083646860229e-38, - "velocityX": 1.418479553941243e-23, - "velocityY": 2.4322727819891274e-38, + "heading": 1.1953764755650808e-32, + "angularVelocity": 6.990086021448287e-33, + "velocityX": 5.494174654732201e-19, + "velocityY": 4.78279637249085e-34, "timestamp": 0 }, { - "x": 1.392444928049753, + "x": 1.3924449280497553, "y": 5.567087650299072, - "heading": -4.6220470147664874e-27, - "angularVelocity": -3.7948407554824907e-26, - "velocityX": 0.6551613945414111, - "velocityY": 1.137046594589441e-15, - "timestamp": 0.12179818106412381 + "heading": 4.1001262381929663e-22, + "angularVelocity": 3.3663279452468632e-21, + "velocityX": 0.6551613945414227, + "velocityY": 5.759043744920587e-16, + "timestamp": 0.12179818106412517 }, { - "x": 1.5386027425923001, + "x": 1.538602742592314, "y": 5.567087650299072, - "heading": -1.9365333413339378e-22, - "angularVelocity": -1.5899146472838326e-21, - "velocityX": 1.1999999775497356, - "velocityY": 2.082625593248517e-15, - "timestamp": 0.24359636212824762 + "heading": -9.345363045002027e-18, + "angularVelocity": -7.673163046652378e-17, + "velocityX": 1.199999977549737, + "velocityY": 1.0548320493532893e-15, + "timestamp": 0.24359636212825034 }, { - "x": 1.6847605571348494, + "x": 1.6847605571348563, "y": 5.567087650299072, - "heading": -1.4409951368437792e-22, - "angularVelocity": 4.0685189233595474e-22, + "heading": -1.2209432374345432e-17, + "angularVelocity": -2.351487757270866e-17, "velocityX": 1.199999977549752, - "velocityY": 2.082625593248546e-15, - "timestamp": 0.3653945431923714 + "velocityY": 1.054832049353261e-15, + "timestamp": 0.3653945431923755 }, { "x": 1.8309183716773985, "y": 5.567087650299072, - "heading": 2.0899654958449072e-23, - "angularVelocity": 1.3546932080698235e-21, - "velocityX": 1.199999977549752, - "velocityY": 2.0826255932485463e-15, - "timestamp": 0.48719272425649524 + "heading": -1.068309383473616e-17, + "angularVelocity": 1.2531702212408767e-17, + "velocityX": 1.1999999775497523, + "velocityY": 1.0548320493532611e-15, + "timestamp": 0.4871927242565007 }, { - "x": 1.9770761862199477, + "x": 1.9770761862199266, "y": 5.567087650299072, - "heading": 1.690438544316869e-22, - "angularVelocity": 1.2163088001720124e-21, - "velocityX": 1.199999977549752, - "velocityY": 2.082625593248546e-15, - "timestamp": 0.6089909053206191 + "heading": -6.939437782754445e-18, + "angularVelocity": 3.0736551271554804e-17, + "velocityX": 1.1999999775497523, + "velocityY": 1.0548320493532611e-15, + "timestamp": 0.6089909053206258 }, { "x": 2.1232340007624972, "y": 5.567087650299072, - "heading": 1.86290605944189e-22, - "angularVelocity": 1.416010597352205e-22, + "heading": -2.9324482572141887e-18, + "angularVelocity": 3.289859892177714e-17, "velocityX": 1.199999977549752, - "velocityY": 2.082625593248546e-15, - "timestamp": 0.7307890863847428 + "velocityY": 1.054832049353261e-15, + "timestamp": 0.730789086384751 }, { - "x": 2.2693918153050445, + "x": 2.269391815305044, "y": 5.567087650299072, - "heading": 4.5629176638525406e-27, - "angularVelocity": -1.5294649016839573e-21, - "velocityX": 1.1999999775497356, - "velocityY": 2.0826255932485175e-15, - "timestamp": 0.8525872674488666 + "heading": -2.924223024384178e-22, + "angularVelocity": 2.407388842978096e-17, + "velocityX": 1.199999977549737, + "velocityY": 1.0548320493532885e-15, + "timestamp": 0.8525872674488763 }, { "x": 2.349189281463623, "y": 5.567087650299072, - "heading": -1.0800252356875386e-31, - "angularVelocity": -3.7463824388368756e-26, - "velocityX": 0.6551613945414111, - "velocityY": 1.1370465945894429e-15, - "timestamp": 0.9743854485129904 + "heading": 1.6673685334753995e-30, + "angularVelocity": 2.4008757783240606e-21, + "velocityX": 0.6551613945414226, + "velocityY": 5.759043744915343e-16, + "timestamp": 0.9743854485130015 }, { "x": 2.349189281463623, "y": 5.567087650299072, - "heading": -1.15233598675018e-31, - "angularVelocity": -5.936794878517772e-32, - "velocityX": -2.2921562380311214e-24, - "velocityY": 2.1917263454721895e-30, - "timestamp": 1.0961836295771141 + "heading": 1.7831791313090364e-30, + "angularVelocity": 8.43744338443602e-31, + "velocityX": -1.487174311186839e-19, + "velocityY": -5.252529696759049e-28, + "timestamp": 1.0961836295771266 }, { - "x": 2.2693918153050445, + "x": 2.269391815305044, "y": 5.567087650299072, - "heading": -3.475580412192629e-21, - "angularVelocity": -2.8535569099611155e-20, - "velocityX": -0.6551613945414112, - "velocityY": 1.824069231632628e-15, - "timestamp": 1.217981810641238 + "heading": 4.900643599154424e-20, + "angularVelocity": 4.023576968464745e-19, + "velocityX": -0.655161394541426, + "velocityY": -7.280112591641159e-16, + "timestamp": 1.2179818106412523 }, { "x": 2.1232340007624972, "y": 5.567087650299072, - "heading": 7.19228614469986e-17, - "angularVelocity": 5.905370375731918e-16, - "velocityX": -1.1999999775497356, - "velocityY": 3.3409829244929845e-15, - "timestamp": 1.3397799917053617 + "heading": 7.594029969730495e-17, + "angularVelocity": 6.23090526860376e-16, + "velocityX": -1.199999977549737, + "velocityY": -1.3334248944822027e-15, + "timestamp": 1.339779991705378 }, { - "x": 1.9770761862199482, + "x": 1.9770761862199409, "y": 5.567087650299072, - "heading": 5.1366024248226334e-17, - "angularVelocity": -1.6877786695311564e-16, + "heading": 4.257511726642054e-17, + "angularVelocity": -2.7393827208826133e-16, "velocityX": -1.199999977549752, - "velocityY": 3.3409829342395925e-15, - "timestamp": 1.4615781727694854 + "velocityY": -1.3334343966180895e-15, + "timestamp": 1.4615781727695039 }, { "x": 1.8309183716773987, "y": 5.567087650299072, - "heading": -1.7979701811530337e-18, - "angularVelocity": -4.3649251544560875e-16, + "heading": -2.2065028106115054e-17, + "angularVelocity": -5.307152037473876e-16, "velocityX": -1.199999977549752, - "velocityY": 3.340982934381562e-15, - "timestamp": 1.5833763538336092 + "velocityY": -1.3334344015361252e-15, + "timestamp": 1.5833763538336296 }, { - "x": 1.6847605571348494, + "x": 1.684760557134842, "y": 5.567087650299072, - "heading": -5.627263788476133e-17, - "angularVelocity": -4.472535404689559e-16, + "heading": -8.169986730219564e-17, + "angularVelocity": -4.896201228598656e-16, "velocityX": -1.199999977549752, - "velocityY": 3.3409829343799955e-15, - "timestamp": 1.705174534897733 + "velocityY": -1.3334344018518055e-15, + "timestamp": 1.7051745348977554 }, { - "x": 1.5386027425923001, + "x": 1.5386027425923, "y": 5.567087650299072, - "heading": -7.146734861941795e-17, - "angularVelocity": -1.2475318269865482e-16, + "heading": -8.15243225571019e-17, + "angularVelocity": 1.4412662667122633e-18, "velocityX": -1.199999977549752, - "velocityY": 3.3409829330177503e-15, - "timestamp": 1.8269727159618567 + "velocityY": -1.3334342978189559e-15, + "timestamp": 1.8269727159618812 }, { - "x": 1.3924449280497528, + "x": 1.392444928049753, "y": 5.567087650299072, - "heading": 2.401745836439516e-21, - "angularVelocity": 5.867883226238598e-16, - "velocityX": -1.1999999775497356, - "velocityY": 3.3409828502355826e-15, - "timestamp": 1.9487708970259805 + "heading": -9.444741136401807e-19, + "angularVelocity": 6.615849837325982e-16, + "velocityX": -1.199999977549737, + "velocityY": -1.3333514424854479e-15, + "timestamp": 1.948770897026007 }, { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": -1.9529795908474793e-22, - "angularVelocity": -2.1322517075995374e-20, - "velocityX": -0.6551613945414112, - "velocityY": 1.824070118744852e-15, - "timestamp": 2.0705690780901045 + "heading": -9.625683020163944e-19, + "angularVelocity": -1.4856176769231114e-19, + "velocityX": -0.6551613945414261, + "velocityY": -7.279052775637916e-16, + "timestamp": 2.070569078090133 }, { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": -2.0997399222925942e-22, - "angularVelocity": -1.20494685567255e-22, - "velocityX": -2.042395408130124e-22, - "velocityY": 8.462664089537296e-22, - "timestamp": 2.1923672591542283 - }, - { - "x": 1.330224348661209, - "y": 5.5781076780240015, - "heading": 1.3752213180599183e-8, - "angularVelocity": 2.2144266656949007e-7, - "velocityX": 0.28302882054194556, - "velocityY": 0.17744811638904515, - "timestamp": 2.2544700751726765 - }, - { - "x": 1.3653781204474067, - "y": 5.6001477324054765, - "heading": 4.561370782539336e-8, - "angularVelocity": 5.130442818459255e-7, - "velocityX": 0.5660576128424637, - "velocityY": 0.3548962155746409, - "timestamp": 2.3165728911911248 - }, - { - "x": 1.4181087719881549, - "y": 5.633207810238353, - "heading": 1.0865567864033805e-7, - "angularVelocity": 0.0000010151225798878058, - "velocityX": 0.8490863204187717, - "velocityY": 0.532344263149933, - "timestamp": 2.378675707209573 - }, - { - "x": 1.4812487494892455, - "y": 5.672794131521707, - "heading": 0.017849702169017073, - "angularVelocity": 0.28742003435135793, - "velocityX": 1.0167007158312162, - "velocityY": 0.6374319849134539, - "timestamp": 2.4407785232280212 - }, - { - "x": 1.5443887266874352, - "y": 5.712380453288194, - "heading": 0.06529469396388879, - "angularVelocity": 0.7639748861110869, - "velocityX": 1.0167007109538098, - "velocityY": 0.6374319926930171, - "timestamp": 2.5028813392464695 - }, - { - "x": 1.607528703885691, - "y": 5.751966775054576, - "heading": 0.13761827273616745, - "angularVelocity": 1.1645780885490655, - "velocityX": 1.0167007109548667, - "velocityY": 0.6374319926913311, - "timestamp": 2.5649841552649177 - }, - { - "x": 1.6706686810840699, - "y": 5.791553096820763, - "heading": 0.22970172720620147, - "angularVelocity": 1.4827581158748078, - "velocityX": 1.016700710956855, - "velocityY": 0.6374319926881596, - "timestamp": 2.627086971283366 - }, - { - "x": 1.7338086582825494, - "y": 5.831139418586787, - "heading": 0.336088558287123, - "angularVelocity": 1.7130757975502817, - "velocityX": 1.0167007109584723, - "velocityY": 0.6374319926855803, - "timestamp": 2.6891897873018142 - }, - { - "x": 1.7969486354809578, - "y": 5.870725740352926, - "heading": 0.4510732003163099, - "angularVelocity": 1.8515205815309534, - "velocityX": 1.0167007109573307, - "velocityY": 0.6374319926874006, - "timestamp": 2.7512926033202625 - }, - { - "x": 1.860088612679118, - "y": 5.910312062119461, - "heading": 0.5688014129904255, - "angularVelocity": 1.895698459779078, - "velocityX": 1.0167007109533306, - "velocityY": 0.6374319926937808, - "timestamp": 2.8133954193387107 - }, - { - "x": 1.9232285898770611, - "y": 5.949898383886342, - "heading": 0.6833720439947638, - "angularVelocity": 1.8448540396349185, - "velocityX": 1.0167007109498374, - "velocityY": 0.6374319926993527, - "timestamp": 2.875498235357159 - }, - { - "x": 1.9863685670748548, - "y": 5.989484705653461, - "heading": 0.7889380109564055, - "angularVelocity": 1.6998579731115984, - "velocityX": 1.0167007109474246, - "velocityY": 0.6374319927032011, - "timestamp": 2.9376010513756072 - }, - { - "x": 2.049508544272936, - "y": 6.029071027420121, - "heading": 0.8798080925206222, - "angularVelocity": 1.4632199856641441, - "velocityX": 1.0167007109520676, - "velocityY": 0.6374319926957958, - "timestamp": 2.9997038673940555 - }, - { - "x": 2.1126484870910645, - "y": 6.068657398223877, + "heading": -1.0240569848515672e-18, + "angularVelocity": -5.048437233348486e-19, + "velocityX": 3.814586359746447e-19, + "velocityY": 1.4611187628635838e-19, + "timestamp": 2.192367259154259 + }, + { + "x": 1.3467835531597558, + "y": 5.582578303497341, + "heading": 2.1822298286556078e-8, + "angularVelocity": 2.6140695812051043e-7, + "velocityX": 0.408912555227134, + "velocityY": 0.18556086376752873, + "timestamp": 2.2758474331723977 + }, + { + "x": 1.4150557303762996, + "y": 5.61355960759438, + "heading": 7.969110326356027e-8, + "angularVelocity": 6.932041753657309e-7, + "velocityX": 0.8178250467198802, + "velocityY": 0.37112169998949157, + "timestamp": 2.3593276071905365 + }, + { + "x": 1.5062786970732867, + "y": 5.65495577384632, + "heading": 0.029442185922842156, + "angularVelocity": 0.35268381478627076, + "velocityX": 1.0927500783256299, + "velocityY": 0.4958802103472522, + "timestamp": 2.4428077812086753 + }, + { + "x": 1.5975016635378336, + "y": 5.696351940610345, + "heading": 0.11439605036500154, + "angularVelocity": 1.0176531786301763, + "velocityX": 1.0927500755419548, + "velocityY": 0.49588021648158287, + "timestamp": 2.526287955226814 + }, + { + "x": 1.6887246300035204, + "y": 5.737748107371955, + "heading": 0.24137697471085723, + "angularVelocity": 1.5210907959806668, + "velocityX": 1.0927500755551556, + "velocityY": 0.4958802164524925, + "timestamp": 2.609768129244953 + }, + { + "x": 1.779947596469001, + "y": 5.779144274133999, + "heading": 0.394686770242847, + "angularVelocity": 1.836481503963777, + "velocityX": 1.0927500755527955, + "velocityY": 0.4958802164576932, + "timestamp": 2.6932483032630916 + }, + { + "x": 1.8711705629341966, + "y": 5.8205404408966785, + "heading": 0.5569437718053947, + "angularVelocity": 1.9436591199162039, + "velocityX": 1.0927500755492763, + "velocityY": 0.49588021646544844, + "timestamp": 2.7767284772812304 + }, + { + "x": 1.9623935293992807, + "y": 5.861936607659589, + "heading": 0.7101331870008885, + "angularVelocity": 1.8350394808976778, + "velocityX": 1.0927500755480895, + "velocityY": 0.4958802164680637, + "timestamp": 2.860208651299369 + }, + { + "x": 2.053616495864439, + "y": 5.903332774422348, + "heading": 0.8368837129910643, + "angularVelocity": 1.518330878929826, + "velocityX": 1.0927500755488537, + "velocityY": 0.49588021646637936, + "timestamp": 2.943688825317508 + }, + { + "x": 2.144839462330586, + "y": 5.944728941182871, + "heading": 0.9215115215035561, + "angularVelocity": 1.01374738981861, + "velocityX": 1.0927500755610373, + "velocityY": 0.4958802164395315, + "timestamp": 3.0271689993356468 + }, + { + "x": 2.236062428594661, + "y": 5.986125108388746, + "heading": 0.9505466187573102, + "angularVelocity": 0.34780829814088937, + "velocityX": 1.0927500731401507, + "velocityY": 0.49588022177427094, + "timestamp": 3.1106491733537855 + }, + { + "x": 2.304334606053628, + "y": 6.017106411951638, + "heading": 0.950546676609113, + "angularVelocity": 6.93000507489643e-7, + "velocityX": 0.8178250496233883, + "velocityY": 0.371121693591152, + "timestamp": 3.1941293473719243 + }, + { + "x": 2.338470697402953, + "y": 6.032597064971924, "heading": 0.9505466984245466, - "angularVelocity": 1.1390563333377834, - "velocityX": 1.0167001573547376, - "velocityY": 0.63743278230727, - "timestamp": 3.0618066834125037 - }, - { - "x": 2.1886009078042274, - "y": 6.143554890170836, - "heading": 1.0124745802683783, - "angularVelocity": 0.6966694651666266, - "velocityX": 0.8544411780429775, - "velocityY": 0.8425735565862382, - "timestamp": 3.150698022763827 - }, - { - "x": 2.264553290701356, - "y": 6.2184524204664475, - "heading": 1.0234437942174555, - "angularVelocity": 0.12340025506561028, - "velocityX": 0.8544407526243177, - "velocityY": 0.8425739879966911, - "timestamp": 3.2395893621151504 - }, - { - "x": 2.3405056735984604, - "y": 6.293349950762083, - "heading": 1.0023010522506035, - "angularVelocity": -0.23784929016864031, - "velocityX": 0.8544407526240461, - "velocityY": 0.8425739879969667, - "timestamp": 3.3284807014664737 - }, - { - "x": 2.4164580564955584, - "y": 6.368247481057726, - "heading": 0.9705071197009111, - "angularVelocity": -0.3576718809920707, - "velocityX": 0.8544407526239729, - "velocityY": 0.8425739879970408, - "timestamp": 3.417372040817797 - }, - { - "x": 2.492410439384703, - "y": 6.443145011361432, - "heading": 0.950546735518749, - "angularVelocity": -0.22454813177325503, - "velocityX": 0.8544407525345075, - "velocityY": 0.8425739880877493, - "timestamp": 3.5062633801691203 - }, - { - "x": 2.5529385454627884, - "y": 6.502832482446322, - "heading": 0.9505467092628421, - "angularVelocity": -2.953708100289274e-7, - "velocityX": 0.6809224219117733, - "velocityY": 0.6714655389428754, - "timestamp": 3.5951547195204436 - }, - { - "x": 2.583202600479126, - "y": 6.5326762199401855, + "angularVelocity": 2.613247280768537e-7, + "velocityX": 0.40891255619464567, + "velocityY": 0.18556086163546412, + "timestamp": 3.277609521390063 + }, + { + "x": 2.338470697402954, + "y": 6.032597064971924, "heading": 0.9505466984245466, - "angularVelocity": -1.2192746279649323e-7, - "velocityX": 0.3404612331998472, - "velocityY": 0.335732791424291, - "timestamp": 3.684046058871767 + "angularVelocity": -5.839076180969031e-19, + "velocityX": 1.0474679300431341e-18, + "velocityY": 9.135743131461031e-19, + "timestamp": 3.361089695408202 }, { - "x": 2.583202600479126, - "y": 6.5326762199401855, + "x": 2.369756744125885, + "y": 6.076174051247526, "heading": 0.9505466984245466, - "angularVelocity": 1.0876323700343221e-22, - "velocityX": 1.7516641677537848e-21, - "velocityY": -1.1441920806239185e-21, - "timestamp": 3.7729373982230903 - }, - { - "x": 2.542403880498974, - "y": 6.501670263198367, - "heading": 0.9505466903049901, - "angularVelocity": -8.318907821701634e-8, - "velocityX": -0.41800409947017914, - "velocityY": -0.3176721483512302, - "timestamp": 3.8705410342445177 - }, - { - "x": 2.460806445400462, - "y": 6.439658353412451, - "heading": 0.9505466688810346, - "angularVelocity": -2.194995637368503e-7, - "velocityX": -0.8360081491287711, - "velocityY": -0.6353442588173928, - "timestamp": 3.968144670265945 - }, - { - "x": 2.367555279184838, - "y": 6.368789913715819, - "heading": 0.9153741187078941, - "angularVelocity": -0.3603610644732418, - "velocityX": -0.9554066837751003, - "velocityY": -0.7260840127008578, - "timestamp": 4.0657483062873725 - }, - { - "x": 2.2743041129628643, - "y": 6.297921474027541, - "heading": 0.8483526672144899, - "angularVelocity": -0.6866696183192453, - "velocityX": -0.9554066838401561, - "velocityY": -0.7260840126152636, - "timestamp": 4.1633519423088 - }, - { - "x": 2.1810529467408974, - "y": 6.227053034339255, - "heading": 0.7577571566962064, - "angularVelocity": -0.9281981103490298, - "velocityX": -0.9554066838400862, - "velocityY": -0.7260840126153555, - "timestamp": 4.260955578330228 - }, - { - "x": 2.087801780518917, - "y": 6.156184594650985, - "heading": 0.651002233808638, - "angularVelocity": -1.0937596921505226, - "velocityX": -0.9554066838402226, - "velocityY": -0.726084012615176, - "timestamp": 4.358559214351656 - }, - { - "x": 1.994550614296896, - "y": 6.0853161549627695, - "heading": 0.5348344163006928, - "angularVelocity": -1.190199691766012, - "velocityX": -0.9554066838406402, - "velocityY": -0.7260840126146266, - "timestamp": 4.456162850373084 - }, - { - "x": 1.9012994480748335, - "y": 6.014447715274608, - "heading": 0.41557973663701897, - "angularVelocity": -1.2218261995638482, - "velocityX": -0.9554066838410652, - "velocityY": -0.7260840126140673, - "timestamp": 4.553766486394512 - }, - { - "x": 1.8080482818527632, - "y": 5.943579275586458, - "heading": 0.2994210690519452, - "angularVelocity": -1.1901059460486971, - "velocityX": -0.9554066838411454, - "velocityY": -0.7260840126139618, - "timestamp": 4.65137012241594 - }, - { - "x": 1.7147971156307207, - "y": 5.87271083589827, - "heading": 0.19268402440905563, - "angularVelocity": -1.0935765202380057, - "velocityX": -0.95540668384086, - "velocityY": -0.7260840126143373, - "timestamp": 4.7489737584373675 - }, - { - "x": 1.6215459494087139, - "y": 5.801842396210036, - "heading": 0.10211451516138965, - "angularVelocity": -0.9279317138122054, - "velocityX": -0.9554066838404965, - "velocityY": -0.7260840126148156, - "timestamp": 4.846577394458795 - }, - { - "x": 1.5282947831867273, - "y": 5.730973956521775, - "heading": 0.035127860642323504, - "angularVelocity": -0.6863131052244829, - "velocityX": -0.955406683840287, - "velocityY": -0.7260840126150913, - "timestamp": 4.944181030480223 - }, - { - "x": 1.4350436169609884, - "y": 5.660105516838453, - "heading": 2.9548859844204172e-8, - "angularVelocity": -0.35990289425029026, - "velocityX": -0.9554066838787327, - "velocityY": -0.7260840125644947, - "timestamp": 5.041784666501651 - }, - { - "x": 1.353446181869114, - "y": 5.598093607043802, - "heading": 8.120889195643221e-9, - "angularVelocity": -2.1954070075685228e-7, - "velocityX": -0.8360081490607667, - "velocityY": -0.6353442589068752, - "timestamp": 5.139388302523079 + "angularVelocity": 8.034071812555556e-18, + "velocityX": 0.31328557933308754, + "velocityY": 0.43636198308634433, + "timestamp": 3.4609540006517374 + }, + { + "x": 2.4323288339012104, + "y": 6.163328018686257, + "heading": 0.9505466984245466, + "angularVelocity": 1.2139716514941585e-17, + "velocityX": 0.6265711219112612, + "velocityY": 0.8727239149783513, + "timestamp": 3.560818305895273 + }, + { + "x": 2.5022186881182034, + "y": 6.260674584872551, + "heading": 0.9505466984245466, + "angularVelocity": 1.081029516788744e-15, + "velocityX": 0.6998481994802278, + "velocityY": 0.9747883986064685, + "timestamp": 3.6606826111388084 + }, + { + "x": 2.572108542335245, + "y": 6.358021151058831, + "heading": 0.9505466984245466, + "angularVelocity": 1.3587937947073691e-16, + "velocityX": 0.6998481994802291, + "velocityY": 0.974788398606472, + "timestamp": 3.760546916382344 + }, + { + "x": 2.641998396552295, + "y": 6.455367717245125, + "heading": 0.9505466984245468, + "angularVelocity": 5.109002093997956e-16, + "velocityX": 0.6998481994802279, + "velocityY": 0.9747883986064684, + "timestamp": 3.8604112216258795 + }, + { + "x": 2.7045704863276203, + "y": 6.54252168468387, + "heading": 0.9505466984245466, + "angularVelocity": 9.967273568896952e-18, + "velocityX": 0.6265711219112611, + "velocityY": 0.8727239149783513, + "timestamp": 3.960275526869415 + }, + { + "x": 2.735856533050537, + "y": 6.586098670959473, + "heading": 0.9505466984245466, + "angularVelocity": 2.3352525714710484e-17, + "velocityX": 0.3132855793330876, + "velocityY": 0.43636198308634433, + "timestamp": 4.0601398321129505 + }, + { + "x": 2.735856533050537, + "y": 6.586098670959473, + "heading": 0.9505466984245466, + "angularVelocity": 4.489031866337397e-20, + "velocityX": -2.841253050020481e-19, + "velocityY": -1.4670872630302605e-19, + "timestamp": 4.160004137356486 + }, + { + "x": 2.6926372151215485, + "y": 6.555153842004882, + "heading": 0.9505466918112597, + "angularVelocity": -6.652694848851377e-8, + "velocityX": -0.4347685747061206, + "velocityY": -0.3112922605861828, + "timestamp": 4.259411777290483 + }, + { + "x": 2.6061985842528834, + "y": 6.493264187669074, + "heading": 0.9505466743461212, + "angularVelocity": -1.756921161749788e-7, + "velocityX": -0.8695370992212547, + "velocityY": -0.6225844852256829, + "timestamp": 4.358819417224479 + }, + { + "x": 2.5092074864577216, + "y": 6.423819016493211, + "heading": 0.9190781371048612, + "angularVelocity": -0.3165605506996643, + "velocityX": -0.9756905793112982, + "velocityY": -0.6985898792283688, + "timestamp": 4.458227057158476 + }, + { + "x": 2.4122163886604104, + "y": 6.354373845320345, + "heading": 0.8601974459911542, + "angularVelocity": -0.5923155519314354, + "velocityX": -0.9756905793328975, + "velocityY": -0.6985898791982086, + "timestamp": 4.557634697092473 + }, + { + "x": 2.3152252908630935, + "y": 6.28492867414746, + "heading": 0.7807467252149503, + "angularVelocity": -0.7992415958064828, + "velocityX": -0.975690579332817, + "velocityY": -0.698589879198321, + "timestamp": 4.6570423370264695 + }, + { + "x": 2.2182341930657996, + "y": 6.215483502974575, + "heading": 0.6866750516739581, + "angularVelocity": -0.9463223712327561, + "velocityX": -0.9756905793328342, + "velocityY": -0.6985898791982971, + "timestamp": 4.756449976960466 + }, + { + "x": 2.121243095268475, + "y": 6.146038331801719, + "heading": 0.5832432706368847, + "angularVelocity": -1.0404812055265393, + "velocityX": -0.9756905793329995, + "velocityY": -0.698589879198066, + "timestamp": 4.855857616894463 + }, + { + "x": 2.024251997471116, + "y": 6.076593160628913, + "heading": 0.4752487029161645, + "angularVelocity": -1.0863809642038045, + "velocityX": -0.9756905793332569, + "velocityY": -0.6985898791977067, + "timestamp": 4.95526525682846 + }, + { + "x": 1.9272608996737375, + "y": 6.007147989456111, + "heading": 0.36725581200699825, + "angularVelocity": -1.0863640961687604, + "velocityX": -0.9756905793334377, + "velocityY": -0.6985898791974542, + "timestamp": 5.054672896762456 + }, + { + "x": 1.8302698018763681, + "y": 5.937702818283318, + "heading": 0.26382879729241876, + "angularVelocity": -1.040433258281267, + "velocityX": -0.975690579333424, + "velocityY": -0.6985898791974733, + "timestamp": 5.154080536696453 + }, + { + "x": 1.7332787040790516, + "y": 5.868257647110507, + "heading": 0.16976442296144847, + "angularVelocity": -0.9462489441799997, + "velocityX": -0.9756905793332555, + "velocityY": -0.6985898791977087, + "timestamp": 5.25348817663045 + }, + { + "x": 1.6362876062816873, + "y": 5.798812475937657, + "heading": 0.09032312899825462, + "angularVelocity": -0.7991467659421343, + "velocityX": -0.9756905793330708, + "velocityY": -0.6985898791979667, + "timestamp": 5.3528958165644465 + }, + { + "x": 1.5392965084843735, + "y": 5.72936730476478, + "heading": 0.03145409331145777, + "angularVelocity": -0.5921983031272416, + "velocityX": -0.975690579332948, + "velocityY": -0.6985898791981381, + "timestamp": 5.452303456498443 + }, + { + "x": 1.4423054106856323, + "y": 5.6599221335939, + "heading": 2.4080089212053166e-8, + "angularVelocity": -0.31641500846668225, + "velocityX": -0.975690579347126, + "velocityY": -0.6985898791783296, + "timestamp": 5.55171109643244 + }, + { + "x": 1.3558667798193893, + "y": 5.598032479254774, + "heading": 6.6137151436793805e-9, + "angularVelocity": -1.757045442719512e-7, + "velocityX": -0.8695370991973227, + "velocityY": -0.6225844852591078, + "timestamp": 5.651118736366437 }, { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": 2.2803993876993828e-23, - "angularVelocity": -8.320273226153553e-8, - "velocityX": -0.41800409944751094, - "velocityY": -0.31767214838105773, - "timestamp": 5.236991938544507 + "heading": 1.3033321736184719e-19, + "angularVelocity": -6.653125604117621e-8, + "velocityX": -0.43476857469814373, + "velocityY": -0.3112922605973239, + "timestamp": 5.750526376300433 }, { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": 2.991326698553366e-23, - "angularVelocity": 2.478347927470716e-23, - "velocityX": 9.764266009341063e-23, - "velocityY": -5.1068246254861434e-23, - "timestamp": 5.334595574565935 - }, - { - "x": 1.3367227529770005, - "y": 5.54401651323739, - "heading": -8.745942749209055e-9, - "angularVelocity": -1.1108213753593968e-7, - "velocityX": 0.30578004822354515, - "velocityY": -0.2930262972997385, - "timestamp": 5.413329590162092 - }, - { - "x": 1.3848733307698786, - "y": 5.497874243310031, - "heading": -3.2419454048962964e-8, - "angularVelocity": -3.0067704689648114e-7, - "velocityX": 0.611560040832321, - "velocityY": -0.5860525413060693, - "timestamp": 5.492063605758249 - }, - { - "x": 1.4530888046304524, - "y": 5.432503962156357, - "heading": -0.006001262639786551, - "angularVelocity": -0.07622156922763929, - "velocityX": 0.866404099220153, - "velocityY": -0.8302673331050667, - "timestamp": 5.570797621354406 - }, - { - "x": 1.5213042784906459, - "y": 5.367133681002277, - "heading": -0.03774158111147184, - "angularVelocity": -0.403133489780173, - "velocityX": 0.8664040992153226, - "velocityY": -0.8302673331102199, - "timestamp": 5.649531636950563 - }, - { - "x": 1.5895197523508426, - "y": 5.301763399848201, - "heading": -0.08991573959916936, - "angularVelocity": -0.6626635018250512, - "velocityX": 0.866404099215366, - "velocityY": -0.8302673331101746, - "timestamp": 5.72826565254672 - }, - { - "x": 1.6577352262110532, - "y": 5.2363931186941395, - "heading": -0.15757332647853733, - "angularVelocity": -0.8593183818592182, - "velocityX": 0.8664040992155391, - "velocityY": -0.830267333109994, - "timestamp": 5.806999668142877 - }, - { - "x": 1.7259507000712884, - "y": 5.171022837540104, - "heading": -0.2360864752633604, - "angularVelocity": -0.9971947726829187, - "velocityX": 0.8664040992158549, - "velocityY": -0.8302673331096645, - "timestamp": 5.885733683739034 - }, - { - "x": 1.7941661739315489, - "y": 5.105652556386094, - "heading": -0.32108338261130676, - "angularVelocity": -1.079544929905686, - "velocityX": 0.8664040992161722, - "velocityY": -0.8302673331093331, - "timestamp": 5.9644676993351915 - }, - { - "x": 1.8623816477918176, - "y": 5.040282275232093, - "heading": -0.40835744473560676, - "angularVelocity": -1.1084670515466504, - "velocityX": 0.8664040992162807, - "velocityY": -0.8302673331092201, - "timestamp": 6.043201714931349 - }, - { - "x": 1.9305971216520723, - "y": 4.974911994078077, - "heading": -0.49376220718458297, - "angularVelocity": -1.0847250937515367, - "velocityX": 0.8664040992161026, - "velocityY": -0.8302673331094058, - "timestamp": 6.121935730527506 - }, - { - "x": 1.9988125955123008, - "y": 4.9095417129240335, - "heading": -0.5731014303336502, - "angularVelocity": -1.007686735502157, - "velocityX": 0.8664040992157647, - "velocityY": -0.8302673331097585, - "timestamp": 6.200669746123663 - }, - { - "x": 2.06702806937251, - "y": 4.844171431769971, - "heading": -0.6420233840895226, - "angularVelocity": -0.8753770938013302, - "velocityX": 0.8664040992155239, - "velocityY": -0.8302673331100098, - "timestamp": 6.27940376171982 - }, - { - "x": 2.1352435432327193, - "y": 4.778801150615908, - "heading": -0.6959284119501224, - "angularVelocity": -0.6846472576362663, - "velocityX": 0.866404099215522, - "velocityY": -0.8302673331100118, - "timestamp": 6.358137777315977 + "heading": 2.2172969897005315e-19, + "angularVelocity": 2.0015491793199464e-19, + "velocityX": -1.0501490690724046e-19, + "velocityY": -4.272854375601257e-19, + "timestamp": 5.84993401623443 + }, + { + "x": 1.3441215983818582, + "y": 5.53692626623967, + "heading": -1.1643991871335638e-8, + "angularVelocity": -1.2934461900203075e-7, + "velocityX": 0.34962324193127553, + "velocityY": -0.33504083198930457, + "timestamp": 5.9399570259085595 + }, + { + "x": 1.4070698670672521, + "y": 5.476603502232459, + "heading": -4.233223250468571e-8, + "angularVelocity": -3.4089329819791224e-7, + "velocityX": 0.6992464361418851, + "velocityY": -0.6700816183059229, + "timestamp": 6.029980035582689 + }, + { + "x": 1.485066172875745, + "y": 5.401860338613426, + "heading": -0.031252361434063125, + "angularVelocity": -0.347159234233111, + "velocityX": 0.8664041125799371, + "velocityY": -0.8302673270932495, + "timestamp": 6.120003045256818 + }, + { + "x": 1.5630624786713805, + "y": 5.327117174980967, + "heading": -0.0999659837507643, + "angularVelocity": -0.7632895474771944, + "velocityX": 0.8664041124370567, + "velocityY": -0.8302673272423656, + "timestamp": 6.210026054930948 + }, + { + "x": 1.6410587844670608, + "y": 5.2523740113485955, + "heading": -0.19520871175979404, + "angularVelocity": -1.0579820465211633, + "velocityX": 0.866404112437784, + "velocityY": -0.8302673272416067, + "timestamp": 6.300049064605077 + }, + { + "x": 1.7190550902628128, + "y": 5.177630847716277, + "heading": -0.30625153833554625, + "angularVelocity": -1.2334938253865513, + "velocityX": 0.8664041124384794, + "velocityY": -0.8302673272408809, + "timestamp": 6.390072074279207 + }, + { + "x": 1.797051396058594, + "y": 5.1028876840839885, + "heading": -0.4225688522263227, + "angularVelocity": -1.292084260588809, + "velocityX": 0.8664041124387727, + "velocityY": -0.8302673272405751, + "timestamp": 6.480095083953336 + }, + { + "x": 1.8750477018543286, + "y": 5.028144520451638, + "heading": -0.5337184986736393, + "angularVelocity": -1.234680409482674, + "velocityX": 0.8664041124382615, + "velocityY": -0.8302673272411084, + "timestamp": 6.570118093627466 + }, + { + "x": 1.9530440076500057, + "y": 4.953401356819256, + "heading": -0.6291756131928835, + "angularVelocity": -1.0603635100046716, + "velocityX": 0.8664041124376008, + "velocityY": -0.8302673272417979, + "timestamp": 6.660141103301595 + }, + { + "x": 2.0310403134456294, + "y": 4.878658193186804, + "heading": -0.6982128332545968, + "angularVelocity": -0.7668841589680074, + "velocityX": 0.8664041124370991, + "velocityY": -0.8302673272423214, + "timestamp": 6.750164112975725 + }, + { + "x": 2.109036619249582, + "y": 4.803915029563043, + "heading": -0.7298996645397342, + "angularVelocity": -0.3519859133774726, + "velocityX": 0.8664041125295305, + "velocityY": -0.83026732714585, + "timestamp": 6.840187122649854 + }, + { + "x": 2.1719848879377293, + "y": 4.743592265558695, + "heading": -0.7298996951613214, + "angularVelocity": -3.401528940808921e-7, + "velocityX": 0.6992464361723661, + "velocityY": -0.670081618274115, + "timestamp": 6.9302101323239835 + }, + { + "x": 2.2034590244293213, + "y": 4.713430881500244, + "heading": -0.7298997067825479, + "angularVelocity": -1.2909173503647302e-7, + "velocityX": 0.3496232419414347, + "velocityY": -0.3350408319787032, + "timestamp": 7.020233141998113 }, { "x": 2.2034590244293213, "y": 4.713430881500244, "heading": -0.7298997067825479, - "angularVelocity": -0.43146909979380765, - "velocityX": 0.8664041923949785, - "velocityY": -0.8302671802104146, - "timestamp": 6.436871792912134 + "angularVelocity": 1.6899611320835774e-19, + "velocityX": 5.175181532248611e-19, + "velocityY": -4.71372364987181e-19, + "timestamp": 7.1102561516722425 + }, + { + "x": 2.2414956338795715, + "y": 4.681346676235305, + "heading": -0.7298997067825479, + "angularVelocity": -2.46282456581248e-17, + "velocityX": 0.39546677329122837, + "velocityY": -0.333579604310823, + "timestamp": 7.206437707289899 }, { - "x": 2.284274256756373, - "y": 4.64687721210592, - "heading": -0.7358958248166633, - "angularVelocity": -0.06872842162702524, - "velocityX": 0.9263165484164835, - "velocityY": -0.7628483336942082, - "timestamp": 6.524115438148963 + "x": 2.3175688480689023, + "y": 4.617178269679347, + "heading": -0.729899706782548, + "angularVelocity": -4.562002127415535e-17, + "velocityX": 0.7909334976002727, + "velocityY": -0.6671591673047552, + "timestamp": 7.302619262907555 }, { - "x": 2.3650894966282934, - "y": 4.580323551873232, - "heading": -0.7298997308132567, - "angularVelocity": 0.06872814618336746, - "velocityX": 0.9263166348969202, - "velocityY": -0.762848228682137, - "timestamp": 6.611359083385793 + "x": 2.405792236328125, + "y": 4.5427610874176025, + "heading": -0.729899706782548, + "angularVelocity": 1.0687559485237639e-16, + "velocityX": 0.917258903671292, + "velocityY": -0.7737157273435987, + "timestamp": 7.398800818525212 }, { - "x": 2.428299134503931, - "y": 4.5282686081612855, - "heading": -0.7298997143064122, - "angularVelocity": 1.8920397482236636e-7, - "velocityX": 0.7245185331728233, - "velocityY": -0.5966617232766905, - "timestamp": 6.698602728622622 + "x": 2.494015624587376, + "y": 4.468343905155829, + "heading": -0.7298997067825479, + "angularVelocity": 2.54215551828638e-16, + "velocityX": 0.917258903671292, + "velocityY": -0.7737157273435986, + "timestamp": 7.494982374142868 + }, + { + "x": 2.5700888387766927, + "y": 4.4041754985999, + "heading": -0.7298997067825479, + "angularVelocity": -6.314437383419688e-17, + "velocityX": 0.7909334976002727, + "velocityY": -0.6671591673047553, + "timestamp": 7.5911639297605245 }, { - "x": 2.4599039554595947, - "y": 4.502241134643555, + "x": 2.6081254482269287, + "y": 4.372091293334961, "heading": -0.7298997067825479, - "angularVelocity": 8.623968367420298e-8, - "velocityX": 0.3622592897152577, - "velocityY": -0.29833088068566255, - "timestamp": 6.785846373859451 + "angularVelocity": -7.295944708803859e-17, + "velocityX": 0.3954667732912283, + "velocityY": -0.333579604310823, + "timestamp": 7.687345485378181 }, { - "x": 2.4599039554595947, - "y": 4.502241134643555, + "x": 2.6081254482269287, + "y": 4.372091293334961, "heading": -0.7298997067825479, - "angularVelocity": -9.04164627803996e-23, - "velocityX": -1.4473589854434152e-22, - "velocityY": -2.1145820655457915e-22, - "timestamp": 6.8730900190962805 - }, - { - "x": 2.418647701705629, - "y": 4.540533860206957, - "heading": -0.7298997001349049, - "angularVelocity": 6.498469080235459e-8, - "velocityX": -0.40330458085897497, - "velocityY": 0.374334318510709, - "timestamp": 6.975385543898147 - }, - { - "x": 2.336135198753465, - "y": 4.61711930710535, - "heading": -0.7298996827304232, - "angularVelocity": 1.7013922856843512e-7, - "velocityX": -0.8066091171825944, - "velocityY": 0.748668595686158, - "timestamp": 7.077681068700014 - }, - { - "x": 2.2461633007376833, - "y": 4.700628324178363, - "heading": -0.6961908922790889, - "angularVelocity": 0.3295236083555346, - "velocityX": -0.879529169922592, - "velocityY": 0.8163506393340244, - "timestamp": 7.1799765935018804 - }, - { - "x": 2.1561914027221256, - "y": 4.78413734125162, - "heading": -0.6354601214689052, - "angularVelocity": 0.5936796446160405, - "velocityX": -0.8795291699203955, - "velocityY": 0.8163506393363948, - "timestamp": 7.282272118303747 - }, - { - "x": 2.0662195047065577, - "y": 4.8676463583248655, - "heading": -0.555497965787572, - "angularVelocity": 0.7816779456990843, - "velocityX": -0.8795291699204953, - "velocityY": 0.8163506393362874, - "timestamp": 7.384567643105614 - }, - { - "x": 1.976247606690973, - "y": 4.951155375398092, - "heading": -0.4632200489828426, - "angularVelocity": 0.9020718842144826, - "velocityX": -0.8795291699206637, - "velocityY": 0.8163506393361059, - "timestamp": 7.48686316790748 - }, - { - "x": 1.8862757086753756, - "y": 5.0346643924713055, - "heading": -0.3649382393995183, - "angularVelocity": 0.96076353069876, - "velocityX": -0.8795291699207887, - "velocityY": 0.8163506393359712, - "timestamp": 7.589158692709347 - }, - { - "x": 1.7963038106597808, - "y": 5.118173409544521, - "heading": -0.2666575817787772, - "angularVelocity": 0.9607522695748207, - "velocityX": -0.8795291699207628, - "velocityY": 0.816350639335999, - "timestamp": 7.691454217511214 - }, - { - "x": 1.7063319126442014, - "y": 5.201682426617753, - "heading": -0.17438289577352772, - "angularVelocity": 0.9020403012152627, - "velocityX": -0.8795291699206121, - "velocityY": 0.8163506393361615, - "timestamp": 7.79374974231308 - }, - { - "x": 1.6163600146286374, - "y": 5.2851914436910015, - "heading": -0.09442558399173479, - "angularVelocity": 0.7816305936810019, - "velocityX": -0.8795291699204603, - "velocityY": 0.8163506393363251, - "timestamp": 7.896045267114947 - }, - { - "x": 1.526388116613081, - "y": 5.368700460764259, - "heading": -0.033700979293950294, - "angularVelocity": 0.5936193671756421, - "velocityX": -0.8795291699203863, - "velocityY": 0.8163506393364047, - "timestamp": 7.998340791916814 - }, - { - "x": 1.436416218597363, - "y": 5.452209477837341, - "heading": -2.4053492503793295e-8, - "angularVelocity": 0.32944701447822, - "velocityX": -0.8795291699219676, - "velocityY": 0.8163506393346972, - "timestamp": 8.100636316718681 - }, - { - "x": 1.353903715645155, - "y": 5.528794924735686, - "heading": -6.648008517386852e-9, - "angularVelocity": 1.701490267547733e-7, - "velocityX": -0.8066091171830237, - "velocityY": 0.7486685956856953, - "timestamp": 8.202931841520549 + "angularVelocity": -3.557756419099644e-20, + "velocityX": -1.1158976027767978e-18, + "velocityY": 1.0302990290883233e-18, + "timestamp": 7.783527040995837 + }, + { + "x": 2.563674001371605, + "y": 4.41309493647991, + "heading": -0.7298997015558017, + "angularVelocity": 4.929434515061794e-8, + "velocityX": -0.41922926520033577, + "velocityY": 0.38671243350444034, + "timestamp": 7.889558395543702 + }, + { + "x": 2.474771112276091, + "y": 4.495102218512861, + "heading": -0.7298996878700399, + "angularVelocity": 1.2907278142001575e-7, + "velocityX": -0.838458486875625, + "velocityY": 0.773424826859854, + "timestamp": 7.9955897500915665 + }, + { + "x": 2.381246629145657, + "y": 4.581372628204462, + "heading": -0.7000859323738574, + "angularVelocity": 0.28117867232115834, + "velocityX": -0.8820455376541905, + "velocityY": 0.8136311193942835, + "timestamp": 8.101621104639431 + }, + { + "x": 2.287722146015324, + "y": 4.667643037896195, + "heading": -0.6472777997396291, + "angularVelocity": 0.498042610692009, + "velocityX": -0.8820455376540477, + "velocityY": 0.81363111939444, + "timestamp": 8.207652459187296 + }, + { + "x": 2.1941976628849664, + "y": 4.753913447587866, + "heading": -0.577776535255652, + "angularVelocity": 0.6554784174958657, + "velocityX": -0.8820455376540902, + "velocityY": 0.813631119394394, + "timestamp": 8.31368381373516 + }, + { + "x": 2.10067317975465, + "y": 4.8401838572795235, + "heading": -0.49698505133773596, + "angularVelocity": 0.7619584250566693, + "velocityX": -0.8820455376541692, + "velocityY": 0.8136311193943084, + "timestamp": 8.419715168283025 + }, + { + "x": 2.0071486966242564, + "y": 4.9264542669711755, + "heading": -0.4096708626062039, + "angularVelocity": 0.8234751796188454, + "velocityX": -0.8820455376542478, + "velocityY": 0.8136311193942233, + "timestamp": 8.52574652283089 + }, + { + "x": 1.913624213493877, + "y": 5.0127246766628355, + "heading": -0.3202243151295472, + "angularVelocity": 0.8435858228735447, + "velocityX": -0.882045537654274, + "velocityY": 0.8136311193941947, + "timestamp": 8.631777877378754 + }, + { + "x": 1.8200997303635116, + "y": 5.098995086354495, + "heading": -0.23291079154548175, + "angularVelocity": 0.8234689064983195, + "velocityX": -0.8820455376542292, + "velocityY": 0.8136311193942434, + "timestamp": 8.737809231926619 + }, + { + "x": 1.7265752472331424, + "y": 5.185265496046131, + "heading": -0.15212040474401956, + "angularVelocity": 0.7619480779620905, + "velocityX": -0.8820455376541468, + "velocityY": 0.8136311193943326, + "timestamp": 8.843840586474483 + }, + { + "x": 1.6330507641028076, + "y": 5.27153590573785, + "heading": -0.0826202834773307, + "angularVelocity": 0.6554676356163622, + "velocityX": -0.8820455376540777, + "velocityY": 0.8136311193944077, + "timestamp": 8.949871941022348 + }, + { + "x": 1.5395262809724208, + "y": 5.357806315429548, + "heading": -0.02981305940491789, + "angularVelocity": 0.49803404188875805, + "velocityX": -0.8820455376540449, + "velocityY": 0.8136311193944432, + "timestamp": 9.055903295570213 + }, + { + "x": 1.4460017978420001, + "y": 5.444076725121205, + "heading": -1.8912690296449957e-8, + "angularVelocity": 0.2811719289954887, + "velocityX": -0.8820455376541747, + "velocityY": 0.8136311193943003, + "timestamp": 9.161934650118077 + }, + { + "x": 1.3570989087465364, + "y": 5.526084007154097, + "heading": -5.2268050645253455e-9, + "angularVelocity": 1.290739450532968e-7, + "velocityX": -0.8384584868756361, + "velocityY": 0.7734248268598417, + "timestamp": 9.267966004665942 }, { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": 6.220225099112246e-24, - "angularVelocity": 6.498826346766567e-8, - "velocityX": -0.403304580859118, - "velocityY": 0.37433431851055476, - "timestamp": 8.305227366322416 + "heading": 1.130585771980045e-19, + "angularVelocity": 4.9294900420321916e-8, + "velocityX": -0.41922926520033954, + "velocityY": 0.3867124335044363, + "timestamp": 9.373997359213806 }, { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": 3.0379270142045743e-24, - "angularVelocity": -1.4113145535339382e-24, - "velocityX": -1.5623932740219278e-24, - "velocityY": 1.5729984496793032e-24, - "timestamp": 8.407522891124284 + "heading": 5.546193231131035e-20, + "angularVelocity": -2.0132747430807326e-20, + "velocityX": -6.460753207500427e-19, + "velocityY": 5.964321607162322e-19, + "timestamp": 9.480028713761671 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 1.0961836295771266, + "isStopPoint": true, + "x": 2.349189281463623, + "y": 5.567087650299072, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 2.192367259154259, + "isStopPoint": true, + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 3.361089695408202, + "isStopPoint": true, + "x": 2.338470697402954, + "y": 6.032597064971924, + "heading": 0.9505466984245466, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "timestamp": 4.160004137356486, + "isStopPoint": true, + "x": 2.735856533050537, + "y": 6.586098670959473, + "heading": 0.9505466984245466, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 5.84993401623443, + "isStopPoint": true, + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 7.1102561516722425, + "isStopPoint": true, + "x": 2.2034590244293213, + "y": 4.713430881500244, + "heading": -0.7298997067825479, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "timestamp": 7.783527040995837, + "isStopPoint": true, + "x": 2.6081254482269287, + "y": 4.372091293334961, + "heading": -0.7298997067825479, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 9.480028713761671, + "isStopPoint": true, + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 } ], - "trajectoryWaypoints": [], "constraints": [ { "scope": [ @@ -3759,6 +3895,18 @@ 8 ], "type": "WptZeroVelocity" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 6 + ], + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -3766,7 +3914,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "amp exit": { "waypoints": [ diff --git a/src/main/deploy/choreo/4 piece.1.traj b/src/main/deploy/choreo/4 piece.1.traj index 401b1ba..c78b875 100644 --- a/src/main/deploy/choreo/4 piece.1.traj +++ b/src/main/deploy/choreo/4 piece.1.traj @@ -3,92 +3,92 @@ { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": -1.5437737592748072e-37, - "angularVelocity": -7.89083646860229e-38, - "velocityX": 1.418479553941243e-23, - "velocityY": 2.4322727819891274e-38, + "heading": 1.1953764755650808e-32, + "angularVelocity": 6.990086021448287e-33, + "velocityX": 5.494174654732201e-19, + "velocityY": 4.78279637249085e-34, "timestamp": 0 }, { - "x": 1.392444928049753, + "x": 1.3924449280497553, "y": 5.567087650299072, - "heading": -4.6220470147664874e-27, - "angularVelocity": -3.7948407554824907e-26, - "velocityX": 0.6551613945414111, - "velocityY": 1.137046594589441e-15, - "timestamp": 0.12179818106412381 + "heading": 4.1001262381929663e-22, + "angularVelocity": 3.3663279452468632e-21, + "velocityX": 0.6551613945414227, + "velocityY": 5.759043744920587e-16, + "timestamp": 0.12179818106412517 }, { - "x": 1.5386027425923001, + "x": 1.538602742592314, "y": 5.567087650299072, - "heading": -1.9365333413339378e-22, - "angularVelocity": -1.5899146472838326e-21, - "velocityX": 1.1999999775497356, - "velocityY": 2.082625593248517e-15, - "timestamp": 0.24359636212824762 + "heading": -9.345363045002027e-18, + "angularVelocity": -7.673163046652378e-17, + "velocityX": 1.199999977549737, + "velocityY": 1.0548320493532893e-15, + "timestamp": 0.24359636212825034 }, { - "x": 1.6847605571348494, + "x": 1.6847605571348563, "y": 5.567087650299072, - "heading": -1.4409951368437792e-22, - "angularVelocity": 4.0685189233595474e-22, + "heading": -1.2209432374345432e-17, + "angularVelocity": -2.351487757270866e-17, "velocityX": 1.199999977549752, - "velocityY": 2.082625593248546e-15, - "timestamp": 0.3653945431923714 + "velocityY": 1.054832049353261e-15, + "timestamp": 0.3653945431923755 }, { "x": 1.8309183716773985, "y": 5.567087650299072, - "heading": 2.0899654958449072e-23, - "angularVelocity": 1.3546932080698235e-21, - "velocityX": 1.199999977549752, - "velocityY": 2.0826255932485463e-15, - "timestamp": 0.48719272425649524 + "heading": -1.068309383473616e-17, + "angularVelocity": 1.2531702212408767e-17, + "velocityX": 1.1999999775497523, + "velocityY": 1.0548320493532611e-15, + "timestamp": 0.4871927242565007 }, { - "x": 1.9770761862199477, + "x": 1.9770761862199266, "y": 5.567087650299072, - "heading": 1.690438544316869e-22, - "angularVelocity": 1.2163088001720124e-21, - "velocityX": 1.199999977549752, - "velocityY": 2.082625593248546e-15, - "timestamp": 0.6089909053206191 + "heading": -6.939437782754445e-18, + "angularVelocity": 3.0736551271554804e-17, + "velocityX": 1.1999999775497523, + "velocityY": 1.0548320493532611e-15, + "timestamp": 0.6089909053206258 }, { "x": 2.1232340007624972, "y": 5.567087650299072, - "heading": 1.86290605944189e-22, - "angularVelocity": 1.416010597352205e-22, + "heading": -2.9324482572141887e-18, + "angularVelocity": 3.289859892177714e-17, "velocityX": 1.199999977549752, - "velocityY": 2.082625593248546e-15, - "timestamp": 0.7307890863847428 + "velocityY": 1.054832049353261e-15, + "timestamp": 0.730789086384751 }, { - "x": 2.2693918153050445, + "x": 2.269391815305044, "y": 5.567087650299072, - "heading": 4.5629176638525406e-27, - "angularVelocity": -1.5294649016839573e-21, - "velocityX": 1.1999999775497356, - "velocityY": 2.0826255932485175e-15, - "timestamp": 0.8525872674488666 + "heading": -2.924223024384178e-22, + "angularVelocity": 2.407388842978096e-17, + "velocityX": 1.199999977549737, + "velocityY": 1.0548320493532885e-15, + "timestamp": 0.8525872674488763 }, { "x": 2.349189281463623, "y": 5.567087650299072, - "heading": -1.0800252356875386e-31, - "angularVelocity": -3.7463824388368756e-26, - "velocityX": 0.6551613945414111, - "velocityY": 1.1370465945894429e-15, - "timestamp": 0.9743854485129904 + "heading": 1.6673685334753995e-30, + "angularVelocity": 2.4008757783240606e-21, + "velocityX": 0.6551613945414226, + "velocityY": 5.759043744915343e-16, + "timestamp": 0.9743854485130015 }, { "x": 2.349189281463623, "y": 5.567087650299072, - "heading": -1.15233598675018e-31, - "angularVelocity": -5.936794878517772e-32, - "velocityX": -2.2921562380311214e-24, - "velocityY": 2.1917263454721895e-30, - "timestamp": 1.0961836295771141 + "heading": 1.7831791313090364e-30, + "angularVelocity": 8.43744338443602e-31, + "velocityX": -1.487174311186839e-19, + "velocityY": -5.252529696759049e-28, + "timestamp": 1.0961836295771266 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/4 piece.2.traj b/src/main/deploy/choreo/4 piece.2.traj index 2b2ab00..50f1bdf 100644 --- a/src/main/deploy/choreo/4 piece.2.traj +++ b/src/main/deploy/choreo/4 piece.2.traj @@ -3,92 +3,92 @@ { "x": 2.349189281463623, "y": 5.567087650299072, - "heading": -1.15233598675018e-31, - "angularVelocity": -5.936794878517772e-32, - "velocityX": -2.2921562380311214e-24, - "velocityY": 2.1917263454721895e-30, + "heading": 1.7831791313090364e-30, + "angularVelocity": 8.43744338443602e-31, + "velocityX": -1.487174311186839e-19, + "velocityY": -5.252529696759049e-28, "timestamp": 0 }, { - "x": 2.2693918153050445, + "x": 2.269391815305044, "y": 5.567087650299072, - "heading": -3.475580412192629e-21, - "angularVelocity": -2.8535569099611155e-20, - "velocityX": -0.6551613945414112, - "velocityY": 1.824069231632628e-15, - "timestamp": 0.12179818106412377 + "heading": 4.900643599154424e-20, + "angularVelocity": 4.023576968464745e-19, + "velocityX": -0.655161394541426, + "velocityY": -7.280112591641159e-16, + "timestamp": 0.12179818106412577 }, { "x": 2.1232340007624972, "y": 5.567087650299072, - "heading": 7.19228614469986e-17, - "angularVelocity": 5.905370375731918e-16, - "velocityX": -1.1999999775497356, - "velocityY": 3.3409829244929845e-15, - "timestamp": 0.24359636212824753 + "heading": 7.594029969730495e-17, + "angularVelocity": 6.23090526860376e-16, + "velocityX": -1.199999977549737, + "velocityY": -1.3334248944822027e-15, + "timestamp": 0.24359636212825153 }, { - "x": 1.9770761862199482, + "x": 1.9770761862199409, "y": 5.567087650299072, - "heading": 5.1366024248226334e-17, - "angularVelocity": -1.6877786695311564e-16, + "heading": 4.257511726642054e-17, + "angularVelocity": -2.7393827208826133e-16, "velocityX": -1.199999977549752, - "velocityY": 3.3409829342395925e-15, - "timestamp": 0.3653945431923713 + "velocityY": -1.3334343966180895e-15, + "timestamp": 0.3653945431923773 }, { "x": 1.8309183716773987, "y": 5.567087650299072, - "heading": -1.7979701811530337e-18, - "angularVelocity": -4.3649251544560875e-16, + "heading": -2.2065028106115054e-17, + "angularVelocity": -5.307152037473876e-16, "velocityX": -1.199999977549752, - "velocityY": 3.340982934381562e-15, - "timestamp": 0.48719272425649507 + "velocityY": -1.3334344015361252e-15, + "timestamp": 0.48719272425650306 }, { - "x": 1.6847605571348494, + "x": 1.684760557134842, "y": 5.567087650299072, - "heading": -5.627263788476133e-17, - "angularVelocity": -4.472535404689559e-16, + "heading": -8.169986730219564e-17, + "angularVelocity": -4.896201228598656e-16, "velocityX": -1.199999977549752, - "velocityY": 3.3409829343799955e-15, - "timestamp": 0.6089909053206188 + "velocityY": -1.3334344018518055e-15, + "timestamp": 0.6089909053206288 }, { - "x": 1.5386027425923001, + "x": 1.5386027425923, "y": 5.567087650299072, - "heading": -7.146734861941795e-17, - "angularVelocity": -1.2475318269865482e-16, + "heading": -8.15243225571019e-17, + "angularVelocity": 1.4412662667122633e-18, "velocityX": -1.199999977549752, - "velocityY": 3.3409829330177503e-15, - "timestamp": 0.7307890863847426 + "velocityY": -1.3334342978189559e-15, + "timestamp": 0.7307890863847546 }, { - "x": 1.3924449280497528, + "x": 1.392444928049753, "y": 5.567087650299072, - "heading": 2.401745836439516e-21, - "angularVelocity": 5.867883226238598e-16, - "velocityX": -1.1999999775497356, - "velocityY": 3.3409828502355826e-15, - "timestamp": 0.8525872674488664 + "heading": -9.444741136401807e-19, + "angularVelocity": 6.615849837325982e-16, + "velocityX": -1.199999977549737, + "velocityY": -1.3333514424854479e-15, + "timestamp": 0.8525872674488804 }, { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": -1.9529795908474793e-22, - "angularVelocity": -2.1322517075995374e-20, - "velocityX": -0.6551613945414112, - "velocityY": 1.824070118744852e-15, - "timestamp": 0.9743854485129904 + "heading": -9.625683020163944e-19, + "angularVelocity": -1.4856176769231114e-19, + "velocityX": -0.6551613945414261, + "velocityY": -7.279052775637916e-16, + "timestamp": 0.9743854485130063 }, { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": -2.0997399222925942e-22, - "angularVelocity": -1.20494685567255e-22, - "velocityX": -2.042395408130124e-22, - "velocityY": 8.462664089537296e-22, - "timestamp": 1.0961836295771141 + "heading": -1.0240569848515672e-18, + "angularVelocity": -5.048437233348486e-19, + "velocityX": 3.814586359746447e-19, + "velocityY": 1.4611187628635838e-19, + "timestamp": 1.0961836295771323 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/4 piece.3.traj b/src/main/deploy/choreo/4 piece.3.traj index 6bb753e..536292b 100644 --- a/src/main/deploy/choreo/4 piece.3.traj +++ b/src/main/deploy/choreo/4 piece.3.traj @@ -3,209 +3,137 @@ { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": -2.0997399222925942e-22, - "angularVelocity": -1.20494685567255e-22, - "velocityX": -2.042395408130124e-22, - "velocityY": 8.462664089537296e-22, + "heading": -1.0240569848515672e-18, + "angularVelocity": -5.048437233348486e-19, + "velocityX": 3.814586359746447e-19, + "velocityY": 1.4611187628635838e-19, "timestamp": 0 }, { - "x": 1.330224348661209, - "y": 5.5781076780240015, - "heading": 1.3752213180599183e-8, - "angularVelocity": 2.2144266656949007e-7, - "velocityX": 0.28302882054194556, - "velocityY": 0.17744811638904515, - "timestamp": 0.06210281601844825 - }, - { - "x": 1.3653781204474067, - "y": 5.6001477324054765, - "heading": 4.561370782539336e-8, - "angularVelocity": 5.130442818459255e-7, - "velocityX": 0.5660576128424637, - "velocityY": 0.3548962155746409, - "timestamp": 0.1242056320368965 - }, - { - "x": 1.4181087719881549, - "y": 5.633207810238353, - "heading": 1.0865567864033805e-7, - "angularVelocity": 0.0000010151225798878058, - "velocityX": 0.8490863204187717, - "velocityY": 0.532344263149933, - "timestamp": 0.18630844805534474 - }, - { - "x": 1.4812487494892455, - "y": 5.672794131521707, - "heading": 0.017849702169017073, - "angularVelocity": 0.28742003435135793, - "velocityX": 1.0167007158312162, - "velocityY": 0.6374319849134539, - "timestamp": 0.248411264073793 - }, - { - "x": 1.5443887266874352, - "y": 5.712380453288194, - "heading": 0.06529469396388879, - "angularVelocity": 0.7639748861110869, - "velocityX": 1.0167007109538098, - "velocityY": 0.6374319926930171, - "timestamp": 0.31051408009224124 - }, - { - "x": 1.607528703885691, - "y": 5.751966775054576, - "heading": 0.13761827273616745, - "angularVelocity": 1.1645780885490655, - "velocityX": 1.0167007109548667, - "velocityY": 0.6374319926913311, - "timestamp": 0.3726168961106895 - }, - { - "x": 1.6706686810840699, - "y": 5.791553096820763, - "heading": 0.22970172720620147, - "angularVelocity": 1.4827581158748078, - "velocityX": 1.016700710956855, - "velocityY": 0.6374319926881596, - "timestamp": 0.43471971212913774 - }, - { - "x": 1.7338086582825494, - "y": 5.831139418586787, - "heading": 0.336088558287123, - "angularVelocity": 1.7130757975502817, - "velocityX": 1.0167007109584723, - "velocityY": 0.6374319926855803, - "timestamp": 0.496822528147586 - }, - { - "x": 1.7969486354809578, - "y": 5.870725740352926, - "heading": 0.4510732003163099, - "angularVelocity": 1.8515205815309534, - "velocityX": 1.0167007109573307, - "velocityY": 0.6374319926874006, - "timestamp": 0.5589253441660342 - }, - { - "x": 1.860088612679118, - "y": 5.910312062119461, - "heading": 0.5688014129904255, - "angularVelocity": 1.895698459779078, - "velocityX": 1.0167007109533306, - "velocityY": 0.6374319926937808, - "timestamp": 0.6210281601844825 - }, - { - "x": 1.9232285898770611, - "y": 5.949898383886342, - "heading": 0.6833720439947638, - "angularVelocity": 1.8448540396349185, - "velocityX": 1.0167007109498374, - "velocityY": 0.6374319926993527, - "timestamp": 0.6831309762029307 - }, - { - "x": 1.9863685670748548, - "y": 5.989484705653461, - "heading": 0.7889380109564055, - "angularVelocity": 1.6998579731115984, - "velocityX": 1.0167007109474246, - "velocityY": 0.6374319927032011, - "timestamp": 0.745233792221379 - }, - { - "x": 2.049508544272936, - "y": 6.029071027420121, - "heading": 0.8798080925206222, - "angularVelocity": 1.4632199856641441, - "velocityX": 1.0167007109520676, - "velocityY": 0.6374319926957958, - "timestamp": 0.8073366082398272 - }, - { - "x": 2.1126484870910645, - "y": 6.068657398223877, - "heading": 0.9505466984245466, - "angularVelocity": 1.1390563333377834, - "velocityX": 1.0167001573547376, - "velocityY": 0.63743278230727, - "timestamp": 0.8694394242582755 - }, - { - "x": 2.1886009078042274, - "y": 6.143554890170836, - "heading": 1.0124745802683783, - "angularVelocity": 0.6966694651666266, - "velocityX": 0.8544411780429775, - "velocityY": 0.8425735565862382, - "timestamp": 0.9583307636095988 - }, - { - "x": 2.264553290701356, - "y": 6.2184524204664475, - "heading": 1.0234437942174555, - "angularVelocity": 0.12340025506561028, - "velocityX": 0.8544407526243177, - "velocityY": 0.8425739879966911, - "timestamp": 1.047222102960922 - }, - { - "x": 2.3405056735984604, - "y": 6.293349950762083, - "heading": 1.0023010522506035, - "angularVelocity": -0.23784929016864031, - "velocityX": 0.8544407526240461, - "velocityY": 0.8425739879969667, - "timestamp": 1.1361134423122454 - }, - { - "x": 2.4164580564955584, - "y": 6.368247481057726, - "heading": 0.9705071197009111, - "angularVelocity": -0.3576718809920707, - "velocityX": 0.8544407526239729, - "velocityY": 0.8425739879970408, - "timestamp": 1.2250047816635687 - }, - { - "x": 2.492410439384703, - "y": 6.443145011361432, - "heading": 0.950546735518749, - "angularVelocity": -0.22454813177325503, - "velocityX": 0.8544407525345075, - "velocityY": 0.8425739880877493, - "timestamp": 1.313896121014892 - }, - { - "x": 2.5529385454627884, - "y": 6.502832482446322, - "heading": 0.9505467092628421, - "angularVelocity": -2.953708100289274e-7, - "velocityX": 0.6809224219117733, - "velocityY": 0.6714655389428754, - "timestamp": 1.4027874603662154 - }, - { - "x": 2.583202600479126, - "y": 6.5326762199401855, + "x": 1.3467835531597558, + "y": 5.582578303497341, + "heading": 2.1822298286556078e-8, + "angularVelocity": 2.6140695812051043e-7, + "velocityX": 0.408912555227134, + "velocityY": 0.18556086376752873, + "timestamp": 0.08348017401813879 + }, + { + "x": 1.4150557303762996, + "y": 5.61355960759438, + "heading": 7.969110326356027e-8, + "angularVelocity": 6.932041753657309e-7, + "velocityX": 0.8178250467198802, + "velocityY": 0.37112169998949157, + "timestamp": 0.16696034803627757 + }, + { + "x": 1.5062786970732867, + "y": 5.65495577384632, + "heading": 0.029442185922842156, + "angularVelocity": 0.35268381478627076, + "velocityX": 1.0927500783256299, + "velocityY": 0.4958802103472522, + "timestamp": 0.25044052205441636 + }, + { + "x": 1.5975016635378336, + "y": 5.696351940610345, + "heading": 0.11439605036500154, + "angularVelocity": 1.0176531786301763, + "velocityX": 1.0927500755419548, + "velocityY": 0.49588021648158287, + "timestamp": 0.33392069607255515 + }, + { + "x": 1.6887246300035204, + "y": 5.737748107371955, + "heading": 0.24137697471085723, + "angularVelocity": 1.5210907959806668, + "velocityX": 1.0927500755551556, + "velocityY": 0.4958802164524925, + "timestamp": 0.41740087009069393 + }, + { + "x": 1.779947596469001, + "y": 5.779144274133999, + "heading": 0.394686770242847, + "angularVelocity": 1.836481503963777, + "velocityX": 1.0927500755527955, + "velocityY": 0.4958802164576932, + "timestamp": 0.5008810441088327 + }, + { + "x": 1.8711705629341966, + "y": 5.8205404408966785, + "heading": 0.5569437718053947, + "angularVelocity": 1.9436591199162039, + "velocityX": 1.0927500755492763, + "velocityY": 0.49588021646544844, + "timestamp": 0.5843612181269715 + }, + { + "x": 1.9623935293992807, + "y": 5.861936607659589, + "heading": 0.7101331870008885, + "angularVelocity": 1.8350394808976778, + "velocityX": 1.0927500755480895, + "velocityY": 0.4958802164680637, + "timestamp": 0.6678413921451103 + }, + { + "x": 2.053616495864439, + "y": 5.903332774422348, + "heading": 0.8368837129910643, + "angularVelocity": 1.518330878929826, + "velocityX": 1.0927500755488537, + "velocityY": 0.49588021646637936, + "timestamp": 0.7513215661632491 + }, + { + "x": 2.144839462330586, + "y": 5.944728941182871, + "heading": 0.9215115215035561, + "angularVelocity": 1.01374738981861, + "velocityX": 1.0927500755610373, + "velocityY": 0.4958802164395315, + "timestamp": 0.8348017401813879 + }, + { + "x": 2.236062428594661, + "y": 5.986125108388746, + "heading": 0.9505466187573102, + "angularVelocity": 0.34780829814088937, + "velocityX": 1.0927500731401507, + "velocityY": 0.49588022177427094, + "timestamp": 0.9182819141995266 + }, + { + "x": 2.304334606053628, + "y": 6.017106411951638, + "heading": 0.950546676609113, + "angularVelocity": 6.93000507489643e-7, + "velocityX": 0.8178250496233883, + "velocityY": 0.371121693591152, + "timestamp": 1.0017620882176654 + }, + { + "x": 2.338470697402953, + "y": 6.032597064971924, "heading": 0.9505466984245466, - "angularVelocity": -1.2192746279649323e-7, - "velocityX": 0.3404612331998472, - "velocityY": 0.335732791424291, - "timestamp": 1.4916787997175387 + "angularVelocity": 2.613247280768537e-7, + "velocityX": 0.40891255619464567, + "velocityY": 0.18556086163546412, + "timestamp": 1.0852422622358042 }, { - "x": 2.583202600479126, - "y": 6.5326762199401855, + "x": 2.338470697402954, + "y": 6.032597064971924, "heading": 0.9505466984245466, - "angularVelocity": 1.0876323700343221e-22, - "velocityX": 1.7516641677537848e-21, - "velocityY": -1.1441920806239185e-21, - "timestamp": 1.580570139068862 + "angularVelocity": -5.839076180969031e-19, + "velocityX": 1.0474679300431341e-18, + "velocityY": 9.135743131461031e-19, + "timestamp": 1.168722436253943 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/4 piece.4.traj b/src/main/deploy/choreo/4 piece.4.traj index a2526be..f1d89d1 100644 --- a/src/main/deploy/choreo/4 piece.4.traj +++ b/src/main/deploy/choreo/4 piece.4.traj @@ -1,157 +1,85 @@ { "samples": [ { - "x": 2.583202600479126, - "y": 6.5326762199401855, + "x": 2.338470697402954, + "y": 6.032597064971924, "heading": 0.9505466984245466, - "angularVelocity": 1.0876323700343221e-22, - "velocityX": 1.7516641677537848e-21, - "velocityY": -1.1441920806239185e-21, + "angularVelocity": -5.839076180969031e-19, + "velocityX": 1.0474679300431341e-18, + "velocityY": 9.135743131461031e-19, "timestamp": 0 }, { - "x": 2.542403880498974, - "y": 6.501670263198367, - "heading": 0.9505466903049901, - "angularVelocity": -8.318907821701634e-8, - "velocityX": -0.41800409947017914, - "velocityY": -0.3176721483512302, - "timestamp": 0.09760363602142741 - }, - { - "x": 2.460806445400462, - "y": 6.439658353412451, - "heading": 0.9505466688810346, - "angularVelocity": -2.194995637368503e-7, - "velocityX": -0.8360081491287711, - "velocityY": -0.6353442588173928, - "timestamp": 0.19520727204285482 - }, - { - "x": 2.367555279184838, - "y": 6.368789913715819, - "heading": 0.9153741187078941, - "angularVelocity": -0.3603610644732418, - "velocityX": -0.9554066837751003, - "velocityY": -0.7260840127008578, - "timestamp": 0.2928109080642822 - }, - { - "x": 2.2743041129628643, - "y": 6.297921474027541, - "heading": 0.8483526672144899, - "angularVelocity": -0.6866696183192453, - "velocityX": -0.9554066838401561, - "velocityY": -0.7260840126152636, - "timestamp": 0.3904145440857101 - }, - { - "x": 2.1810529467408974, - "y": 6.227053034339255, - "heading": 0.7577571566962064, - "angularVelocity": -0.9281981103490298, - "velocityX": -0.9554066838400862, - "velocityY": -0.7260840126153555, - "timestamp": 0.48801818010713793 - }, - { - "x": 2.087801780518917, - "y": 6.156184594650985, - "heading": 0.651002233808638, - "angularVelocity": -1.0937596921505226, - "velocityX": -0.9554066838402226, - "velocityY": -0.726084012615176, - "timestamp": 0.5856218161285658 - }, - { - "x": 1.994550614296896, - "y": 6.0853161549627695, - "heading": 0.5348344163006928, - "angularVelocity": -1.190199691766012, - "velocityX": -0.9554066838406402, - "velocityY": -0.7260840126146266, - "timestamp": 0.6832254521499936 - }, - { - "x": 1.9012994480748335, - "y": 6.014447715274608, - "heading": 0.41557973663701897, - "angularVelocity": -1.2218261995638482, - "velocityX": -0.9554066838410652, - "velocityY": -0.7260840126140673, - "timestamp": 0.7808290881714215 - }, - { - "x": 1.8080482818527632, - "y": 5.943579275586458, - "heading": 0.2994210690519452, - "angularVelocity": -1.1901059460486971, - "velocityX": -0.9554066838411454, - "velocityY": -0.7260840126139618, - "timestamp": 0.8784327241928493 + "x": 2.369756744125885, + "y": 6.076174051247526, + "heading": 0.9505466984245466, + "angularVelocity": 8.034071812555556e-18, + "velocityX": 0.31328557933308754, + "velocityY": 0.43636198308634433, + "timestamp": 0.09986430524353551 }, { - "x": 1.7147971156307207, - "y": 5.87271083589827, - "heading": 0.19268402440905563, - "angularVelocity": -1.0935765202380057, - "velocityX": -0.95540668384086, - "velocityY": -0.7260840126143373, - "timestamp": 0.9760363602142772 + "x": 2.4323288339012104, + "y": 6.163328018686257, + "heading": 0.9505466984245466, + "angularVelocity": 1.2139716514941585e-17, + "velocityX": 0.6265711219112612, + "velocityY": 0.8727239149783513, + "timestamp": 0.19972861048707102 }, { - "x": 1.6215459494087139, - "y": 5.801842396210036, - "heading": 0.10211451516138965, - "angularVelocity": -0.9279317138122054, - "velocityX": -0.9554066838404965, - "velocityY": -0.7260840126148156, - "timestamp": 1.073639996235705 + "x": 2.5022186881182034, + "y": 6.260674584872551, + "heading": 0.9505466984245466, + "angularVelocity": 1.081029516788744e-15, + "velocityX": 0.6998481994802278, + "velocityY": 0.9747883986064685, + "timestamp": 0.29959291573060653 }, { - "x": 1.5282947831867273, - "y": 5.730973956521775, - "heading": 0.035127860642323504, - "angularVelocity": -0.6863131052244829, - "velocityX": -0.955406683840287, - "velocityY": -0.7260840126150913, - "timestamp": 1.171243632257133 + "x": 2.572108542335245, + "y": 6.358021151058831, + "heading": 0.9505466984245466, + "angularVelocity": 1.3587937947073691e-16, + "velocityX": 0.6998481994802291, + "velocityY": 0.974788398606472, + "timestamp": 0.39945722097414205 }, { - "x": 1.4350436169609884, - "y": 5.660105516838453, - "heading": 2.9548859844204172e-8, - "angularVelocity": -0.35990289425029026, - "velocityX": -0.9554066838787327, - "velocityY": -0.7260840125644947, - "timestamp": 1.2688472682785608 + "x": 2.641998396552295, + "y": 6.455367717245125, + "heading": 0.9505466984245468, + "angularVelocity": 5.109002093997956e-16, + "velocityX": 0.6998481994802279, + "velocityY": 0.9747883986064684, + "timestamp": 0.49932152621767756 }, { - "x": 1.353446181869114, - "y": 5.598093607043802, - "heading": 8.120889195643221e-9, - "angularVelocity": -2.1954070075685228e-7, - "velocityX": -0.8360081490607667, - "velocityY": -0.6353442589068752, - "timestamp": 1.3664509042999886 + "x": 2.7045704863276203, + "y": 6.54252168468387, + "heading": 0.9505466984245466, + "angularVelocity": 9.967273568896952e-18, + "velocityX": 0.6265711219112611, + "velocityY": 0.8727239149783513, + "timestamp": 0.5991858314612131 }, { - "x": 1.3126474618911743, - "y": 5.567087650299072, - "heading": 2.2803993876993828e-23, - "angularVelocity": -8.320273226153553e-8, - "velocityX": -0.41800409944751094, - "velocityY": -0.31767214838105773, - "timestamp": 1.4640545403214165 + "x": 2.735856533050537, + "y": 6.586098670959473, + "heading": 0.9505466984245466, + "angularVelocity": 2.3352525714710484e-17, + "velocityX": 0.3132855793330876, + "velocityY": 0.43636198308634433, + "timestamp": 0.6990501367047486 }, { - "x": 1.3126474618911743, - "y": 5.567087650299072, - "heading": 2.991326698553366e-23, - "angularVelocity": 2.478347927470716e-23, - "velocityX": 9.764266009341063e-23, - "velocityY": -5.1068246254861434e-23, - "timestamp": 1.5616581763428443 + "x": 2.735856533050537, + "y": 6.586098670959473, + "heading": 0.9505466984245466, + "angularVelocity": 4.489031866337397e-20, + "velocityX": -2.841253050020481e-19, + "velocityY": -1.4670872630302605e-19, + "timestamp": 0.7989144419482841 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/4 piece.5.traj b/src/main/deploy/choreo/4 piece.5.traj index 6606b87..74303c0 100644 --- a/src/main/deploy/choreo/4 piece.5.traj +++ b/src/main/deploy/choreo/4 piece.5.traj @@ -1,184 +1,166 @@ { "samples": [ + { + "x": 2.735856533050537, + "y": 6.586098670959473, + "heading": 0.9505466984245466, + "angularVelocity": 4.489031866337397e-20, + "velocityX": -2.841253050020481e-19, + "velocityY": -1.4670872630302605e-19, + "timestamp": 0 + }, + { + "x": 2.6926372151215485, + "y": 6.555153842004882, + "heading": 0.9505466918112597, + "angularVelocity": -6.652694848851377e-8, + "velocityX": -0.4347685747061206, + "velocityY": -0.3112922605861828, + "timestamp": 0.0994076399339967 + }, + { + "x": 2.6061985842528834, + "y": 6.493264187669074, + "heading": 0.9505466743461212, + "angularVelocity": -1.756921161749788e-7, + "velocityX": -0.8695370992212547, + "velocityY": -0.6225844852256829, + "timestamp": 0.1988152798679934 + }, + { + "x": 2.5092074864577216, + "y": 6.423819016493211, + "heading": 0.9190781371048612, + "angularVelocity": -0.3165605506996643, + "velocityX": -0.9756905793112982, + "velocityY": -0.6985898792283688, + "timestamp": 0.2982229198019901 + }, + { + "x": 2.4122163886604104, + "y": 6.354373845320345, + "heading": 0.8601974459911542, + "angularVelocity": -0.5923155519314354, + "velocityX": -0.9756905793328975, + "velocityY": -0.6985898791982086, + "timestamp": 0.3976305597359868 + }, + { + "x": 2.3152252908630935, + "y": 6.28492867414746, + "heading": 0.7807467252149503, + "angularVelocity": -0.7992415958064828, + "velocityX": -0.975690579332817, + "velocityY": -0.698589879198321, + "timestamp": 0.4970381996699835 + }, + { + "x": 2.2182341930657996, + "y": 6.215483502974575, + "heading": 0.6866750516739581, + "angularVelocity": -0.9463223712327561, + "velocityX": -0.9756905793328342, + "velocityY": -0.6985898791982971, + "timestamp": 0.5964458396039802 + }, + { + "x": 2.121243095268475, + "y": 6.146038331801719, + "heading": 0.5832432706368847, + "angularVelocity": -1.0404812055265393, + "velocityX": -0.9756905793329995, + "velocityY": -0.698589879198066, + "timestamp": 0.6958534795379769 + }, + { + "x": 2.024251997471116, + "y": 6.076593160628913, + "heading": 0.4752487029161645, + "angularVelocity": -1.0863809642038045, + "velocityX": -0.9756905793332569, + "velocityY": -0.6985898791977067, + "timestamp": 0.7952611194719736 + }, + { + "x": 1.9272608996737375, + "y": 6.007147989456111, + "heading": 0.36725581200699825, + "angularVelocity": -1.0863640961687604, + "velocityX": -0.9756905793334377, + "velocityY": -0.6985898791974542, + "timestamp": 0.8946687594059703 + }, + { + "x": 1.8302698018763681, + "y": 5.937702818283318, + "heading": 0.26382879729241876, + "angularVelocity": -1.040433258281267, + "velocityX": -0.975690579333424, + "velocityY": -0.6985898791974733, + "timestamp": 0.994076399339967 + }, + { + "x": 1.7332787040790516, + "y": 5.868257647110507, + "heading": 0.16976442296144847, + "angularVelocity": -0.9462489441799997, + "velocityX": -0.9756905793332555, + "velocityY": -0.6985898791977087, + "timestamp": 1.0934840392739638 + }, + { + "x": 1.6362876062816873, + "y": 5.798812475937657, + "heading": 0.09032312899825462, + "angularVelocity": -0.7991467659421343, + "velocityX": -0.9756905793330708, + "velocityY": -0.6985898791979667, + "timestamp": 1.1928916792079605 + }, + { + "x": 1.5392965084843735, + "y": 5.72936730476478, + "heading": 0.03145409331145777, + "angularVelocity": -0.5921983031272416, + "velocityX": -0.975690579332948, + "velocityY": -0.6985898791981381, + "timestamp": 1.2922993191419572 + }, + { + "x": 1.4423054106856323, + "y": 5.6599221335939, + "heading": 2.4080089212053166e-8, + "angularVelocity": -0.31641500846668225, + "velocityX": -0.975690579347126, + "velocityY": -0.6985898791783296, + "timestamp": 1.3917069590759539 + }, + { + "x": 1.3558667798193893, + "y": 5.598032479254774, + "heading": 6.6137151436793805e-9, + "angularVelocity": -1.757045442719512e-7, + "velocityX": -0.8695370991973227, + "velocityY": -0.6225844852591078, + "timestamp": 1.4911145990099506 + }, { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": 2.991326698553366e-23, - "angularVelocity": 2.478347927470716e-23, - "velocityX": 9.764266009341063e-23, - "velocityY": -5.1068246254861434e-23, - "timestamp": 0 + "heading": 1.3033321736184719e-19, + "angularVelocity": -6.653125604117621e-8, + "velocityX": -0.43476857469814373, + "velocityY": -0.3112922605973239, + "timestamp": 1.5905222389439473 }, { - "x": 1.3367227529770005, - "y": 5.54401651323739, - "heading": -8.745942749209055e-9, - "angularVelocity": -1.1108213753593968e-7, - "velocityX": 0.30578004822354515, - "velocityY": -0.2930262972997385, - "timestamp": 0.07873401559615711 - }, - { - "x": 1.3848733307698786, - "y": 5.497874243310031, - "heading": -3.2419454048962964e-8, - "angularVelocity": -3.0067704689648114e-7, - "velocityX": 0.611560040832321, - "velocityY": -0.5860525413060693, - "timestamp": 0.15746803119231423 - }, - { - "x": 1.4530888046304524, - "y": 5.432503962156357, - "heading": -0.006001262639786551, - "angularVelocity": -0.07622156922763929, - "velocityX": 0.866404099220153, - "velocityY": -0.8302673331050667, - "timestamp": 0.23620204678847134 - }, - { - "x": 1.5213042784906459, - "y": 5.367133681002277, - "heading": -0.03774158111147184, - "angularVelocity": -0.403133489780173, - "velocityX": 0.8664040992153226, - "velocityY": -0.8302673331102199, - "timestamp": 0.31493606238462846 - }, - { - "x": 1.5895197523508426, - "y": 5.301763399848201, - "heading": -0.08991573959916936, - "angularVelocity": -0.6626635018250512, - "velocityX": 0.866404099215366, - "velocityY": -0.8302673331101746, - "timestamp": 0.39367007798078557 - }, - { - "x": 1.6577352262110532, - "y": 5.2363931186941395, - "heading": -0.15757332647853733, - "angularVelocity": -0.8593183818592182, - "velocityX": 0.8664040992155391, - "velocityY": -0.830267333109994, - "timestamp": 0.4724040935769427 - }, - { - "x": 1.7259507000712884, - "y": 5.171022837540104, - "heading": -0.2360864752633604, - "angularVelocity": -0.9971947726829187, - "velocityX": 0.8664040992158549, - "velocityY": -0.8302673331096645, - "timestamp": 0.5511381091730998 - }, - { - "x": 1.7941661739315489, - "y": 5.105652556386094, - "heading": -0.32108338261130676, - "angularVelocity": -1.079544929905686, - "velocityX": 0.8664040992161722, - "velocityY": -0.8302673331093331, - "timestamp": 0.6298721247692569 - }, - { - "x": 1.8623816477918176, - "y": 5.040282275232093, - "heading": -0.40835744473560676, - "angularVelocity": -1.1084670515466504, - "velocityX": 0.8664040992162807, - "velocityY": -0.8302673331092201, - "timestamp": 0.708606140365414 - }, - { - "x": 1.9305971216520723, - "y": 4.974911994078077, - "heading": -0.49376220718458297, - "angularVelocity": -1.0847250937515367, - "velocityX": 0.8664040992161026, - "velocityY": -0.8302673331094058, - "timestamp": 0.7873401559615711 - }, - { - "x": 1.9988125955123008, - "y": 4.9095417129240335, - "heading": -0.5731014303336502, - "angularVelocity": -1.007686735502157, - "velocityX": 0.8664040992157647, - "velocityY": -0.8302673331097585, - "timestamp": 0.8660741715577283 - }, - { - "x": 2.06702806937251, - "y": 4.844171431769971, - "heading": -0.6420233840895226, - "angularVelocity": -0.8753770938013302, - "velocityX": 0.8664040992155239, - "velocityY": -0.8302673331100098, - "timestamp": 0.9448081871538854 - }, - { - "x": 2.1352435432327193, - "y": 4.778801150615908, - "heading": -0.6959284119501224, - "angularVelocity": -0.6846472576362663, - "velocityX": 0.866404099215522, - "velocityY": -0.8302673331100118, - "timestamp": 1.0235422027500425 - }, - { - "x": 2.2034590244293213, - "y": 4.713430881500244, - "heading": -0.7298997067825479, - "angularVelocity": -0.43146909979380765, - "velocityX": 0.8664041923949785, - "velocityY": -0.8302671802104146, - "timestamp": 1.1022762183461996 - }, - { - "x": 2.284274256756373, - "y": 4.64687721210592, - "heading": -0.7358958248166633, - "angularVelocity": -0.06872842162702524, - "velocityX": 0.9263165484164835, - "velocityY": -0.7628483336942082, - "timestamp": 1.1895198635830289 - }, - { - "x": 2.3650894966282934, - "y": 4.580323551873232, - "heading": -0.7298997308132567, - "angularVelocity": 0.06872814618336746, - "velocityX": 0.9263166348969202, - "velocityY": -0.762848228682137, - "timestamp": 1.2767635088198581 - }, - { - "x": 2.428299134503931, - "y": 4.5282686081612855, - "heading": -0.7298997143064122, - "angularVelocity": 1.8920397482236636e-7, - "velocityX": 0.7245185331728233, - "velocityY": -0.5966617232766905, - "timestamp": 1.3640071540566874 - }, - { - "x": 2.4599039554595947, - "y": 4.502241134643555, - "heading": -0.7298997067825479, - "angularVelocity": 8.623968367420298e-8, - "velocityX": 0.3622592897152577, - "velocityY": -0.29833088068566255, - "timestamp": 1.4512507992935166 - }, - { - "x": 2.4599039554595947, - "y": 4.502241134643555, - "heading": -0.7298997067825479, - "angularVelocity": -9.04164627803996e-23, - "velocityX": -1.4473589854434152e-22, - "velocityY": -2.1145820655457915e-22, - "timestamp": 1.538494444530346 + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": 2.2172969897005315e-19, + "angularVelocity": 2.0015491793199464e-19, + "velocityX": -1.0501490690724046e-19, + "velocityY": -4.272854375601257e-19, + "timestamp": 1.689929878877944 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/4 piece.6.traj b/src/main/deploy/choreo/4 piece.6.traj index 0f06597..4c1c790 100644 --- a/src/main/deploy/choreo/4 piece.6.traj +++ b/src/main/deploy/choreo/4 piece.6.traj @@ -1,148 +1,139 @@ { "samples": [ { - "x": 2.4599039554595947, - "y": 4.502241134643555, - "heading": -0.7298997067825479, - "angularVelocity": -9.04164627803996e-23, - "velocityX": -1.4473589854434152e-22, - "velocityY": -2.1145820655457915e-22, + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": 2.2172969897005315e-19, + "angularVelocity": 2.0015491793199464e-19, + "velocityX": -1.0501490690724046e-19, + "velocityY": -4.272854375601257e-19, "timestamp": 0 }, { - "x": 2.418647701705629, - "y": 4.540533860206957, - "heading": -0.7298997001349049, - "angularVelocity": 6.498469080235459e-8, - "velocityX": -0.40330458085897497, - "velocityY": 0.374334318510709, - "timestamp": 0.10229552480186666 + "x": 1.3441215983818582, + "y": 5.53692626623967, + "heading": -1.1643991871335638e-8, + "angularVelocity": -1.2934461900203075e-7, + "velocityX": 0.34962324193127553, + "velocityY": -0.33504083198930457, + "timestamp": 0.09002300967412946 }, { - "x": 2.336135198753465, - "y": 4.61711930710535, - "heading": -0.7298996827304232, - "angularVelocity": 1.7013922856843512e-7, - "velocityX": -0.8066091171825944, - "velocityY": 0.748668595686158, - "timestamp": 0.2045910496037333 + "x": 1.4070698670672521, + "y": 5.476603502232459, + "heading": -4.233223250468571e-8, + "angularVelocity": -3.4089329819791224e-7, + "velocityX": 0.6992464361418851, + "velocityY": -0.6700816183059229, + "timestamp": 0.18004601934825892 }, { - "x": 2.2461633007376833, - "y": 4.700628324178363, - "heading": -0.6961908922790889, - "angularVelocity": 0.3295236083555346, - "velocityX": -0.879529169922592, - "velocityY": 0.8163506393340244, - "timestamp": 0.30688657440559997 + "x": 1.485066172875745, + "y": 5.401860338613426, + "heading": -0.031252361434063125, + "angularVelocity": -0.347159234233111, + "velocityX": 0.8664041125799371, + "velocityY": -0.8302673270932495, + "timestamp": 0.2700690290223884 }, { - "x": 2.1561914027221256, - "y": 4.78413734125162, - "heading": -0.6354601214689052, - "angularVelocity": 0.5936796446160405, - "velocityX": -0.8795291699203955, - "velocityY": 0.8163506393363948, - "timestamp": 0.4091820992074666 + "x": 1.5630624786713805, + "y": 5.327117174980967, + "heading": -0.0999659837507643, + "angularVelocity": -0.7632895474771944, + "velocityX": 0.8664041124370567, + "velocityY": -0.8302673272423656, + "timestamp": 0.36009203869651785 }, { - "x": 2.0662195047065577, - "y": 4.8676463583248655, - "heading": -0.555497965787572, - "angularVelocity": 0.7816779456990843, - "velocityX": -0.8795291699204953, - "velocityY": 0.8163506393362874, - "timestamp": 0.5114776240093333 + "x": 1.6410587844670608, + "y": 5.2523740113485955, + "heading": -0.19520871175979404, + "angularVelocity": -1.0579820465211633, + "velocityX": 0.866404112437784, + "velocityY": -0.8302673272416067, + "timestamp": 0.4501150483706473 }, { - "x": 1.976247606690973, - "y": 4.951155375398092, - "heading": -0.4632200489828426, - "angularVelocity": 0.9020718842144826, - "velocityX": -0.8795291699206637, - "velocityY": 0.8163506393361059, - "timestamp": 0.6137731488111999 + "x": 1.7190550902628128, + "y": 5.177630847716277, + "heading": -0.30625153833554625, + "angularVelocity": -1.2334938253865513, + "velocityX": 0.8664041124384794, + "velocityY": -0.8302673272408809, + "timestamp": 0.5401380580447768 }, { - "x": 1.8862757086753756, - "y": 5.0346643924713055, - "heading": -0.3649382393995183, - "angularVelocity": 0.96076353069876, - "velocityX": -0.8795291699207887, - "velocityY": 0.8163506393359712, - "timestamp": 0.7160686736130666 + "x": 1.797051396058594, + "y": 5.1028876840839885, + "heading": -0.4225688522263227, + "angularVelocity": -1.292084260588809, + "velocityX": 0.8664041124387727, + "velocityY": -0.8302673272405751, + "timestamp": 0.6301610677189062 }, { - "x": 1.7963038106597808, - "y": 5.118173409544521, - "heading": -0.2666575817787772, - "angularVelocity": 0.9607522695748207, - "velocityX": -0.8795291699207628, - "velocityY": 0.816350639335999, - "timestamp": 0.8183641984149332 + "x": 1.8750477018543286, + "y": 5.028144520451638, + "heading": -0.5337184986736393, + "angularVelocity": -1.234680409482674, + "velocityX": 0.8664041124382615, + "velocityY": -0.8302673272411084, + "timestamp": 0.7201840773930357 }, { - "x": 1.7063319126442014, - "y": 5.201682426617753, - "heading": -0.17438289577352772, - "angularVelocity": 0.9020403012152627, - "velocityX": -0.8795291699206121, - "velocityY": 0.8163506393361615, - "timestamp": 0.9206597232167999 + "x": 1.9530440076500057, + "y": 4.953401356819256, + "heading": -0.6291756131928835, + "angularVelocity": -1.0603635100046716, + "velocityX": 0.8664041124376008, + "velocityY": -0.8302673272417979, + "timestamp": 0.8102070870671652 }, { - "x": 1.6163600146286374, - "y": 5.2851914436910015, - "heading": -0.09442558399173479, - "angularVelocity": 0.7816305936810019, - "velocityX": -0.8795291699204603, - "velocityY": 0.8163506393363251, - "timestamp": 1.0229552480186666 + "x": 2.0310403134456294, + "y": 4.878658193186804, + "heading": -0.6982128332545968, + "angularVelocity": -0.7668841589680074, + "velocityX": 0.8664041124370991, + "velocityY": -0.8302673272423214, + "timestamp": 0.9002300967412946 }, { - "x": 1.526388116613081, - "y": 5.368700460764259, - "heading": -0.033700979293950294, - "angularVelocity": 0.5936193671756421, - "velocityX": -0.8795291699203863, - "velocityY": 0.8163506393364047, - "timestamp": 1.1252507728205332 + "x": 2.109036619249582, + "y": 4.803915029563043, + "heading": -0.7298996645397342, + "angularVelocity": -0.3519859133774726, + "velocityX": 0.8664041125295305, + "velocityY": -0.83026732714585, + "timestamp": 0.9902531064154241 }, { - "x": 1.436416218597363, - "y": 5.452209477837341, - "heading": -2.4053492503793295e-8, - "angularVelocity": 0.32944701447822, - "velocityX": -0.8795291699219676, - "velocityY": 0.8163506393346972, - "timestamp": 1.2275462976224008 + "x": 2.1719848879377293, + "y": 4.743592265558695, + "heading": -0.7298996951613214, + "angularVelocity": -3.401528940808921e-7, + "velocityX": 0.6992464361723661, + "velocityY": -0.670081618274115, + "timestamp": 1.0802761160895535 }, { - "x": 1.353903715645155, - "y": 5.528794924735686, - "heading": -6.648008517386852e-9, - "angularVelocity": 1.701490267547733e-7, - "velocityX": -0.8066091171830237, - "velocityY": 0.7486685956856953, - "timestamp": 1.3298418224242683 - }, - { - "x": 1.3126474618911743, - "y": 5.567087650299072, - "heading": 6.220225099112246e-24, - "angularVelocity": 6.498826346766567e-8, - "velocityX": -0.403304580859118, - "velocityY": 0.37433431851055476, - "timestamp": 1.4321373472261358 + "x": 2.2034590244293213, + "y": 4.713430881500244, + "heading": -0.7298997067825479, + "angularVelocity": -1.2909173503647302e-7, + "velocityX": 0.3496232419414347, + "velocityY": -0.3350408319787032, + "timestamp": 1.170299125763683 }, { - "x": 1.3126474618911743, - "y": 5.567087650299072, - "heading": 3.0379270142045743e-24, - "angularVelocity": -1.4113145535339382e-24, - "velocityX": -1.5623932740219278e-24, - "velocityY": 1.5729984496793032e-24, - "timestamp": 1.5344328720280034 + "x": 2.2034590244293213, + "y": 4.713430881500244, + "heading": -0.7298997067825479, + "angularVelocity": 1.6899611320835774e-19, + "velocityX": 5.175181532248611e-19, + "velocityY": -4.71372364987181e-19, + "timestamp": 1.2603221354378125 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/4 piece.7.traj b/src/main/deploy/choreo/4 piece.7.traj new file mode 100644 index 0000000..098aed3 --- /dev/null +++ b/src/main/deploy/choreo/4 piece.7.traj @@ -0,0 +1,77 @@ +{ + "samples": [ + { + "x": 2.2034590244293213, + "y": 4.713430881500244, + "heading": -0.7298997067825479, + "angularVelocity": 1.6899611320835774e-19, + "velocityX": 5.175181532248611e-19, + "velocityY": -4.71372364987181e-19, + "timestamp": 0 + }, + { + "x": 2.2414956338795715, + "y": 4.681346676235305, + "heading": -0.7298997067825479, + "angularVelocity": -2.46282456581248e-17, + "velocityX": 0.39546677329122837, + "velocityY": -0.333579604310823, + "timestamp": 0.0961815556176564 + }, + { + "x": 2.3175688480689023, + "y": 4.617178269679347, + "heading": -0.729899706782548, + "angularVelocity": -4.562002127415535e-17, + "velocityX": 0.7909334976002727, + "velocityY": -0.6671591673047552, + "timestamp": 0.1923631112353128 + }, + { + "x": 2.405792236328125, + "y": 4.5427610874176025, + "heading": -0.729899706782548, + "angularVelocity": 1.0687559485237639e-16, + "velocityX": 0.917258903671292, + "velocityY": -0.7737157273435987, + "timestamp": 0.2885446668529692 + }, + { + "x": 2.494015624587376, + "y": 4.468343905155829, + "heading": -0.7298997067825479, + "angularVelocity": 2.54215551828638e-16, + "velocityX": 0.917258903671292, + "velocityY": -0.7737157273435986, + "timestamp": 0.3847262224706256 + }, + { + "x": 2.5700888387766927, + "y": 4.4041754985999, + "heading": -0.7298997067825479, + "angularVelocity": -6.314437383419688e-17, + "velocityX": 0.7909334976002727, + "velocityY": -0.6671591673047553, + "timestamp": 0.480907778088282 + }, + { + "x": 2.6081254482269287, + "y": 4.372091293334961, + "heading": -0.7298997067825479, + "angularVelocity": -7.295944708803859e-17, + "velocityX": 0.3954667732912283, + "velocityY": -0.333579604310823, + "timestamp": 0.5770893337059384 + }, + { + "x": 2.6081254482269287, + "y": 4.372091293334961, + "heading": -0.7298997067825479, + "angularVelocity": -3.557756419099644e-20, + "velocityX": -1.1158976027767978e-18, + "velocityY": 1.0302990290883233e-18, + "timestamp": 0.6732708893235948 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4 piece.8.traj b/src/main/deploy/choreo/4 piece.8.traj new file mode 100644 index 0000000..c3e4a7a --- /dev/null +++ b/src/main/deploy/choreo/4 piece.8.traj @@ -0,0 +1,158 @@ +{ + "samples": [ + { + "x": 2.6081254482269287, + "y": 4.372091293334961, + "heading": -0.7298997067825479, + "angularVelocity": -3.557756419099644e-20, + "velocityX": -1.1158976027767978e-18, + "velocityY": 1.0302990290883233e-18, + "timestamp": 0 + }, + { + "x": 2.563674001371605, + "y": 4.41309493647991, + "heading": -0.7298997015558017, + "angularVelocity": 4.929434515061794e-8, + "velocityX": -0.41922926520033577, + "velocityY": 0.38671243350444034, + "timestamp": 0.10603135454786461 + }, + { + "x": 2.474771112276091, + "y": 4.495102218512861, + "heading": -0.7298996878700399, + "angularVelocity": 1.2907278142001575e-7, + "velocityX": -0.838458486875625, + "velocityY": 0.773424826859854, + "timestamp": 0.21206270909572922 + }, + { + "x": 2.381246629145657, + "y": 4.581372628204462, + "heading": -0.7000859323738574, + "angularVelocity": 0.28117867232115834, + "velocityX": -0.8820455376541905, + "velocityY": 0.8136311193942835, + "timestamp": 0.3180940636435938 + }, + { + "x": 2.287722146015324, + "y": 4.667643037896195, + "heading": -0.6472777997396291, + "angularVelocity": 0.498042610692009, + "velocityX": -0.8820455376540477, + "velocityY": 0.81363111939444, + "timestamp": 0.42412541819145844 + }, + { + "x": 2.1941976628849664, + "y": 4.753913447587866, + "heading": -0.577776535255652, + "angularVelocity": 0.6554784174958657, + "velocityX": -0.8820455376540902, + "velocityY": 0.813631119394394, + "timestamp": 0.530156772739323 + }, + { + "x": 2.10067317975465, + "y": 4.8401838572795235, + "heading": -0.49698505133773596, + "angularVelocity": 0.7619584250566693, + "velocityX": -0.8820455376541692, + "velocityY": 0.8136311193943084, + "timestamp": 0.6361881272871877 + }, + { + "x": 2.0071486966242564, + "y": 4.9264542669711755, + "heading": -0.4096708626062039, + "angularVelocity": 0.8234751796188454, + "velocityX": -0.8820455376542478, + "velocityY": 0.8136311193942233, + "timestamp": 0.7422194818350523 + }, + { + "x": 1.913624213493877, + "y": 5.0127246766628355, + "heading": -0.3202243151295472, + "angularVelocity": 0.8435858228735447, + "velocityX": -0.882045537654274, + "velocityY": 0.8136311193941947, + "timestamp": 0.8482508363829169 + }, + { + "x": 1.8200997303635116, + "y": 5.098995086354495, + "heading": -0.23291079154548175, + "angularVelocity": 0.8234689064983195, + "velocityX": -0.8820455376542292, + "velocityY": 0.8136311193942434, + "timestamp": 0.9542821909307815 + }, + { + "x": 1.7265752472331424, + "y": 5.185265496046131, + "heading": -0.15212040474401956, + "angularVelocity": 0.7619480779620905, + "velocityX": -0.8820455376541468, + "velocityY": 0.8136311193943326, + "timestamp": 1.060313545478646 + }, + { + "x": 1.6330507641028076, + "y": 5.27153590573785, + "heading": -0.0826202834773307, + "angularVelocity": 0.6554676356163622, + "velocityX": -0.8820455376540777, + "velocityY": 0.8136311193944077, + "timestamp": 1.1663449000265107 + }, + { + "x": 1.5395262809724208, + "y": 5.357806315429548, + "heading": -0.02981305940491789, + "angularVelocity": 0.49803404188875805, + "velocityX": -0.8820455376540449, + "velocityY": 0.8136311193944432, + "timestamp": 1.2723762545743753 + }, + { + "x": 1.4460017978420001, + "y": 5.444076725121205, + "heading": -1.8912690296449957e-8, + "angularVelocity": 0.2811719289954887, + "velocityX": -0.8820455376541747, + "velocityY": 0.8136311193943003, + "timestamp": 1.37840760912224 + }, + { + "x": 1.3570989087465364, + "y": 5.526084007154097, + "heading": -5.2268050645253455e-9, + "angularVelocity": 1.290739450532968e-7, + "velocityX": -0.8384584868756361, + "velocityY": 0.7734248268598417, + "timestamp": 1.4844389636701045 + }, + { + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": 1.130585771980045e-19, + "angularVelocity": 4.9294900420321916e-8, + "velocityX": -0.41922926520033954, + "velocityY": 0.3867124335044363, + "timestamp": 1.5904703182179691 + }, + { + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": 5.546193231131035e-20, + "angularVelocity": -2.0132747430807326e-20, + "velocityX": -6.460753207500427e-19, + "velocityY": 5.964321607162322e-19, + "timestamp": 1.6965016727658337 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4 piece.traj b/src/main/deploy/choreo/4 piece.traj index 166a4cb..d5e4535 100644 --- a/src/main/deploy/choreo/4 piece.traj +++ b/src/main/deploy/choreo/4 piece.traj @@ -3,821 +3,857 @@ { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": -1.5437737592748072e-37, - "angularVelocity": -7.89083646860229e-38, - "velocityX": 1.418479553941243e-23, - "velocityY": 2.4322727819891274e-38, + "heading": 1.1953764755650808e-32, + "angularVelocity": 6.990086021448287e-33, + "velocityX": 5.494174654732201e-19, + "velocityY": 4.78279637249085e-34, "timestamp": 0 }, { - "x": 1.392444928049753, + "x": 1.3924449280497553, "y": 5.567087650299072, - "heading": -4.6220470147664874e-27, - "angularVelocity": -3.7948407554824907e-26, - "velocityX": 0.6551613945414111, - "velocityY": 1.137046594589441e-15, - "timestamp": 0.12179818106412381 + "heading": 4.1001262381929663e-22, + "angularVelocity": 3.3663279452468632e-21, + "velocityX": 0.6551613945414227, + "velocityY": 5.759043744920587e-16, + "timestamp": 0.12179818106412517 }, { - "x": 1.5386027425923001, + "x": 1.538602742592314, "y": 5.567087650299072, - "heading": -1.9365333413339378e-22, - "angularVelocity": -1.5899146472838326e-21, - "velocityX": 1.1999999775497356, - "velocityY": 2.082625593248517e-15, - "timestamp": 0.24359636212824762 + "heading": -9.345363045002027e-18, + "angularVelocity": -7.673163046652378e-17, + "velocityX": 1.199999977549737, + "velocityY": 1.0548320493532893e-15, + "timestamp": 0.24359636212825034 }, { - "x": 1.6847605571348494, + "x": 1.6847605571348563, "y": 5.567087650299072, - "heading": -1.4409951368437792e-22, - "angularVelocity": 4.0685189233595474e-22, + "heading": -1.2209432374345432e-17, + "angularVelocity": -2.351487757270866e-17, "velocityX": 1.199999977549752, - "velocityY": 2.082625593248546e-15, - "timestamp": 0.3653945431923714 + "velocityY": 1.054832049353261e-15, + "timestamp": 0.3653945431923755 }, { "x": 1.8309183716773985, "y": 5.567087650299072, - "heading": 2.0899654958449072e-23, - "angularVelocity": 1.3546932080698235e-21, - "velocityX": 1.199999977549752, - "velocityY": 2.0826255932485463e-15, - "timestamp": 0.48719272425649524 + "heading": -1.068309383473616e-17, + "angularVelocity": 1.2531702212408767e-17, + "velocityX": 1.1999999775497523, + "velocityY": 1.0548320493532611e-15, + "timestamp": 0.4871927242565007 }, { - "x": 1.9770761862199477, + "x": 1.9770761862199266, "y": 5.567087650299072, - "heading": 1.690438544316869e-22, - "angularVelocity": 1.2163088001720124e-21, - "velocityX": 1.199999977549752, - "velocityY": 2.082625593248546e-15, - "timestamp": 0.6089909053206191 + "heading": -6.939437782754445e-18, + "angularVelocity": 3.0736551271554804e-17, + "velocityX": 1.1999999775497523, + "velocityY": 1.0548320493532611e-15, + "timestamp": 0.6089909053206258 }, { "x": 2.1232340007624972, "y": 5.567087650299072, - "heading": 1.86290605944189e-22, - "angularVelocity": 1.416010597352205e-22, + "heading": -2.9324482572141887e-18, + "angularVelocity": 3.289859892177714e-17, "velocityX": 1.199999977549752, - "velocityY": 2.082625593248546e-15, - "timestamp": 0.7307890863847428 + "velocityY": 1.054832049353261e-15, + "timestamp": 0.730789086384751 }, { - "x": 2.2693918153050445, + "x": 2.269391815305044, "y": 5.567087650299072, - "heading": 4.5629176638525406e-27, - "angularVelocity": -1.5294649016839573e-21, - "velocityX": 1.1999999775497356, - "velocityY": 2.0826255932485175e-15, - "timestamp": 0.8525872674488666 + "heading": -2.924223024384178e-22, + "angularVelocity": 2.407388842978096e-17, + "velocityX": 1.199999977549737, + "velocityY": 1.0548320493532885e-15, + "timestamp": 0.8525872674488763 }, { "x": 2.349189281463623, "y": 5.567087650299072, - "heading": -1.0800252356875386e-31, - "angularVelocity": -3.7463824388368756e-26, - "velocityX": 0.6551613945414111, - "velocityY": 1.1370465945894429e-15, - "timestamp": 0.9743854485129904 + "heading": 1.6673685334753995e-30, + "angularVelocity": 2.4008757783240606e-21, + "velocityX": 0.6551613945414226, + "velocityY": 5.759043744915343e-16, + "timestamp": 0.9743854485130015 }, { "x": 2.349189281463623, "y": 5.567087650299072, - "heading": -1.15233598675018e-31, - "angularVelocity": -5.936794878517772e-32, - "velocityX": -2.2921562380311214e-24, - "velocityY": 2.1917263454721895e-30, - "timestamp": 1.0961836295771141 + "heading": 1.7831791313090364e-30, + "angularVelocity": 8.43744338443602e-31, + "velocityX": -1.487174311186839e-19, + "velocityY": -5.252529696759049e-28, + "timestamp": 1.0961836295771266 }, { - "x": 2.2693918153050445, + "x": 2.269391815305044, "y": 5.567087650299072, - "heading": -3.475580412192629e-21, - "angularVelocity": -2.8535569099611155e-20, - "velocityX": -0.6551613945414112, - "velocityY": 1.824069231632628e-15, - "timestamp": 1.217981810641238 + "heading": 4.900643599154424e-20, + "angularVelocity": 4.023576968464745e-19, + "velocityX": -0.655161394541426, + "velocityY": -7.280112591641159e-16, + "timestamp": 1.2179818106412523 }, { "x": 2.1232340007624972, "y": 5.567087650299072, - "heading": 7.19228614469986e-17, - "angularVelocity": 5.905370375731918e-16, - "velocityX": -1.1999999775497356, - "velocityY": 3.3409829244929845e-15, - "timestamp": 1.3397799917053617 + "heading": 7.594029969730495e-17, + "angularVelocity": 6.23090526860376e-16, + "velocityX": -1.199999977549737, + "velocityY": -1.3334248944822027e-15, + "timestamp": 1.339779991705378 }, { - "x": 1.9770761862199482, + "x": 1.9770761862199409, "y": 5.567087650299072, - "heading": 5.1366024248226334e-17, - "angularVelocity": -1.6877786695311564e-16, + "heading": 4.257511726642054e-17, + "angularVelocity": -2.7393827208826133e-16, "velocityX": -1.199999977549752, - "velocityY": 3.3409829342395925e-15, - "timestamp": 1.4615781727694854 + "velocityY": -1.3334343966180895e-15, + "timestamp": 1.4615781727695039 }, { "x": 1.8309183716773987, "y": 5.567087650299072, - "heading": -1.7979701811530337e-18, - "angularVelocity": -4.3649251544560875e-16, + "heading": -2.2065028106115054e-17, + "angularVelocity": -5.307152037473876e-16, "velocityX": -1.199999977549752, - "velocityY": 3.340982934381562e-15, - "timestamp": 1.5833763538336092 + "velocityY": -1.3334344015361252e-15, + "timestamp": 1.5833763538336296 }, { - "x": 1.6847605571348494, + "x": 1.684760557134842, "y": 5.567087650299072, - "heading": -5.627263788476133e-17, - "angularVelocity": -4.472535404689559e-16, + "heading": -8.169986730219564e-17, + "angularVelocity": -4.896201228598656e-16, "velocityX": -1.199999977549752, - "velocityY": 3.3409829343799955e-15, - "timestamp": 1.705174534897733 + "velocityY": -1.3334344018518055e-15, + "timestamp": 1.7051745348977554 }, { - "x": 1.5386027425923001, + "x": 1.5386027425923, "y": 5.567087650299072, - "heading": -7.146734861941795e-17, - "angularVelocity": -1.2475318269865482e-16, + "heading": -8.15243225571019e-17, + "angularVelocity": 1.4412662667122633e-18, "velocityX": -1.199999977549752, - "velocityY": 3.3409829330177503e-15, - "timestamp": 1.8269727159618567 + "velocityY": -1.3334342978189559e-15, + "timestamp": 1.8269727159618812 }, { - "x": 1.3924449280497528, + "x": 1.392444928049753, "y": 5.567087650299072, - "heading": 2.401745836439516e-21, - "angularVelocity": 5.867883226238598e-16, - "velocityX": -1.1999999775497356, - "velocityY": 3.3409828502355826e-15, - "timestamp": 1.9487708970259805 + "heading": -9.444741136401807e-19, + "angularVelocity": 6.615849837325982e-16, + "velocityX": -1.199999977549737, + "velocityY": -1.3333514424854479e-15, + "timestamp": 1.948770897026007 }, { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": -1.9529795908474793e-22, - "angularVelocity": -2.1322517075995374e-20, - "velocityX": -0.6551613945414112, - "velocityY": 1.824070118744852e-15, - "timestamp": 2.0705690780901045 + "heading": -9.625683020163944e-19, + "angularVelocity": -1.4856176769231114e-19, + "velocityX": -0.6551613945414261, + "velocityY": -7.279052775637916e-16, + "timestamp": 2.070569078090133 }, { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": -2.0997399222925942e-22, - "angularVelocity": -1.20494685567255e-22, - "velocityX": -2.042395408130124e-22, - "velocityY": 8.462664089537296e-22, - "timestamp": 2.1923672591542283 - }, - { - "x": 1.330224348661209, - "y": 5.5781076780240015, - "heading": 1.3752213180599183e-8, - "angularVelocity": 2.2144266656949007e-7, - "velocityX": 0.28302882054194556, - "velocityY": 0.17744811638904515, - "timestamp": 2.2544700751726765 - }, - { - "x": 1.3653781204474067, - "y": 5.6001477324054765, - "heading": 4.561370782539336e-8, - "angularVelocity": 5.130442818459255e-7, - "velocityX": 0.5660576128424637, - "velocityY": 0.3548962155746409, - "timestamp": 2.3165728911911248 - }, - { - "x": 1.4181087719881549, - "y": 5.633207810238353, - "heading": 1.0865567864033805e-7, - "angularVelocity": 0.0000010151225798878058, - "velocityX": 0.8490863204187717, - "velocityY": 0.532344263149933, - "timestamp": 2.378675707209573 - }, - { - "x": 1.4812487494892455, - "y": 5.672794131521707, - "heading": 0.017849702169017073, - "angularVelocity": 0.28742003435135793, - "velocityX": 1.0167007158312162, - "velocityY": 0.6374319849134539, - "timestamp": 2.4407785232280212 - }, - { - "x": 1.5443887266874352, - "y": 5.712380453288194, - "heading": 0.06529469396388879, - "angularVelocity": 0.7639748861110869, - "velocityX": 1.0167007109538098, - "velocityY": 0.6374319926930171, - "timestamp": 2.5028813392464695 - }, - { - "x": 1.607528703885691, - "y": 5.751966775054576, - "heading": 0.13761827273616745, - "angularVelocity": 1.1645780885490655, - "velocityX": 1.0167007109548667, - "velocityY": 0.6374319926913311, - "timestamp": 2.5649841552649177 - }, - { - "x": 1.6706686810840699, - "y": 5.791553096820763, - "heading": 0.22970172720620147, - "angularVelocity": 1.4827581158748078, - "velocityX": 1.016700710956855, - "velocityY": 0.6374319926881596, - "timestamp": 2.627086971283366 - }, - { - "x": 1.7338086582825494, - "y": 5.831139418586787, - "heading": 0.336088558287123, - "angularVelocity": 1.7130757975502817, - "velocityX": 1.0167007109584723, - "velocityY": 0.6374319926855803, - "timestamp": 2.6891897873018142 - }, - { - "x": 1.7969486354809578, - "y": 5.870725740352926, - "heading": 0.4510732003163099, - "angularVelocity": 1.8515205815309534, - "velocityX": 1.0167007109573307, - "velocityY": 0.6374319926874006, - "timestamp": 2.7512926033202625 - }, - { - "x": 1.860088612679118, - "y": 5.910312062119461, - "heading": 0.5688014129904255, - "angularVelocity": 1.895698459779078, - "velocityX": 1.0167007109533306, - "velocityY": 0.6374319926937808, - "timestamp": 2.8133954193387107 - }, - { - "x": 1.9232285898770611, - "y": 5.949898383886342, - "heading": 0.6833720439947638, - "angularVelocity": 1.8448540396349185, - "velocityX": 1.0167007109498374, - "velocityY": 0.6374319926993527, - "timestamp": 2.875498235357159 - }, - { - "x": 1.9863685670748548, - "y": 5.989484705653461, - "heading": 0.7889380109564055, - "angularVelocity": 1.6998579731115984, - "velocityX": 1.0167007109474246, - "velocityY": 0.6374319927032011, - "timestamp": 2.9376010513756072 - }, - { - "x": 2.049508544272936, - "y": 6.029071027420121, - "heading": 0.8798080925206222, - "angularVelocity": 1.4632199856641441, - "velocityX": 1.0167007109520676, - "velocityY": 0.6374319926957958, - "timestamp": 2.9997038673940555 - }, - { - "x": 2.1126484870910645, - "y": 6.068657398223877, + "heading": -1.0240569848515672e-18, + "angularVelocity": -5.048437233348486e-19, + "velocityX": 3.814586359746447e-19, + "velocityY": 1.4611187628635838e-19, + "timestamp": 2.192367259154259 + }, + { + "x": 1.3467835531597558, + "y": 5.582578303497341, + "heading": 2.1822298286556078e-8, + "angularVelocity": 2.6140695812051043e-7, + "velocityX": 0.408912555227134, + "velocityY": 0.18556086376752873, + "timestamp": 2.2758474331723977 + }, + { + "x": 1.4150557303762996, + "y": 5.61355960759438, + "heading": 7.969110326356027e-8, + "angularVelocity": 6.932041753657309e-7, + "velocityX": 0.8178250467198802, + "velocityY": 0.37112169998949157, + "timestamp": 2.3593276071905365 + }, + { + "x": 1.5062786970732867, + "y": 5.65495577384632, + "heading": 0.029442185922842156, + "angularVelocity": 0.35268381478627076, + "velocityX": 1.0927500783256299, + "velocityY": 0.4958802103472522, + "timestamp": 2.4428077812086753 + }, + { + "x": 1.5975016635378336, + "y": 5.696351940610345, + "heading": 0.11439605036500154, + "angularVelocity": 1.0176531786301763, + "velocityX": 1.0927500755419548, + "velocityY": 0.49588021648158287, + "timestamp": 2.526287955226814 + }, + { + "x": 1.6887246300035204, + "y": 5.737748107371955, + "heading": 0.24137697471085723, + "angularVelocity": 1.5210907959806668, + "velocityX": 1.0927500755551556, + "velocityY": 0.4958802164524925, + "timestamp": 2.609768129244953 + }, + { + "x": 1.779947596469001, + "y": 5.779144274133999, + "heading": 0.394686770242847, + "angularVelocity": 1.836481503963777, + "velocityX": 1.0927500755527955, + "velocityY": 0.4958802164576932, + "timestamp": 2.6932483032630916 + }, + { + "x": 1.8711705629341966, + "y": 5.8205404408966785, + "heading": 0.5569437718053947, + "angularVelocity": 1.9436591199162039, + "velocityX": 1.0927500755492763, + "velocityY": 0.49588021646544844, + "timestamp": 2.7767284772812304 + }, + { + "x": 1.9623935293992807, + "y": 5.861936607659589, + "heading": 0.7101331870008885, + "angularVelocity": 1.8350394808976778, + "velocityX": 1.0927500755480895, + "velocityY": 0.4958802164680637, + "timestamp": 2.860208651299369 + }, + { + "x": 2.053616495864439, + "y": 5.903332774422348, + "heading": 0.8368837129910643, + "angularVelocity": 1.518330878929826, + "velocityX": 1.0927500755488537, + "velocityY": 0.49588021646637936, + "timestamp": 2.943688825317508 + }, + { + "x": 2.144839462330586, + "y": 5.944728941182871, + "heading": 0.9215115215035561, + "angularVelocity": 1.01374738981861, + "velocityX": 1.0927500755610373, + "velocityY": 0.4958802164395315, + "timestamp": 3.0271689993356468 + }, + { + "x": 2.236062428594661, + "y": 5.986125108388746, + "heading": 0.9505466187573102, + "angularVelocity": 0.34780829814088937, + "velocityX": 1.0927500731401507, + "velocityY": 0.49588022177427094, + "timestamp": 3.1106491733537855 + }, + { + "x": 2.304334606053628, + "y": 6.017106411951638, + "heading": 0.950546676609113, + "angularVelocity": 6.93000507489643e-7, + "velocityX": 0.8178250496233883, + "velocityY": 0.371121693591152, + "timestamp": 3.1941293473719243 + }, + { + "x": 2.338470697402953, + "y": 6.032597064971924, + "heading": 0.9505466984245466, + "angularVelocity": 2.613247280768537e-7, + "velocityX": 0.40891255619464567, + "velocityY": 0.18556086163546412, + "timestamp": 3.277609521390063 + }, + { + "x": 2.338470697402954, + "y": 6.032597064971924, + "heading": 0.9505466984245466, + "angularVelocity": -5.839076180969031e-19, + "velocityX": 1.0474679300431341e-18, + "velocityY": 9.135743131461031e-19, + "timestamp": 3.361089695408202 + }, + { + "x": 2.369756744125885, + "y": 6.076174051247526, + "heading": 0.9505466984245466, + "angularVelocity": 8.034071812555556e-18, + "velocityX": 0.31328557933308754, + "velocityY": 0.43636198308634433, + "timestamp": 3.4609540006517374 + }, + { + "x": 2.4323288339012104, + "y": 6.163328018686257, + "heading": 0.9505466984245466, + "angularVelocity": 1.2139716514941585e-17, + "velocityX": 0.6265711219112612, + "velocityY": 0.8727239149783513, + "timestamp": 3.560818305895273 + }, + { + "x": 2.5022186881182034, + "y": 6.260674584872551, + "heading": 0.9505466984245466, + "angularVelocity": 1.081029516788744e-15, + "velocityX": 0.6998481994802278, + "velocityY": 0.9747883986064685, + "timestamp": 3.6606826111388084 + }, + { + "x": 2.572108542335245, + "y": 6.358021151058831, "heading": 0.9505466984245466, - "angularVelocity": 1.1390563333377834, - "velocityX": 1.0167001573547376, - "velocityY": 0.63743278230727, - "timestamp": 3.0618066834125037 - }, - { - "x": 2.1886009078042274, - "y": 6.143554890170836, - "heading": 1.0124745802683783, - "angularVelocity": 0.6966694651666266, - "velocityX": 0.8544411780429775, - "velocityY": 0.8425735565862382, - "timestamp": 3.150698022763827 - }, - { - "x": 2.264553290701356, - "y": 6.2184524204664475, - "heading": 1.0234437942174555, - "angularVelocity": 0.12340025506561028, - "velocityX": 0.8544407526243177, - "velocityY": 0.8425739879966911, - "timestamp": 3.2395893621151504 - }, - { - "x": 2.3405056735984604, - "y": 6.293349950762083, - "heading": 1.0023010522506035, - "angularVelocity": -0.23784929016864031, - "velocityX": 0.8544407526240461, - "velocityY": 0.8425739879969667, - "timestamp": 3.3284807014664737 - }, - { - "x": 2.4164580564955584, - "y": 6.368247481057726, - "heading": 0.9705071197009111, - "angularVelocity": -0.3576718809920707, - "velocityX": 0.8544407526239729, - "velocityY": 0.8425739879970408, - "timestamp": 3.417372040817797 - }, - { - "x": 2.492410439384703, - "y": 6.443145011361432, - "heading": 0.950546735518749, - "angularVelocity": -0.22454813177325503, - "velocityX": 0.8544407525345075, - "velocityY": 0.8425739880877493, - "timestamp": 3.5062633801691203 - }, - { - "x": 2.5529385454627884, - "y": 6.502832482446322, - "heading": 0.9505467092628421, - "angularVelocity": -2.953708100289274e-7, - "velocityX": 0.6809224219117733, - "velocityY": 0.6714655389428754, - "timestamp": 3.5951547195204436 - }, - { - "x": 2.583202600479126, - "y": 6.5326762199401855, + "angularVelocity": 1.3587937947073691e-16, + "velocityX": 0.6998481994802291, + "velocityY": 0.974788398606472, + "timestamp": 3.760546916382344 + }, + { + "x": 2.641998396552295, + "y": 6.455367717245125, + "heading": 0.9505466984245468, + "angularVelocity": 5.109002093997956e-16, + "velocityX": 0.6998481994802279, + "velocityY": 0.9747883986064684, + "timestamp": 3.8604112216258795 + }, + { + "x": 2.7045704863276203, + "y": 6.54252168468387, + "heading": 0.9505466984245466, + "angularVelocity": 9.967273568896952e-18, + "velocityX": 0.6265711219112611, + "velocityY": 0.8727239149783513, + "timestamp": 3.960275526869415 + }, + { + "x": 2.735856533050537, + "y": 6.586098670959473, "heading": 0.9505466984245466, - "angularVelocity": -1.2192746279649323e-7, - "velocityX": 0.3404612331998472, - "velocityY": 0.335732791424291, - "timestamp": 3.684046058871767 + "angularVelocity": 2.3352525714710484e-17, + "velocityX": 0.3132855793330876, + "velocityY": 0.43636198308634433, + "timestamp": 4.0601398321129505 }, { - "x": 2.583202600479126, - "y": 6.5326762199401855, + "x": 2.735856533050537, + "y": 6.586098670959473, "heading": 0.9505466984245466, - "angularVelocity": 1.0876323700343221e-22, - "velocityX": 1.7516641677537848e-21, - "velocityY": -1.1441920806239185e-21, - "timestamp": 3.7729373982230903 - }, - { - "x": 2.542403880498974, - "y": 6.501670263198367, - "heading": 0.9505466903049901, - "angularVelocity": -8.318907821701634e-8, - "velocityX": -0.41800409947017914, - "velocityY": -0.3176721483512302, - "timestamp": 3.8705410342445177 - }, - { - "x": 2.460806445400462, - "y": 6.439658353412451, - "heading": 0.9505466688810346, - "angularVelocity": -2.194995637368503e-7, - "velocityX": -0.8360081491287711, - "velocityY": -0.6353442588173928, - "timestamp": 3.968144670265945 - }, - { - "x": 2.367555279184838, - "y": 6.368789913715819, - "heading": 0.9153741187078941, - "angularVelocity": -0.3603610644732418, - "velocityX": -0.9554066837751003, - "velocityY": -0.7260840127008578, - "timestamp": 4.0657483062873725 - }, - { - "x": 2.2743041129628643, - "y": 6.297921474027541, - "heading": 0.8483526672144899, - "angularVelocity": -0.6866696183192453, - "velocityX": -0.9554066838401561, - "velocityY": -0.7260840126152636, - "timestamp": 4.1633519423088 - }, - { - "x": 2.1810529467408974, - "y": 6.227053034339255, - "heading": 0.7577571566962064, - "angularVelocity": -0.9281981103490298, - "velocityX": -0.9554066838400862, - "velocityY": -0.7260840126153555, - "timestamp": 4.260955578330228 - }, - { - "x": 2.087801780518917, - "y": 6.156184594650985, - "heading": 0.651002233808638, - "angularVelocity": -1.0937596921505226, - "velocityX": -0.9554066838402226, - "velocityY": -0.726084012615176, - "timestamp": 4.358559214351656 - }, - { - "x": 1.994550614296896, - "y": 6.0853161549627695, - "heading": 0.5348344163006928, - "angularVelocity": -1.190199691766012, - "velocityX": -0.9554066838406402, - "velocityY": -0.7260840126146266, - "timestamp": 4.456162850373084 - }, - { - "x": 1.9012994480748335, - "y": 6.014447715274608, - "heading": 0.41557973663701897, - "angularVelocity": -1.2218261995638482, - "velocityX": -0.9554066838410652, - "velocityY": -0.7260840126140673, - "timestamp": 4.553766486394512 - }, - { - "x": 1.8080482818527632, - "y": 5.943579275586458, - "heading": 0.2994210690519452, - "angularVelocity": -1.1901059460486971, - "velocityX": -0.9554066838411454, - "velocityY": -0.7260840126139618, - "timestamp": 4.65137012241594 - }, - { - "x": 1.7147971156307207, - "y": 5.87271083589827, - "heading": 0.19268402440905563, - "angularVelocity": -1.0935765202380057, - "velocityX": -0.95540668384086, - "velocityY": -0.7260840126143373, - "timestamp": 4.7489737584373675 - }, - { - "x": 1.6215459494087139, - "y": 5.801842396210036, - "heading": 0.10211451516138965, - "angularVelocity": -0.9279317138122054, - "velocityX": -0.9554066838404965, - "velocityY": -0.7260840126148156, - "timestamp": 4.846577394458795 - }, - { - "x": 1.5282947831867273, - "y": 5.730973956521775, - "heading": 0.035127860642323504, - "angularVelocity": -0.6863131052244829, - "velocityX": -0.955406683840287, - "velocityY": -0.7260840126150913, - "timestamp": 4.944181030480223 - }, - { - "x": 1.4350436169609884, - "y": 5.660105516838453, - "heading": 2.9548859844204172e-8, - "angularVelocity": -0.35990289425029026, - "velocityX": -0.9554066838787327, - "velocityY": -0.7260840125644947, - "timestamp": 5.041784666501651 - }, - { - "x": 1.353446181869114, - "y": 5.598093607043802, - "heading": 8.120889195643221e-9, - "angularVelocity": -2.1954070075685228e-7, - "velocityX": -0.8360081490607667, - "velocityY": -0.6353442589068752, - "timestamp": 5.139388302523079 + "angularVelocity": 4.489031866337397e-20, + "velocityX": -2.841253050020481e-19, + "velocityY": -1.4670872630302605e-19, + "timestamp": 4.160004137356486 + }, + { + "x": 2.6926372151215485, + "y": 6.555153842004882, + "heading": 0.9505466918112597, + "angularVelocity": -6.652694848851377e-8, + "velocityX": -0.4347685747061206, + "velocityY": -0.3112922605861828, + "timestamp": 4.259411777290483 + }, + { + "x": 2.6061985842528834, + "y": 6.493264187669074, + "heading": 0.9505466743461212, + "angularVelocity": -1.756921161749788e-7, + "velocityX": -0.8695370992212547, + "velocityY": -0.6225844852256829, + "timestamp": 4.358819417224479 + }, + { + "x": 2.5092074864577216, + "y": 6.423819016493211, + "heading": 0.9190781371048612, + "angularVelocity": -0.3165605506996643, + "velocityX": -0.9756905793112982, + "velocityY": -0.6985898792283688, + "timestamp": 4.458227057158476 + }, + { + "x": 2.4122163886604104, + "y": 6.354373845320345, + "heading": 0.8601974459911542, + "angularVelocity": -0.5923155519314354, + "velocityX": -0.9756905793328975, + "velocityY": -0.6985898791982086, + "timestamp": 4.557634697092473 + }, + { + "x": 2.3152252908630935, + "y": 6.28492867414746, + "heading": 0.7807467252149503, + "angularVelocity": -0.7992415958064828, + "velocityX": -0.975690579332817, + "velocityY": -0.698589879198321, + "timestamp": 4.6570423370264695 + }, + { + "x": 2.2182341930657996, + "y": 6.215483502974575, + "heading": 0.6866750516739581, + "angularVelocity": -0.9463223712327561, + "velocityX": -0.9756905793328342, + "velocityY": -0.6985898791982971, + "timestamp": 4.756449976960466 + }, + { + "x": 2.121243095268475, + "y": 6.146038331801719, + "heading": 0.5832432706368847, + "angularVelocity": -1.0404812055265393, + "velocityX": -0.9756905793329995, + "velocityY": -0.698589879198066, + "timestamp": 4.855857616894463 + }, + { + "x": 2.024251997471116, + "y": 6.076593160628913, + "heading": 0.4752487029161645, + "angularVelocity": -1.0863809642038045, + "velocityX": -0.9756905793332569, + "velocityY": -0.6985898791977067, + "timestamp": 4.95526525682846 + }, + { + "x": 1.9272608996737375, + "y": 6.007147989456111, + "heading": 0.36725581200699825, + "angularVelocity": -1.0863640961687604, + "velocityX": -0.9756905793334377, + "velocityY": -0.6985898791974542, + "timestamp": 5.054672896762456 + }, + { + "x": 1.8302698018763681, + "y": 5.937702818283318, + "heading": 0.26382879729241876, + "angularVelocity": -1.040433258281267, + "velocityX": -0.975690579333424, + "velocityY": -0.6985898791974733, + "timestamp": 5.154080536696453 + }, + { + "x": 1.7332787040790516, + "y": 5.868257647110507, + "heading": 0.16976442296144847, + "angularVelocity": -0.9462489441799997, + "velocityX": -0.9756905793332555, + "velocityY": -0.6985898791977087, + "timestamp": 5.25348817663045 + }, + { + "x": 1.6362876062816873, + "y": 5.798812475937657, + "heading": 0.09032312899825462, + "angularVelocity": -0.7991467659421343, + "velocityX": -0.9756905793330708, + "velocityY": -0.6985898791979667, + "timestamp": 5.3528958165644465 + }, + { + "x": 1.5392965084843735, + "y": 5.72936730476478, + "heading": 0.03145409331145777, + "angularVelocity": -0.5921983031272416, + "velocityX": -0.975690579332948, + "velocityY": -0.6985898791981381, + "timestamp": 5.452303456498443 + }, + { + "x": 1.4423054106856323, + "y": 5.6599221335939, + "heading": 2.4080089212053166e-8, + "angularVelocity": -0.31641500846668225, + "velocityX": -0.975690579347126, + "velocityY": -0.6985898791783296, + "timestamp": 5.55171109643244 + }, + { + "x": 1.3558667798193893, + "y": 5.598032479254774, + "heading": 6.6137151436793805e-9, + "angularVelocity": -1.757045442719512e-7, + "velocityX": -0.8695370991973227, + "velocityY": -0.6225844852591078, + "timestamp": 5.651118736366437 }, { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": 2.2803993876993828e-23, - "angularVelocity": -8.320273226153553e-8, - "velocityX": -0.41800409944751094, - "velocityY": -0.31767214838105773, - "timestamp": 5.236991938544507 + "heading": 1.3033321736184719e-19, + "angularVelocity": -6.653125604117621e-8, + "velocityX": -0.43476857469814373, + "velocityY": -0.3112922605973239, + "timestamp": 5.750526376300433 }, { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": 2.991326698553366e-23, - "angularVelocity": 2.478347927470716e-23, - "velocityX": 9.764266009341063e-23, - "velocityY": -5.1068246254861434e-23, - "timestamp": 5.334595574565935 - }, - { - "x": 1.3367227529770005, - "y": 5.54401651323739, - "heading": -8.745942749209055e-9, - "angularVelocity": -1.1108213753593968e-7, - "velocityX": 0.30578004822354515, - "velocityY": -0.2930262972997385, - "timestamp": 5.413329590162092 - }, - { - "x": 1.3848733307698786, - "y": 5.497874243310031, - "heading": -3.2419454048962964e-8, - "angularVelocity": -3.0067704689648114e-7, - "velocityX": 0.611560040832321, - "velocityY": -0.5860525413060693, - "timestamp": 5.492063605758249 - }, - { - "x": 1.4530888046304524, - "y": 5.432503962156357, - "heading": -0.006001262639786551, - "angularVelocity": -0.07622156922763929, - "velocityX": 0.866404099220153, - "velocityY": -0.8302673331050667, - "timestamp": 5.570797621354406 - }, - { - "x": 1.5213042784906459, - "y": 5.367133681002277, - "heading": -0.03774158111147184, - "angularVelocity": -0.403133489780173, - "velocityX": 0.8664040992153226, - "velocityY": -0.8302673331102199, - "timestamp": 5.649531636950563 - }, - { - "x": 1.5895197523508426, - "y": 5.301763399848201, - "heading": -0.08991573959916936, - "angularVelocity": -0.6626635018250512, - "velocityX": 0.866404099215366, - "velocityY": -0.8302673331101746, - "timestamp": 5.72826565254672 - }, - { - "x": 1.6577352262110532, - "y": 5.2363931186941395, - "heading": -0.15757332647853733, - "angularVelocity": -0.8593183818592182, - "velocityX": 0.8664040992155391, - "velocityY": -0.830267333109994, - "timestamp": 5.806999668142877 - }, - { - "x": 1.7259507000712884, - "y": 5.171022837540104, - "heading": -0.2360864752633604, - "angularVelocity": -0.9971947726829187, - "velocityX": 0.8664040992158549, - "velocityY": -0.8302673331096645, - "timestamp": 5.885733683739034 - }, - { - "x": 1.7941661739315489, - "y": 5.105652556386094, - "heading": -0.32108338261130676, - "angularVelocity": -1.079544929905686, - "velocityX": 0.8664040992161722, - "velocityY": -0.8302673331093331, - "timestamp": 5.9644676993351915 - }, - { - "x": 1.8623816477918176, - "y": 5.040282275232093, - "heading": -0.40835744473560676, - "angularVelocity": -1.1084670515466504, - "velocityX": 0.8664040992162807, - "velocityY": -0.8302673331092201, - "timestamp": 6.043201714931349 - }, - { - "x": 1.9305971216520723, - "y": 4.974911994078077, - "heading": -0.49376220718458297, - "angularVelocity": -1.0847250937515367, - "velocityX": 0.8664040992161026, - "velocityY": -0.8302673331094058, - "timestamp": 6.121935730527506 - }, - { - "x": 1.9988125955123008, - "y": 4.9095417129240335, - "heading": -0.5731014303336502, - "angularVelocity": -1.007686735502157, - "velocityX": 0.8664040992157647, - "velocityY": -0.8302673331097585, - "timestamp": 6.200669746123663 - }, - { - "x": 2.06702806937251, - "y": 4.844171431769971, - "heading": -0.6420233840895226, - "angularVelocity": -0.8753770938013302, - "velocityX": 0.8664040992155239, - "velocityY": -0.8302673331100098, - "timestamp": 6.27940376171982 - }, - { - "x": 2.1352435432327193, - "y": 4.778801150615908, - "heading": -0.6959284119501224, - "angularVelocity": -0.6846472576362663, - "velocityX": 0.866404099215522, - "velocityY": -0.8302673331100118, - "timestamp": 6.358137777315977 + "heading": 2.2172969897005315e-19, + "angularVelocity": 2.0015491793199464e-19, + "velocityX": -1.0501490690724046e-19, + "velocityY": -4.272854375601257e-19, + "timestamp": 5.84993401623443 + }, + { + "x": 1.3441215983818582, + "y": 5.53692626623967, + "heading": -1.1643991871335638e-8, + "angularVelocity": -1.2934461900203075e-7, + "velocityX": 0.34962324193127553, + "velocityY": -0.33504083198930457, + "timestamp": 5.9399570259085595 + }, + { + "x": 1.4070698670672521, + "y": 5.476603502232459, + "heading": -4.233223250468571e-8, + "angularVelocity": -3.4089329819791224e-7, + "velocityX": 0.6992464361418851, + "velocityY": -0.6700816183059229, + "timestamp": 6.029980035582689 + }, + { + "x": 1.485066172875745, + "y": 5.401860338613426, + "heading": -0.031252361434063125, + "angularVelocity": -0.347159234233111, + "velocityX": 0.8664041125799371, + "velocityY": -0.8302673270932495, + "timestamp": 6.120003045256818 + }, + { + "x": 1.5630624786713805, + "y": 5.327117174980967, + "heading": -0.0999659837507643, + "angularVelocity": -0.7632895474771944, + "velocityX": 0.8664041124370567, + "velocityY": -0.8302673272423656, + "timestamp": 6.210026054930948 + }, + { + "x": 1.6410587844670608, + "y": 5.2523740113485955, + "heading": -0.19520871175979404, + "angularVelocity": -1.0579820465211633, + "velocityX": 0.866404112437784, + "velocityY": -0.8302673272416067, + "timestamp": 6.300049064605077 + }, + { + "x": 1.7190550902628128, + "y": 5.177630847716277, + "heading": -0.30625153833554625, + "angularVelocity": -1.2334938253865513, + "velocityX": 0.8664041124384794, + "velocityY": -0.8302673272408809, + "timestamp": 6.390072074279207 + }, + { + "x": 1.797051396058594, + "y": 5.1028876840839885, + "heading": -0.4225688522263227, + "angularVelocity": -1.292084260588809, + "velocityX": 0.8664041124387727, + "velocityY": -0.8302673272405751, + "timestamp": 6.480095083953336 + }, + { + "x": 1.8750477018543286, + "y": 5.028144520451638, + "heading": -0.5337184986736393, + "angularVelocity": -1.234680409482674, + "velocityX": 0.8664041124382615, + "velocityY": -0.8302673272411084, + "timestamp": 6.570118093627466 + }, + { + "x": 1.9530440076500057, + "y": 4.953401356819256, + "heading": -0.6291756131928835, + "angularVelocity": -1.0603635100046716, + "velocityX": 0.8664041124376008, + "velocityY": -0.8302673272417979, + "timestamp": 6.660141103301595 + }, + { + "x": 2.0310403134456294, + "y": 4.878658193186804, + "heading": -0.6982128332545968, + "angularVelocity": -0.7668841589680074, + "velocityX": 0.8664041124370991, + "velocityY": -0.8302673272423214, + "timestamp": 6.750164112975725 + }, + { + "x": 2.109036619249582, + "y": 4.803915029563043, + "heading": -0.7298996645397342, + "angularVelocity": -0.3519859133774726, + "velocityX": 0.8664041125295305, + "velocityY": -0.83026732714585, + "timestamp": 6.840187122649854 + }, + { + "x": 2.1719848879377293, + "y": 4.743592265558695, + "heading": -0.7298996951613214, + "angularVelocity": -3.401528940808921e-7, + "velocityX": 0.6992464361723661, + "velocityY": -0.670081618274115, + "timestamp": 6.9302101323239835 }, { "x": 2.2034590244293213, "y": 4.713430881500244, "heading": -0.7298997067825479, - "angularVelocity": -0.43146909979380765, - "velocityX": 0.8664041923949785, - "velocityY": -0.8302671802104146, - "timestamp": 6.436871792912134 + "angularVelocity": -1.2909173503647302e-7, + "velocityX": 0.3496232419414347, + "velocityY": -0.3350408319787032, + "timestamp": 7.020233141998113 }, { - "x": 2.284274256756373, - "y": 4.64687721210592, - "heading": -0.7358958248166633, - "angularVelocity": -0.06872842162702524, - "velocityX": 0.9263165484164835, - "velocityY": -0.7628483336942082, - "timestamp": 6.524115438148963 + "x": 2.2034590244293213, + "y": 4.713430881500244, + "heading": -0.7298997067825479, + "angularVelocity": 1.6899611320835774e-19, + "velocityX": 5.175181532248611e-19, + "velocityY": -4.71372364987181e-19, + "timestamp": 7.1102561516722425 }, { - "x": 2.3650894966282934, - "y": 4.580323551873232, - "heading": -0.7298997308132567, - "angularVelocity": 0.06872814618336746, - "velocityX": 0.9263166348969202, - "velocityY": -0.762848228682137, - "timestamp": 6.611359083385793 + "x": 2.2414956338795715, + "y": 4.681346676235305, + "heading": -0.7298997067825479, + "angularVelocity": -2.46282456581248e-17, + "velocityX": 0.39546677329122837, + "velocityY": -0.333579604310823, + "timestamp": 7.206437707289899 + }, + { + "x": 2.3175688480689023, + "y": 4.617178269679347, + "heading": -0.729899706782548, + "angularVelocity": -4.562002127415535e-17, + "velocityX": 0.7909334976002727, + "velocityY": -0.6671591673047552, + "timestamp": 7.302619262907555 + }, + { + "x": 2.405792236328125, + "y": 4.5427610874176025, + "heading": -0.729899706782548, + "angularVelocity": 1.0687559485237639e-16, + "velocityX": 0.917258903671292, + "velocityY": -0.7737157273435987, + "timestamp": 7.398800818525212 }, { - "x": 2.428299134503931, - "y": 4.5282686081612855, - "heading": -0.7298997143064122, - "angularVelocity": 1.8920397482236636e-7, - "velocityX": 0.7245185331728233, - "velocityY": -0.5966617232766905, - "timestamp": 6.698602728622622 + "x": 2.494015624587376, + "y": 4.468343905155829, + "heading": -0.7298997067825479, + "angularVelocity": 2.54215551828638e-16, + "velocityX": 0.917258903671292, + "velocityY": -0.7737157273435986, + "timestamp": 7.494982374142868 + }, + { + "x": 2.5700888387766927, + "y": 4.4041754985999, + "heading": -0.7298997067825479, + "angularVelocity": -6.314437383419688e-17, + "velocityX": 0.7909334976002727, + "velocityY": -0.6671591673047553, + "timestamp": 7.5911639297605245 }, { - "x": 2.4599039554595947, - "y": 4.502241134643555, + "x": 2.6081254482269287, + "y": 4.372091293334961, "heading": -0.7298997067825479, - "angularVelocity": 8.623968367420298e-8, - "velocityX": 0.3622592897152577, - "velocityY": -0.29833088068566255, - "timestamp": 6.785846373859451 + "angularVelocity": -7.295944708803859e-17, + "velocityX": 0.3954667732912283, + "velocityY": -0.333579604310823, + "timestamp": 7.687345485378181 }, { - "x": 2.4599039554595947, - "y": 4.502241134643555, + "x": 2.6081254482269287, + "y": 4.372091293334961, "heading": -0.7298997067825479, - "angularVelocity": -9.04164627803996e-23, - "velocityX": -1.4473589854434152e-22, - "velocityY": -2.1145820655457915e-22, - "timestamp": 6.8730900190962805 - }, - { - "x": 2.418647701705629, - "y": 4.540533860206957, - "heading": -0.7298997001349049, - "angularVelocity": 6.498469080235459e-8, - "velocityX": -0.40330458085897497, - "velocityY": 0.374334318510709, - "timestamp": 6.975385543898147 - }, - { - "x": 2.336135198753465, - "y": 4.61711930710535, - "heading": -0.7298996827304232, - "angularVelocity": 1.7013922856843512e-7, - "velocityX": -0.8066091171825944, - "velocityY": 0.748668595686158, - "timestamp": 7.077681068700014 - }, - { - "x": 2.2461633007376833, - "y": 4.700628324178363, - "heading": -0.6961908922790889, - "angularVelocity": 0.3295236083555346, - "velocityX": -0.879529169922592, - "velocityY": 0.8163506393340244, - "timestamp": 7.1799765935018804 - }, - { - "x": 2.1561914027221256, - "y": 4.78413734125162, - "heading": -0.6354601214689052, - "angularVelocity": 0.5936796446160405, - "velocityX": -0.8795291699203955, - "velocityY": 0.8163506393363948, - "timestamp": 7.282272118303747 - }, - { - "x": 2.0662195047065577, - "y": 4.8676463583248655, - "heading": -0.555497965787572, - "angularVelocity": 0.7816779456990843, - "velocityX": -0.8795291699204953, - "velocityY": 0.8163506393362874, - "timestamp": 7.384567643105614 - }, - { - "x": 1.976247606690973, - "y": 4.951155375398092, - "heading": -0.4632200489828426, - "angularVelocity": 0.9020718842144826, - "velocityX": -0.8795291699206637, - "velocityY": 0.8163506393361059, - "timestamp": 7.48686316790748 - }, - { - "x": 1.8862757086753756, - "y": 5.0346643924713055, - "heading": -0.3649382393995183, - "angularVelocity": 0.96076353069876, - "velocityX": -0.8795291699207887, - "velocityY": 0.8163506393359712, - "timestamp": 7.589158692709347 - }, - { - "x": 1.7963038106597808, - "y": 5.118173409544521, - "heading": -0.2666575817787772, - "angularVelocity": 0.9607522695748207, - "velocityX": -0.8795291699207628, - "velocityY": 0.816350639335999, - "timestamp": 7.691454217511214 - }, - { - "x": 1.7063319126442014, - "y": 5.201682426617753, - "heading": -0.17438289577352772, - "angularVelocity": 0.9020403012152627, - "velocityX": -0.8795291699206121, - "velocityY": 0.8163506393361615, - "timestamp": 7.79374974231308 - }, - { - "x": 1.6163600146286374, - "y": 5.2851914436910015, - "heading": -0.09442558399173479, - "angularVelocity": 0.7816305936810019, - "velocityX": -0.8795291699204603, - "velocityY": 0.8163506393363251, - "timestamp": 7.896045267114947 - }, - { - "x": 1.526388116613081, - "y": 5.368700460764259, - "heading": -0.033700979293950294, - "angularVelocity": 0.5936193671756421, - "velocityX": -0.8795291699203863, - "velocityY": 0.8163506393364047, - "timestamp": 7.998340791916814 - }, - { - "x": 1.436416218597363, - "y": 5.452209477837341, - "heading": -2.4053492503793295e-8, - "angularVelocity": 0.32944701447822, - "velocityX": -0.8795291699219676, - "velocityY": 0.8163506393346972, - "timestamp": 8.100636316718681 - }, - { - "x": 1.353903715645155, - "y": 5.528794924735686, - "heading": -6.648008517386852e-9, - "angularVelocity": 1.701490267547733e-7, - "velocityX": -0.8066091171830237, - "velocityY": 0.7486685956856953, - "timestamp": 8.202931841520549 + "angularVelocity": -3.557756419099644e-20, + "velocityX": -1.1158976027767978e-18, + "velocityY": 1.0302990290883233e-18, + "timestamp": 7.783527040995837 + }, + { + "x": 2.563674001371605, + "y": 4.41309493647991, + "heading": -0.7298997015558017, + "angularVelocity": 4.929434515061794e-8, + "velocityX": -0.41922926520033577, + "velocityY": 0.38671243350444034, + "timestamp": 7.889558395543702 + }, + { + "x": 2.474771112276091, + "y": 4.495102218512861, + "heading": -0.7298996878700399, + "angularVelocity": 1.2907278142001575e-7, + "velocityX": -0.838458486875625, + "velocityY": 0.773424826859854, + "timestamp": 7.9955897500915665 + }, + { + "x": 2.381246629145657, + "y": 4.581372628204462, + "heading": -0.7000859323738574, + "angularVelocity": 0.28117867232115834, + "velocityX": -0.8820455376541905, + "velocityY": 0.8136311193942835, + "timestamp": 8.101621104639431 + }, + { + "x": 2.287722146015324, + "y": 4.667643037896195, + "heading": -0.6472777997396291, + "angularVelocity": 0.498042610692009, + "velocityX": -0.8820455376540477, + "velocityY": 0.81363111939444, + "timestamp": 8.207652459187296 + }, + { + "x": 2.1941976628849664, + "y": 4.753913447587866, + "heading": -0.577776535255652, + "angularVelocity": 0.6554784174958657, + "velocityX": -0.8820455376540902, + "velocityY": 0.813631119394394, + "timestamp": 8.31368381373516 + }, + { + "x": 2.10067317975465, + "y": 4.8401838572795235, + "heading": -0.49698505133773596, + "angularVelocity": 0.7619584250566693, + "velocityX": -0.8820455376541692, + "velocityY": 0.8136311193943084, + "timestamp": 8.419715168283025 + }, + { + "x": 2.0071486966242564, + "y": 4.9264542669711755, + "heading": -0.4096708626062039, + "angularVelocity": 0.8234751796188454, + "velocityX": -0.8820455376542478, + "velocityY": 0.8136311193942233, + "timestamp": 8.52574652283089 + }, + { + "x": 1.913624213493877, + "y": 5.0127246766628355, + "heading": -0.3202243151295472, + "angularVelocity": 0.8435858228735447, + "velocityX": -0.882045537654274, + "velocityY": 0.8136311193941947, + "timestamp": 8.631777877378754 + }, + { + "x": 1.8200997303635116, + "y": 5.098995086354495, + "heading": -0.23291079154548175, + "angularVelocity": 0.8234689064983195, + "velocityX": -0.8820455376542292, + "velocityY": 0.8136311193942434, + "timestamp": 8.737809231926619 + }, + { + "x": 1.7265752472331424, + "y": 5.185265496046131, + "heading": -0.15212040474401956, + "angularVelocity": 0.7619480779620905, + "velocityX": -0.8820455376541468, + "velocityY": 0.8136311193943326, + "timestamp": 8.843840586474483 + }, + { + "x": 1.6330507641028076, + "y": 5.27153590573785, + "heading": -0.0826202834773307, + "angularVelocity": 0.6554676356163622, + "velocityX": -0.8820455376540777, + "velocityY": 0.8136311193944077, + "timestamp": 8.949871941022348 + }, + { + "x": 1.5395262809724208, + "y": 5.357806315429548, + "heading": -0.02981305940491789, + "angularVelocity": 0.49803404188875805, + "velocityX": -0.8820455376540449, + "velocityY": 0.8136311193944432, + "timestamp": 9.055903295570213 + }, + { + "x": 1.4460017978420001, + "y": 5.444076725121205, + "heading": -1.8912690296449957e-8, + "angularVelocity": 0.2811719289954887, + "velocityX": -0.8820455376541747, + "velocityY": 0.8136311193943003, + "timestamp": 9.161934650118077 + }, + { + "x": 1.3570989087465364, + "y": 5.526084007154097, + "heading": -5.2268050645253455e-9, + "angularVelocity": 1.290739450532968e-7, + "velocityX": -0.8384584868756361, + "velocityY": 0.7734248268598417, + "timestamp": 9.267966004665942 }, { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": 6.220225099112246e-24, - "angularVelocity": 6.498826346766567e-8, - "velocityX": -0.403304580859118, - "velocityY": 0.37433431851055476, - "timestamp": 8.305227366322416 + "heading": 1.130585771980045e-19, + "angularVelocity": 4.9294900420321916e-8, + "velocityX": -0.41922926520033954, + "velocityY": 0.3867124335044363, + "timestamp": 9.373997359213806 }, { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": 3.0379270142045743e-24, - "angularVelocity": -1.4113145535339382e-24, - "velocityX": -1.5623932740219278e-24, - "velocityY": 1.5729984496793032e-24, - "timestamp": 8.407522891124284 + "heading": 5.546193231131035e-20, + "angularVelocity": -2.0132747430807326e-20, + "velocityX": -6.460753207500427e-19, + "velocityY": 5.964321607162322e-19, + "timestamp": 9.480028713761671 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/amp.2.traj b/src/main/deploy/choreo/amp.2.traj new file mode 100644 index 0000000..ea9c0ef --- /dev/null +++ b/src/main/deploy/choreo/amp.2.traj @@ -0,0 +1,50 @@ +{ + "samples": [ + { + "x": 2.832932472229004, + "y": 7.730569839477539, + "heading": -1.57, + "angularVelocity": 2.5384798880891626e-19, + "velocityX": 1.3213424450152277e-19, + "velocityY": -1.3296186785152276e-19, + "timestamp": 0 + }, + { + "x": 2.8500669725287007, + "y": 7.6734549316320955, + "heading": -1.57, + "angularVelocity": 9.592821781304562e-18, + "velocityX": 0.16273980802388974, + "velocityY": -0.5424651419939025, + "timestamp": 0.10528770131756326 + }, + { + "x": 2.8742703668725444, + "y": 7.592777082070295, + "heading": -1.57, + "angularVelocity": -5.573303219326481e-16, + "velocityX": 0.22987864718256742, + "velocityY": -0.7662609075153389, + "timestamp": 0.21057540263512653 + }, + { + "x": 2.891404867172241, + "y": 7.535662174224853, + "heading": -1.57, + "angularVelocity": -7.4565914005893e-18, + "velocityX": 0.1627398080238898, + "velocityY": -0.5424651419939025, + "timestamp": 0.3158631039526898 + }, + { + "x": 2.891404867172241, + "y": 7.535662174224853, + "heading": -1.57, + "angularVelocity": 5.895806889158938e-29, + "velocityX": 5.9907163144707855e-27, + "velocityY": -3.7935830975500606e-26, + "timestamp": 0.42115080527025306 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/amp.3.traj b/src/main/deploy/choreo/amp.3.traj new file mode 100644 index 0000000..5bfb150 --- /dev/null +++ b/src/main/deploy/choreo/amp.3.traj @@ -0,0 +1,50 @@ +{ + "samples": [ + { + "x": 2.891404867172241, + "y": 7.535662174224853, + "heading": -1.57, + "angularVelocity": 5.895806889158938e-29, + "velocityX": 5.9907163144707855e-27, + "velocityY": -3.7935830975500606e-26, + "timestamp": 0 + }, + { + "x": 2.8742703668725444, + "y": 7.592777082070296, + "heading": -1.57, + "angularVelocity": 7.315345766882995e-18, + "velocityX": -0.16273980802388938, + "velocityY": 0.5424651419939036, + "timestamp": 0.10528770131756326 + }, + { + "x": 2.8500669725287007, + "y": 7.6734549316320955, + "heading": -1.57, + "angularVelocity": -6.153512022231716e-17, + "velocityX": -0.22987864718256681, + "velocityY": 0.7662609075153392, + "timestamp": 0.21057540263512653 + }, + { + "x": 2.832932472229004, + "y": 7.730569839477539, + "heading": -1.57, + "angularVelocity": -5.2354046153489826e-18, + "velocityX": -0.16273980802388938, + "velocityY": 0.5424651419939036, + "timestamp": 0.3158631039526898 + }, + { + "x": 2.832932472229004, + "y": 7.730569839477539, + "heading": -1.57, + "angularVelocity": -2.4683037136660597e-29, + "velocityX": 7.654567110201289e-26, + "velocityY": 5.537318655690251e-26, + "timestamp": 0.42115080527025306 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/amp.4.traj b/src/main/deploy/choreo/amp.4.traj new file mode 100644 index 0000000..1910839 --- /dev/null +++ b/src/main/deploy/choreo/amp.4.traj @@ -0,0 +1,122 @@ +{ + "samples": [ + { + "x": 2.832932472229004, + "y": 7.730569839477539, + "heading": -1.57, + "angularVelocity": -2.4683037136660597e-29, + "velocityX": 7.654567110201289e-26, + "velocityY": 5.537318655690251e-26, + "timestamp": 0 + }, + { + "x": 2.8359579349757227, + "y": 7.645857104619916, + "heading": -1.57, + "angularVelocity": -2.316144918509908e-20, + "velocityX": 0.024100874771484475, + "velocityY": -0.6748227247444478, + "timestamp": 0.1255333167532311 + }, + { + "x": 2.8417825691833203, + "y": 7.482767774299788, + "heading": -1.57, + "angularVelocity": 1.056701907673452e-18, + "velocityX": 0.04639911027800991, + "velocityY": -1.2991716823727628, + "timestamp": 0.2510666335064622 + }, + { + "x": 2.847607203390918, + "y": 7.3196784439796545, + "heading": -1.57, + "angularVelocity": 8.605980709731148e-17, + "velocityX": 0.0463991102780115, + "velocityY": -1.2991716823728066, + "timestamp": 0.3765999502596933 + }, + { + "x": 2.8534318375985155, + "y": 7.156589113659521, + "heading": -1.57, + "angularVelocity": 1.4141187262457141e-16, + "velocityX": 0.04639911027801154, + "velocityY": -1.2991716823728066, + "timestamp": 0.5021332670129244 + }, + { + "x": 2.859256471806113, + "y": 6.993499783339388, + "heading": -1.57, + "angularVelocity": 1.8082426171008665e-16, + "velocityX": 0.04639911027801156, + "velocityY": -1.2991716823728066, + "timestamp": 0.6276665837661555 + }, + { + "x": 2.865081106013711, + "y": 6.830410453019255, + "heading": -1.57, + "angularVelocity": 2.0331609320152467e-16, + "velocityX": 0.04639911027801156, + "velocityY": -1.2991716823728066, + "timestamp": 0.7531999005193866 + }, + { + "x": 2.8709057402213087, + "y": 6.667321122699121, + "heading": -1.57, + "angularVelocity": 2.071461137440704e-16, + "velocityX": 0.04639911027801156, + "velocityY": -1.2991716823728066, + "timestamp": 0.8787332172726177 + }, + { + "x": 2.8767303744289063, + "y": 6.504231792378988, + "heading": -1.57, + "angularVelocity": 1.9302961552437798e-16, + "velocityX": 0.04639911027801154, + "velocityY": -1.2991716823728066, + "timestamp": 1.0042665340258488 + }, + { + "x": 2.882555008636504, + "y": 6.341142462058855, + "heading": -1.57, + "angularVelocity": 1.4589103259398445e-16, + "velocityX": 0.04639911027801151, + "velocityY": -1.2991716823728066, + "timestamp": 1.1297998507790799 + }, + { + "x": 2.8883796428441015, + "y": 6.178053131738727, + "heading": -1.57, + "angularVelocity": 7.476959541422744e-17, + "velocityX": 0.04639911027800991, + "velocityY": -1.2991716823727628, + "timestamp": 1.255333167532311 + }, + { + "x": 2.8914051055908203, + "y": 6.0933403968811035, + "heading": -1.57, + "angularVelocity": -2.6078332231156146e-19, + "velocityX": 0.02410087477148447, + "velocityY": -0.6748227247444478, + "timestamp": 1.3808664842855416 + }, + { + "x": 2.8914051055908203, + "y": 6.0933403968811035, + "heading": -1.57, + "angularVelocity": -9.147702321715024e-31, + "velocityX": 2.3496522050032264e-27, + "velocityY": -3.7036224885223456e-27, + "timestamp": 1.5063998010387722 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/amp.5.traj b/src/main/deploy/choreo/amp.5.traj new file mode 100644 index 0000000..31c0911 --- /dev/null +++ b/src/main/deploy/choreo/amp.5.traj @@ -0,0 +1,122 @@ +{ + "samples": [ + { + "x": 2.8914051055908203, + "y": 6.0933403968811035, + "heading": -1.57, + "angularVelocity": -9.147702321715024e-31, + "velocityX": 2.3496522050032264e-27, + "velocityY": -3.7036224885223456e-27, + "timestamp": 0 + }, + { + "x": 2.8883796428441015, + "y": 6.178053131738727, + "heading": -1.57, + "angularVelocity": -4.029927402920424e-24, + "velocityX": -0.024100874771485054, + "velocityY": 0.6748227247444473, + "timestamp": 0.12553331675323065 + }, + { + "x": 2.882555008636504, + "y": 6.341142462058855, + "heading": -1.57, + "angularVelocity": -2.9814554460737063e-23, + "velocityX": -0.046399110278010976, + "velocityY": 1.2991716823727628, + "timestamp": 0.2510666335064613 + }, + { + "x": 2.8767303744289063, + "y": 6.504231792378988, + "heading": -1.57, + "angularVelocity": -7.536931833879997e-20, + "velocityX": -0.04639911027801265, + "velocityY": 1.2991716823728066, + "timestamp": 0.37659995025969195 + }, + { + "x": 2.8709057402213087, + "y": 6.667321122699121, + "heading": -1.57, + "angularVelocity": 1.297039546672288e-20, + "velocityX": -0.04639911027801267, + "velocityY": 1.2991716823728066, + "timestamp": 0.5021332670129226 + }, + { + "x": 2.865081106013711, + "y": 6.830410453019255, + "heading": -1.57, + "angularVelocity": -5.623390515121104e-20, + "velocityX": -0.046399110278012676, + "velocityY": 1.2991716823728066, + "timestamp": 0.6276665837661533 + }, + { + "x": 2.859256471806113, + "y": 6.993499783339388, + "heading": -1.57, + "angularVelocity": 1.7227474112408243e-20, + "velocityX": -0.04639911027801268, + "velocityY": 1.2991716823728066, + "timestamp": 0.7531999005193839 + }, + { + "x": 2.8534318375985155, + "y": 7.156589113659521, + "heading": -1.57, + "angularVelocity": 1.069337924652968e-19, + "velocityX": -0.046399110278012676, + "velocityY": 1.2991716823728066, + "timestamp": 0.8787332172726146 + }, + { + "x": 2.847607203390918, + "y": 7.3196784439796545, + "heading": -1.57, + "angularVelocity": 1.0591455457918656e-19, + "velocityX": -0.04639911027801267, + "velocityY": 1.2991716823728066, + "timestamp": 1.0042665340258452 + }, + { + "x": 2.8417825691833203, + "y": 7.482767774299788, + "heading": -1.57, + "angularVelocity": -1.1072216404355553e-19, + "velocityX": -0.04639911027801265, + "velocityY": 1.2991716823728066, + "timestamp": 1.1297998507790759 + }, + { + "x": 2.8359579349757227, + "y": 7.645857104619916, + "heading": -1.57, + "angularVelocity": -7.667119644110267e-24, + "velocityX": -0.046399110278010976, + "velocityY": 1.2991716823727628, + "timestamp": 1.2553331675323065 + }, + { + "x": 2.832932472229004, + "y": 7.730569839477539, + "heading": -1.57, + "angularVelocity": -2.0172959631122012e-24, + "velocityX": -0.024100874771485054, + "velocityY": 0.6748227247444473, + "timestamp": 1.3808664842855372 + }, + { + "x": 2.832932472229004, + "y": 7.730569839477539, + "heading": -1.57, + "angularVelocity": -4.5369158421897955e-36, + "velocityX": -2.686713154731724e-27, + "velocityY": -7.77822559972659e-28, + "timestamp": 1.5063998010387678 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 piece amp side.auto b/src/main/deploy/pathplanner/autos/2 piece amp side.auto index 3617285..e5d7f8b 100644 --- a/src/main/deploy/pathplanner/autos/2 piece amp side.auto +++ b/src/main/deploy/pathplanner/autos/2 piece amp side.auto @@ -76,7 +76,7 @@ { "type": "wait", "data": { - "waitTime": 1.0 + "waitTime": 0.3 } }, { @@ -109,6 +109,12 @@ "name": "SpeakerSetting" } }, + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/4 piece.auto b/src/main/deploy/pathplanner/autos/4 piece.auto index 5f7dcae..6a6e0e1 100644 --- a/src/main/deploy/pathplanner/autos/4 piece.auto +++ b/src/main/deploy/pathplanner/autos/4 piece.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.3126474618911605, + "x": 1.3126474618911743, "y": 5.567087650299072 }, - "rotation": 5.013350972124652e-28 + "rotation": -8.520581699199449e-32 }, "command": { "type": "sequential", @@ -23,6 +23,12 @@ "name": "SpeakerSetting" } }, + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + }, { "type": "named", "data": { @@ -88,7 +94,7 @@ { "type": "wait", "data": { - "waitTime": 1.0 + "waitTime": 0.6 } }, { @@ -128,6 +134,18 @@ "data": { "pathName": "4 piece.3" } + }, + { + "type": "wait", + "data": { + "waitTime": 0.15 + } + }, + { + "type": "path", + "data": { + "pathName": "4 piece.4" + } } ] } @@ -144,7 +162,7 @@ { "type": "path", "data": { - "pathName": "4 piece.4" + "pathName": "4 piece.5" } }, { @@ -156,7 +174,7 @@ { "type": "wait", "data": { - "waitTime": 1.0 + "waitTime": 0.6 } }, { @@ -194,7 +212,19 @@ { "type": "path", "data": { - "pathName": "4 piece.5" + "pathName": "4 piece.6" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.15 + } + }, + { + "type": "path", + "data": { + "pathName": "4 piece.7" } } ] @@ -212,7 +242,7 @@ { "type": "path", "data": { - "pathName": "4 piece.6" + "pathName": "4 piece.8" } }, { @@ -224,7 +254,7 @@ { "type": "wait", "data": { - "waitTime": 1.0 + "waitTime": 0.6 } }, { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8963b71..3c40ac2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,7 +35,6 @@ import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.util.PixelFormat; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.Relay; @@ -122,8 +121,8 @@ public RobotContainer() { // .withPosition(0, 0); var camera = CameraServer.startAutomaticCapture(); camera.setResolution(160, 120); - camera.setFPS(15); - camera.setPixelFormat(PixelFormat.kMJPEG); + camera.setFPS(10); + // camera.setPixelFormat(PixelFormat.kMJPEG); driveTab.add("camera", camera).withWidget(BuiltInWidgets.kCameraStream).withProperties(Map.of("comp", 50)) .withPosition(0, 0).withSize(11, 5); @@ -145,7 +144,7 @@ public RobotContainer() { NamedCommands.registerCommand("WaitUntilVeryEnd", Commands.idle().until(() -> DriverStation.getMatchTime() <= 2)); NamedCommands.registerCommand("WaitIntake", IntakeWithArm.intakeWithDelay(intake, arm).beforeStarting(() -> intake.resetNoteDetection())); - NamedCommands.registerCommand("Index", new SetIntakeSpeed(intake, true).withTimeout(1)); + NamedCommands.registerCommand("Index", new SetIntakeSpeed(intake, true).withTimeout(.2)); NamedCommands.registerCommand("ArmDown", new SetArmPosition(arm, 4)); NamedCommands.registerCommand("ArmCarry", new SetArmPosition(arm, 40)); NamedCommands.registerCommand("AmpSetting", new SetScoringPosition(arm, shooter, ScoringConstants.ampPosition)); diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 2af6aa9..18a5c8a 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -2,10 +2,10 @@ public final class AutoConstants { // both defaults are 5.0 - public static final double autoDriveKp = 5.0; + public static final double autoDriveKp = 1.8; public static final double autoDriveKi = 0.0; - public static final double autoDriveKd = 0.0; - public static final double autoAngleKp = 5.0; + public static final double autoDriveKd = 0.5; + public static final double autoAngleKp = .9; public static final double autoAngleKi = 0.0; - public static final double autoAngleKd = 0.0; + public static final double autoAngleKd = 0.55; } diff --git a/src/main/java/frc/robot/constants/SwerveConstants.java b/src/main/java/frc/robot/constants/SwerveConstants.java index 9a5fdeb..c0a60b3 100644 --- a/src/main/java/frc/robot/constants/SwerveConstants.java +++ b/src/main/java/frc/robot/constants/SwerveConstants.java @@ -7,10 +7,10 @@ public final class SwerveConstants { // FL, FR, BL, BR (matches AdvantageScope convention) public static final SwerveModuleConstants moduleConstants[] = { - new SwerveModuleConstants(21, 22, .719), - new SwerveModuleConstants(23, 24, 1.921), - new SwerveModuleConstants(25, 26, 2.619), - new SwerveModuleConstants(27, 28, 2.877) + new SwerveModuleConstants(21, 22, .676), + new SwerveModuleConstants(23, 24, 1.919), + new SwerveModuleConstants(25, 26, 2.168), + new SwerveModuleConstants(27, 28, 2.814) }; // the left-to-right distance between the drivetrain wheels, should be measured from center to center diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 48eb8fe..a6aaa59 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -257,29 +257,28 @@ public void periodic() { // todo: estimate without using gyro? // Every 0.02s, updating pose2d - if (!DriverStation.isAutonomous()) { - if (vision.fieldInputs.hasTarget == true) { - visionSeenCount++; - if (visionSeenCount > 2) { // The if statement is used to eliminate "hallucinations". Schizophrenic robot smh - var visionPose = new Pose2d(vision.fieldInputs.fieldspaceTranslationX_m, - vision.fieldInputs.fieldspaceTranslationY_m, - new Rotation2d(vision.fieldInputs.fieldspaceRotationX_rad) - .plus(Rotation2d.fromDegrees(Robot.isBlue() ? 0 : 180))); // continuation of lines 259-261. Create a new - // pose2d - // using X,Y and rotation - Logger.recordOutput("Odometry/VisionPose", visionPose); // log the odometry to advantage kit - var distance = Math.hypot( // use pythagorean theorem to find the distance from the last position - Math.abs(translationX - vision.fieldInputs.fieldspaceTranslationX_m), - Math.abs(translationY - vision.fieldInputs.fieldspaceTranslationY_m)); - poseEstimator.addVisionMeasurement(visionPose, - timer.get() - vision.fieldInputs.fieldspaceTotalLatency_s); // remove lag from the time in the calculation - // of - // estimated pose - VecBuilder.fill(distance / 2, distance / 2, 100); - } - } else - visionSeenCount = 0; - } + // if (!DriverStation.isAutonomous()) { + if (vision.fieldInputs.targetAprilTag != -1) { + visionSeenCount++; + if (visionSeenCount > 2) { // The if statement is used to eliminate "hallucinations". Schizophrenic robot smh + var visionPose = new Pose2d(vision.fieldInputs.fieldspaceTranslationX_m, + vision.fieldInputs.fieldspaceTranslationY_m, + new Rotation2d(vision.fieldInputs.fieldspaceRotationZ_rad)); // continuation of lines 259-261. Create a new + // pose2d + // using X,Y and rotation + Logger.recordOutput("Odometry/VisionPose", visionPose); // log the odometry to advantage kit + var distance = Math.hypot( // use pythagorean theorem to find the distance from the last position + Math.abs(translationX - vision.fieldInputs.fieldspaceTranslationX_m), + Math.abs(translationY - vision.fieldInputs.fieldspaceTranslationY_m)); + poseEstimator.addVisionMeasurement(visionPose, + timer.get() - vision.fieldInputs.fieldspaceTotalLatency_s); // remove lag from the time in the calculation + // of + // estimated pose + VecBuilder.fill(distance / 2, distance / 2, 100); + } + } else + visionSeenCount = 0; + // } Logger.recordOutput("Odometry/Robot", pose); // update field pose for (int i = 0; i < modules.length; i++) { diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index a019706..6dc648d 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.7", + "version": "2024.2.8", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.7" + "version": "2024.2.8" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.7", + "version": "2024.2.8", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false,