diff --git a/src/main/deploy/choreo/2025-project.chor b/src/main/deploy/choreo/2025-project.chor index 73c85d7..f94de15 100644 --- a/src/main/deploy/choreo/2025-project.chor +++ b/src/main/deploy/choreo/2025-project.chor @@ -4,6 +4,20 @@ "type":"Swerve", "variables":{ "expressions":{ + "FieldMaxX":{ + "dimension":"Length", + "var":{ + "exp":"17.526 m", + "val":17.526 + } + }, + "FieldMaxY":{ + "dimension":"Length", + "var":{ + "exp":"8.0518 m", + "val":8.0518 + } + }, "endEffectorOffset":{ "dimension":"Number", "var":{ diff --git a/src/main/deploy/choreo/HToBarge.traj b/src/main/deploy/choreo/HToBarge.traj index 2bec3c2..5e152e4 100644 --- a/src/main/deploy/choreo/HToBarge.traj +++ b/src/main/deploy/choreo/HToBarge.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":5.827323, "y":4.0509, "heading":3.141592653589793, "intervals":62, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.58463382720947, "y":5.08871412277, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":5.827323, "y":4.0509, "heading":3.141592653589793, "intervals":64, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.7, "y":5.08871412277, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -30,71 +30,73 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,2.37576], + "waypoints":[0.0,2.47606], "samples":[ - {"t":0.0, "x":5.82732, "y":4.0509, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.58166, "ay":1.52477, "alpha":9.2621, "fx":[66.00707,63.41176,20.76279,21.95822], "fy":[1.90634,46.81558,50.89239,2.05432]}, - {"t":0.03832, "x":5.82922, "y":4.05202, "heading":-3.14159, "vx":0.09893, "vy":0.05843, "omega":0.35491, "ax":2.58231, "ay":1.52509, "alpha":7.24976, "fx":[61.03224,59.03401,25.43155,26.6854], "fy":[6.82975,42.27177,45.21888,7.36973]}, - {"t":0.07664, "x":5.8349, "y":4.05538, "heading":-3.12799, "vx":0.19788, "vy":0.11687, "omega":0.63271, "ax":2.58212, "ay":1.52509, "alpha":5.59944, "fx":[57.46362,55.42415,28.94091,30.34229], "fy":[11.419,38.61318,39.96653,11.69104]}, - {"t":0.11496, "x":5.84438, "y":4.06098, "heading":-3.10375, "vx":0.29682, "vy":0.17531, "omega":0.84728, "ax":2.58199, "ay":1.52487, "alpha":4.26248, "fx":[53.68202,52.11616,31.61019,34.75378], "fy":[14.68058,35.96425,36.62516,14.40517]}, - {"t":0.15327, "x":5.85765, "y":4.06881, "heading":-3.07128, "vx":0.39576, "vy":0.23374, "omega":1.01061, "ax":2.58159, "ay":1.52488, "alpha":3.27939, "fx":[51.69267,49.794,34.39943,36.24925], "fy":[17.51295,33.01687,34.31584,16.8306]}, - {"t":0.19159, "x":5.87471, "y":4.07889, "heading":-3.03256, "vx":0.49468, "vy":0.29217, "omega":1.13627, "ax":2.58118, "ay":1.52447, "alpha":2.18967, "fx":[48.1797,47.00043,37.52917,39.39881], "fy":[19.89139,31.81972,31.07165,18.86585]}, - {"t":0.22991, "x":5.89556, "y":4.0912, "heading":-2.98902, "vx":0.59359, "vy":0.35058, "omega":1.22018, "ax":2.57965, "ay":1.52485, "alpha":1.82109, "fx":[47.92859,46.22186,38.09765,39.75777], "fy":[21.29714,30.72605,29.47018,20.18099]}, - {"t":0.26823, "x":5.9202, "y":4.10576, "heading":-2.94226, "vx":0.69244, "vy":0.40901, "omega":1.28996, "ax":2.57873, "ay":1.5198, "alpha":1.32176, "fx":[46.80792,45.64626,38.98981,40.50049], "fy":[22.76216,28.8964,27.95595,21.72305]}, - {"t":0.30655, "x":5.94863, "y":4.12255, "heading":-2.89283, "vx":0.79125, "vy":0.46725, "omega":1.3406, "ax":1.81656, "ay":1.07206, "alpha":0.95754, "fx":[33.06741,31.84719,27.49789,28.71239], "fy":[16.09173,20.70599,19.72956,14.95588]}, - {"t":0.34487, "x":5.98028, "y":4.14124, "heading":-2.84146, "vx":0.86086, "vy":0.50833, "omega":1.3773, "ax":0.00125, "ay":-0.00201, "alpha":0.75757, "fx":[2.25018,1.54134,-2.41031,-1.29815], "fy":[-1.19559,2.17234,1.13056,-2.24152]}, - {"t":0.38319, "x":6.01327, "y":4.16071, "heading":-2.78868, "vx":0.86091, "vy":0.50825, "omega":1.40633, "ax":-0.00313, "ay":0.00529, "alpha":0.55156, "fx":[1.66468,0.73469,-1.76894,-0.83886], "fy":[-0.69671,1.75391,0.97759,-1.6819]}, - {"t":0.42151, "x":6.04626, "y":4.18019, "heading":-2.7348, "vx":0.86079, "vy":0.50846, "omega":1.42746, "ax":0.00213, "ay":-0.00361, "alpha":0.42128, "fx":[1.53287,0.36664,-1.30823,-0.44901], "fy":[-0.58505,1.27114,0.46461,-1.39158]}, - {"t":0.45982, "x":6.07924, "y":4.19967, "heading":-2.6801, "vx":0.86087, "vy":0.50832, "omega":1.4436, "ax":-0.00144, "ay":0.00244, "alpha":0.32544, "fx":[1.00593,0.31801,-1.05397,-0.36606], "fy":[-0.32316,1.13365,0.41148,-1.0593]}, - {"t":0.49814, "x":6.11223, "y":4.21915, "heading":-2.62478, "vx":0.86081, "vy":0.50841, "omega":1.45607, "ax":0.00087, "ay":-0.00148, "alpha":0.2456, "fx":[0.89764,0.15747,-0.80898,-0.18798], "fy":[-0.2403,0.76978,0.19108,-0.819]}, - {"t":0.53646, "x":6.14521, "y":4.23863, "heading":-2.56899, "vx":0.86085, "vy":0.50836, "omega":1.46548, "ax":-0.00054, "ay":0.00092, "alpha":0.18816, "fx":[0.60189,0.12067,-0.61991,-0.1387], "fy":[-0.12435,0.67159,0.15576,-0.64199]}, - {"t":0.57478, "x":6.1782, "y":4.25811, "heading":-2.51283, "vx":0.86083, "vy":0.50839, "omega":1.47269, "ax":0.0002, "ay":-0.00034, "alpha":0.14279, "fx":[0.51762,0.05599,-0.49394,-0.06641], "fy":[-0.07786,0.46328,0.06664,-0.4745]}, - {"t":0.6131, "x":6.21119, "y":4.2776, "heading":-2.4564, "vx":0.86083, "vy":0.50838, "omega":1.47817, "ax":-0.00011, "ay":0.00018, "alpha":0.10961, "fx":[0.35743,0.03294,-0.36095,-0.03646], "fy":[-0.03681,0.39739,0.03866,-0.38732]}, - {"t":0.65142, "x":6.24417, "y":4.29708, "heading":-2.39976, "vx":0.86083, "vy":0.50838, "omega":1.48237, "ax":0.00001, "ay":-0.00002, "alpha":0.08316, "fx":[0.30109,0.00286,-0.29445,-0.00861], "fy":[-0.01124,0.27451,0.0105,-0.27526]}, - {"t":0.68974, "x":6.27716, "y":4.31656, "heading":-2.34296, "vx":0.86083, "vy":0.50838, "omega":1.48555, "ax":0.0, "ay":-0.00001, "alpha":0.06434, "fx":[0.21006,-0.00359,-0.2099,0.00375], "fy":[0.00118,0.23554,-0.00655,-0.23074]}, - {"t":0.72805, "x":6.31014, "y":4.33604, "heading":-2.28603, "vx":0.86083, "vy":0.50838, "omega":1.48802, "ax":-0.00002, "ay":0.00004, "alpha":0.04879, "fx":[0.17597,-0.01875,-0.1732,0.01434], "fy":[0.01267,0.16098,-0.01128,-0.15959]}, - {"t":0.76637, "x":6.34313, "y":4.35552, "heading":-2.22901, "vx":0.86083, "vy":0.50838, "omega":1.48989, "ax":0.00003, "ay":-0.00004, "alpha":0.03806, "fx":[0.12249,-0.01576,-0.12164,0.01661], "fy":[0.01465,0.13942,-0.02059,-0.13639]}, - {"t":0.80469, "x":6.37612, "y":4.375, "heading":-2.17192, "vx":0.86083, "vy":0.50838, "omega":1.49135, "ax":-0.00003, "ay":0.00005, "alpha":0.02885, "fx":[0.10271,-0.02374,-0.10105,0.02028], "fy":[0.01854,0.0936,-0.01701,-0.09207]}, - {"t":0.84301, "x":6.4091, "y":4.39448, "heading":-2.11478, "vx":0.86083, "vy":0.50838, "omega":1.49245, "ax":0.00003, "ay":-0.00005, "alpha":0.02264, "fx":[0.0708,-0.01724,-0.06988,0.01816], "fy":[0.01691,0.08213,-0.02204,-0.08013]}, - {"t":0.88133, "x":6.44209, "y":4.41396, "heading":-2.05759, "vx":0.86083, "vy":0.50838, "omega":1.49332, "ax":-0.00002, "ay":0.00004, "alpha":0.01711, "fx":[0.05943,-0.02162,-0.0583,0.01899], "fy":[0.01729,0.05373,-0.01602,-0.05246]}, - {"t":0.91965, "x":6.47507, "y":4.43344, "heading":-2.00036, "vx":0.86083, "vy":0.50838, "omega":1.49398, "ax":0.00003, "ay":-0.00004, "alpha":0.01343, "fx":[0.04019,-0.0146,-0.03934,0.01545], "fy":[0.01464,0.04777,-0.01884,-0.04647]}, - {"t":0.95797, "x":6.50806, "y":4.45292, "heading":-1.94312, "vx":0.86083, "vy":0.50838, "omega":1.49449, "ax":-0.00002, "ay":0.00003, "alpha":0.01002, "fx":[0.03349,-0.01705,-0.03274,0.01509], "fy":[0.01362,0.02997,-0.0126,-0.02895]}, - {"t":0.99629, "x":6.54105, "y":4.4724, "heading":-1.88585, "vx":0.86083, "vy":0.50838, "omega":1.49487, "ax":0.00002, "ay":-0.00004, "alpha":0.00773, "fx":[0.02177,-0.01063,-0.02099,0.01141], "fy":[0.01075,0.02679,-0.01422,-0.02598]}, - {"t":1.0346, "x":6.57403, "y":4.49188, "heading":-1.82857, "vx":0.86083, "vy":0.50838, "omega":1.49517, "ax":-0.00002, "ay":0.00003, "alpha":0.0055, "fx":[0.01743,-0.01185,-0.01698,0.01037], "fy":[0.00929,0.01543,-0.00841,-0.01456]}, - {"t":1.07292, "x":6.60702, "y":4.51136, "heading":-1.77128, "vx":0.86083, "vy":0.50838, "omega":1.49538, "ax":0.00002, "ay":-0.00004, "alpha":0.00393, "fx":[0.01018,-0.00621,-0.00945,0.00695], "fy":[0.00618,0.01345,-0.00916,-0.01295]}, - {"t":1.11124, "x":6.64, "y":4.53084, "heading":-1.71397, "vx":0.86083, "vy":0.50838, "omega":1.49553, "ax":-0.00001, "ay":0.00002, "alpha":0.00228, "fx":[0.00678,-0.00633,-0.00656,0.00515], "fy":[0.0046,0.00593,-0.00377,-0.00511]}, - {"t":1.14956, "x":6.67299, "y":4.55032, "heading":-1.65667, "vx":0.86083, "vy":0.50838, "omega":1.49562, "ax":0.00002, "ay":-0.00004, "alpha":0.001, "fx":[0.00217,-0.0012,-0.00146,0.00192], "fy":[0.00078,0.00422,-0.00353,-0.00389]}, - {"t":1.18788, "x":6.70597, "y":4.5698, "heading":-1.59936, "vx":0.86083, "vy":0.50838, "omega":1.49566, "ax":-0.00001, "ay":0.00003, "alpha":-0.00048, "fx":[-0.00117,0.00004,0.00123,-0.00109], "fy":[-0.00097,-0.00107,0.00181,0.00191]}, - {"t":1.2262, "x":6.73896, "y":4.58928, "heading":-1.54205, "vx":0.86083, "vy":0.50838, "omega":1.49564, "ax":0.00002, "ay":-0.00004, "alpha":-0.0018, "fx":[-0.00422,0.0052,0.00494,-0.00448], "fy":[-0.00631,-0.003,0.00357,0.0033]}, - {"t":1.26452, "x":6.77195, "y":4.60877, "heading":-1.48474, "vx":0.86083, "vy":0.50838, "omega":1.49557, "ax":-0.00002, "ay":0.00003, "alpha":-0.00345, "fx":[-0.00803,0.00846,0.00802,-0.00953], "fy":[-0.00851,-0.0071,0.00943,0.00801]}, - {"t":1.30283, "x":6.80493, "y":4.62825, "heading":-1.42743, "vx":0.86083, "vy":0.50838, "omega":1.49544, "ax":0.00002, "ay":-0.00004, "alpha":-0.00515, "fx":[-0.01014,0.01443,0.01091,-0.01366], "fy":[-0.01666,-0.00946,0.01366,0.00986]}, - {"t":1.34115, "x":6.83792, "y":4.64773, "heading":-1.37012, "vx":0.86083, "vy":0.50838, "omega":1.49524, "ax":-0.00002, "ay":0.00003, "alpha":-0.00734, "fx":[-0.01471,0.0209,0.01473,-0.02217], "fy":[-0.01986,-0.01301,0.02092,0.01407]}, - {"t":1.37947, "x":6.8709, "y":4.66721, "heading":-1.31283, "vx":0.86083, "vy":0.50838, "omega":1.49496, "ax":0.00003, "ay":-0.00004, "alpha":-0.00985, "fx":[-0.01619,0.02884,0.01705,-0.02799], "fy":[-0.03282,-0.01571,0.02927,0.01637]}, - {"t":1.41779, "x":6.90389, "y":4.68669, "heading":-1.25554, "vx":0.86083, "vy":0.50838, "omega":1.49458, "ax":-0.00002, "ay":0.00004, "alpha":-0.01309, "fx":[-0.02155,0.04057,0.02167,-0.04228], "fy":[-0.038,-0.01908,0.03934,0.02043]}, - {"t":1.45611, "x":6.93688, "y":4.70617, "heading":-1.19827, "vx":0.86083, "vy":0.50838, "omega":1.49408, "ax":0.00003, "ay":-0.00005, "alpha":-0.01708, "fx":[-0.02237,0.05235,0.02336,-0.05136], "fy":[-0.05909,-0.02161,0.05457,0.02278]}, - {"t":1.49443, "x":6.96986, "y":4.72565, "heading":-1.14102, "vx":0.86083, "vy":0.50838, "omega":1.49343, "ax":-0.00003, "ay":0.00006, "alpha":-0.02221, "fx":[-0.02816,0.07294,0.02836,-0.07545], "fy":[-0.06796,-0.02483,0.06991,0.02679]}, - {"t":1.53275, "x":7.00285, "y":4.74513, "heading":-1.0838, "vx":0.86083, "vy":0.50838, "omega":1.49257, "ax":0.00003, "ay":-0.00006, "alpha":-0.02885, "fx":[-0.02773,0.0917,0.02889,-0.09055], "fy":[-0.10285,-0.02581,0.0968,0.02795]}, - {"t":1.57107, "x":7.03583, "y":4.76461, "heading":-1.0266, "vx":0.86083, "vy":0.50838, "omega":1.49147, "ax":-0.00006, "ay":0.0001, "alpha":-0.03736, "fx":[-0.03269,0.12757,0.0327,-0.13147], "fy":[-0.11847,-0.02828,0.12177,0.03157]}, - {"t":1.60938, "x":7.06882, "y":4.78409, "heading":-0.96945, "vx":0.86083, "vy":0.50838, "omega":1.49004, "ax":0.00004, "ay":-0.00007, "alpha":-0.04872, "fx":[-0.02919,0.15878,0.03066,-0.15731], "fy":[-0.17716,-0.02455,0.16845,0.02828]}, - {"t":1.6477, "x":7.10181, "y":4.80357, "heading":-0.91235, "vx":0.86083, "vy":0.50838, "omega":1.48817, "ax":-0.00011, "ay":0.00018, "alpha":-0.06333, "fx":[-0.03023,0.2217,0.02893,-0.22763], "fy":[-0.20479,-0.02413,0.21092,0.03025]}, - {"t":1.68602, "x":7.13479, "y":4.82305, "heading":-0.85533, "vx":0.86083, "vy":0.50839, "omega":1.48574, "ax":0.0001, "ay":-0.00016, "alpha":-0.08306, "fx":[-0.01839,0.27489,0.02163,-0.27165], "fy":[-0.30585,-0.00914,0.29018,0.01383]}, - {"t":1.72434, "x":7.16778, "y":4.84253, "heading":-0.7984, "vx":0.86083, "vy":0.50838, "omega":1.48256, "ax":-0.00021, "ay":0.00036, "alpha":-0.10901, "fx":[-0.00982,0.38807,0.00243,-0.39493], "fy":[-0.35324,0.00021,0.36531,0.01185]}, - {"t":1.76266, "x":7.20076, "y":4.86201, "heading":-0.74159, "vx":0.86082, "vy":0.5084, "omega":1.47838, "ax":0.00038, "ay":-0.00064, "alpha":-0.14338, "fx":[0.02548,0.47865,-0.01289,-0.46607], "fy":[-0.5334,0.03786,0.49385,-0.04094]}, - {"t":1.80098, "x":7.23375, "y":4.88149, "heading":-0.68494, "vx":0.86084, "vy":0.50837, "omega":1.47289, "ax":-0.00049, "ay":0.00083, "alpha":-0.19148, "fx":[0.04874,0.69325,-0.08355,-0.69109], "fy":[-0.60682,0.07436,0.63444,-0.04673]}, - {"t":1.8393, "x":7.26673, "y":4.90097, "heading":-0.6285, "vx":0.86082, "vy":0.5084, "omega":1.46555, "ax":0.00118, "ay":-0.002, "alpha":-0.25027, "fx":[0.146,0.83506,-0.10664,-0.7957], "fy":[-0.93159,0.14852,0.83536,-0.18558]}, - {"t":1.87761, "x":7.29972, "y":4.92045, "heading":-0.57234, "vx":0.86086, "vy":0.50833, "omega":1.45596, "ax":-0.00149, "ay":0.00252, "alpha":-0.3419, "fx":[0.18074,1.25461,-0.31314,-1.22136], "fy":[-1.02684,0.26968,1.11078,-0.18573]}, - {"t":1.91593, "x":7.33271, "y":4.93993, "heading":-0.51655, "vx":0.86081, "vy":0.50842, "omega":1.44286, "ax":0.00138, "ay":-0.00234, "alpha":-0.43962, "fx":[0.40445,1.4245,-0.35843,-1.37846], "fy":[-1.56818,0.43017,1.45238,-0.47027]}, - {"t":1.95425, "x":7.36569, "y":4.95941, "heading":-0.46126, "vx":0.86086, "vy":0.50833, "omega":1.42602, "ax":-0.00196, "ay":0.00332, "alpha":-0.58901, "fx":[0.59532,2.01728,-0.71674,-2.02653], "fy":[-1.77748,0.66501,1.88807,-0.55428]}, - {"t":1.99257, "x":7.39868, "y":4.97889, "heading":-0.40662, "vx":0.86078, "vy":0.50846, "omega":1.40345, "ax":0.00009, "ay":-0.00025, "alpha":-0.77257, "fx":[0.95027,2.39424,-0.94924,-2.38912], "fy":[-2.5574,0.96241,2.57734,-0.99921]}, - {"t":2.03089, "x":7.43166, "y":4.99838, "heading":-0.35284, "vx":0.86079, "vy":0.50845, "omega":1.37384, "ax":-1.81378, "ay":-1.07511, "alpha":-0.97813, "fx":[-28.53463,-27.41735,-31.65959,-33.32798], "fy":[-21.04475,-16.55954,-14.80923,-19.27242]}, - {"t":2.06921, "x":7.46332, "y":5.01707, "heading":-0.3002, "vx":0.79129, "vy":0.46725, "omega":1.33636, "ax":-2.57749, "ay":-1.5219, "alpha":-1.34889, "fx":[-40.77766,-39.13099,-45.19012,-46.76351], "fy":[-29.77385,-23.37218,-20.87776,-27.45328]}, - {"t":2.10753, "x":7.49174, "y":5.03386, "heading":-0.24899, "vx":0.69252, "vy":0.40894, "omega":1.28467, "ax":-2.58067, "ay":-1.52311, "alpha":-1.47208, "fx":[-41.1839,-39.73771,-44.71133,-46.44144], "fy":[-30.95411,-22.28862,-19.84046,-28.47515]}, - {"t":2.14585, "x":7.51639, "y":5.04841, "heading":-0.19976, "vx":0.59363, "vy":0.35057, "omega":1.22827, "ax":-2.58113, "ay":-1.52456, "alpha":-2.46619, "fx":[-38.14894,-36.30218,-47.95776,-49.69579], "fy":[-33.7804,-21.24388,-17.88647,-28.74367]}, - {"t":2.18416, "x":7.53724, "y":5.06073, "heading":-0.1527, "vx":0.49473, "vy":0.29215, "omega":1.13376, "ax":-2.58174, "ay":-1.52463, "alpha":-3.15464, "fx":[-35.6542,-36.17622,-49.39729,-50.91773], "fy":[-35.28379,-18.80587,-15.75331,-31.81621]}, - {"t":2.22248, "x":7.5543, "y":5.0708, "heading":-0.10925, "vx":0.3958, "vy":0.23373, "omega":1.01288, "ax":-2.58206, "ay":-1.52474, "alpha":-4.27306, "fx":[-33.48111,-31.91046,-52.89119,-53.88427], "fy":[-37.57387,-16.80188,-13.3789,-33.91225]}, - {"t":2.2608, "x":7.56757, "y":5.07864, "heading":-0.07044, "vx":0.29686, "vy":0.17531, "omega":0.84915, "ax":-2.58222, "ay":-1.52492, "alpha":-5.61271, "fx":[-29.82739,-30.09013,-56.03099,-56.22899], "fy":[-41.63542,-12.58549,-9.85182,-37.60595]}, - {"t":2.29912, "x":7.57705, "y":5.08424, "heading":-0.0379, "vx":0.19791, "vy":0.11687, "omega":0.63407, "ax":-2.58237, "ay":-1.52498, "alpha":-7.28116, "fx":[-25.78491,-25.54586,-60.96719,-59.88976], "fy":[-45.5528,-8.48207,-6.25032,-41.3973]}, - {"t":2.33744, "x":7.58274, "y":5.08759, "heading":-0.01361, "vx":0.09896, "vy":0.05844, "omega":0.35507, "ax":-2.58246, "ay":-1.52506, "alpha":-9.26622, "fx":[-20.79165,-21.4452,-66.08445,-63.87235], "fy":[-50.99733,-2.61838,-1.83289,-46.23967]}, - {"t":2.37576, "x":7.58463, "y":5.08871, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":5.82732, "y":4.0509, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.62253, "ay":1.45339, "alpha":8.81663, "fx":[65.40103,63.11913,22.5829,23.76237], "fy":[1.76578,44.73276,48.51771,1.89319]}, + {"t":0.03869, "x":5.82929, "y":4.05199, "heading":-3.14159, "vx":0.10146, "vy":0.05623, "omega":0.3411, "ax":2.62316, "ay":1.45374, "alpha":6.87873, "fx":[60.65991,58.89633,27.07639,28.27434], "fy":[6.53563,40.32513,43.04439,7.02733]}, + {"t":0.07738, "x":5.83517, "y":4.05525, "heading":-3.1284, "vx":0.20295, "vy":0.11247, "omega":0.60723, "ax":2.62297, "ay":1.45374, "alpha":5.29993, "fx":[57.2955,55.4493,30.41977,31.73014], "fy":[10.94721,36.76293,38.02653,11.19615]}, + {"t":0.11607, "x":5.84499, "y":4.06069, "heading":-3.1049, "vx":0.30443, "vy":0.16872, "omega":0.81227, "ax":2.62286, "ay":1.4535, "alpha":4.0151, "fx":[53.64336,52.28996,32.91107,36.04256], "fy":[14.07198,34.21244,34.81758,13.81432]}, + {"t":0.15475, "x":5.85873, "y":4.06831, "heading":-3.07348, "vx":0.4059, "vy":0.22495, "omega":0.96761, "ax":2.62243, "ay":1.45358, "alpha":3.07479, "fx":[51.79418,50.07761,35.64975,37.33695], "fy":[16.79769,31.35392,32.59081,16.17951]}, + {"t":0.19344, "x":5.8764, "y":4.0781, "heading":-3.03604, "vx":0.50736, "vy":0.28119, "omega":1.08657, "ax":2.62188, "ay":1.45342, "alpha":2.14844, "fx":[48.92367,47.80788,36.84026,41.25032], "fy":[19.01974,30.18284,29.62564,18.0831]}, + {"t":0.23213, "x":5.89799, "y":4.09006, "heading":-2.99401, "vx":0.60879, "vy":0.33742, "omega":1.16969, "ax":2.62094, "ay":1.45283, "alpha":1.68739, "fx":[48.24206,46.71546,39.1554,40.64646], "fy":[20.38753,29.07555,28.0233,19.38585]}, + {"t":0.27082, "x":5.9235, "y":4.1042, "heading":-2.94875, "vx":0.71019, "vy":0.39362, "omega":1.23497, "ax":2.61841, "ay":1.45057, "alpha":1.22334, "fx":[47.1969,46.0551,39.99262,41.34561], "fy":[21.7663,27.49048,26.63629,20.82812]}, + {"t":0.30951, "x":5.95294, "y":4.12052, "heading":-2.90097, "vx":0.8115, "vy":0.44974, "omega":1.2823, "ax":1.62846, "ay":0.89827, "alpha":0.89074, "fx":[29.73461,28.64294,24.55863,25.64659], "fy":[13.31429,17.56692,16.73216,12.28176]}, + {"t":0.3482, "x":5.98555, "y":4.13859, "heading":-2.85136, "vx":0.8745, "vy":0.4845, "omega":1.31676, "ax":-0.00123, "ay":0.0023, "alpha":0.71544, "fx":[1.95923,1.65151,-2.3457,-1.34718], "fy":[-1.06269,2.08096,1.14072,-2.00592]}, + {"t":0.38688, "x":6.01938, "y":4.15734, "heading":-2.80042, "vx":0.87445, "vy":0.48459, "omega":1.34444, "ax":-0.00154, "ay":0.00278, "alpha":0.51357, "fx":[1.56222,0.72491,-1.61363,-0.77626], "fy":[-0.70597,1.59451,0.88847,-1.59164]}, + {"t":0.42557, "x":6.05321, "y":4.17609, "heading":-2.74841, "vx":0.87439, "vy":0.48469, "omega":1.36431, "ax":0.00219, "ay":-0.00395, "alpha":0.3905, "fx":[1.4219,0.35096,-1.19932,-0.42746], "fy":[-0.57052,1.16453,0.43871,-1.29633]}, + {"t":0.46426, "x":6.08704, "y":4.19484, "heading":-2.69562, "vx":0.87448, "vy":0.48454, "omega":1.37942, "ax":-0.00119, "ay":0.00215, "alpha":0.30189, "fx":[0.93126,0.31252,-0.97106,-0.35233], "fy":[-0.31727,1.04372,0.39532,-0.9781]}, + {"t":0.50295, "x":6.12087, "y":4.21358, "heading":-2.64225, "vx":0.87443, "vy":0.48462, "omega":1.3911, "ax":0.00067, "ay":-0.0012, "alpha":0.22762, "fx":[0.82338,0.1619,-0.74905,-0.19176], "fy":[-0.23306,0.71334,0.19294,-0.75345]}, + {"t":0.54164, "x":6.15471, "y":4.23233, "heading":-2.58844, "vx":0.87445, "vy":0.48458, "omega":1.3999, "ax":-0.00051, "ay":0.00092, "alpha":0.17362, "fx":[0.55566,0.12278,-0.57272,-0.13984], "fy":[-0.12312,0.61262,0.15984,-0.58778]}, + {"t":0.58033, "x":6.18854, "y":4.25108, "heading":-2.53428, "vx":0.87443, "vy":0.48461, "omega":1.40662, "ax":0.00019, "ay":-0.00033, "alpha":0.13234, "fx":[0.4758,0.06689,-0.45718,-0.07314], "fy":[-0.0819,0.42781,0.07076,-0.43896]}, + {"t":0.61901, "x":6.22237, "y":4.26983, "heading":-2.47986, "vx":0.87444, "vy":0.4846, "omega":1.41174, "ax":-0.00012, "ay":0.00021, "alpha":0.10095, "fx":[0.33064,0.03809,-0.33448,-0.04193], "fy":[-0.03946,0.36082,0.04727,-0.35478]}, + {"t":0.6577, "x":6.2562, "y":4.28858, "heading":-2.42524, "vx":0.87444, "vy":0.48461, "omega":1.41565, "ax":0.00005, "ay":-0.0001, "alpha":0.07719, "fx":[0.27832,0.01384,-0.27308,-0.01545], "fy":[-0.01822,0.25342,0.01494,-0.2567]}, + {"t":0.69639, "x":6.29003, "y":4.30733, "heading":-2.37047, "vx":0.87444, "vy":0.4846, "omega":1.41863, "ax":-0.00002, "ay":0.00004, "alpha":0.05927, "fx":[0.19516,0.00159,-0.19585,-0.00228], "fy":[-0.00202,0.21379,0.00239,-0.21167]}, + {"t":0.73508, "x":6.32386, "y":4.32607, "heading":-2.31558, "vx":0.87444, "vy":0.4846, "omega":1.42093, "ax":0.00002, "ay":-0.00003, "alpha":0.04545, "fx":[0.16395,-0.00956,-0.16173,0.0084], "fy":[0.00631,0.14935,-0.00726,-0.1503]}, + {"t":0.77377, "x":6.35769, "y":4.34482, "heading":-2.26061, "vx":0.87444, "vy":0.4846, "omega":1.42268, "ax":0.0, "ay":0.00001, "alpha":0.03516, "fx":[0.11466,-0.01166,-0.11482,0.0115], "fy":[0.01215,0.12695,-0.01297,-0.12555]}, + {"t":0.81246, "x":6.39152, "y":4.36357, "heading":-2.20557, "vx":0.87444, "vy":0.4846, "omega":1.42404, "ax":0.0, "ay":-0.00001, "alpha":0.02707, "fx":[0.09684,-0.01679,-0.09553,0.01572], "fy":[0.01369,0.08772,-0.01392,-0.08795]}, + {"t":0.85114, "x":6.42535, "y":4.38232, "heading":-2.15048, "vx":0.87444, "vy":0.4846, "omega":1.42509, "ax":0.0, "ay":0.0, "alpha":0.02107, "fx":[0.0671,-0.01444,-0.06717,0.01437], "fy":[0.0154,0.07533,-0.01622,-0.07428]}, + {"t":0.88983, "x":6.45918, "y":4.40107, "heading":-2.09534, "vx":0.87444, "vy":0.4846, "omega":1.42591, "ax":0.0, "ay":0.0, "alpha":0.01627, "fx":[0.05711,-0.01678,-0.05622,0.0159], "fy":[0.01399,0.05121,-0.01401,-0.05122]}, + {"t":0.92852, "x":6.49301, "y":4.41982, "heading":-2.04018, "vx":0.87444, "vy":0.4846, "omega":1.42654, "ax":0.0, "ay":0.0, "alpha":0.01269, "fx":[0.03892,-0.013,-0.03892,0.013], "fy":[0.01409,0.04443,-0.0148,-0.04369]}, + {"t":0.96721, "x":6.52684, "y":4.43856, "heading":-1.98498, "vx":0.87444, "vy":0.4846, "omega":1.42703, "ax":0.0, "ay":0.0, "alpha":0.00977, "fx":[0.03327,-0.01404,-0.03267,0.01336], "fy":[0.0117,0.02942,-0.01163,-0.02935]}, + {"t":1.0059, "x":6.56067, "y":4.45731, "heading":-1.92978, "vx":0.87444, "vy":0.4846, "omega":1.42741, "ax":0.0, "ay":0.0, "alpha":0.00756, "fx":[0.02197,-0.01012,-0.02191,0.01018], "fy":[0.01112,0.02564,-0.01181,-0.02518]}, + {"t":1.04459, "x":6.5945, "y":4.47606, "heading":-1.87455, "vx":0.87444, "vy":0.4846, "omega":1.4277, "ax":0.0, "ay":0.0, "alpha":0.00569, "fx":[0.01857,-0.01046,-0.01821,0.00994], "fy":[0.00861,0.01614,-0.00847,-0.016]}, + {"t":1.08327, "x":6.62833, "y":4.49481, "heading":-1.81932, "vx":0.87444, "vy":0.4846, "omega":1.42792, "ax":0.0, "ay":-0.00001, "alpha":0.00423, "fx":[0.01144,-0.0068,-0.01129,0.00694], "fy":[0.00758,0.01383,-0.00834,-0.01357]}, + {"t":1.12196, "x":6.66217, "y":4.51356, "heading":-1.76407, "vx":0.87444, "vy":0.4846, "omega":1.42808, "ax":0.0, "ay":0.00001, "alpha":0.00291, "fx":[0.009,-0.00659,-0.00883,0.00617], "fy":[0.00524,0.00763,-0.00503,-0.00741]}, + {"t":1.16065, "x":6.696, "y":4.53231, "heading":-1.70882, "vx":0.87444, "vy":0.4846, "omega":1.42819, "ax":0.00001, "ay":-0.00001, "alpha":0.00181, "fx":[0.0044,-0.00318,-0.00417,0.00341], "fy":[0.0036,0.00588,-0.00453,-0.00577]}, + {"t":1.19934, "x":6.72983, "y":4.55106, "heading":-1.65357, "vx":0.87444, "vy":0.4846, "omega":1.42826, "ax":0.0, "ay":0.00001, "alpha":0.00072, "fx":[0.00218,-0.00225,-0.00213,0.00188], "fy":[0.00143,0.00163,-0.00115,-0.00134]}, + {"t":1.23803, "x":6.76366, "y":4.5698, "heading":-1.59831, "vx":0.87444, "vy":0.4846, "omega":1.42829, "ax":0.00001, "ay":-0.00002, "alpha":-0.00028, "fx":[-0.00088,0.00113,0.0012,-0.0008], "fy":[-0.00122,-0.00009,0.00005,0.0001]}, + {"t":1.27672, "x":6.79749, "y":4.58855, "heading":-1.54305, "vx":0.87444, "vy":0.4846, "omega":1.42828, "ax":-0.00001, "ay":0.00001, "alpha":-0.0014, "fx":[-0.00335,0.00317,0.00334,-0.00353], "fy":[-0.00341,-0.00323,0.00373,0.00355]}, + {"t":1.3154, "x":6.83132, "y":4.6073, "heading":-1.48779, "vx":0.87444, "vy":0.4846, "omega":1.42823, "ax":0.00001, "ay":-0.00002, "alpha":-0.00253, "fx":[-0.00544,0.00688,0.00587,-0.00645], "fy":[-0.0077,-0.00521,0.00622,0.00516]}, + {"t":1.35409, "x":6.86515, "y":4.62605, "heading":-1.43254, "vx":0.87444, "vy":0.4846, "omega":1.42813, "ax":-0.00001, "ay":0.00001, "alpha":-0.0039, "fx":[-0.00842,0.01073,0.00844,-0.01112], "fy":[-0.01027,-0.00774,0.01059,0.00806]}, + {"t":1.39278, "x":6.89898, "y":4.6448, "heading":-1.37729, "vx":0.87444, "vy":0.4846, "omega":1.42798, "ax":0.00002, "ay":-0.00003, "alpha":-0.00541, "fx":[-0.00989,0.01534,0.01044,-0.01479], "fy":[-0.01723,-0.0101,0.01532,0.01002]}, + {"t":1.43147, "x":6.93281, "y":4.66355, "heading":-1.32204, "vx":0.87444, "vy":0.4846, "omega":1.42777, "ax":0.0, "ay":0.00001, "alpha":-0.00733, "fx":[-0.01349,0.02215,0.01363,-0.02263], "fy":[-0.02073,-0.01233,0.02102,0.01262]}, + {"t":1.47016, "x":6.96664, "y":4.6823, "heading":-1.2668, "vx":0.87444, "vy":0.4846, "omega":1.42749, "ax":0.00002, "ay":-0.00004, "alpha":-0.0096, "fx":[-0.01447,0.02856,0.01517,-0.02786], "fy":[-0.03204,-0.01498,0.02957,0.01494]}, + {"t":1.50885, "x":7.00047, "y":4.70104, "heading":-1.21158, "vx":0.87444, "vy":0.4846, "omega":1.42711, "ax":0.0, "ay":0.00001, "alpha":-0.0125, "fx":[-0.01862,0.0403,0.01896,-0.04096], "fy":[-0.03744,-0.01704,0.03773,0.01733]}, + {"t":1.54753, "x":7.0343, "y":4.71979, "heading":-1.15636, "vx":0.87444, "vy":0.4846, "omega":1.42663, "ax":0.00003, "ay":-0.00005, "alpha":-0.01613, "fx":[-0.01903,0.05006,0.01991,-0.04918], "fy":[-0.05598,-0.01959,0.05273,0.01968]}, + {"t":1.58622, "x":7.06813, "y":4.73854, "heading":-1.10117, "vx":0.87444, "vy":0.4846, "omega":1.42601, "ax":-0.00001, "ay":0.00001, "alpha":-0.02078, "fx":[-0.02329,0.07014,0.02381,-0.07118], "fy":[-0.06497,-0.02129,0.06544,0.02176]}, + {"t":1.62491, "x":7.10196, "y":4.75729, "heading":-1.046, "vx":0.87444, "vy":0.4846, "omega":1.4252, "ax":0.00003, "ay":-0.00006, "alpha":-0.02684, "fx":[-0.02257,0.08598,0.02361,-0.08494], "fy":[-0.09574,-0.02261,0.09148,0.02311]}, + {"t":1.6636, "x":7.13579, "y":4.77604, "heading":-0.99086, "vx":0.87444, "vy":0.4846, "omega":1.42416, "ax":-0.00002, "ay":0.00003, "alpha":-0.03464, "fx":[-0.02566,0.12055,0.02613,-0.1223], "fy":[-0.11138,-0.02314,0.11254,0.0243]}, + {"t":1.70229, "x":7.16963, "y":4.79479, "heading":-0.93576, "vx":0.87444, "vy":0.4846, "omega":1.42282, "ax":0.00004, "ay":-0.00006, "alpha":-0.04503, "fx":[-0.02213,0.14727,0.02332,-0.14608], "fy":[-0.16321,-0.02055,0.15753,0.02192]}, + {"t":1.74098, "x":7.20346, "y":4.81353, "heading":-0.88071, "vx":0.87444, "vy":0.4846, "omega":1.42108, "ax":-0.00005, "ay":0.00009, "alpha":-0.05854, "fx":[-0.02098,0.20751,0.02037,-0.21033], "fy":[-0.19077,-0.01748,0.19387,0.02059]}, + {"t":1.77966, "x":7.23729, "y":4.83228, "heading":-0.82574, "vx":0.87444, "vy":0.4846, "omega":1.41882, "ax":0.00006, "ay":-0.00011, "alpha":-0.07665, "fx":[-0.00998,0.25344,0.01207,-0.25136], "fy":[-0.27998,-0.00529,0.27056,0.00716]}, + {"t":1.81835, "x":7.27112, "y":4.85103, "heading":-0.77084, "vx":0.87444, "vy":0.4846, "omega":1.41585, "ax":-0.00013, "ay":0.00024, "alpha":-0.1008, "fx":[0.00125,0.36142,-0.00729,-0.36407], "fy":[-0.32761,0.00766,0.33546,0.00019]}, + {"t":1.85704, "x":7.30495, "y":4.86978, "heading":-0.71607, "vx":0.87444, "vy":0.4846, "omega":1.41195, "ax":0.00025, "ay":-0.00044, "alpha":-0.13247, "fx":[0.03294,0.43993,-0.02473,-0.43172], "fy":[-0.48641,0.03946,0.46143,-0.04413]}, + {"t":1.89573, "x":7.33878, "y":4.88853, "heading":-0.66144, "vx":0.87445, "vy":0.48459, "omega":1.40683, "ax":-0.00035, "ay":0.00063, "alpha":-0.17749, "fx":[0.06038,0.64497,-0.09128,-0.63726], "fy":[-0.56234,0.07995,0.58328,-0.05902]}, + {"t":1.93442, "x":7.37261, "y":4.90728, "heading":-0.60701, "vx":0.87444, "vy":0.48461, "omega":1.39996, "ax":0.00085, "ay":-0.00154, "alpha":-0.23179, "fx":[0.14753,0.76742,-0.1191,-0.73899], "fy":[-0.84955,0.14262,0.78272,-0.17844]}, + {"t":1.97311, "x":7.40644, "y":4.92602, "heading":-0.55285, "vx":0.87447, "vy":0.48455, "omega":1.39099, "ax":-0.00112, "ay":0.00202, "alpha":-0.31844, "fx":[0.1899,1.17478,-0.31065,-1.12871], "fy":[-0.95467,0.26451,1.02207,-0.1971]}, + {"t":2.01179, "x":7.44027, "y":4.94477, "heading":-0.49904, "vx":0.87442, "vy":0.48463, "omega":1.37867, "ax":0.00112, "ay":-0.00202, "alpha":-0.40833, "fx":[0.39649,1.31661,-0.3592,-1.27932], "fy":[-1.43856,0.39251,1.35944,-0.44805]}, + {"t":2.05048, "x":7.4741, "y":4.96352, "heading":-0.4457, "vx":0.87447, "vy":0.48455, "omega":1.36287, "ax":-0.00179, "ay":0.00322, "alpha":-0.55664, "fx":[0.55687,1.95443,-0.72356,-1.90681], "fy":[-1.64569,0.64872,1.75311,-0.54118]}, + {"t":2.08917, "x":7.50793, "y":4.98227, "heading":-0.39297, "vx":0.8744, "vy":0.48468, "omega":1.34134, "ax":-0.00077, "ay":0.00132, "alpha":-0.72219, "fx":[0.90355,2.2114,-0.93081,-2.23554], "fy":[-2.35232,0.96465,2.4191,-0.94358]}, + {"t":2.12786, "x":7.54176, "y":5.00102, "heading":-0.34108, "vx":0.87437, "vy":0.48473, "omega":1.3134, "ax":-1.62398, "ay":-0.90465, "alpha":-0.9217, "fx":[-25.45788,-24.40517,-28.44629,-29.97433], "fy":[-17.9928,-13.75774,-12.17657,-16.39323]}, + {"t":2.16655, "x":7.57437, "y":5.0191, "heading":-0.29026, "vx":0.81154, "vy":0.44973, "omega":1.27774, "ax":-2.61808, "ay":-1.45115, "alpha":-1.26358, "fx":[-41.5701,-40.08444,-45.74472,-47.16959], "fy":[-28.3123,-22.19765,-20.03713,-26.21283]}, + {"t":2.20524, "x":7.60381, "y":5.03541, "heading":-0.24083, "vx":0.71025, "vy":0.39359, "omega":1.22885, "ax":-2.62122, "ay":-1.45233, "alpha":-1.35094, "fx":[-42.07493,-40.80022,-45.16756,-46.73549], "fy":[-29.40031,-21.24409,-19.03396,-27.15989]}, + {"t":2.24393, "x":7.62933, "y":5.04955, "heading":-0.19329, "vx":0.60884, "vy":0.3374, "omega":1.17659, "ax":-2.62199, "ay":-1.45324, "alpha":-2.37253, "fx":[-39.14414,-37.47077,-48.36589,-49.8482], "fy":[-31.63844,-19.80281,-16.79119,-28.66649]}, + {"t":2.28261, "x":7.65092, "y":5.06152, "heading":-0.14777, "vx":0.5074, "vy":0.28117, "omega":1.0848, "ax":-2.62262, "ay":-1.45324, "alpha":-2.96237, "fx":[-36.7086,-37.42037,-49.67765,-51.06454], "fy":[-33.49859,-17.92668,-15.14815,-30.32563]}, + {"t":2.3213, "x":7.66859, "y":5.07131, "heading":-0.1058, "vx":0.40593, "vy":0.22495, "omega":0.97019, "ax":-2.6229, "ay":-1.45341, "alpha":-4.03166, "fx":[-34.68661,-33.27518,-53.00465,-53.92382], "fy":[-35.67878,-16.02994,-12.88439,-32.31724]}, + {"t":2.35999, "x":7.68233, "y":5.07892, "heading":-0.06826, "vx":0.30446, "vy":0.16872, "omega":0.81421, "ax":-2.62305, "ay":-1.4536, "alpha":-5.30901, "fx":[-31.16544,-31.63906,-55.94724,-56.14818], "fy":[-39.58256,-11.98887,-9.4855,-35.86649]}, + {"t":2.39868, "x":7.69215, "y":5.08436, "heading":-0.03676, "vx":0.20298, "vy":0.11248, "omega":0.60881, "ax":-2.62318, "ay":-1.45369, "alpha":-6.91264, "fx":[-27.34966,-27.18028,-60.66734,-59.71147], "fy":[-43.3333,-8.08421,-6.01709,-39.49466]}, + {"t":2.43737, "x":7.69804, "y":5.08763, "heading":-0.01321, "vx":0.10149, "vy":0.05624, "omega":0.34138, "ax":-2.62328, "ay":-1.45375, "alpha":-8.82372, "fx":[-22.57813,-23.25902,-65.52604,-63.55241], "fy":[-48.60911,-2.43029,-1.71364,-44.18021]}, + {"t":2.47606, "x":7.7, "y":5.08871, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/HToOppE.traj b/src/main/deploy/choreo/HToOppE.traj new file mode 100644 index 0000000..0e982ce --- /dev/null +++ b/src/main/deploy/choreo/HToOppE.traj @@ -0,0 +1,208 @@ +{ + "name":"HToOppE", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":5.827323, "y":4.0509, "heading":3.141592653589793, "intervals":48, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.160315799713134, "y":5.6167577, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.365684200286866, "y":5.6167577, "heading":0.0, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":11.47006893157959, "y":5.786170482635498, "heading":0.0, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":12.055767059326172, "y":5.666640281677246, "heading":-0.9034901014083804, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":12.631814748155, "y":5.3371419902636, "heading":-1.047197551196598, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":4.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"H.x", "val":5.827323}, "y":{"exp":"H.y", "val":4.0509}, "heading":{"exp":"H.heading", "val":3.141592653589793}, "intervals":48, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.160315799713134 m", "val":8.160315799713134}, "y":{"exp":"5.6167577 m", "val":5.6167577}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.365684200286866 m", "val":9.365684200286866}, "y":{"exp":"5.6167577 m", "val":5.6167577}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"11.47006893157959 m", "val":11.47006893157959}, "y":{"exp":"5.786170482635498 m", "val":5.786170482635498}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"12.055767059326172 m", "val":12.055767059326172}, "y":{"exp":"5.666640281677246 m", "val":5.666640281677246}, "heading":{"exp":"-0.9034901014083804 rad", "val":-0.9034901014083804}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"FieldMaxX - E.x", "val":12.631814748155}, "y":{"exp":"FieldMaxY - E.y", "val":5.3371419902636}, "heading":{"exp":"E.heading - 180 deg", "val":-1.047197551196598}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"4 rad / s", "val":4.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.22512,1.82846,2.51826,2.82159,3.97529], + "samples":[ + {"t":0.0, "x":5.82732, "y":4.0509, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":5.02221, "ay":3.25354, "alpha":41.42014, "fx":[198.93149,141.07699,-0.00967,-5.12776], "fy":[-0.90738,140.44115,198.93155,-121.5254]}, + {"t":0.02552, "x":5.82896, "y":4.05196, "heading":3.14159, "vx":0.12818, "vy":0.08304, "omega":1.05718, "ax":4.89633, "ay":3.4556, "alpha":41.09144, "fx":[198.71106,141.00492,-0.07594,-13.16215], "fy":[-1.0842,140.29264,198.71064,-107.50608]}, + {"t":0.05105, "x":5.83383, "y":4.0552, "heading":-3.11461, "vx":0.25315, "vy":0.17124, "omega":2.10596, "ax":4.80513, "ay":3.5826, "alpha":40.89356, "fx":[198.05497,137.27371,-5.42839,-9.50373], "fy":[3.07137,143.3183,197.99063,-105.49912]}, + {"t":0.07657, "x":5.84185, "y":4.06074, "heading":-3.06086, "vx":0.3758, "vy":0.26268, "omega":3.1497, "ax":4.69645, "ay":3.72488, "alpha":33.16687, "fx":[174.75436,125.31205,-9.07466,22.15884], "fy":[-3.24032,129.94664,171.41416,-49.75293]}, + {"t":0.10209, "x":5.85297, "y":4.06866, "heading":-2.98047, "vx":0.49566, "vy":0.35775, "omega":3.99623, "ax":4.68947, "ay":3.73274, "alpha":0.00743, "fx":[79.28639,75.13273,76.72663,81.53878], "fy":[59.38614,60.42003,66.04791,63.03786]}, + {"t":0.12762, "x":5.86715, "y":4.07901, "heading":-2.87847, "vx":0.61536, "vy":0.45302, "omega":3.99642, "ax":4.68372, "ay":3.73872, "alpha":-0.00004, "fx":[78.09054,77.99743,77.87813,78.33516], "fy":[61.98846,61.99325,62.81892,62.48986]}, + {"t":0.15314, "x":5.88438, "y":4.09179, "heading":-2.77647, "vx":0.7349, "vy":0.54845, "omega":3.99642, "ax":4.67637, "ay":3.74633, "alpha":-0.00003, "fx":[77.99755,77.86031,77.91357,78.03983], "fy":[62.25397,62.37516,62.6243,62.54486]}, + {"t":0.17866, "x":5.90466, "y":4.10701, "heading":-2.67447, "vx":0.85426, "vy":0.64406, "omega":3.99642, "ax":4.66663, "ay":3.75638, "alpha":-0.00004, "fx":[77.72687,77.97669,77.70657,77.75193], "fy":[62.58279,62.53026,62.69364,62.66168]}, + {"t":0.20419, "x":5.92799, "y":4.12467, "heading":-2.57247, "vx":0.97336, "vy":0.73994, "omega":3.99642, "ax":4.65312, "ay":3.77025, "alpha":-0.00005, "fx":[77.52928,77.51419,77.6504,77.56713], "fy":[62.81035,62.99507,62.74012,62.84764]}, + {"t":0.22971, "x":5.95435, "y":4.14478, "heading":-2.47047, "vx":1.09213, "vy":0.83617, "omega":3.99641, "ax":4.63317, "ay":3.79055, "alpha":-0.00008, "fx":[77.17215,77.52322,77.12777,77.1076], "fy":[63.24813,63.1191,63.17568,63.20393]}, + {"t":0.25523, "x":5.98373, "y":4.16736, "heading":-2.36846, "vx":1.21038, "vy":0.93292, "omega":3.99641, "ax":4.60059, "ay":3.82329, "alpha":-0.00013, "fx":[76.47477,76.76184,76.76001,76.76216], "fy":[64.03884,63.759,63.66095,63.47091]}, + {"t":0.28076, "x":6.01612, "y":4.19241, "heading":-2.26646, "vx":1.3278, "vy":1.0305, "omega":3.99641, "ax":4.53808, "ay":3.88471, "alpha":-0.00027, "fx":[75.77353,76.11236,75.3672,75.33725], "fy":[64.83521,64.55429,64.73185,64.90388]}, + {"t":0.30628, "x":6.05149, "y":4.21998, "heading":-2.16446, "vx":1.44363, "vy":1.12965, "omega":3.9964, "ax":4.37106, "ay":4.04, "alpha":-0.00121, "fx":[72.82758,73.12131,72.88507,72.61965], "fy":[67.59485,67.4634,66.85166,67.46971]}, + {"t":0.3318, "x":6.08976, "y":4.25013, "heading":-2.06246, "vx":1.55519, "vy":1.23276, "omega":3.99637, "ax":2.91298, "ay":4.96643, "alpha":-3.64172, "fx":[37.0952,54.18077,59.74073,43.21514], "fy":[82.32726,69.54987,83.68555,95.58952]}, + {"t":0.35733, "x":6.1304, "y":4.28321, "heading":-1.96046, "vx":1.62954, "vy":1.35952, "omega":3.90342, "ax":4.22922, "ay":3.68013, "alpha":-15.00158, "fx":[23.68803,100.02831,110.40547,47.87431], "fy":[48.67973,8.69913,74.72106,113.28416]}, + {"t":0.38285, "x":6.17337, "y":4.31911, "heading":-1.86083, "vx":1.73748, "vy":1.45345, "omega":3.52053, "ax":4.98513, "ay":2.42992, "alpha":-13.67157, "fx":[43.6269,112.42336,116.67903,59.66934], "fy":[22.75093,-5.83326,56.84161,88.26329]}, + {"t":0.40837, "x":6.21934, "y":4.357, "heading":-1.77098, "vx":1.86472, "vy":1.51547, "omega":3.17159, "ax":5.20227, "ay":1.72938, "alpha":-12.18952, "fx":[54.99456,113.46622,115.10418,63.31264], "fy":[7.36347,-10.56208,47.70382,70.80619]}, + {"t":0.4339, "x":6.26863, "y":4.39624, "heading":-1.69003, "vx":1.9975, "vy":1.55961, "omega":2.86047, "ax":5.16, "ay":1.54074, "alpha":-11.2377, "fx":[58.13247,112.75411,110.95987,62.21245], "fy":[2.52922,-7.97967,45.89883,62.28535]}, + {"t":0.45942, "x":6.32129, "y":4.43655, "heading":-1.61702, "vx":2.1292, "vy":1.59894, "omega":2.57365, "ax":4.94561, "ay":1.32669, "alpha":-10.21175, "fx":[59.1491,106.88123,104.52628,59.20717], "fy":[-1.97849,-6.56696,43.64413,53.36278]}, + {"t":0.48494, "x":6.37725, "y":4.47779, "heading":-1.55133, "vx":2.25543, "vy":1.6328, "omega":2.31301, "ax":4.55783, "ay":1.35565, "alpha":-9.43508, "fx":[54.78689,100.565,95.66134,52.89417], "fy":[-0.90282,-1.53631,44.20774,48.62386]}, + {"t":0.51047, "x":6.4363, "y":4.51991, "heading":-1.49229, "vx":2.37176, "vy":1.6674, "omega":2.0722, "ax":3.79235, "ay":1.0891, "alpha":-7.67074, "fx":[47.1579,83.2087,79.00298,43.49706], "fy":[-2.26772,-0.21882,37.39162,37.71426]}, + {"t":0.53599, "x":6.49807, "y":4.56282, "heading":-1.4394, "vx":2.46855, "vy":1.6952, "omega":1.87642, "ax":3.06635, "ay":0.89823, "alpha":-6.12489, "fx":[38.48522,68.13774,63.23494,34.60018], "fy":[-1.75834,1.61699,31.12719,28.90649]}, + {"t":0.56151, "x":6.56207, "y":4.60638, "heading":-1.39151, "vx":2.54681, "vy":1.71812, "omega":1.72009, "ax":2.26283, "ay":0.54224, "alpha":-4.16515, "fx":[29.77831,49.33652,45.6442,26.1224], "fy":[-2.73461,0.30929,20.66505,17.91577]}, + {"t":0.58703, "x":6.62781, "y":4.65041, "heading":-1.34761, "vx":2.60457, "vy":1.73196, "omega":1.61378, "ax":1.69899, "ay":0.24708, "alpha":-2.69072, "fx":[23.34482,36.15472,33.31477,20.47132], "fy":[-3.66982,-0.88071,11.8233,9.20235]}, + {"t":0.61256, "x":6.69484, "y":4.69469, "heading":-1.30642, "vx":2.64793, "vy":1.73827, "omega":1.54511, "ax":1.18578, "ay":-0.04281, "alpha":-1.25367, "fx":[17.72847,23.35185,21.80405,16.18124], "fy":[-4.38418,-3.3014,3.17228,1.6585]}, + {"t":0.63808, "x":6.76281, "y":4.73905, "heading":-1.26699, "vx":2.6782, "vy":1.73717, "omega":1.51311, "ax":0.99269, "ay":-0.17411, "alpha":-0.63094, "fx":[15.54672,18.43454,17.69637,14.51291], "fy":[-4.78427,-3.85949,-1.02517,-1.94028]}, + {"t":0.6636, "x":6.83149, "y":4.78333, "heading":-1.22837, "vx":2.70353, "vy":1.73273, "omega":1.497, "ax":0.6817, "ay":-0.30631, "alpha":0.27599, "fx":[11.84632,10.36448,10.88076,12.36279], "fy":[-4.16864,-5.09471,-5.78884,-5.37223]}, + {"t":0.68913, "x":6.90072, "y":4.82745, "heading":-1.19016, "vx":2.72093, "vy":1.72491, "omega":1.50405, "ax":0.68109, "ay":-0.13565, "alpha":0.10038, "fx":[11.49812,11.01855,11.2882,11.60908], "fy":[-1.91705,-2.10321,-2.60593,-2.41837]}, + {"t":0.71465, "x":6.97039, "y":4.87143, "heading":-1.15177, "vx":2.73832, "vy":1.72145, "omega":1.50661, "ax":0.34417, "ay":0.00838, "alpha":0.67101, "fx":[6.54784,3.52071,4.92362,7.95652], "fy":[2.29683,0.85832,-1.97857,-0.61782]}, + {"t":0.74017, "x":7.04039, "y":4.91537, "heading":-1.11332, "vx":2.7471, "vy":1.72166, "omega":1.52374, "ax":0.22662, "ay":0.53278, "alpha":0.1399, "fx":[3.90098,3.17904,3.70455,4.32613], "fy":[9.27211,8.9387,8.48908,8.82492]}, + {"t":0.7657, "x":7.11058, "y":4.95949, "heading":-1.07442, "vx":2.75289, "vy":1.73526, "omega":1.52731, "ax":-0.38949, "ay":1.22984, "alpha":0.30567, "fx":[-6.35322,-7.39862,-6.65237,-5.56646], "fy":[21.25447,21.4688,19.19345,20.0868]}, + {"t":0.79122, "x":7.18071, "y":5.00418, "heading":-1.03544, "vx":2.74294, "vy":1.76665, "omega":1.53511, "ax":-1.00822, "ay":2.03324, "alpha":0.17352, "fx":[-16.7013,-17.95094,-16.59803,-15.97604], "fy":[34.19624,33.72277,33.59082,34.06293]}, + {"t":0.81674, "x":7.25039, "y":5.04993, "heading":-0.99626, "vx":2.71721, "vy":1.81855, "omega":1.53954, "ax":-2.04561, "ay":3.20255, "alpha":0.01478, "fx":[-34.40158,-34.06295,-33.84999,-34.08313], "fy":[52.48122,55.46099,52.6789,52.91876]}, + {"t":0.84227, "x":7.31908, "y":5.09739, "heading":-0.95697, "vx":2.665, "vy":1.90029, "omega":1.53992, "ax":-3.39746, "ay":3.07537, "alpha":2.71823, "fx":[-54.81243,-67.04099,-57.37883,-47.30379], "fy":[60.03681,52.10446,42.50813,50.41005]}, + {"t":0.86779, "x":7.38599, "y":5.14689, "heading":-0.91766, "vx":2.57829, "vy":1.97878, "omega":1.60929, "ax":-4.26413, "ay":2.69527, "alpha":4.70531, "fx":[-69.3766,-86.98616,-72.97766,-54.98327], "fy":[60.3925,48.21462,28.18764,42.92049]}, + {"t":0.89331, "x":7.45041, "y":5.19828, "heading":-0.87659, "vx":2.46945, "vy":2.04757, "omega":1.72939, "ax":-5.15367, "ay":-0.53709, "alpha":10.95997, "fx":[-83.3977,-124.74571,-87.08355,-48.40953], "fy":[28.3701,-6.37816,-46.08166,-11.72273]}, + {"t":0.91884, "x":7.51176, "y":5.25036, "heading":-0.83245, "vx":2.33791, "vy":2.03386, "omega":2.00912, "ax":-3.6121, "ay":-4.11792, "alpha":11.47668, "fx":[-61.4731,-100.07873,-59.69737,-19.59841], "fy":[-28.94011,-68.41495,-106.72239,-70.49751]}, + {"t":0.94436, "x":7.57025, "y":5.30093, "heading":-0.78117, "vx":2.24572, "vy":1.92876, "omega":2.30204, "ax":-2.34994, "ay":-5.24104, "alpha":9.17087, "fx":[-42.53904,-72.77981,-35.76083,-5.60991], "fy":[-57.16207,-87.93023,-116.30252,-88.0674]}, + {"t":0.96988, "x":7.62681, "y":5.34845, "heading":-0.72242, "vx":2.18574, "vy":1.79499, "omega":2.53612, "ax":-1.40493, "ay":-5.6743, "alpha":7.06897, "fx":[-28.70302,-49.28434,-19.1082,3.41755], "fy":[-71.7516,-97.1517,-115.79879,-93.64917]}, + {"t":0.99541, "x":7.68214, "y":5.39242, "heading":-0.65769, "vx":2.14988, "vy":1.65017, "omega":2.71654, "ax":-0.8674, "ay":-5.83023, "alpha":5.92512, "fx":[-20.97352,-36.60313,-9.1198,8.85957], "fy":[-79.18429,-99.65438,-114.01135,-95.89819]}, + {"t":1.02093, "x":7.73673, "y":5.43264, "heading":-0.58835, "vx":2.12775, "vy":1.50136, "omega":2.86777, "ax":-0.58319, "ay":-5.8932, "alpha":5.52087, "fx":[-17.9127,-30.06572,-3.18189,12.27454], "fy":[-81.90915,-101.97054,-113.00449,-96.06273]}, + {"t":1.04645, "x":7.79084, "y":5.46904, "heading":-0.51516, "vx":2.11286, "vy":1.35095, "omega":3.00868, "ax":-0.38006, "ay":-5.92691, "alpha":5.49845, "fx":[-16.52808,-26.10608,1.55829,15.73397], "fy":[-83.20707,-102.67773,-113.15012,-96.15991]}, + {"t":1.07198, "x":7.84465, "y":5.50159, "heading":-0.43836, "vx":2.10316, "vy":1.19967, "omega":3.14902, "ax":-0.26382, "ay":-5.94468, "alpha":5.86818, "fx":[-17.10415,-24.63364,5.43955,18.70742], "fy":[-82.76033,-105.0268,-113.60081,-94.99179]}, + {"t":1.0975, "x":7.89824, "y":5.53027, "heading":-0.35799, "vx":2.09643, "vy":1.04794, "omega":3.29879, "ax":-0.17914, "ay":-5.9559, "alpha":6.54068, "fx":[-18.60396,-24.34729,9.02239,21.98418], "fy":[-81.38832,-106.32056,-115.73028,-93.68852]}, + {"t":1.12302, "x":7.95169, "y":5.55508, "heading":-0.2738, "vx":2.09185, "vy":0.89593, "omega":3.46573, "ax":-0.13954, "ay":-5.96289, "alpha":4.23134, "fx":[-7.24144,-12.81022,-0.25621,11.00383], "fy":[-84.99904,-110.34344,-112.53293,-89.71842]}, + {"t":1.14855, "x":8.00503, "y":5.576, "heading":-0.18534, "vx":2.08829, "vy":0.74374, "omega":3.57373, "ax":-1.5821, "ay":-5.72781, "alpha":-45.05422, "fx":[33.49789,186.47283,-195.69135,-129.77106], "fy":[-195.14485,-58.60889,21.52102,-149.68674]}, + {"t":1.17407, "x":8.05782, "y":5.59312, "heading":-0.09413, "vx":2.04791, "vy":0.59755, "omega":2.4238, "ax":-1.59554, "ay":-5.58676, "alpha":-45.43986, "fx":[30.04742,192.72337,-198.03053,-131.12767], "fy":[-196.3872,-38.35676,11.63698,-149.40745]}, + {"t":1.19959, "x":8.10957, "y":5.60655, "heading":-0.03226, "vx":2.00719, "vy":0.45495, "omega":1.26402, "ax":-1.4825, "ay":-4.31531, "alpha":-49.52442, "fx":[37.28126,193.47022,-197.85747,-131.7442], "fy":[-195.34909,37.86187,18.87107,-149.12072]}, + {"t":1.22512, "x":8.16032, "y":5.61676, "heading":0.0, "vx":1.96935, "vy":0.34481, "omega":0.0, "ax":0.74821, "ay":-5.91288, "alpha":0.0, "fx":[12.55462,12.5486,12.39458,12.3912], "fy":[-98.48466,-98.97088,-98.31751,-98.48629]}, + {"t":1.25687, "x":8.22323, "y":5.62473, "heading":0.0, "vx":1.99311, "vy":0.15705, "omega":0.0, "ax":0.18917, "ay":-5.93125, "alpha":0.0, "fx":[2.99335,3.00208,3.39229,3.22552], "fy":[-99.62966,-98.71734,-98.71368,-98.42374]}, + {"t":1.28863, "x":8.28662, "y":5.62672, "heading":0.0, "vx":1.99912, "vy":-0.0313, "omega":0.0, "ax":-0.0192, "ay":-0.86221, "alpha":0.0, "fx":[-0.33446,-0.33429,-0.30578,-0.3057], "fy":[-14.38692,-14.31989,-14.39665,-14.38689]}, + {"t":1.32038, "x":8.35009, "y":5.62529, "heading":0.0, "vx":1.99851, "vy":-0.05868, "omega":0.0, "ax":0.01054, "ay":0.40293, "alpha":0.0, "fx":[0.23687,0.23687,0.15296,0.07575], "fy":[6.98421,6.65534,6.65534,6.57148]}, + {"t":1.35214, "x":8.41356, "y":5.62363, "heading":0.0, "vx":1.99884, "vy":-0.04588, "omega":0.0, "ax":0.00601, "ay":0.29145, "alpha":0.0, "fx":[0.08792,0.08787,0.1126,0.11258], "fy":[4.84598,4.92971,4.81162,4.84596]}, + {"t":1.38389, "x":8.47703, "y":5.62232, "heading":0.0, "vx":1.99903, "vy":-0.03663, "omega":0.0, "ax":0.00284, "ay":0.16709, "alpha":0.0, "fx":[0.05174,0.05178,0.07161,0.01415], "fy":[2.77863,2.78089,2.7809,2.80093]}, + {"t":1.41565, "x":8.54051, "y":5.62125, "heading":0.0, "vx":1.99912, "vy":-0.03132, "omega":0.0, "ax":0.00167, "ay":0.11343, "alpha":0.0, "fx":[0.03612,0.03611,0.0197,0.0197], "fy":[1.89898,1.85991,1.90522,1.89898]}, + {"t":1.4474, "x":8.604, "y":5.62031, "heading":0.0, "vx":1.99918, "vy":-0.02772, "omega":0.0, "ax":0.00114, "ay":0.08685, "alpha":0.0, "fx":[0.02166,0.02167,0.04123,-0.00828], "fy":[1.436,1.44518,1.44518,1.46474]}, + {"t":1.47916, "x":8.66748, "y":5.61947, "heading":0.0, "vx":1.99921, "vy":-0.02496, "omega":0.0, "ax":0.00078, "ay":0.06478, "alpha":0.0, "fx":[0.01417,0.01417,0.01167,0.01167], "fy":[1.08102,1.07721,1.07984,1.08102]}, + {"t":1.51091, "x":8.73097, "y":5.61871, "heading":0.0, "vx":1.99924, "vy":-0.0229, "omega":0.0, "ax":0.00048, "ay":0.04321, "alpha":0.0, "fx":[0.01873,0.01873,0.0285,-0.03394], "fy":[0.74277,0.70964,0.70964,0.71941]}, + {"t":1.54267, "x":8.79445, "y":5.61801, "heading":0.0, "vx":1.99925, "vy":-0.02153, "omega":0.0, "ax":0.00025, "ay":0.02373, "alpha":0.0, "fx":[-0.00246,-0.00246,0.01083,0.01083], "fy":[0.38887,0.42645,0.37787,0.38887]}, + {"t":1.57442, "x":8.85794, "y":5.61733, "heading":0.0, "vx":1.99926, "vy":-0.02078, "omega":0.0, "ax":0.00009, "ay":0.0083, "alpha":0.0, "fx":[0.0227,0.0227,0.03148,-0.07117], "fy":[0.19333,0.11701,0.11701,0.12577]}, + {"t":1.60618, "x":8.92143, "y":5.61668, "heading":0.0, "vx":1.99926, "vy":-0.02052, "omega":0.0, "ax":-0.00004, "ay":-0.00407, "alpha":0.0, "fx":[-0.01152,-0.01152,0.01012,0.01012], "fy":[-0.07868,-0.0204,-0.09367,-0.07868]}, + {"t":1.63793, "x":8.98491, "y":5.61602, "heading":0.0, "vx":1.99926, "vy":-0.02064, "omega":0.0, "ax":-0.00021, "ay":-0.02018, "alpha":0.0, "fx":[0.02558,0.02558,0.05991,-0.12518], "fy":[-0.28339,-0.36558,-0.36558,-0.33133]}, + {"t":1.66969, "x":9.0484, "y":5.61536, "heading":0.0, "vx":1.99926, "vy":-0.02129, "omega":0.0, "ax":-0.00058, "ay":-0.05267, "alpha":0.0, "fx":[-0.00725,-0.00725,-0.01217,-0.01217], "fy":[-0.87545,-0.89686,-0.86389,-0.87545]}, + {"t":1.70144, "x":9.11188, "y":5.61466, "heading":0.0, "vx":1.99924, "vy":-0.02296, "omega":0.0, "ax":-0.00101, "ay":-0.08312, "alpha":0.0, "fx":[-0.01857,-0.01857,0.06953,-0.09969], "fy":[-1.47897,-1.3839,-1.3839,-1.29582]}, + {"t":1.7332, "x":9.17537, "y":5.61389, "heading":0.0, "vx":1.99921, "vy":-0.0256, "omega":0.0, "ax":0.0014, "ay":0.11802, "alpha":0.0, "fx":[0.06011,0.06008,-0.01341,-0.01342], "fy":[2.00414,1.79013,2.07113,2.00413]}, + {"t":1.76495, "x":9.23886, "y":5.61313, "heading":0.0, "vx":1.99925, "vy":-0.02185, "omega":0.0, "ax":-0.00233, "ay":1.56139, "alpha":0.0, "fx":[-0.02881,-0.0279,-0.1428,0.04411], "fy":[26.16503,26.01706,26.01707,25.91108]}, + {"t":1.79671, "x":9.30234, "y":5.61323, "heading":0.0, "vx":1.99918, "vy":0.02773, "omega":0.0, "ax":-0.27756, "ay":5.25862, "alpha":0.0, "fx":[-4.81643,-4.81626,-4.43697,-4.43752], "fy":[87.46923,88.57767,87.11877,87.46901]}, + {"t":1.82846, "x":9.36568, "y":5.61676, "heading":0.0, "vx":1.99036, "vy":0.19472, "omega":0.0, "ax":5.94346, "ay":0.71204, "alpha":-21.62462, "fx":[142.34817,137.20951,55.03946,61.70143], "fy":[-41.64872,60.55567,92.42108,-63.8508]}, + {"t":1.85145, "x":9.41302, "y":5.62142, "heading":0.0, "vx":2.12702, "vy":0.21109, "omega":-0.49722, "ax":5.94945, "ay":0.71363, "alpha":-16.93183, "fx":[133.45371,129.20525,64.53045,69.50834], "fy":[-32.88909,51.76584,72.64963,-43.9429]}, + {"t":1.87445, "x":9.4635, "y":5.62647, "heading":-0.01143, "vx":2.26382, "vy":0.2275, "omega":-0.88654, "ax":5.94819, "ay":0.71457, "alpha":-11.94257, "fx":[123.60428,120.75492,74.52726,77.7275], "fy":[-21.51621,40.87417,53.22082,-24.93291]}, + {"t":1.89744, "x":9.51712, "y":5.63189, "heading":-0.03182, "vx":2.40059, "vy":0.24393, "omega":-1.16113, "ax":5.94652, "ay":0.71571, "alpha":-6.91353, "fx":[113.15398,111.83031,85.0271,86.49136], "fy":[-8.95481,29.12743,35.30695,-7.75721]}, + {"t":1.92043, "x":9.57389, "y":5.63768, "heading":-0.05852, "vx":2.53732, "vy":0.26039, "omega":-1.3201, "ax":5.94418, "ay":0.71731, "alpha":-2.14297, "fx":[103.22737,102.90226,94.93462,95.28237], "fy":[4.94522,17.34799,19.44548,6.0902]}, + {"t":1.94343, "x":9.63381, "y":5.64386, "heading":-0.08887, "vx":2.67399, "vy":0.27688, "omega":-1.36937, "ax":5.94065, "ay":0.71978, "alpha":1.77816, "fx":[95.3125,94.95476,102.5109,103.33286], "fy":[17.23894,7.80998,6.73159,16.21305]}, + {"t":1.96642, "x":9.69686, "y":5.65042, "heading":-0.12035, "vx":2.81059, "vy":0.29343, "omega":-1.32849, "ax":5.93475, "ay":0.72376, "alpha":4.69906, "fx":[89.80191,88.52718,108.25292,109.13558], "fy":[27.56789,1.01111,-2.79051,22.47074]}, + {"t":1.98941, "x":9.76305, "y":5.65735, "heading":-0.1509, "vx":2.94704, "vy":0.31007, "omega":-1.22044, "ax":5.92286, "ay":0.73181, "alpha":6.50786, "fx":[86.5527,83.88658,110.91608,113.56975], "fy":[34.2635,-2.87456,-8.35899,25.76542]}, + {"t":2.01241, "x":9.83238, "y":5.66468, "heading":-0.17896, "vx":3.08323, "vy":0.3269, "omega":-1.0708, "ax":5.88697, "ay":0.75442, "alpha":6.95202, "fx":[85.51277,81.47203,111.44056,114.10655], "fy":[36.26617,-2.71953,-9.54167,26.29804]}, + {"t":2.0354, "x":9.90483, "y":5.67239, "heading":-0.20358, "vx":3.21859, "vy":0.34425, "omega":-0.91095, "ax":2.57561, "ay":1.07394, "alpha":6.16893, "fx":[31.38999,26.14171,55.02126,59.18347], "fy":[37.08144,5.70988,-0.51441,29.33154]}, + {"t":2.05839, "x":9.97952, "y":5.68059, "heading":-0.22453, "vx":3.27781, "vy":0.36894, "omega":-0.76911, "ax":-0.04973, "ay":0.44915, "alpha":3.52768, "fx":[-7.33782,-10.99402,5.76473,9.25136], "fy":[17.83487,0.82374,-2.76658,14.05622]}, + {"t":2.08139, "x":10.05487, "y":5.68919, "heading":-0.24221, "vx":3.27667, "vy":0.37927, "omega":-0.688, "ax":-0.0272, "ay":0.23364, "alpha":1.57021, "fx":[-3.1189,-4.94846,2.90599,3.34762], "fy":[8.75188,0.8793,-0.94924,6.89683]}, + {"t":2.10438, "x":10.1302, "y":5.69798, "heading":-0.25803, "vx":3.27604, "vy":0.38464, "omega":-0.65189, "ax":-0.01368, "ay":0.11607, "alpha":0.62558, "fx":[-1.2213,-1.87347,0.7661,1.41649], "fy":[3.97502,0.74179,-0.1405,3.16296]}, + {"t":2.12737, "x":10.20553, "y":5.70685, "heading":-0.27302, "vx":3.27573, "vy":0.38731, "omega":-0.63751, "ax":-0.00667, "ay":0.0565, "alpha":0.24117, "fx":[-0.45096,-0.7702,0.48312,0.29309], "fy":[1.8214,0.44327,0.12419,1.37825]}, + {"t":2.15037, "x":10.28085, "y":5.71577, "heading":-0.28768, "vx":3.27557, "vy":0.38861, "omega":-0.63196, "ax":-0.00436, "ay":0.03658, "alpha":0.08973, "fx":[-0.17424,-0.24116,0.02879,0.09598], "fy":[0.96785,0.50471,0.16818,0.79864]}, + {"t":2.17336, "x":10.35616, "y":5.72472, "heading":-0.30221, "vx":3.27547, "vy":0.38945, "omega":-0.6299, "ax":-0.00438, "ay":0.03692, "alpha":0.02641, "fx":[-0.05173,-0.08309,0.0683,-0.22583], "fy":[0.89083,0.49422,0.46304,0.61389]}, + {"t":2.19635, "x":10.43147, "y":5.73368, "heading":-0.3167, "vx":3.27537, "vy":0.3903, "omega":-0.62929, "ax":-0.00411, "ay":0.03444, "alpha":0.0858, "fx":[-0.19271,-0.30446,0.05543,0.16739], "fy":[0.84547,0.62892,0.14333,0.67883]}, + {"t":2.21935, "x":10.50678, "y":5.74266, "heading":-0.33116, "vx":3.27528, "vy":0.39109, "omega":-0.62732, "ax":-0.00228, "ay":0.01901, "alpha":0.02022, "fx":[-0.04558,-0.01249,-0.03087,-0.06298], "fy":[0.58813,0.21037,0.24278,0.2266]}, + {"t":2.24234, "x":10.58209, "y":5.75166, "heading":-0.34559, "vx":3.27523, "vy":0.39153, "omega":-0.62686, "ax":0.00642, "ay":-0.0536, "alpha":0.17561, "fx":[-0.14907,-0.43777,0.36245,0.65221], "fy":[-0.37053,-0.86338,-1.6376,-0.70212]}, + {"t":2.26533, "x":10.6574, "y":5.76065, "heading":-0.36, "vx":3.27537, "vy":0.39029, "omega":-0.62282, "ax":0.02506, "ay":-0.21189, "alpha":0.01909, "fx":[0.34217,0.45344,0.31995,0.55525], "fy":[-3.32498,-3.63474,-3.52581,-3.64321]}, + {"t":2.28833, "x":10.73272, "y":5.76957, "heading":-0.37432, "vx":3.27595, "vy":0.38542, "omega":-0.62238, "ax":0.0638, "ay":-0.55103, "alpha":0.25989, "fx":[0.69489,0.22069,1.43056,1.90802], "fy":[-8.41528,-9.33867,-10.11127,-8.87634]}, + {"t":2.31132, "x":10.80806, "y":5.77828, "heading":-0.38863, "vx":3.27742, "vy":0.37275, "omega":-0.6164, "ax":0.12789, "ay":-1.16727, "alpha":-0.12434, "fx":[2.13334,2.58137,1.86727,1.94552], "fy":[-19.95986,-19.36337,-18.93712,-19.5712]}, + {"t":2.33431, "x":10.88345, "y":5.78655, "heading":-0.40281, "vx":3.28036, "vy":0.34591, "omega":-0.61926, "ax":0.21716, "ay":-2.22642, "alpha":0.0362, "fx":[3.51428,3.46629,3.72269,3.77625], "fy":[-37.0409,-37.56361,-36.92662,-36.92251]}, + {"t":2.3573, "x":10.95894, "y":5.79391, "heading":-0.41705, "vx":3.28535, "vy":0.29472, "omega":-0.61843, "ax":0.29191, "ay":-3.83251, "alpha":-0.99193, "fx":[5.6756,8.03338,3.47799,2.27695], "fy":[-67.83322,-62.67366,-60.5071,-64.53014]}, + {"t":2.3803, "x":11.03455, "y":5.79968, "heading":-0.43126, "vx":3.29206, "vy":0.2066, "omega":-0.64124, "ax":0.23244, "ay":-5.24496, "alpha":-1.73739, "fx":[5.9885,9.95339,1.6583,-2.10159], "fy":[-92.52263,-87.05512,-81.31689,-88.82931]}, + {"t":2.40329, "x":11.11031, "y":5.80304, "heading":-0.44601, "vx":3.29741, "vy":0.086, "omega":-0.68119, "ax":0.01886, "ay":-5.63871, "alpha":-4.03912, "fx":[5.1602,14.21566,-6.53069,-11.5876], "fy":[-107.42368,-90.761,-80.61246,-97.18123]}, + {"t":2.42628, "x":11.18613, "y":5.80353, "heading":-0.46167, "vx":3.29784, "vy":-0.04365, "omega":-0.77406, "ax":-3.18537, "ay":-4.87956, "alpha":-6.38329, "fx":[-43.7831,-32.46188,-64.09777,-72.05184], "fy":[-102.89305,-78.60225,-58.73552,-85.12901]}, + {"t":2.44928, "x":11.26112, "y":5.80123, "heading":-0.47947, "vx":3.2246, "vy":-0.15585, "omega":-0.92083, "ax":-5.5652, "ay":-2.10229, "alpha":-8.12259, "fx":[-82.68552,-68.38476,-104.51455,-115.49215], "fy":[-65.11485,-30.28464,-4.82534,-39.95165]}, + {"t":2.47227, "x":11.33379, "y":5.79709, "heading":-0.50064, "vx":3.09664, "vy":-0.20419, "omega":-1.1076, "ax":-5.77753, "ay":-1.50964, "alpha":-8.47205, "fx":[-87.10686,-71.78754,-107.61029,-118.73024], "fy":[-58.00555,-19.66751,7.49279,-30.47965]}, + {"t":2.49526, "x":11.40347, "y":5.792, "heading":-0.52611, "vx":2.96379, "vy":-0.2389, "omega":-1.30239, "ax":-5.84384, "ay":-1.27034, "alpha":-7.79523, "fx":[-89.8722,-74.6849,-106.488,-118.61099], "fy":[-51.38459,-16.51929,8.93693,-25.73712]}, + {"t":2.51826, "x":11.47007, "y":5.78617, "heading":-0.55606, "vx":2.82942, "vy":-0.26811, "omega":-1.48163, "ax":-5.84727, "ay":-1.12772, "alpha":-6.41294, "fx":[-92.12939,-78.45932,-104.0858,-115.21031], "fy":[-43.69983,-15.10005,5.81726,-22.21172]}, + {"t":2.53422, "x":11.5145, "y":5.78175, "heading":-0.57971, "vx":2.73607, "vy":-0.28611, "omega":-1.58401, "ax":-5.89152, "ay":-1.02922, "alpha":-6.32556, "fx":[-93.41145,-79.83227,-103.87807,-115.71331], "fy":[-42.168,-14.09949,7.56821,-19.92689]}, + {"t":2.55019, "x":11.55743, "y":5.77705, "heading":-0.605, "vx":2.64201, "vy":-0.30254, "omega":-1.685, "ax":-5.90703, "ay":-0.95162, "alpha":-6.19025, "fx":[-94.36617,-80.42668,-103.71675,-115.35973], "fy":[-40.52174,-13.35557,8.63726,-18.2121]}, + {"t":2.56615, "x":11.59885, "y":5.7721, "heading":-0.6319, "vx":2.54771, "vy":-0.31774, "omega":-1.78383, "ax":-5.91835, "ay":-0.89129, "alpha":-5.57584, "fx":[-95.43297,-82.27714,-102.5365,-114.3779], "fy":[-36.8216,-13.1457,7.16607,-16.62842]}, + {"t":2.58212, "x":11.63877, "y":5.76691, "heading":-0.66038, "vx":2.45322, "vy":-0.33197, "omega":-1.87285, "ax":-5.92699, "ay":-0.8428, "alpha":-4.90395, "fx":[-96.44984,-84.42613,-101.89448,-112.42972], "fy":[-33.6793,-12.7182,5.40806,-15.20651]}, + {"t":2.59808, "x":11.67718, "y":5.7615, "heading":-0.69028, "vx":2.3586, "vy":-0.34542, "omega":-1.95114, "ax":-5.93373, "ay":-0.80321, "alpha":-3.76922, "fx":[-97.33222,-87.78093,-100.61548,-109.9212], "fy":[-28.19815,-12.82968,1.40345,-13.9323]}, + {"t":2.61405, "x":11.71408, "y":5.75589, "heading":-0.72143, "vx":2.26387, "vy":-0.35825, "omega":-2.01132, "ax":-5.93916, "ay":-0.77009, "alpha":-2.56278, "fx":[-98.23144,-91.32429,-99.99178,-106.46429], "fy":[-22.95232,-12.4451,-2.86889,-13.08169]}, + {"t":2.63001, "x":11.74947, "y":5.75007, "heading":-0.75354, "vx":2.16905, "vy":-0.37054, "omega":-2.05223, "ax":-5.9436, "ay":-0.74211, "alpha":-0.94558, "fx":[-98.95127,-95.77405,-99.28363,-102.29876], "fy":[-15.50847,-12.34594,-9.01932,-12.60842]}, + {"t":2.64598, "x":11.78334, "y":5.74406, "heading":-0.7863, "vx":2.07416, "vy":-0.38239, "omega":-2.06733, "ax":-5.9473, "ay":-0.71805, "alpha":0.73037, "fx":[-99.33601,-100.72352,-98.94849,-97.54668], "fy":[-8.57114,-11.33128,-15.45514,-12.52031]}, + {"t":2.66194, "x":11.8157, "y":5.73786, "heading":-0.81931, "vx":1.97921, "vy":-0.39385, "omega":-2.05567, "ax":-5.95043, "ay":-0.69721, "alpha":2.7392, "fx":[-99.40417,-106.23126,-98.9335,-92.19428], "fy":[0.33384,-10.46204,-23.31427,-13.04591]}, + {"t":2.67791, "x":11.84654, "y":5.73148, "heading":-0.85213, "vx":1.88421, "vy":-0.40498, "omega":-2.01194, "ax":-5.95311, "ay":-0.67895, "alpha":4.82577, "fx":[-99.49411,-111.92999,-99.6047,-85.91288], "fy":[9.03979,-8.80153,-31.19497,-14.31429]}, + {"t":2.69387, "x":11.87586, "y":5.72493, "heading":-0.88425, "vx":1.78917, "vy":-0.41582, "omega":-1.93489, "ax":-5.95542, "ay":-0.66281, "alpha":7.11911, "fx":[-99.10518,-117.95263,-100.75429,-79.28404], "fy":[18.98328,-6.99726,-39.80819,-16.37281]}, + {"t":2.70984, "x":11.90366, "y":5.71821, "heading":-0.91514, "vx":1.69409, "vy":-0.4264, "omega":-1.82124, "ax":-5.95744, "ay":-0.64849, "alpha":9.4539, "fx":[-98.65636,-123.93705,-102.34165,-72.29566], "fy":[28.95939,-4.6509,-48.36193,-19.18683]}, + {"t":2.7258, "x":11.92995, "y":5.71132, "heading":-0.94422, "vx":1.59898, "vy":-0.43676, "omega":-1.6703, "ax":-5.95922, "ay":-0.63562, "alpha":11.83671, "fx":[-97.75543,-129.85742,-104.38492,-65.35173], "fy":[39.44936,-2.1675,-56.80972,-22.85439]}, + {"t":2.74177, "x":11.95472, "y":5.70427, "heading":-0.97088, "vx":1.50384, "vy":-0.4469, "omega":-1.48133, "ax":-5.96079, "ay":-0.62411, "alpha":14.22052, "fx":[-97.07984,-135.60747,-106.80958,-57.9574], "fy":[49.87853,0.54461,-64.74131,-27.29618]}, + {"t":2.75773, "x":11.97797, "y":5.69705, "heading":-0.99453, "vx":1.40867, "vy":-0.45687, "omega":-1.2543, "ax":-5.9622, "ay":-0.61363, "alpha":16.49714, "fx":[-95.98078,-140.97894,-109.56368,-51.02477], "fy":[60.17142,3.25385,-71.69848,-32.64264]}, + {"t":2.7737, "x":11.9997, "y":5.68968, "heading":-1.01456, "vx":1.31349, "vy":-0.46666, "omega":-0.99092, "ax":-5.96345, "ay":-0.60421, "alpha":18.70058, "fx":[-95.24106,-146.02822,-112.28934,-44.07309], "fy":[70.12361,5.93271,-78.19581,-38.14791]}, + {"t":2.78966, "x":12.01991, "y":5.68215, "heading":-1.03038, "vx":1.21828, "vy":-0.47631, "omega":-0.69237, "ax":-5.96459, "ay":-0.59555, "alpha":20.73863, "fx":[-94.16865,-150.58916,-114.90215,-38.04749], "fy":[79.70236,8.55501,-83.90456,-44.06295]}, + {"t":2.80563, "x":12.0386, "y":5.67447, "heading":-1.04143, "vx":1.12306, "vy":-0.48582, "omega":-0.36127, "ax":-5.96561, "ay":-0.58774, "alpha":22.62896, "fx":[-93.78621,-154.78243,-117.52843,-31.67817], "fy":[88.35865,10.83032,-88.72867,-49.64999]}, + {"t":2.82159, "x":12.05577, "y":5.66664, "heading":-1.0472, "vx":1.02781, "vy":-0.4952, "omega":0.0, "ax":-0.99523, "ay":-0.02324, "alpha":0.0, "fx":[-16.58914,-16.5933,-16.5909,-16.58674], "fy":[-0.36986,-0.42782,-0.37713,-0.37447]}, + {"t":2.85655, "x":12.09109, "y":5.64931, "heading":-1.0472, "vx":0.99302, "vy":-0.49601, "omega":0.0, "ax":-0.99383, "ay":0.09209, "alpha":0.0, "fx":[-16.50693,-16.57003,-16.53372,-16.65604], "fy":[1.56374,1.55585,1.49217,1.52874]}, + {"t":2.89151, "x":12.1252, "y":5.63203, "heading":-1.0472, "vx":0.95828, "vy":-0.4928, "omega":0.0, "ax":-0.98276, "ay":0.17575, "alpha":0.0, "fx":[-16.3819,-16.38311,-16.38243,-16.38117], "fy":[2.93312,2.9212,2.93217,2.93251]}, + {"t":2.92647, "x":12.1581, "y":5.61491, "heading":-1.0472, "vx":0.92392, "vy":-0.48665, "omega":0.0, "ax":-0.96979, "ay":0.23796, "alpha":0.0, "fx":[-16.11792,-16.16334,-16.13739,-16.2452], "fy":[4.00241,3.97656,3.93103,3.95688]}, + {"t":2.96143, "x":12.18981, "y":5.59804, "heading":-1.0472, "vx":0.89001, "vy":-0.47833, "omega":0.0, "ax":-0.95705, "ay":0.28549, "alpha":0.0, "fx":[-15.95361,-15.95355,-15.95363,-15.95354], "fy":[4.75707,4.76255,4.75834,4.75787]}, + {"t":2.9964, "x":12.22034, "y":5.58149, "heading":-1.0472, "vx":0.85655, "vy":-0.46835, "omega":0.0, "ax":-0.94529, "ay":0.32272, "alpha":0.0, "fx":[-15.71852,-15.75225,-15.7331,-15.82597], "fy":[5.41593,5.38365,5.35006,5.36892]}, + {"t":3.03136, "x":12.24971, "y":5.56532, "heading":-1.0472, "vx":0.82351, "vy":-0.45707, "omega":0.0, "ax":-0.93469, "ay":0.35255, "alpha":0.0, "fx":[-15.581,-15.58013,-15.5807,-15.58136], "fy":[5.87182,5.88779,5.87436,5.87342]}, + {"t":3.06632, "x":12.27793, "y":5.54955, "heading":-1.0472, "vx":0.79083, "vy":-0.44474, "omega":0.0, "ax":-0.92523, "ay":0.37691, "alpha":0.0, "fx":[-15.39135,-15.41678,-15.40243,-15.48214], "fy":[6.31763,6.28342,6.25824,6.27223]}, + {"t":3.10128, "x":12.30501, "y":5.53423, "heading":-1.0472, "vx":0.75848, "vy":-0.43157, "omega":0.0, "ax":-0.91682, "ay":0.39713, "alpha":0.0, "fx":[-15.28319,-15.28189,-15.28271,-15.28376], "fy":[6.6135,6.63452,6.61661,6.61546]}, + {"t":3.13624, "x":12.33097, "y":5.51939, "heading":-1.0472, "vx":0.72643, "vy":-0.41768, "omega":0.0, "ax":-0.90932, "ay":0.41417, "alpha":0.0, "fx":[-15.13126,-15.15108,-15.13995,-15.20935], "fy":[6.93664,6.90257,6.88305,6.89379]}, + {"t":3.1712, "x":12.35581, "y":5.50504, "heading":-1.0472, "vx":0.69464, "vy":-0.4032, "omega":0.0, "ax":-0.90262, "ay":0.4287, "alpha":0.0, "fx":[-15.04663,-15.04502,-15.04603,-15.04738], "fy":[7.13861,7.1634,7.14215,7.14084]}, + {"t":3.20616, "x":12.37954, "y":5.4912, "heading":-1.0472, "vx":0.66309, "vy":-0.38821, "omega":0.0, "ax":-0.89662, "ay":0.44123, "alpha":0.0, "fx":[-14.92364,-14.9391,-14.93048,-14.99148], "fy":[7.38582,7.35226,7.33712,7.34535]}, + {"t":3.24112, "x":12.40217, "y":5.4779, "heading":-1.0472, "vx":0.63174, "vy":-0.37279, "omega":0.0, "ax":-0.89121, "ay":0.45214, "alpha":0.0, "fx":[-14.85649,-14.85475,-14.85583,-14.85731], "fy":[7.52904,7.55504,7.53266,7.53132]}, + {"t":3.27608, "x":12.42371, "y":5.46515, "heading":-1.0472, "vx":0.60058, "vy":-0.35698, "omega":0.0, "ax":-0.88633, "ay":0.46172, "alpha":0.0, "fx":[-14.75547,-14.76752,-14.76085,-14.81497], "fy":[7.72563,7.6928,7.68108,7.68737]}, + {"t":3.31104, "x":12.44417, "y":5.45295, "heading":-1.0472, "vx":0.5696, "vy":-0.34084, "omega":0.0, "ax":-0.8819, "ay":0.4702, "alpha":0.0, "fx":[-14.70129,-14.69942,-14.70058,-14.70219], "fy":[7.82965,7.85704,7.8334,7.83201]}, + {"t":3.346, "x":12.46354, "y":5.44132, "heading":-1.0472, "vx":0.53876, "vy":-0.3244, "omega":0.0, "ax":-0.87787, "ay":0.47775, "alpha":0.0, "fx":[-14.61715,-14.62636,-14.62131,-14.66974], "fy":[7.99136,7.95911,7.95022,7.9549]}, + {"t":3.38096, "x":12.48184, "y":5.43027, "heading":-1.0472, "vx":0.50807, "vy":-0.3077, "omega":0.0, "ax":-0.87418, "ay":0.48452, "alpha":0.0, "fx":[-14.57264,-14.57069,-14.57189,-14.57359], "fy":[8.06811,8.0963,8.07192,8.07051]}, + {"t":3.41592, "x":12.49907, "y":5.41981, "heading":-1.0472, "vx":0.47751, "vy":-0.29076, "omega":0.0, "ax":-0.8708, "ay":0.49062, "alpha":0.0, "fx":[-14.50159,-14.50862,-14.50481,-14.54847], "fy":[8.20442,8.17302,8.1663,8.16976]}, + {"t":3.45088, "x":12.51523, "y":5.40994, "heading":-1.0472, "vx":0.44707, "vy":-0.27361, "omega":0.0, "ax":-0.86769, "ay":0.49614, "alpha":0.0, "fx":[-14.46449,-14.46247,-14.46371,-14.46549], "fy":[8.26165,8.29055,8.26552,8.26408]}, + {"t":3.48584, "x":12.53033, "y":5.40068, "heading":-1.0472, "vx":0.41673, "vy":-0.25626, "omega":0.0, "ax":-0.86483, "ay":0.50117, "alpha":0.0, "fx":[-14.40383,-14.40896,-14.40623,-14.44584], "fy":[8.3791,8.34834,8.34352,8.34591]}, + {"t":3.5208, "x":12.54437, "y":5.39203, "heading":-1.0472, "vx":0.3865, "vy":-0.23874, "omega":0.0, "ax":-0.86217, "ay":0.50576, "alpha":0.0, "fx":[-14.37242,-14.37037,-14.37163,-14.37344], "fy":[8.42192,8.45097,8.42577,8.42435]}, + {"t":3.55576, "x":12.55736, "y":5.38399, "heading":-1.0472, "vx":0.35636, "vy":-0.22106, "omega":0.0, "ax":-0.85971, "ay":0.50997, "alpha":0.0, "fx":[-14.32005,-14.32373,-14.32182,-14.35799], "fy":[8.52467,8.49476,8.49137,8.49295]}, + {"t":3.59072, "x":12.56929, "y":5.37657, "heading":-1.0472, "vx":0.3263, "vy":-0.20323, "omega":0.0, "ax":-0.85742, "ay":0.51384, "alpha":0.0, "fx":[-14.29316,-14.29107,-14.29235,-14.29421], "fy":[8.55659,8.58604,8.56047,8.55903]}, + {"t":3.62568, "x":12.58018, "y":5.36978, "heading":-1.0472, "vx":0.29632, "vy":-0.18527, "omega":0.0, "ax":-0.85528, "ay":0.51742, "alpha":0.0, "fx":[-14.24769,-14.24988,-14.24882,-14.28194], "fy":[8.6481,8.61854,8.61664,8.61739]}, + {"t":3.66064, "x":12.59001, "y":5.36362, "heading":-1.0472, "vx":0.26642, "vy":-0.16718, "omega":0.0, "ax":-0.85328, "ay":0.52073, "alpha":0.0, "fx":[-14.22426,-14.22214,-14.22343,-14.22533], "fy":[8.67139,8.70108,8.67528,8.67384]}, + {"t":3.6956, "x":12.59881, "y":5.3581, "heading":-1.0472, "vx":0.23659, "vy":-0.14897, "omega":0.0, "ax":-0.85141, "ay":0.52381, "alpha":0.0, "fx":[-14.18439,-14.18548,-14.18504,-14.21555], "fy":[8.75375,8.72483,8.72402,8.72416]}, + {"t":3.73056, "x":12.60656, "y":5.35321, "heading":-1.0472, "vx":0.20683, "vy":-0.13066, "omega":0.0, "ax":-0.84966, "ay":0.52668, "alpha":0.0, "fx":[-14.16383,-14.1617,-14.163,-14.16491], "fy":[8.77045,8.80012,8.77432,8.77289]}, + {"t":3.76552, "x":12.61327, "y":5.34896, "heading":-1.0472, "vx":0.17712, "vy":-0.11225, "omega":0.0, "ax":-0.84801, "ay":0.52935, "alpha":0.0, "fx":[-14.12868,-14.12876,-14.1289,-14.15708], "fy":[8.84532,8.81693,8.81712,8.8167]}, + {"t":3.80049, "x":12.61894, "y":5.34536, "heading":-1.0472, "vx":0.14748, "vy":-0.09374, "omega":0.0, "ax":-0.84645, "ay":0.53185, "alpha":0.0, "fx":[-14.11043,-14.10828,-14.10958,-14.11152], "fy":[8.8567,8.88645,8.86056,8.85913]}, + {"t":3.83545, "x":12.62358, "y":5.34241, "heading":-1.0472, "vx":0.11788, "vy":-0.07515, "omega":0.0, "ax":-0.84499, "ay":0.5342, "alpha":0.0, "fx":[-14.07933,-14.07841,-14.07911,-14.1052], "fy":[8.92552,8.89742,8.89861,8.89763]}, + {"t":3.87041, "x":12.62719, "y":5.34011, "heading":-1.0472, "vx":0.08834, "vy":-0.05647, "omega":0.0, "ax":-0.8436, "ay":0.5364, "alpha":0.0, "fx":[-14.0629,-14.06073,-14.06205,-14.06401], "fy":[8.93246,8.96236,8.93632,8.9349]}, + {"t":3.90537, "x":12.62976, "y":5.33846, "heading":-1.0472, "vx":0.05885, "vy":-0.03772, "omega":0.0, "ax":-0.84229, "ay":0.53847, "alpha":0.0, "fx":[-14.04104,-14.03887,-14.04019,-14.04216], "fy":[8.96702,8.99692,8.97087,8.96945]}, + {"t":3.94033, "x":12.6313, "y":5.33747, "heading":-1.0472, "vx":0.0294, "vy":-0.01889, "omega":0.0, "ax":-0.84105, "ay":0.54043, "alpha":0.0, "fx":[-14.01621,-14.02984,-14.01297,-14.0204], "fy":[9.00472,9.00745,9.01268,9.00969]}, + {"t":3.97529, "x":12.63181, "y":5.33714, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/OppBargeToOppG.traj b/src/main/deploy/choreo/OppBargeToOppG.traj new file mode 100644 index 0000000..667d099 --- /dev/null +++ b/src/main/deploy/choreo/OppBargeToOppG.traj @@ -0,0 +1,107 @@ +{ + "name":"OppBargeToOppG", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":9.826, "y":5.08871412277, "heading":3.141592653589793, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":10.958967590332032, "y":4.449285507202148, "heading":1.0534404506460873, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":11.698677, "y":4.3309, "heading":0.0, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"FieldMaxX - barge.x", "val":9.826}, "y":{"exp":"barge.y", "val":5.08871412277}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"10.958967590332032 m", "val":10.958967590332032}, "y":{"exp":"4.449285507202148 m", "val":4.449285507202148}, "heading":{"exp":"1.0534404506460873 rad", "val":1.0534404506460873}, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"FieldMaxX - G.x", "val":11.698677}, "y":{"exp":"FieldMaxY - G.y", "val":4.3309}, "heading":{"exp":"G.heading - 180 deg", "val":0.0}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.66398,1.8885], + "samples":[ + {"t":0.0, "x":9.826, "y":5.08871, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.96008, "ay":-1.56652, "alpha":-47.17883, "fx":[-14.43164,-2.40097,150.44109,197.12036], "fy":[197.00808,-199.06127,-130.53944,28.1399]}, + {"t":0.02656, "x":9.82775, "y":5.08816, "heading":3.14159, "vx":0.13174, "vy":-0.04161, "omega":-1.25304, "ax":5.55676, "ay":-1.66775, "alpha":-44.84832, "fx":[19.97268,2.70248,150.09696,197.74224], "fy":[195.71476,-199.00605,-130.88274,22.97192]}, + {"t":0.05312, "x":9.83321, "y":5.08647, "heading":3.10831, "vx":0.27932, "vy":-0.0859, "omega":-2.44418, "ax":6.73365, "ay":-2.24782, "alpha":-39.31745, "fx":[97.53381,5.52511,147.32887,198.59899], "fy":[170.08653,-198.86947,-133.92002,12.82252]}, + {"t":0.07968, "x":9.843, "y":5.08339, "heading":3.0434, "vx":0.45816, "vy":-0.1456, "omega":-3.48843, "ax":7.75994, "ay":-3.58542, "alpha":-32.04061, "fx":[167.9009,8.32457,142.2818,198.91069], "fy":[101.49674,-198.64772,-139.16866,-2.74926]}, + {"t":0.10624, "x":9.85791, "y":5.07826, "heading":2.95075, "vx":0.66426, "vy":-0.24083, "omega":-4.3394, "ax":8.16049, "ay":-5.18526, "alpha":-25.62305, "fx":[195.33054,16.17305,135.40136,197.22094], "fy":[22.91611,-197.95271,-145.71058,-24.99589]}, + {"t":0.1328, "x":9.87843, "y":5.07004, "heading":2.83549, "vx":0.881, "vy":-0.37854, "omega":-5.01994, "ax":8.21963, "ay":-6.87661, "alpha":-18.37108, "fx":[189.37392,40.83275,127.99787,189.86454], "fy":[-54.50215,-193.98658,-151.95201,-58.07835]}, + {"t":0.15936, "x":9.90473, "y":5.05756, "heading":2.70217, "vx":1.09931, "vy":-0.56118, "omega":-5.50786, "ax":8.05104, "ay":-8.53298, "alpha":-6.10513, "fx":[149.42199,103.18492,125.4268,158.79391], "fy":[-128.99381,-168.59013,-153.24695,-118.13215]}, + {"t":0.18592, "x":9.93676, "y":5.03964, "heading":2.55588, "vx":1.31314, "vy":-0.78781, "omega":-5.67001, "ax":6.06961, "ay":-7.94066, "alpha":23.1413, "fx":[80.50434,172.72672,183.58971,-32.111], "fy":[-180.61459,-95.66402,-59.22724,-193.96216]}, + {"t":0.21247, "x":9.97378, "y":5.01592, "heading":2.40529, "vx":1.47434, "vy":-0.99871, "omega":-5.05539, "ax":6.61412, "ay":-6.86481, "alpha":26.90399, "fx":[85.92301,176.79519,193.89812,-15.59958], "fy":[-177.51736,-87.21955,1.21092,-194.20659]}, + {"t":0.23903, "x":10.01527, "y":4.98697, "heading":2.27102, "vx":1.65001, "vy":-1.18104, "omega":-4.34084, "ax":7.17439, "ay":-6.27587, "alpha":26.37894, "fx":[91.86597,179.46597,192.39175,14.65118], "fy":[-173.74082,-80.64581,27.00689,-191.08304]}, + {"t":0.26559, "x":10.06162, "y":4.95339, "heading":2.15573, "vx":1.84056, "vy":-1.34772, "omega":-3.64023, "ax":7.58301, "ay":-5.74697, "alpha":25.15931, "fx":[93.09758,180.9567,188.79498,42.77135], "fy":[-172.07721,-75.97189,45.16989,-180.31762]}, + {"t":0.29215, "x":10.11318, "y":4.91557, "heading":2.05905, "vx":2.04196, "vy":-1.50036, "omega":-2.97202, "ax":7.90441, "ay":-5.17968, "alpha":23.79278, "fx":[89.51777,180.81112,185.08165,71.64057], "fy":[-172.55577,-74.42555,56.41299,-154.80257]}, + {"t":0.31871, "x":10.1702, "y":4.8739, "heading":1.98012, "vx":2.25189, "vy":-1.63792, "omega":-2.3401, "ax":8.34915, "ay":-4.2153, "alpha":22.44721, "fx":[85.03266,178.8367,183.0594,109.77641], "fy":[-172.54168,-75.8576,58.00833,-90.67737]}, + {"t":0.34527, "x":10.23296, "y":4.82891, "heading":1.91797, "vx":2.47364, "vy":-1.74988, "omega":-1.74391, "ax":8.57783, "ay":-3.53028, "alpha":21.37538, "fx":[87.41563,175.00604,182.35442,127.1772], "fy":[-166.67148,-77.76076,48.04102,-39.00082]}, + {"t":0.37183, "x":10.30168, "y":4.78119, "heading":1.87165, "vx":2.70146, "vy":-1.84364, "omega":-1.1762, "ax":8.33352, "ay":-2.73005, "alpha":19.5938, "fx":[104.6436,168.5363,174.07216,108.41078], "fy":[-140.11656,-70.10669,34.8483,-6.65944]}, + {"t":0.39839, "x":10.37637, "y":4.73126, "heading":1.84041, "vx":2.92279, "vy":-1.91615, "omega":-0.6558, "ax":-1.03738, "ay":4.83649, "alpha":-7.93049, "fx":[-3.62486,-42.16404,-32.26013,8.8787], "fy":[103.19156,92.01253,57.46543,69.81828]}, + {"t":0.42495, "x":10.45363, "y":4.68207, "heading":1.82299, "vx":2.89524, "vy":-1.7877, "omega":-0.86643, "ax":-6.25971, "ay":6.63592, "alpha":-20.18629, "fx":[-49.89247,-145.50006,-177.57745,-44.41531], "fy":[175.6108,115.63929,19.88306,131.33756]}, + {"t":0.45151, "x":10.52832, "y":4.63693, "heading":1.79998, "vx":2.72899, "vy":-1.61145, "omega":-1.40256, "ax":-6.4889, "ay":7.05356, "alpha":-21.17981, "fx":[-46.08572,-150.34084,-188.30421,-47.93685], "fy":[184.78749,119.95543,9.744,155.83077]}, + {"t":0.47807, "x":10.59851, "y":4.59662, "heading":1.76273, "vx":2.55665, "vy":-1.42411, "omega":-1.96508, "ax":-6.36115, "ay":7.36158, "alpha":-21.63323, "fx":[-39.31243,-149.07882,-191.87914,-43.8791], "fy":[189.30154,125.18531,10.4647,165.90454]}, + {"t":0.50463, "x":10.66417, "y":4.56139, "heading":1.71054, "vx":2.3877, "vy":-1.22859, "omega":-2.53965, "ax":-6.13425, "ay":7.53492, "alpha":-22.23316, "fx":[-29.75776,-146.22008,-193.81565,-39.2262], "fy":[192.52332,130.3455,10.43825,169.10719]}, + {"t":0.53119, "x":10.72542, "y":4.53142, "heading":1.64309, "vx":2.22478, "vy":-1.02847, "omega":-3.13015, "ax":-5.91312, "ay":7.46347, "alpha":-23.26384, "fx":[-16.73794,-142.80406,-195.22793,-39.5058], "fy":[194.9579,135.18035,6.99896,160.51292]}, + {"t":0.55775, "x":10.78242, "y":4.50674, "heading":1.55995, "vx":2.06773, "vy":-0.83025, "omega":-3.74802, "ax":-6.79032, "ay":5.94044, "alpha":-25.35902, "fx":[-4.50977,-139.48828,-196.21898,-112.54819], "fy":[196.1417,139.31867,4.16558,56.47149]}, + {"t":0.58431, "x":10.83495, "y":4.48678, "heading":1.46041, "vx":1.88738, "vy":-0.67247, "omega":-4.42154, "ax":-7.83896, "ay":4.49731, "alpha":-27.88447, "fx":[-10.74227,-138.69162,-196.60179,-176.65135], "fy":[196.09919,140.56287,11.24364,-48.03386]}, + {"t":0.61086, "x":10.88231, "y":4.47051, "heading":1.34297, "vx":1.67918, "vy":-0.55303, "omega":-5.16213, "ax":-8.7766, "ay":5.22112, "alpha":-21.72906, "fx":[-53.31663,-143.10884,-195.37231,-193.40888], "fy":[189.04784,136.34135,28.1493,-5.40424]}, + {"t":0.63742, "x":10.92381, "y":4.45766, "heading":1.20587, "vx":1.44608, "vy":-0.41436, "omega":-5.73924, "ax":-9.21523, "ay":7.45737, "alpha":0.78948, "fx":[-156.60007,-155.17004,-150.52008,-152.16369], "fy":[120.62282,122.353,128.06374,126.20374]}, + {"t":0.66398, "x":10.95897, "y":4.44929, "heading":1.05344, "vx":1.20133, "vy":-0.21629, "omega":-5.71827, "ax":-0.87447, "ay":0.43251, "alpha":33.61154, "fx":[-123.31738,12.60216,99.31385,-46.90645], "fy":[-20.22892,-108.78418,42.72587,115.12636]}, + {"t":0.69246, "x":10.99282, "y":4.4433, "heading":0.8906, "vx":1.17643, "vy":-0.20398, "omega":-4.76112, "ax":-0.94333, "ay":0.31044, "alpha":27.99604, "fx":[-108.44043,-8.34346,82.81384,-28.92906], "fy":[-4.32139,-92.64287,18.30847,99.35517]}, + {"t":0.72094, "x":11.02594, "y":4.43762, "heading":0.75502, "vx":1.14957, "vy":-0.19514, "omega":-3.96387, "ax":-0.96094, "ay":0.25895, "alpha":23.40936, "fx":[-94.02924,-19.89592,66.4692,-16.61792], "fy":[6.01007,-77.28959,3.86418,84.68154]}, + {"t":0.74941, "x":11.05829, "y":4.43217, "heading":0.64214, "vx":1.1222, "vy":-0.18776, "omega":-3.29724, "ax":-0.96911, "ay":0.23139, "alpha":19.46, "fx":[-80.87239,-26.25443,51.79265,-9.28435], "fy":[11.97154,-62.88005,-4.19255,70.52951]}, + {"t":0.77789, "x":11.08985, "y":4.42691, "heading":0.54824, "vx":1.09461, "vy":-0.18117, "omega":-2.74308, "ax":-0.97376, "ay":0.21426, "alpha":16.19306, "fx":[-69.37289,-29.14179,39.08632,-5.4999], "fy":[15.0979,-51.08362,-8.31057,58.5829]}, + {"t":0.80637, "x":11.12063, "y":4.42184, "heading":0.47013, "vx":1.06688, "vy":-0.17507, "omega":-2.28195, "ax":-0.97673, "ay":0.2026, "alpha":13.31105, "fx":[-59.41236,-30.08061,28.26595,-3.89979], "fy":[16.0413,-40.50785,-9.76218,47.73778]}, + {"t":0.83485, "x":11.15061, "y":4.41694, "heading":0.40515, "vx":1.03906, "vy":-0.1693, "omega":-1.90289, "ax":-0.9788, "ay":0.19415, "alpha":11.07148, "fx":[-51.54298,-29.7796,19.8784,-3.82028], "fy":[15.96512,-32.5961,-9.93118,39.50784]}, + {"t":0.86332, "x":11.17981, "y":4.4122, "heading":0.35096, "vx":1.01119, "vy":-0.16377, "omega":-1.58761, "ax":-0.98031, "ay":0.18775, "alpha":9.04037, "fx":[-44.62831,-28.82524,12.53653,-4.44831], "fy":[15.05059,-25.48335,-9.15215,32.10369]}, + {"t":0.8918, "x":11.20821, "y":4.40761, "heading":0.30575, "vx":0.98327, "vy":-0.15843, "omega":-1.33017, "ax":-0.98147, "ay":0.18273, "alpha":7.50329, "fx":[-39.4904,-27.54956,7.19191,-5.59425], "fy":[13.86931,-20.32423,-8.04653,26.68564]}, + {"t":0.92028, "x":11.23581, "y":4.40317, "heading":0.26787, "vx":0.95532, "vy":-0.15322, "omega":-1.1165, "ax":-0.98238, "ay":0.17869, "alpha":6.16213, "fx":[-35.00044,-26.28164,2.50316,-6.72402], "fy":[12.63869,-15.87771,-6.89285,22.04678]}, + {"t":0.94875, "x":11.26261, "y":4.39888, "heading":0.23607, "vx":0.92735, "vy":-0.14813, "omega":-0.94102, "ax":-0.98311, "ay":0.17537, "alpha":5.03285, "fx":[-31.48734,-24.77035,-1.1054,-8.18883], "fy":[11.15822,-12.31002,-5.44184,18.28716]}, + {"t":0.97723, "x":11.28862, "y":4.39473, "heading":0.20928, "vx":0.89935, "vy":-0.14314, "omega":-0.7977, "ax":-0.98372, "ay":0.17259, "alpha":4.23914, "fx":[-28.82382,-23.81694,-3.85496,-9.09658], "fy":[10.23237,-9.80218,-4.59471,15.6728]}, + {"t":1.00571, "x":11.31384, "y":4.39073, "heading":0.18656, "vx":0.87134, "vy":-0.13823, "omega":-0.67698, "ax":-0.98422, "ay":0.17024, "alpha":3.33804, "fx":[-26.20725,-22.30211,-6.52679,-10.58999], "fy":[8.7279,-7.09649,-3.10649,12.82612]}, + {"t":1.03418, "x":11.33825, "y":4.38686, "heading":0.16728, "vx":0.84331, "vy":-0.13338, "omega":-0.58192, "ax":-0.98466, "ay":0.16821, "alpha":2.93917, "fx":[-24.81836,-21.78669,-7.95099,-11.09888], "fy":[8.22291,-5.8984,-2.67582,11.56719]}, + {"t":1.06266, "x":11.36187, "y":4.38313, "heading":0.15071, "vx":0.81527, "vy":-0.12859, "omega":-0.49822, "ax":-0.98503, "ay":0.16645, "alpha":2.19903, "fx":[-22.76577,-20.41425,-10.03919,-12.46046], "fy":[6.84926,-3.72545,-1.32086,9.29552]}, + {"t":1.09114, "x":11.38468, "y":4.37953, "heading":0.13652, "vx":0.78722, "vy":-0.12385, "omega":-0.4356, "ax":-0.98535, "ay":0.1649, "alpha":2.03984, "fx":[-22.16112,-20.20354,-10.65973,-12.67681], "fy":[6.63106,-3.31095,-1.16182,8.83722]}, + {"t":1.11962, "x":11.4067, "y":4.37607, "heading":0.12412, "vx":0.75916, "vy":-0.11915, "omega":-0.37751, "ax":-0.98563, "ay":0.16354, "alpha":1.45431, "fx":[-20.56143,-19.10995,-12.28327,-13.76544], "fy":[5.52295,-1.5642,-0.07916,7.02493]}, + {"t":1.14809, "x":11.42792, "y":4.37275, "heading":0.11337, "vx":0.73109, "vy":-0.11449, "omega":-0.3361, "ax":-0.98588, "ay":0.16232, "alpha":1.4052, "fx":[-20.35932,-19.00332,-12.49434,-13.87982], "fy":[5.39721,-1.54203,0.00471,6.96358]}, + {"t":1.17657, "x":11.44834, "y":4.36955, "heading":0.1038, "vx":0.70302, "vy":-0.10987, "omega":-0.29608, "ax":-0.98611, "ay":0.16124, "alpha":0.97859, "fx":[-19.16783,-18.27575,-13.7012,-14.60689], "fy":[4.63798,-0.18449,0.73333,5.56406]}, + {"t":1.20505, "x":11.46796, "y":4.36649, "heading":0.09537, "vx":0.67494, "vy":-0.10528, "omega":-0.26821, "ax":-0.98631, "ay":0.16025, "alpha":0.95412, "fx":[-19.11966,-18.12179,-13.75655,-14.76702], "fy":[4.46506,-0.30355,0.87774,5.64621]}, + {"t":1.23352, "x":11.48678, "y":4.36356, "heading":0.08773, "vx":0.64685, "vy":-0.10072, "omega":-0.24104, "ax":-0.98649, "ay":0.15937, "alpha":0.67692, "fx":[-18.2885,-17.76295,-14.59712,-15.12847], "fy":[4.05897,0.70023,1.25159,4.6154]}, + {"t":1.262, "x":11.5048, "y":4.36075, "heading":0.08086, "vx":0.61876, "vy":-0.09618, "omega":-0.22177, "ax":-0.98665, "ay":0.15856, "alpha":0.6357, "fx":[-18.26045,-17.50354,-14.63183,-15.39211], "fy":[3.78897,0.57819,1.50121,4.70389]}, + {"t":1.29048, "x":11.52202, "y":4.35808, "heading":0.07455, "vx":0.59066, "vy":-0.09166, "omega":-0.20366, "ax":-0.9868, "ay":0.15782, "alpha":0.48283, "fx":[-17.72748,-17.44224,-15.17081,-15.45731], "fy":[3.67272,1.27391,1.58688,3.98946]}, + {"t":1.31895, "x":11.53844, "y":4.35553, "heading":0.06875, "vx":0.56256, "vy":-0.08717, "omega":-0.18991, "ax":-0.98693, "ay":0.15714, "alpha":0.41577, "fx":[-17.66514,-17.09819,-15.23866,-15.80491], "fy":[3.32752,1.21053,1.91592,4.02379]}, + {"t":1.34743, "x":11.55406, "y":4.35311, "heading":0.06334, "vx":0.53445, "vy":-0.08269, "omega":-0.17807, "ax":-0.98706, "ay":0.15652, "alpha":0.35409, "fx":[-17.36228,-17.22656,-15.54642,-15.67995], "fy":[3.4039,1.64955,1.81259,3.57009]}, + {"t":1.37591, "x":11.56888, "y":4.35082, "heading":0.05827, "vx":0.50634, "vy":-0.07824, "omega":-0.16799, "ax":-0.98717, "ay":0.15594, "alpha":0.27028, "fx":[-17.25938,-16.8588,-15.65283,-16.05186], "fy":[3.03997,1.6582,2.16258,3.53691]}, + {"t":1.40439, "x":11.5829, "y":4.34866, "heading":0.05348, "vx":0.47823, "vy":-0.0738, "omega":-0.16029, "ax":-0.98728, "ay":0.1554, "alpha":0.26663, "fx":[-17.12028,-17.07037,-15.79719,-15.8421], "fy":[3.21171,1.89435,1.96794,3.28802]}, + {"t":1.43286, "x":11.59612, "y":4.34662, "heading":0.04892, "vx":0.45012, "vy":-0.06937, "omega":-0.1527, "ax":-0.98738, "ay":0.15491, "alpha":0.1819, "fx":[-16.99703,-16.74333,-15.92166,-16.17448], "fy":[2.88624,1.95983,2.28071,3.20214]}, + {"t":1.46134, "x":11.60853, "y":4.34471, "heading":0.04457, "vx":0.422, "vy":-0.06496, "omega":-0.14752, "ax":-0.98747, "ay":0.15444, "alpha":0.20865, "fx":[-16.96176,-16.95867,-15.96284,-15.95931], "fy":[3.07958,2.04647,2.06827,3.10376]}, + {"t":1.48982, "x":11.62015, "y":4.34292, "heading":0.04037, "vx":0.39388, "vy":-0.06056, "omega":-0.14158, "ax":-0.98755, "ay":0.15401, "alpha":0.13811, "fx":[-16.85012,-16.71762,-16.07403,-16.20648], "fy":[2.83063,2.13846,2.30541,2.99477]}, + {"t":1.51829, "x":11.63097, "y":4.34126, "heading":0.03634, "vx":0.36576, "vy":-0.05618, "omega":-0.13765, "ax":-0.98763, "ay":0.15361, "alpha":0.17622, "fx":[-16.87024,-16.89314,-16.06055,-16.02962], "fy":[3.00539,2.12411,2.11468,2.99814]}, + {"t":1.54677, "x":11.64098, "y":4.33972, "heading":0.03242, "vx":0.33763, "vy":-0.0518, "omega":-0.13263, "ax":-0.98771, "ay":0.15323, "alpha":0.13061, "fx":[-16.80234,-16.75899,-16.12683,-16.17035], "fy":[2.84701,2.20863,2.2621,2.89929]}, + {"t":1.57525, "x":11.6502, "y":4.33831, "heading":0.02864, "vx":0.30951, "vy":-0.04744, "omega":-0.12891, "ax":-0.98778, "ay":0.15287, "alpha":0.17083, "fx":[-16.84591,-16.88562,-16.09001,-16.04162], "fy":[2.99559,2.12978,2.09993,2.96797]}, + {"t":1.60372, "x":11.65861, "y":4.33702, "heading":0.02497, "vx":0.28138, "vy":-0.04308, "omega":-0.12404, "ax":-0.98784, "ay":0.15254, "alpha":0.15577, "fx":[-16.84688,-16.85925,-16.08704,-16.07436], "fy":[2.92395,2.17805,2.1612,2.90769]}, + {"t":1.6322, "x":11.66622, "y":4.33585, "heading":0.02144, "vx":0.25325, "vy":-0.03874, "omega":-0.11961, "ax":-0.98791, "ay":0.15222, "alpha":0.19819, "fx":[-16.90324,-16.95258,-16.03726,-15.97858], "fy":[3.06371,2.05186,2.00993,3.02429]}, + {"t":1.66068, "x":11.67303, "y":4.33481, "heading":0.01803, "vx":0.22511, "vy":-0.03441, "omega":-0.11396, "ax":-0.98796, "ay":0.15192, "alpha":0.21593, "fx":[-16.98849,-17.02711,-15.94991,-15.91003], "fy":[3.06903,2.04297,1.99453,3.02332]}, + {"t":1.68916, "x":11.67904, "y":4.33389, "heading":0.01479, "vx":0.19698, "vy":-0.03008, "omega":-0.10782, "ax":-0.98802, "ay":0.15164, "alpha":0.26855, "fx":[-17.0689,-17.11908,-15.87533,-15.81588], "fy":[3.23276,1.86501,1.82123,3.19199]}, + {"t":1.71763, "x":11.68425, "y":4.3331, "heading":0.01172, "vx":0.16884, "vy":-0.02576, "omega":-0.10017, "ax":-0.98807, "ay":0.15137, "alpha":0.32116, "fx":[-17.24846,-17.29094,-15.69394,-15.64933], "fy":[3.3114,1.78297,1.73192,3.26682]}, + {"t":1.74611, "x":11.68866, "y":4.33243, "heading":0.00886, "vx":0.14071, "vy":-0.02145, "omega":-0.09102, "ax":-0.98812, "ay":0.15112, "alpha":0.39797, "fx":[-17.38337,-17.42332,-15.56397,-15.51529], "fy":[3.54084,1.52891,1.49536,3.51103]}, + {"t":1.77459, "x":11.69227, "y":4.33188, "heading":0.00627, "vx":0.11257, "vy":-0.01715, "omega":-0.07969, "ax":-0.98817, "ay":0.15087, "alpha":0.49169, "fx":[-17.67133,-17.70379,-15.27433,-15.23963], "fy":[3.70593,1.35312,1.31741,3.68355]}, + {"t":1.80306, "x":11.69507, "y":4.33145, "heading":0.004, "vx":0.08443, "vy":-0.01285, "omega":-0.06569, "ax":-0.98821, "ay":0.15064, "alpha":0.61142, "fx":[-17.90545,-17.9278,-15.04422,-15.01457], "fy":[4.05004,0.98378,0.96973,4.04113]}, + {"t":1.83154, "x":11.69707, "y":4.33114, "heading":0.00213, "vx":0.05629, "vy":-0.00856, "omega":-0.04828, "ax":-0.98825, "ay":0.15043, "alpha":0.76096, "fx":[-18.33268,-18.35185,-14.61514,-14.5952], "fy":[4.33995,0.67816,0.66138,4.35057]}, + {"t":1.86002, "x":11.69828, "y":4.33096, "heading":0.00076, "vx":0.02814, "vy":-0.00428, "omega":-0.02661, "ax":-0.98829, "ay":0.15022, "alpha":0.93433, "fx":[-18.72271,-18.74086,-14.22854,-14.20544], "fy":[4.78209,0.21599,0.20554,4.81251]}, + {"t":1.8885, "x":11.69868, "y":4.3309, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/OppEToOppbarge.traj b/src/main/deploy/choreo/OppEToOppbarge.traj new file mode 100644 index 0000000..c09d1c1 --- /dev/null +++ b/src/main/deploy/choreo/OppEToOppbarge.traj @@ -0,0 +1,117 @@ +{ + "name":"OppEToOppbarge", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":12.631814748155, "y":5.3371419902636, "heading":-1.047197551196598, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":10.687749862670898, "y":5.166319847106934, "heading":-2.864219521109, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.826, "y":5.08871412277, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":3.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":2.0}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"FieldMaxX - E.x", "val":12.631814748155}, "y":{"exp":"FieldMaxY - E.y", "val":5.3371419902636}, "heading":{"exp":"E.heading - 180 deg", "val":-1.047197551196598}, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"10.687749862670898 m", "val":10.687749862670898}, "y":{"exp":"5.166319847106934 m", "val":5.166319847106934}, "heading":{"exp":"-2.8642195211090002 rad", "val":-2.864219521109}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"FieldMaxX - barge.x", "val":9.826}, "y":{"exp":"barge.y", "val":5.08871412277}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"3 m / s ^ 2", "val":3.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"2 rad / s", "val":2.0}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.93839,1.69813], + "samples":[ + {"t":0.0, "x":12.63181, "y":5.33714, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.5815, "ay":-0.48008, "alpha":-54.95768, "fx":[-113.15872,173.81349,-36.39012,-196.3939], "fy":[-163.53734,-95.58569,195.36742,31.7447]}, + {"t":0.0204, "x":12.63128, "y":5.33704, "heading":-1.0472, "vx":-0.05266, "vy":-0.00979, "omega":-1.12112, "ax":-5.84702, "ay":-1.17728, "alpha":-43.01302, "fx":[-158.13421,64.86486,-102.63891,-193.95976], "fy":[-117.44467,-166.27889,167.69816,37.52676]}, + {"t":0.0408, "x":12.62899, "y":5.3366, "heading":-1.07007, "vx":-0.17194, "vy":-0.03381, "omega":-1.99858, "ax":-5.95911, "ay":-0.45555, "alpha":-0.01503, "fx":[-97.3794,-104.35161,-102.52483,-93.08643], "fy":[-10.69231,-13.36205,0.33058,-6.65154]}, + {"t":0.0612, "x":12.62424, "y":5.33581, "heading":-1.11084, "vx":-0.2935, "vy":-0.0431, "omega":-1.99888, "ax":-5.95802, "ay":-0.45503, "alpha":0.0, "fx":[-99.13258,-99.76206,-99.28788,-99.08686], "fy":[-7.86954,-7.76085,-7.24025,-7.46972]}, + {"t":0.0816, "x":12.61701, "y":5.33484, "heading":-1.15162, "vx":-0.41505, "vy":-0.05239, "omega":-1.99888, "ax":-5.95688, "ay":-0.45364, "alpha":0.0, "fx":[-99.25809,-99.38276,-99.33889,-99.21347], "fy":[-7.59949,-7.67628,-7.46335,-7.50905]}, + {"t":0.102, "x":12.60731, "y":5.33368, "heading":-1.19239, "vx":-0.53656, "vy":-0.06164, "omega":-1.99888, "ax":-5.95557, "ay":-0.45278, "alpha":0.0, "fx":[-99.17273,-99.59843,-99.09396,-99.24056], "fy":[-7.69857,-7.58684,-7.41736,-7.48777]}, + {"t":0.1224, "x":12.59512, "y":5.33232, "heading":-1.23317, "vx":-0.65806, "vy":-0.07088, "omega":-1.99888, "ax":-5.95415, "ay":-0.4512, "alpha":0.0, "fx":[-99.24721,-99.26425,-99.25927,-99.24083], "fy":[-7.48254,-7.60177,-7.50337,-7.49761]}, + {"t":0.1428, "x":12.58046, "y":5.33078, "heading":-1.27395, "vx":-0.77952, "vy":-0.08008, "omega":-1.99888, "ax":-5.95253, "ay":-0.45, "alpha":0.0, "fx":[-99.10797,-99.61975,-99.00693,-99.16889], "fy":[-7.67633,-7.5566,-7.35125,-7.421]}, + {"t":0.1632, "x":12.56332, "y":5.32906, "heading":-1.31472, "vx":-0.90095, "vy":-0.08926, "omega":-1.99888, "ax":-5.95075, "ay":-0.44816, "alpha":0.0, "fx":[-99.18708,-99.21075,-99.20606,-99.18063], "fy":[-7.4264,-7.55868,-7.45109,-7.44605]}, + {"t":0.1836, "x":12.5437, "y":5.32714, "heading":-1.3555, "vx":-1.02234, "vy":-0.0984, "omega":-1.99888, "ax":-5.9487, "ay":-0.44646, "alpha":0.0, "fx":[-99.03413,-99.62038,-98.9084,-99.08481], "fy":[-7.62721,-7.51641,-7.2852,-7.34024]}, + {"t":0.204, "x":12.52161, "y":5.32504, "heading":-1.39628, "vx":-1.1437, "vy":-0.10751, "omega":-1.99888, "ax":-5.94637, "ay":-0.44425, "alpha":0.0, "fx":[-99.1111,-99.13958,-99.13638,-99.10568], "fy":[-7.35252,-7.50245,-7.38587,-7.38115]}, + {"t":0.2244, "x":12.49704, "y":5.32276, "heading":-1.43705, "vx":-1.265, "vy":-0.11657, "omega":-1.99888, "ax":-5.94369, "ay":-0.44178, "alpha":0.0, "fx":[-98.93714,-99.62685,-98.77301,-98.97676], "fy":[-7.55661,-7.4608,-7.20216,-7.23738]}, + {"t":0.2448, "x":12.47, "y":5.32029, "heading":-1.47783, "vx":-1.38625, "vy":-0.12559, "omega":-1.99888, "ax":-5.94055, "ay":-0.43908, "alpha":0.0, "fx":[-99.01314,-99.04122,-99.04045,-99.0094], "fy":[-7.25102,-7.42546,-7.30214,-7.29819]}, + {"t":0.2652, "x":12.44048, "y":5.31763, "heading":-1.51861, "vx":-1.50744, "vy":-0.13454, "omega":-1.99888, "ax":-5.93686, "ay":-0.43554, "alpha":0.0, "fx":[-98.84543,-99.49243,-98.65748,-98.86329], "fy":[-7.4043,-7.3568,-7.13633,-7.14379]}, + {"t":0.2856, "x":12.40849, "y":5.3148, "heading":-1.55938, "vx":-1.62855, "vy":-0.14343, "omega":-1.99888, "ax":-5.93241, "ay":-0.4319, "alpha":0.0, "fx":[-98.87591,-98.9051,-98.90645,-98.87402], "fy":[-7.11971,-7.31146,-7.18382,-7.18357]}, + {"t":0.306, "x":12.37404, "y":5.31178, "heading":-1.60016, "vx":-1.74956, "vy":-0.15224, "omega":-1.99888, "ax":-5.92707, "ay":-0.4262, "alpha":0.0, "fx":[-98.67621,-99.39731,-98.45769,-98.67423], "fy":[-7.23148,-7.21621,-6.9905,-6.98019]}, + {"t":0.3264, "x":12.33711, "y":5.30859, "heading":-1.64094, "vx":-1.87048, "vy":-0.16093, "omega":-1.99888, "ax":-5.92025, "ay":-0.42137, "alpha":0.0, "fx":[-98.66914,-98.7039,-98.70798,-98.66998], "fy":[-6.93547,-7.14197,-7.00631,-7.01218]}, + {"t":0.3468, "x":12.29772, "y":5.30522, "heading":-1.68171, "vx":-1.99125, "vy":-0.16953, "omega":-1.99888, "ax":-5.91176, "ay":-0.41205, "alpha":0.0, "fx":[-98.36308,-99.42469,-98.05834,-98.33866], "fy":[-7.0234,-7.02471,-6.7342,-6.69239]}, + {"t":0.3672, "x":12.25587, "y":5.30167, "heading":-1.72249, "vx":-2.11185, "vy":-0.17793, "omega":-1.99888, "ax":-5.90017, "ay":-0.40473, "alpha":0.00001, "fx":[-98.32816,-98.37125,-98.37927,-98.33323], "fy":[-6.65045,-6.87251,-6.72484,-6.7389]}, + {"t":0.38759, "x":12.21156, "y":5.29796, "heading":-1.76327, "vx":-2.23221, "vy":-0.18619, "omega":-1.99888, "ax":-5.88469, "ay":-0.38655, "alpha":0.00001, "fx":[-97.81475,-99.43003,-97.38381,-97.75093], "fy":[-6.62531,-6.66738,-6.28529,-6.19614]}, + {"t":0.40799, "x":12.1648, "y":5.29408, "heading":-1.80404, "vx":-2.35225, "vy":-0.19408, "omega":-1.99888, "ax":-5.86077, "ay":-0.37687, "alpha":0.00001, "fx":[-97.68012,-97.70635,-97.71315,-97.68505], "fy":[-6.21609,-6.36136,-6.26739,-6.28381]}, + {"t":0.42839, "x":12.1156, "y":5.29004, "heading":-1.84482, "vx":-2.47181, "vy":-0.20176, "omega":-1.99888, "ax":-5.82371, "ay":-0.33897, "alpha":0.00002, "fx":[-96.79503,-98.38055,-96.4204,-96.71755], "fy":[-5.78598,-5.8492,-5.53542,-5.43152]}, + {"t":0.44879, "x":12.06396, "y":5.28586, "heading":-1.8856, "vx":-2.59061, "vy":-0.20868, "omega":-1.99888, "ax":-5.75131, "ay":-0.32178, "alpha":0.00004, "fx":[-95.86478,-95.87565,-95.87895,-95.86715], "fy":[-5.32436,-5.40482,-5.35551,-5.37109]}, + {"t":0.46919, "x":12.00992, "y":5.28153, "heading":-1.92637, "vx":-2.70794, "vy":-0.21524, "omega":-1.99888, "ax":-5.55752, "ay":-0.23426, "alpha":0.00012, "fx":[-92.2775,-94.26031,-91.86055,-92.1664], "fy":[-4.01525,-4.12233,-3.80434,-3.67791]}, + {"t":0.48959, "x":11.95352, "y":5.27709, "heading":-1.96715, "vx":-2.82131, "vy":-0.22002, "omega":-1.99888, "ax":-3.46457, "ay":-0.06538, "alpha":0.00052, "fx":[-57.76033,-57.75017,-57.74503,-57.7551], "fy":[-1.08626,-1.07844,-1.09417,-1.10025]}, + {"t":0.50999, "x":11.89525, "y":5.27259, "heading":-2.00793, "vx":-2.89199, "vy":-0.22136, "omega":-1.99887, "ax":-0.84645, "ay":0.04635, "alpha":0.00162, "fx":[-14.03935,-14.38736,-13.98932,-14.0233], "fy":[0.76244,0.74731,0.7819,0.79867]}, + {"t":0.53039, "x":11.83607, "y":5.26809, "heading":-2.0487, "vx":-2.90925, "vy":-0.22041, "omega":-1.99883, "ax":0.15602, "ay":0.11524, "alpha":0.00064, "fx":[2.58443,2.6056,2.61704,2.59586], "fy":[1.90291,1.95984,1.90793,1.91327]}, + {"t":0.55079, "x":11.77676, "y":5.26361, "heading":-2.08948, "vx":-2.90607, "vy":-0.21806, "omega":-1.99882, "ax":0.34494, "ay":-0.08095, "alpha":-0.0006, "fx":[5.7169,5.87032,5.70133,5.71142], "fy":[-1.34686,-1.34036,-1.35182,-1.35822]}, + {"t":0.57119, "x":11.71755, "y":5.25915, "heading":-2.13025, "vx":-2.89903, "vy":-0.21971, "omega":-1.99883, "ax":0.1928, "ay":-0.21957, "alpha":0.00001, "fx":[3.23731,3.20835,3.19034,3.21929], "fy":[-3.62325,-3.71578,-3.63812,-3.66306]}, + {"t":0.59159, "x":11.65845, "y":5.25462, "heading":-2.17103, "vx":-2.8951, "vy":-0.22419, "omega":-1.99883, "ax":0.13821, "ay":-0.28268, "alpha":0.00003, "fx":[2.22149,2.606,2.18258,2.20571], "fy":[-4.70874,-4.6944,-4.71538,-4.72971]}, + {"t":0.61199, "x":11.59942, "y":5.24999, "heading":-2.2118, "vx":-2.89228, "vy":-0.22996, "omega":-1.99883, "ax":0.08574, "ay":-0.4002, "alpha":0.00004, "fx":[1.48004,1.42191,1.37861,1.43674], "fy":[-6.60846,-6.77835,-6.62486,-6.67288]}, + {"t":0.63239, "x":11.54043, "y":5.24521, "heading":-2.25258, "vx":-2.89053, "vy":-0.23812, "omega":-1.99883, "ax":0.03352, "ay":-0.38689, "alpha":0.0, "fx":[0.52885,0.65939,0.52131,0.52546], "fy":[-6.44893,-6.44609,-6.44958,-6.45242]}, + {"t":0.65279, "x":11.48147, "y":5.24028, "heading":-2.29336, "vx":-2.88985, "vy":-0.24601, "omega":-1.99883, "ax":-0.02731, "ay":-0.45408, "alpha":0.00001, "fx":[-0.38677,-0.4596,-0.5237,-0.45086], "fy":[-7.49652,-7.70667,-7.5052,-7.56878]}, + {"t":0.67319, "x":11.42252, "y":5.23516, "heading":-2.33413, "vx":-2.89041, "vy":-0.25528, "omega":-1.99883, "ax":-0.08608, "ay":-0.39011, "alpha":-0.00001, "fx":[-1.3764,-1.61744,-1.37173,-1.37411], "fy":[-6.503,-6.50341,-6.50298,-6.50255]}, + {"t":0.69359, "x":11.36354, "y":5.22987, "heading":-2.37491, "vx":-2.89216, "vy":-0.26323, "omega":-1.99883, "ax":-0.14792, "ay":-0.39603, "alpha":-0.00002, "fx":[-2.39807,-2.46457,-2.53347,-2.46695], "fy":[-6.53841,-6.73375,-6.53634,-6.59801]}, + {"t":0.71399, "x":11.30451, "y":5.22442, "heading":-2.41568, "vx":-2.89518, "vy":-0.27131, "omega":-1.99883, "ax":-0.20503, "ay":-0.28979, "alpha":-0.00005, "fx":[-3.26598,-3.85028,-3.28081,-3.27378], "fy":[-4.83124,-4.82133,-4.83019,-4.84009]}, + {"t":0.73439, "x":11.2454, "y":5.21883, "heading":-2.45646, "vx":-2.89936, "vy":-0.27722, "omega":-1.99883, "ax":-0.27443, "ay":-0.22983, "alpha":-0.00002, "fx":[-4.53298,-4.57054,-4.61638,-4.57882], "fy":[-3.79687,-3.91109,-3.79066,-3.82598]}, + {"t":0.75479, "x":11.1862, "y":5.21312, "heading":-2.49723, "vx":-2.90496, "vy":-0.28191, "omega":-1.99883, "ax":-0.44533, "ay":-0.08019, "alpha":0.00059, "fx":[-7.33116,-7.66566,-7.35309,-7.34361], "fy":[-1.33886,-1.32104,-1.3345,-1.35244]}, + {"t":0.77519, "x":11.12685, "y":5.20736, "heading":-2.53801, "vx":-2.91405, "vy":-0.28355, "omega":-1.99882, "ax":-0.34357, "ay":0.04587, "alpha":-0.00033, "fx":[-5.73052,-5.72762,-5.72371,-5.72661], "fy":[0.75934,0.7745,0.75345,0.77103]}, + {"t":0.79559, "x":11.06733, "y":5.20158, "heading":-2.57878, "vx":-2.92105, "vy":-0.28261, "omega":-1.99883, "ax":0.41294, "ay":0.25988, "alpha":-0.00168, "fx":[6.83648,6.96618,6.87273,6.85855], "fy":[4.3342,4.32592,4.3311,4.33708]}, + {"t":0.81599, "x":11.00783, "y":5.19587, "heading":-2.61956, "vx":-2.91263, "vy":-0.27731, "omega":-1.99886, "ax":3.12012, "ay":0.50762, "alpha":-0.00067, "fx":[51.95257,51.99532,52.06982,52.02611], "fy":[8.41076,8.60167,8.3623,8.4726]}, + {"t":0.83639, "x":10.94906, "y":5.19032, "heading":-2.66034, "vx":-2.84898, "vy":-0.26696, "omega":-1.99888, "ax":5.39111, "ay":0.60959, "alpha":-0.00014, "fx":[89.15833,91.48427,89.46646,89.3595], "fy":[10.21918,9.98975,10.11815,10.31889]}, + {"t":0.85679, "x":10.89206, "y":5.185, "heading":-2.70111, "vx":-2.739, "vy":-0.25452, "omega":-1.99888, "ax":5.69813, "ay":0.675, "alpha":-0.00004, "fx":[94.90636,94.95428,95.06562,95.01412], "fy":[11.19741,11.43219,11.14488,11.23317]}, + {"t":0.87719, "x":10.83737, "y":5.17995, "heading":-2.74189, "vx":-2.62276, "vy":-0.24075, "omega":-1.99888, "ax":5.79304, "ay":0.58529, "alpha":-0.00002, "fx":[96.08037,97.85399,96.18105,96.15322], "fy":[9.86069,9.48586,9.67761,10.00177]}, + {"t":0.89759, "x":10.78507, "y":5.17516, "heading":-2.78267, "vx":-2.50459, "vy":-0.22881, "omega":-1.99888, "ax":5.83512, "ay":0.60686, "alpha":0.00024, "fx":[96.88617,97.06858,97.63794,97.4819], "fy":[9.98636,10.51904,10.25925,9.69986]}, + {"t":0.91799, "x":10.73519, "y":5.17062, "heading":-2.82344, "vx":-2.38555, "vy":-0.21643, "omega":-1.99888, "ax":5.86413, "ay":0.5663, "alpha":18.6351, "fx":[142.19376,123.58806,45.56092,79.66626], "fy":[-19.04498,65.82536,56.02498,-65.04587]}, + {"t":0.93839, "x":10.68775, "y":5.16632, "heading":-2.86422, "vx":-2.26593, "vy":-0.20488, "omega":-1.61872, "ax":2.92701, "ay":0.27696, "alpha":12.06761, "fx":[83.3165,68.65568,12.96329,30.23196], "fy":[-15.27212,40.91295,26.93841,-34.11188]}, + {"t":0.96761, "x":10.62279, "y":5.16045, "heading":-2.91152, "vx":-2.1804, "vy":-0.19679, "omega":-1.2661, "ax":2.9732, "ay":0.27372, "alpha":8.84905, "fx":[74.06629,64.74297,23.73634,35.7014], "fy":[-11.48917,30.73722,22.37037,-23.36741]}, + {"t":0.99683, "x":10.56034, "y":5.15482, "heading":-2.94852, "vx":-2.09352, "vy":-0.18879, "omega":-1.00752, "ax":2.97829, "ay":0.2718, "alpha":6.70679, "fx":[67.86118,61.83026,31.11575,37.77938], "fy":[-8.5786,24.1175,18.60652,-16.02253]}, + {"t":1.02605, "x":10.50044, "y":5.14942, "heading":-2.97796, "vx":-2.00649, "vy":-0.18085, "omega":-0.81154, "ax":2.98077, "ay":0.27086, "alpha":5.10159, "fx":[63.27346,59.17349,35.39947,40.90584], "fy":[-5.94947,19.20572,15.63051,-10.8264]}, + {"t":1.05527, "x":10.44308, "y":5.14425, "heading":-3.00167, "vx":-1.91939, "vy":-0.17293, "omega":-0.66247, "ax":2.98225, "ay":0.27031, "alpha":3.9533, "fx":[60.102,57.3702,39.24646,42.13174], "fy":[-3.94917,15.77846,13.28332,-7.08904]}, + {"t":1.08449, "x":10.38827, "y":5.13931, "heading":-3.02103, "vx":-1.83224, "vy":-0.16503, "omega":-0.54695, "ax":2.98322, "ay":0.26994, "alpha":3.04721, "fx":[57.59825,55.68224,41.42541,44.2094], "fy":[-2.19906,13.09955,11.46222,-4.3637]}, + {"t":1.11371, "x":10.336, "y":5.1346, "heading":-3.03701, "vx":-1.74507, "vy":-0.15714, "omega":-0.45791, "ax":2.98391, "ay":0.26968, "alpha":2.4044, "fx":[55.89911,54.59031,43.56192,44.91007], "fy":[-0.93367,11.22635,10.02975,-2.34077]}, + {"t":1.14293, "x":10.28628, "y":5.13013, "heading":-3.05039, "vx":-1.65788, "vy":-0.14926, "omega":-0.38765, "ax":2.98443, "ay":0.26948, "alpha":1.87281, "fx":[54.46453,53.52032,44.71975,46.29126], "fy":[0.18658,9.70282,8.91603,-0.83681]}, + {"t":1.17215, "x":10.23911, "y":5.12588, "heading":-3.06172, "vx":-1.57067, "vy":-0.14139, "omega":-0.33292, "ax":2.98483, "ay":0.26933, "alpha":1.50348, "fx":[53.52864,52.87018,45.9777,46.64605], "fy":[0.97036,8.64492,8.03512,0.30817]}, + {"t":1.20138, "x":10.19449, "y":5.12186, "heading":-3.07145, "vx":-1.48345, "vy":-0.13352, "omega":-0.28899, "ax":2.98515, "ay":0.26921, "alpha":1.18168, "fx":[52.67364,52.18727,46.6048,47.57817], "fy":[1.68902,7.74154,7.35061,1.16933]}, + {"t":1.2306, "x":10.15242, "y":5.11808, "heading":-3.07989, "vx":-1.39622, "vy":-0.12565, "omega":-0.25446, "ax":2.98541, "ay":0.26911, "alpha":0.96737, "fx":[52.15283,51.80759,47.37672,47.72416], "fy":[2.16287,7.13235,6.8096,1.83911]}, + {"t":1.25982, "x":10.11289, "y":5.11452, "heading":-3.08733, "vx":-1.30899, "vy":-0.11779, "omega":-0.22619, "ax":2.98563, "ay":0.26903, "alpha":0.76979, "fx":[51.62928,51.36846,47.70964,48.3684], "fy":[2.61695,6.58987,6.39126,2.34039]}, + {"t":1.28904, "x":10.07592, "y":5.11119, "heading":-3.09394, "vx":-1.22174, "vy":-0.10993, "omega":-0.2037, "ax":2.98581, "ay":0.26896, "alpha":0.64795, "fx":[51.34795,51.15879,48.19603,48.38525], "fy":[2.89569,6.24324,6.06257,2.73233]}, + {"t":1.31826, "x":10.04149, "y":5.1081, "heading":-3.09989, "vx":-1.1345, "vy":-0.10207, "omega":-0.18477, "ax":2.98597, "ay":0.2689, "alpha":0.52781, "fx":[51.02655,50.87962,48.3556,48.83673], "fy":[3.1761,5.91887,5.81647,3.01844]}, + {"t":1.34748, "x":10.00962, "y":5.10523, "heading":-3.10529, "vx":-1.04724, "vy":-0.09421, "omega":-0.16934, "ax":2.9861, "ay":0.26885, "alpha":0.46458, "fx":[50.8916,50.7814,48.6624,48.77216], "fy":[3.32333,5.73503,5.63003,3.23806]}, + {"t":1.3767, "x":9.98029, "y":5.10259, "heading":-3.11024, "vx":-0.95999, "vy":-0.08635, "omega":-0.15577, "ax":2.98622, "ay":0.26881, "alpha":0.39661, "fx":[50.70352,50.61396,48.71068,49.08733], "fy":[3.4818,5.55533,5.50232,3.38401]}, + {"t":1.40592, "x":9.95351, "y":5.10018, "heading":-3.11479, "vx":-0.87273, "vy":-0.0785, "omega":-0.14418, "ax":2.98633, "ay":0.26877, "alpha":0.37406, "fx":[50.66583,50.59462,48.89568,48.96636], "fy":[3.5322,5.48375,5.41907,3.4858]}, + {"t":1.43514, "x":9.92929, "y":5.098, "heading":-3.119, "vx":-0.78546, "vy":-0.07065, "omega":-0.13325, "ax":2.98642, "ay":0.26873, "alpha":0.34522, "fx":[50.57594,50.51386,48.86269,49.17621], "fy":[3.59546,5.40902,5.38314,3.53085]}, + {"t":1.46436, "x":9.90761, "y":5.09605, "heading":-3.1229, "vx":-0.6982, "vy":-0.06279, "omega":-0.12316, "ax":2.9865, "ay":0.2687, "alpha":0.35465, "fx":[50.61127,50.55755,48.95617,49.00927], "fy":[3.56602,5.42568,5.38449,3.54017]}, + {"t":1.49358, "x":9.88848, "y":5.09433, "heading":-3.12649, "vx":-0.61093, "vy":-0.05494, "omega":-0.1128, "ax":2.98658, "ay":0.26867, "alpha":0.3599, "fx":[50.60567,50.55495,48.85242,49.12622], "fy":[3.54425,5.43833,5.43128,3.50062]}, + {"t":1.52281, "x":9.87191, "y":5.09284, "heading":-3.12979, "vx":-0.52366, "vy":-0.04709, "omega":-0.10228, "ax":2.98665, "ay":0.26865, "alpha":0.39896, "fx":[50.70564,50.65788,48.86658,48.91367], "fy":[3.43743,5.53544,5.51516,3.42473]}, + {"t":1.55203, "x":9.85788, "y":5.09158, "heading":-3.13278, "vx":-0.43639, "vy":-0.03924, "omega":-0.09062, "ax":2.98671, "ay":0.26862, "alpha":0.43929, "fx":[50.78598,50.73776,48.68804,48.9361], "fy":[3.32893,5.6353,5.64843,3.29856]}, + {"t":1.58125, "x":9.8464, "y":5.09055, "heading":-3.13543, "vx":-0.34911, "vy":-0.03139, "omega":-0.07779, "ax":2.98676, "ay":0.2686, "alpha":0.51128, "fx":[50.95522,50.90701,48.62108,48.66832], "fy":[3.13121,5.82149,5.82725,3.12985]}, + {"t":1.61047, "x":9.83748, "y":5.08975, "heading":-3.1377, "vx":-0.26184, "vy":-0.02354, "omega":-0.06285, "ax":2.98681, "ay":0.26858, "alpha":0.59357, "fx":[51.13694,51.08607,48.35035,48.5817], "fy":[2.91688,6.02363,6.06665,2.90136]}, + {"t":1.63969, "x":9.8311, "y":5.08917, "heading":-3.13954, "vx":-0.17456, "vy":-0.01569, "omega":-0.0455, "ax":2.98686, "ay":0.26856, "alpha":0.70733, "fx":[51.39419,51.3408,48.18583,48.2374], "fy":[2.60541,6.32131,6.36657,2.61401]}, + {"t":1.66891, "x":9.82728, "y":5.08883, "heading":-3.14087, "vx":-0.08728, "vy":-0.00785, "omega":-0.02483, "ax":2.98691, "ay":0.26855, "alpha":0.84987, "fx":[51.70995,51.81079,47.79262,47.84778], "fy":[2.29805,6.60194,6.68966,2.31658]}, + {"t":1.69813, "x":9.826, "y":5.08871, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/OppGToOppBarge.traj b/src/main/deploy/choreo/OppGToOppBarge.traj new file mode 100644 index 0000000..234436b --- /dev/null +++ b/src/main/deploy/choreo/OppGToOppBarge.traj @@ -0,0 +1,89 @@ +{ + "name":"OppGToOppBarge", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":11.698677, "y":4.3309, "heading":0.0, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":11.333887100219728, "y":4.521003246307373, "heading":-0.9827950870986544, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":10.47326946258545, "y":5.011077404022217, "heading":-3.0750240485246456, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.826, "y":5.08871412277, "heading":3.141592653589793, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"FieldMaxX - G.x", "val":11.698677}, "y":{"exp":"FieldMaxY - G.y", "val":4.3309}, "heading":{"exp":"G.heading - 180 deg", "val":0.0}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"11.333887100219727 m", "val":11.333887100219728}, "y":{"exp":"4.521003246307373 m", "val":4.521003246307373}, "heading":{"exp":"-0.9827950870986543 rad", "val":-0.9827950870986544}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"10.47326946258545 m", "val":10.47326946258545}, "y":{"exp":"5.011077404022217 m", "val":5.011077404022217}, "heading":{"exp":"-3.0750240485246456 rad", "val":-3.0750240485246456}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"FieldMaxX - barge.x", "val":9.826}, "y":{"exp":"barge.y", "val":5.08871412277}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.30677,0.79439,1.12781], + "samples":[ + {"t":0.0, "x":11.69868, "y":4.3309, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.64651, "ay":1.98617, "alpha":-36.04913, "fx":[-122.88069,-30.09616,-159.82774,-197.04998], "fy":[-154.76678,196.66105,118.76195,-28.22204]}, + {"t":0.02045, "x":11.69708, "y":4.33132, "heading":0.0, "vx":-0.15638, "vy":0.04062, "omega":-0.73725, "ax":-7.91136, "ay":2.21277, "alpha":-34.3421, "fx":[-137.29655,-33.14697,-159.70876,-197.36174], "fy":[-141.80121,196.11798,118.8684,-25.64173]}, + {"t":0.0409, "x":11.69223, "y":4.33261, "heading":-0.01508, "vx":-0.31818, "vy":0.08587, "omega":-1.43959, "ax":-8.15259, "ay":2.53431, "alpha":-32.59, "fx":[-151.92816,-35.17625,-158.65432,-197.8403], "fy":[-125.72565,195.69371,120.20486,-21.19007]}, + {"t":0.06135, "x":11.68401, "y":4.3349, "heading":-0.04452, "vx":-0.48491, "vy":0.1377, "omega":-2.1061, "ax":-8.35563, "ay":2.93822, "alpha":-30.84213, "fx":[-165.33984,-36.73082,-156.70606,-198.36089], "fy":[-107.26448,195.31072,122.64726,-14.77854]}, + {"t":0.08181, "x":11.67235, "y":4.33833, "heading":-0.08759, "vx":-0.65579, "vy":0.19779, "omega":-2.73686, "ax":-8.52505, "ay":3.42663, "alpha":-28.99761, "fx":[-177.1432,-38.65719,-153.90002,-198.73346], "fy":[-86.17909,194.80074,126.03473,-6.17553]}, + {"t":0.10226, "x":11.65715, "y":4.34309, "heading":-0.14356, "vx":-0.83014, "vy":0.26787, "omega":-3.3299, "ax":-8.67825, "ay":4.02534, "alpha":-26.77263, "fx":[-187.29678,-42.40951,-150.29548,-198.64744], "fy":[-60.69398,193.81476,130.14906,5.13239]}, + {"t":0.12271, "x":11.63836, "y":4.34941, "heading":-0.21167, "vx":-1.00762, "vy":0.3502, "omega":-3.87744, "ax":-8.83496, "ay":4.78882, "alpha":-23.63427, "fx":[-194.84118,-50.71964,-146.04632,-197.49107], "fy":[-27.14256,191.49038,134.65024,20.31103]}, + {"t":0.14316, "x":11.61591, "y":4.35757, "heading":-0.29096, "vx":-1.18831, "vy":0.44813, "omega":-4.36079, "ax":-8.99253, "ay":5.80046, "alpha":-18.60622, "fx":[-195.43385,-68.93664,-141.61699,-193.61683], "fy":[20.30896,185.18424,138.86264,42.4076]}, + {"t":0.16361, "x":11.58972, "y":4.36795, "heading":-0.38015, "vx":-1.37222, "vy":0.56676, "omega":-4.74131, "ax":-9.00562, "ay":7.12247, "alpha":-9.88873, "fx":[-176.15175,-105.8358,-138.73317,-179.75703], "fy":[86.50478,165.94685,140.73731,81.72365]}, + {"t":0.18406, "x":11.55978, "y":4.38103, "heading":-0.47711, "vx":-1.5564, "vy":0.71243, "omega":-4.94355, "ax":-7.6892, "ay":8.54654, "alpha":8.09243, "fx":[-117.36153,-159.74974,-149.82879,-85.76087], "fy":[157.33952,113.52506,123.67373,175.32858]}, + {"t":0.20451, "x":11.52634, "y":4.39739, "heading":-0.57822, "vx":-1.71365, "vy":0.88721, "omega":-4.77805, "ax":-6.19277, "ay":7.2166, "alpha":25.08967, "fx":[-84.88902,-174.97574,-182.49768,29.44067], "fy":[176.13298,85.87586,28.81022,190.36991]}, + {"t":0.22496, "x":11.49, "y":4.41704, "heading":-0.67593, "vx":-1.8403, "vy":1.0348, "omega":-4.26493, "ax":-6.01527, "ay":7.20236, "alpha":24.57254, "fx":[-79.1792,-168.65509,-177.19013,23.93792], "fy":[175.68958,91.80222,27.25944,185.48807]}, + {"t":0.24542, "x":11.4511, "y":4.43971, "heading":-0.76316, "vx":-1.96332, "vy":1.1821, "omega":-3.76239, "ax":-4.13788, "ay":7.95373, "alpha":20.61863, "fx":[-49.72627,-140.02764,-129.09532,42.94356], "fy":[175.95895,113.72977,74.22583,166.42507]}, + {"t":0.26587, "x":11.41008, "y":4.46555, "heading":-0.8401, "vx":-2.04795, "vy":1.34476, "omega":-3.34071, "ax":8.95102, "ay":1.63754, "alpha":-14.46191, "fx":[155.01153,174.33562,140.94444,126.54494], "fy":[-55.48958,16.54134,95.01144,53.12501]}, + {"t":0.28632, "x":11.37007, "y":4.49339, "heading":-0.90842, "vx":-1.86489, "vy":1.37825, "omega":-3.63648, "ax":9.33978, "ay":-2.76147, "alpha":-19.01615, "fx":[128.20464,187.30644,171.97574,135.27159], "fy":[-138.19624,-37.62589,73.81934,-82.12684]}, + {"t":0.30677, "x":11.33389, "y":4.521, "heading":-0.9828, "vx":-1.67387, "vy":1.32178, "omega":-4.02538, "ax":8.91402, "ay":-4.04743, "alpha":-19.06022, "fx":[113.90483,182.17009,179.96654,118.32834], "fy":[-150.73251,-57.9263,50.8044,-112.02062]}, + {"t":0.32797, "x":11.3004, "y":4.54812, "heading":-1.06814, "vx":-1.48489, "vy":1.23597, "omega":-4.42948, "ax":7.57101, "ay":-6.14838, "alpha":-18.78639, "fx":[86.46709,165.03276,182.4806,70.83992], "fy":[-165.90922,-91.93336,-1.42384,-150.69599]}, + {"t":0.34917, "x":11.27062, "y":4.57294, "heading":-1.16205, "vx":-1.32437, "vy":1.10562, "omega":-4.82777, "ax":5.64447, "ay":-7.88951, "alpha":-17.90314, "fx":[51.98144,137.88276,166.12998,20.36851], "fy":[-177.19079,-124.59053,-58.23583,-166.04037]}, + {"t":0.37037, "x":11.24381, "y":4.59461, "heading":-1.2644, "vx":-1.2047, "vy":0.93835, "omega":-5.20734, "ax":3.03314, "ay":-9.11647, "alpha":-16.20442, "fx":[10.64028,98.69377,123.6021,-30.69196], "fy":[-180.9175,-152.11616,-111.71303,-163.12184]}, + {"t":0.39157, "x":11.21895, "y":4.61245, "heading":-1.3748, "vx":-1.1404, "vy":0.74507, "omega":-5.55089, "ax":-0.34208, "ay":-9.44238, "alpha":-13.11778, "fx":[-35.89634,43.50867,46.19524,-76.61697], "fy":[-171.90996,-168.21382,-146.24843,-143.22782]}, + {"t":0.41278, "x":11.1947, "y":4.62613, "heading":-1.49249, "vx":-1.14765, "vy":0.54488, "omega":-5.829, "ax":-4.43918, "ay":-7.96697, "alpha":-7.28819, "fx":[-85.25111,-42.16832,-60.80109,-107.77577], "fy":[-139.11544,-152.31164,-129.18425,-110.61118]}, + {"t":0.43398, "x":11.16937, "y":4.63589, "heading":-1.61607, "vx":-1.24177, "vy":0.37597, "omega":-5.98352, "ax":-8.03539, "ay":-1.80495, "alpha":4.25656, "fx":[-130.02811,-141.55474,-138.35841,-125.84284], "fy":[-17.19082,-13.57818,-41.17325,-48.40828]}, + {"t":0.45518, "x":11.14124, "y":4.64345, "heading":-1.74293, "vx":-1.41213, "vy":0.33771, "omega":-5.89328, "ax":-5.30646, "ay":6.90352, "alpha":10.78034, "fx":[-41.9105,-98.26082,-132.60714,-81.04581], "fy":[148.4258,131.13325,80.8552,99.89922]}, + {"t":0.47638, "x":11.11011, "y":4.65216, "heading":-1.86787, "vx":-1.52463, "vy":0.48407, "omega":-5.66472, "ax":-1.05956, "ay":9.71744, "alpha":8.97465, "fx":[24.57709,-34.53829,-68.54394,7.85578], "fy":[169.72029,170.89151,150.67228,156.65592]}, + {"t":0.49758, "x":11.07754, "y":4.66461, "heading":-1.98797, "vx":-1.54709, "vy":0.69009, "omega":-5.47445, "ax":1.28041, "ay":9.81549, "alpha":7.26433, "fx":[54.53004,5.42804,-19.16397,44.58104], "fy":[162.73176,173.32368,164.08313,154.33949]}, + {"t":0.51878, "x":11.04503, "y":4.68145, "heading":-2.10403, "vx":-1.51995, "vy":0.89819, "omega":-5.32044, "ax":3.25023, "ay":9.16136, "alpha":5.27498, "fx":[76.80563,41.88573,27.5634,70.46445], "fy":[148.2147,162.74462,158.15433,141.74825]}, + {"t":0.53998, "x":11.01354, "y":4.70255, "heading":-2.21683, "vx":-1.45104, "vy":1.09242, "omega":-5.2086, "ax":4.75189, "ay":8.21206, "alpha":3.78367, "fx":[94.20379,70.72715,62.32354,89.59274], "fy":[131.6129,146.27745,143.11791,126.55628]}, + {"t":0.56118, "x":10.98384, "y":4.72755, "heading":-2.32726, "vx":-1.35029, "vy":1.26652, "omega":-5.12838, "ax":2.68368, "ay":7.1553, "alpha":9.82109, "fx":[84.94208,36.98383,-2.13475,59.15165], "fy":[112.59744,141.98788,128.62494,93.89122]}, + {"t":0.58238, "x":10.95582, "y":4.75601, "heading":-2.43599, "vx":-1.2934, "vy":1.41822, "omega":-4.92017, "ax":-7.33675, "ay":-0.90829, "alpha":22.78135, "fx":[-63.32805,-127.07772,-172.23528,-126.55914], "fy":[-48.6879,96.43278,-2.4663,-105.84142]}, + {"t":0.60359, "x":10.92675, "y":4.78588, "heading":-2.5403, "vx":-1.44894, "vy":1.39897, "omega":-4.43718, "ax":-8.85431, "ay":-2.37146, "alpha":20.9077, "fx":[-105.20003,-156.62413,-185.32435,-143.24009], "fy":[-114.19136,81.44373,-10.74207,-114.63454]}, + {"t":0.62479, "x":10.89404, "y":4.815, "heading":-2.63437, "vx":-1.63666, "vy":1.34869, "omega":-3.99391, "ax":-9.51426, "ay":-2.27517, "alpha":19.72575, "fx":[-124.52914,-164.87936,-189.83435,-155.14976], "fy":[-119.8477,79.55329,-3.97597,-107.4337]}, + {"t":0.64599, "x":10.8572, "y":4.84309, "heading":-2.71905, "vx":-1.83838, "vy":1.30045, "omega":-3.5757, "ax":-9.89384, "ay":-2.06805, "alpha":18.94859, "fx":[-135.38863,-169.05515,-191.97113,-163.28749], "fy":[-119.76204,78.54783,2.99002,-99.66957]}, + {"t":0.66719, "x":10.816, "y":4.87019, "heading":-2.79486, "vx":-2.04814, "vy":1.25661, "omega":-3.17397, "ax":-10.11549, "ay":-1.98205, "alpha":18.46348, "fx":[-140.28542,-172.63902,-193.15286,-168.40392], "fy":[-121.21521,75.50285,7.65992,-94.10703]}, + {"t":0.68839, "x":10.77031, "y":4.89639, "heading":-2.86215, "vx":-2.2626, "vy":1.21459, "omega":-2.78252, "ax":-10.22787, "ay":-2.07411, "alpha":18.21405, "fx":[-140.34266,-176.5423,-193.99626,-171.09349], "fy":[-125.86539,69.61419,9.53775,-91.58451]}, + {"t":0.70959, "x":10.72004, "y":4.92167, "heading":-2.92114, "vx":-2.47944, "vy":1.17061, "omega":-2.39637, "ax":-10.24881, "ay":-2.3447, "alpha":18.16893, "fx":[-136.06536,-180.77793,-194.75155,-171.77581], "fy":[-133.73984,60.80223,8.80273,-92.20528]}, + {"t":0.73079, "x":10.66517, "y":4.94597, "heading":-2.97195, "vx":-2.69672, "vy":1.1209, "omega":-2.01117, "ax":-10.18031, "ay":-2.76436, "alpha":18.31206, "fx":[-127.66404,-184.94348,-195.4574,-170.73859], "fy":[-144.12778,49.41769,6.01758,-95.62953]}, + {"t":0.75199, "x":10.60571, "y":4.96911, "heading":-3.01459, "vx":-2.91256, "vy":1.0623, "omega":-1.62293, "ax":-10.0196, "ay":-3.28389, "alpha":18.63862, "fx":[-115.31774,-188.49399,-196.06182,-168.21397], "fy":[-155.92055,36.17036,2.011,-101.22429]}, + {"t":0.77319, "x":10.5417, "y":4.99089, "heading":-3.04899, "vx":-3.12498, "vy":0.99267, "omega":-1.22777, "ax":-9.70805, "ay":-3.82808, "alpha":19.63514, "fx":[-96.37748,-190.4633,-196.5376,-163.93593], "fy":[-169.59111,24.87322,-1.51913,-109.01176]}, + {"t":0.79439, "x":10.47327, "y":5.01108, "heading":-3.07502, "vx":-3.33081, "vy":0.91151, "omega":-0.81148, "ax":-6.16071, "ay":-9.35881, "alpha":11.3387, "fx":[-41.64858,-90.80289,-163.58205,-114.75105], "fy":[-190.7182,-168.58306,-105.78924,-158.93721]}, + {"t":0.82218, "x":10.37834, "y":5.03279, "heading":-3.09757, "vx":-3.50198, "vy":0.65148, "omega":-0.49644, "ax":8.35613, "ay":-7.97891, "alpha":6.86858, "fx":[142.46903,169.48472,139.55383,105.66317], "fy":[-134.90586,-97.51039,-135.03334,-164.56872]}, + {"t":0.84996, "x":10.28427, "y":5.04781, "heading":-3.11136, "vx":-3.26981, "vy":0.42979, "omega":-0.30559, "ax":11.12951, "ay":-3.95074, "alpha":3.22461, "fx":[183.43331,191.71871,188.95559,177.98656], "fy":[-73.63097,-47.73912,-56.67596,-85.38179]}, + {"t":0.87775, "x":10.19771, "y":5.05823, "heading":-3.11986, "vx":-2.96057, "vy":0.32001, "omega":-0.216, "ax":11.59778, "ay":-2.54951, "alpha":1.97177, "fx":[192.08965,195.67522,194.94305,190.60956], "fy":[-49.0089,-31.65632,-35.24434,-54.08661]}, + {"t":0.90553, "x":10.11993, "y":5.06614, "heading":-3.12586, "vx":-2.63833, "vy":0.24918, "omega":-0.16121, "ax":11.75136, "ay":-1.88345, "alpha":1.38614, "fx":[195.13807,197.08079,196.78378,194.5556], "fy":[-36.58052,-23.92622,-25.80496,-39.27318]}, + {"t":0.93332, "x":10.05116, "y":5.07233, "heading":-3.13034, "vx":-2.31182, "vy":0.19684, "omega":-0.1227, "ax":11.82094, "ay":-1.49874, "alpha":1.05111, "fx":[196.55733,197.75766,197.60768,196.27467], "fy":[-29.18337,-19.40574,-20.53893,-30.80532]}, + {"t":0.9611, "x":9.99149, "y":5.07722, "heading":-3.13375, "vx":-1.98337, "vy":0.1552, "omega":-0.09349, "ax":11.85875, "ay":-1.24913, "alpha":0.83492, "fx":[197.33715,198.14375,198.05727,197.18063], "fy":[-24.29739,-16.44382,-17.1897,-25.35875]}, + {"t":0.98889, "x":9.94096, "y":5.08105, "heading":-3.13634, "vx":-1.65388, "vy":0.12049, "omega":-0.0703, "ax":11.88184, "ay":-1.07436, "alpha":0.68407, "fx":[197.81476,198.38896,198.33447,197.71988], "fy":[-20.83548,-14.35399,-14.87519,-21.57132]}, + {"t":1.01667, "x":9.89959, "y":5.08399, "heading":-3.1383, "vx":-1.32374, "vy":0.09064, "omega":-0.05129, "ax":11.89711, "ay":-0.94525, "alpha":0.57289, "fx":[198.13048,198.55668,198.52007,198.06904], "fy":[-18.25643,-12.80081,-13.18146,-18.78892]}, + {"t":1.04446, "x":9.8674, "y":5.08614, "heading":-3.13972, "vx":-0.99318, "vy":0.06438, "omega":-0.03537, "ax":11.90782, "ay":-0.84603, "alpha":0.48759, "fx":[198.35131,198.67775,198.65194,198.30947], "fy":[-16.26165,-11.60116,-11.88895,-16.66006]}, + {"t":1.07224, "x":9.8444, "y":5.0876, "heading":-3.1407, "vx":-0.66232, "vy":0.04087, "omega":-0.02182, "ax":11.91568, "ay":-0.76742, "alpha":0.42008, "fx":[198.51263,198.76882,198.74991,198.48298], "fy":[-14.67323,-10.6467,-10.87054,-14.97951]}, + {"t":1.10003, "x":9.8306, "y":5.08844, "heading":-3.14131, "vx":-0.33124, "vy":0.01955, "omega":-0.01015, "ax":11.92164, "ay":-0.70361, "alpha":0.36533, "fx":[198.6346,198.83955,198.82528,198.61289], "fy":[-13.37873,-9.86925,-10.04758,-13.61959]}, + {"t":1.12781, "x":9.826, "y":5.08871, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/bargeToOppE.traj b/src/main/deploy/choreo/bargeToOppE.traj new file mode 100644 index 0000000..2dcd1c1 --- /dev/null +++ b/src/main/deploy/choreo/bargeToOppE.traj @@ -0,0 +1,189 @@ +{ + "name":"bargeToOppE", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.7, "y":5.08871412277, "heading":0.0, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.160315799713134, "y":5.6167577, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.365684200286866, "y":5.6167577, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":11.51699161529541, "y":5.808125972747803, "heading":0.0, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":12.02427864074707, "y":5.681303977966309, "heading":-0.5538539145382533, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":12.631814748155, "y":5.3371419902636, "heading":-1.047197551196598, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":3, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"barge.x", "val":7.7}, "y":{"exp":"barge.y", "val":5.08871412277}, "heading":{"exp":"barge.heading", "val":0.0}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.160315799713134 m", "val":8.160315799713134}, "y":{"exp":"5.6167577 m", "val":5.6167577}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.365684200286866 m", "val":9.365684200286866}, "y":{"exp":"5.6167577 m", "val":5.6167577}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"11.51699161529541 m", "val":11.51699161529541}, "y":{"exp":"5.808125972747803 m", "val":5.808125972747803}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"12.02427864074707 m", "val":12.02427864074707}, "y":{"exp":"5.681303977966309 m", "val":5.681303977966309}, "heading":{"exp":"-0.5538539145382533 rad", "val":-0.5538539145382533}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"FieldMaxX - E.x", "val":12.631814748155}, "y":{"exp":"FieldMaxY - E.y", "val":5.3371419902636}, "heading":{"exp":"E.heading - 180 deg", "val":-1.047197551196598}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":3, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.5681,1.17828,1.96654,2.36941,3.56348], + "samples":[ + {"t":0.0, "x":7.7, "y":5.08871, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.23562, "ay":5.86817, "alpha":-0.00506, "fx":[20.59574,20.50084,20.65159,20.6409], "fy":[97.75048,98.06288,97.7236,97.74145]}, + {"t":0.04058, "x":7.70102, "y":5.09355, "heading":0.0, "vx":0.05014, "vy":0.23812, "omega":-0.00021, "ax":1.44988, "ay":5.82031, "alpha":-0.00475, "fx":[24.18038,24.06439,24.21997,24.21056], "fy":[96.95631,97.24464,96.9382,96.94822]}, + {"t":0.08116, "x":7.70425, "y":5.108, "heading":-0.00001, "vx":0.10897, "vy":0.47431, "omega":-0.0004, "ax":1.73716, "ay":5.74078, "alpha":0.03123, "fx":[28.80246,28.97262,28.99902,29.05642], "fy":[95.95899,95.61762,95.6106,95.59728]}, + {"t":0.12174, "x":7.7101, "y":5.13197, "heading":-0.00002, "vx":0.17947, "vy":0.70726, "omega":0.00087, "ax":2.13935, "ay":5.60286, "alpha":-0.00405, "fx":[35.67171,35.57922,35.70335,35.69379], "fy":[93.34483,93.58063,93.32606,93.33667]}, + {"t":0.16232, "x":7.71914, "y":5.16529, "heading":0.00001, "vx":0.26628, "vy":0.93462, "omega":0.0007, "ax":2.7298, "ay":5.3394, "alpha":-0.02267, "fx":[45.68844,45.65047,45.68404,44.99499], "fy":[88.94233,88.95687,88.94441,89.17722]}, + {"t":0.20289, "x":7.73219, "y":5.20761, "heading":0.00004, "vx":0.37705, "vy":1.15128, "omega":-0.00022, "ax":3.62991, "ay":4.77208, "alpha":0.01773, "fx":[60.39711,60.78725,60.4319,60.41919], "fy":[79.72346,79.06637,79.69076,79.71226]}, + {"t":0.24347, "x":7.75048, "y":5.25825, "heading":0.00003, "vx":0.52435, "vy":1.34493, "omega":0.0005, "ax":4.92454, "ay":3.41771, "alpha":-0.00582, "fx":[82.17144,82.09645,82.13145,81.95946], "fy":[56.81569,56.9663,56.94508,57.15926]}, + {"t":0.28405, "x":7.77582, "y":5.31564, "heading":0.00005, "vx":0.72418, "vy":1.48362, "omega":0.00027, "ax":5.96739, "ay":0.5558, "alpha":0.00711, "fx":[99.46216,99.46374,99.48628,99.48192], "fy":[9.29094,9.26942,9.21248,9.28714]}, + {"t":0.32463, "x":7.81012, "y":5.37631, "heading":0.00006, "vx":0.96633, "vy":1.50617, "omega":0.00056, "ax":5.38503, "ay":-2.63233, "alpha":0.01653, "fx":[89.79659,89.70934,89.75353,89.80431], "fy":[-43.69625,-43.95858,-43.93603,-43.92813]}, + {"t":0.36521, "x":7.85376, "y":5.43526, "heading":0.00008, "vx":1.18485, "vy":1.39935, "omega":0.00123, "ax":4.12925, "ay":-4.34681, "alpha":-0.03044, "fx":[68.73928,69.01124,68.77299,68.80718], "fy":[-72.58074,-72.08231,-72.62498,-72.54915]}, + {"t":0.40579, "x":7.90524, "y":5.48846, "heading":0.00013, "vx":1.35241, "vy":1.22297, "omega":-0.00001, "ax":3.17927, "ay":-5.08439, "alpha":-0.0428, "fx":[53.16643,53.13247,53.21513,52.47334], "fy":[-84.6867,-84.71248,-84.68439,-84.93355]}, + {"t":0.44637, "x":7.96274, "y":5.5339, "heading":0.00013, "vx":1.48142, "vy":1.01665, "omega":-0.00174, "ax":2.54948, "ay":-5.4284, "alpha":0.02156, "fx":[42.51449,42.39937,42.53421,42.54627], "fy":[-90.43126,-90.6679,-90.43653,-90.41985]}, + {"t":0.48695, "x":8.02495, "y":5.57069, "heading":0.00006, "vx":1.58488, "vy":0.79637, "omega":-0.00087, "ax":2.12222, "ay":-5.60977, "alpha":0.00448, "fx":[35.20603,35.35961,35.36739,35.57247], "fy":[-93.7586,-93.44294,-93.4378,-93.40904]}, + {"t":0.52753, "x":8.09101, "y":5.59839, "heading":0.00003, "vx":1.67099, "vy":0.56873, "omega":-0.00069, "ax":1.81893, "ay":-5.71567, "alpha":0.01695, "fx":[30.36362,30.23386,30.34316,30.3422], "fy":[-95.21658,-95.47908,-95.19718,-95.21734]}, + {"t":0.5681, "x":8.16032, "y":5.61676, "heading":0.0, "vx":1.7448, "vy":0.33679, "omega":0.0, "ax":1.8041, "ay":-5.71622, "alpha":0.0, "fx":[29.97514,30.11432,30.07718,30.1271], "fy":[-95.33671,-95.15799,-95.35801,-95.29412]}, + {"t":0.60022, "x":8.21728, "y":5.62463, "heading":0.0, "vx":1.80274, "vy":0.15322, "omega":0.0, "ax":2.55937, "ay":-5.41761, "alpha":0.0, "fx":[42.84642,42.63873,42.82957,42.3393], "fy":[-90.08847,-90.39999,-90.37598,-90.37123]}, + {"t":0.63233, "x":8.27649, "y":5.62675, "heading":0.0, "vx":1.88493, "vy":-0.02076, "omega":0.0, "ax":3.53294, "ay":-1.30092, "alpha":0.0, "fx":[58.86552,58.85294,58.9272,58.92389], "fy":[-21.71656,-21.52279,-21.78226,-21.72112]}, + {"t":0.66445, "x":8.33885, "y":5.62542, "heading":0.0, "vx":1.99839, "vy":-0.06254, "omega":0.0, "ax":0.0107, "ay":0.37065, "alpha":0.0, "fx":[0.32114,0.321,0.55624,-0.48494], "fy":[6.37019,6.03603,6.03557,6.27226]}, + {"t":0.69656, "x":8.40303, "y":5.6236, "heading":0.0, "vx":1.99874, "vy":-0.05064, "omega":0.0, "ax":0.0078, "ay":0.34573, "alpha":0.0, "fx":[0.14347,0.14361,0.11644,0.11648], "fy":[5.7767,5.69345,5.80585,5.77676]}, + {"t":0.72868, "x":8.46722, "y":5.62215, "heading":0.0, "vx":1.99899, "vy":-0.03953, "omega":0.0, "ax":0.00319, "ay":0.17394, "alpha":0.0, "fx":[0.01961,0.01962,0.04625,0.12754], "fy":[2.77158,2.93321,2.93319,2.96027]}, + {"t":0.76079, "x":8.53142, "y":5.62097, "heading":0.0, "vx":1.99909, "vy":-0.03395, "omega":0.0, "ax":0.00133, "ay":0.08127, "alpha":0.0, "fx":[0.04707,0.04706,-0.00285,-0.00285], "fy":[1.3797,1.23348,1.42611,1.3797]}, + {"t":0.7929, "x":8.59562, "y":5.61992, "heading":0.0, "vx":1.99913, "vy":-0.03134, "omega":0.0, "ax":0.00074, "ay":0.04839, "alpha":0.0, "fx":[-0.0365,-0.03651,-0.03761,0.15995], "fy":[0.66114,0.8555,0.8555,0.85451]}, + {"t":0.82502, "x":8.65982, "y":5.61894, "heading":0.0, "vx":1.99916, "vy":-0.02978, "omega":0.0, "ax":0.00052, "ay":0.03538, "alpha":0.0, "fx":[0.0297,0.0297,-0.01247,-0.01247], "fy":[0.61093,0.48612,0.65141,0.61093]}, + {"t":0.85713, "x":8.72402, "y":5.618, "heading":0.0, "vx":1.99917, "vy":-0.02865, "omega":0.0, "ax":0.00036, "ay":0.02521, "alpha":0.0, "fx":[0.00667,0.00667,0.06031,-0.0499], "fy":[0.36871,0.41943,0.41943,0.47308]}, + {"t":0.88925, "x":8.78823, "y":5.6171, "heading":0.0, "vx":1.99918, "vy":-0.02784, "omega":0.0, "ax":0.00016, "ay":0.01144, "alpha":0.0, "fx":[0.00438,0.00438,0.00089,0.00089], "fy":[0.1924,0.18004,0.19777,0.1924]}, + {"t":0.92136, "x":8.85243, "y":5.61621, "heading":0.0, "vx":1.99919, "vy":-0.02747, "omega":0.0, "ax":-0.00014, "ay":-0.0099, "alpha":0.0, "fx":[0.02105,0.02105,0.03609,-0.08731], "fy":[-0.11,-0.18832,-0.18832,-0.17331]}, + {"t":0.95348, "x":8.91663, "y":5.61532, "heading":0.0, "vx":1.99918, "vy":-0.02779, "omega":0.0, "ax":-0.00068, "ay":-0.04787, "alpha":0.0, "fx":[-0.02611,-0.02611,0.00331,0.00331], "fy":[-0.81273,-0.72918,-0.83742,-0.81273]}, + {"t":0.98559, "x":8.98083, "y":5.6144, "heading":0.0, "vx":1.99916, "vy":-0.02933, "omega":0.0, "ax":-0.00176, "ay":-0.11327, "alpha":0.0, "fx":[0.00651,0.00651,0.03171,-0.16232], "fy":[-1.80555,-1.92414,-1.92414,-1.89911]}, + {"t":1.01771, "x":9.04503, "y":5.6134, "heading":0.0, "vx":1.99911, "vy":-0.03296, "omega":0.0, "ax":-0.00342, "ay":-0.18966, "alpha":0.0, "fx":[-0.06048,-0.06047,-0.05344,-0.05344], "fy":[-3.16508,-3.149,-3.16708,-3.16507]}, + {"t":1.04982, "x":9.10923, "y":5.61225, "heading":0.0, "vx":1.999, "vy":-0.03905, "omega":0.0, "ax":-0.00264, "ay":-0.12825, "alpha":0.0, "fx":[-0.01104,-0.01105,0.07505,-0.22882], "fy":[-2.12513,-2.17076,-2.17076,-2.08472]}, + {"t":1.08193, "x":9.17343, "y":5.61093, "heading":0.0, "vx":1.99891, "vy":-0.04317, "omega":0.0, "ax":0.0105, "ay":0.63755, "alpha":0.0, "fx":[0.14036,0.14035,0.20977,0.20977], "fy":[10.59295,10.79937,10.52535,10.59294]}, + {"t":1.11405, "x":9.23763, "y":5.60987, "heading":0.0, "vx":1.99925, "vy":-0.0227, "omega":0.0, "ax":-0.05808, "ay":3.48386, "alpha":0.0, "fx":[-0.68374,-0.68437,-0.98806,-1.51666], "fy":[59.22339,57.79023,57.79008,57.49352]}, + {"t":1.14616, "x":9.3018, "y":5.61094, "heading":0.0, "vx":1.99738, "vy":0.08918, "omega":0.0, "ax":-0.50729, "ay":5.73584, "alpha":0.0, "fx":[-8.74247,-8.72928,-8.17845,-8.17514], "fy":[95.33235,96.93167,94.85491,95.33607]}, + {"t":1.17828, "x":9.36568, "y":5.61676, "heading":0.0, "vx":1.98109, "vy":0.27339, "omega":0.0, "ax":5.89286, "ay":1.06234, "alpha":-9.80532, "fx":[119.48924,115.62791,76.95113,80.85658], "fy":[-10.08942,41.46542,50.81126,-11.3527]}, + {"t":1.20455, "x":9.41977, "y":5.62431, "heading":0.0, "vx":2.13593, "vy":0.3013, "omega":-0.25764, "ax":5.89763, "ay":1.06516, "alpha":-6.92163, "fx":[113.26096,110.40666,83.40222,86.1729], "fy":[-2.98686,35.23399,40.65525,-1.87939]}, + {"t":1.23083, "x":9.47793, "y":5.63259, "heading":-0.00677, "vx":2.29089, "vy":0.32929, "omega":-0.43951, "ax":5.89601, "ay":1.06754, "alpha":-4.28879, "fx":[107.60061,105.92108,88.93896,90.67406], "fy":[4.88987,28.42028,31.99061,5.88087]}, + {"t":1.2571, "x":9.54016, "y":5.64161, "heading":-0.01832, "vx":2.44581, "vy":0.35734, "omega":-0.5522, "ax":5.89363, "ay":1.07143, "alpha":-2.07939, "fx":[102.59084,101.8061,93.67992,94.89933], "fy":[10.95222,23.30847,24.75371,12.42676]}, + {"t":1.28338, "x":9.60646, "y":5.65137, "heading":-0.03283, "vx":2.60067, "vy":0.38549, "omega":-0.60684, "ax":5.89031, "ay":1.07587, "alpha":-0.12935, "fx":[98.40326,98.29015,97.96158,98.09937], "fy":[17.38769,18.0512,18.75191,17.54646]}, + {"t":1.30965, "x":9.67683, "y":5.66187, "heading":-0.04877, "vx":2.75544, "vy":0.41376, "omega":-0.61024, "ax":5.88458, "ay":1.0841, "alpha":1.30361, "fx":[95.14757,95.30985,100.74302,101.17176], "fy":[21.79787,14.93664,14.25675,21.29465]}, + {"t":1.33593, "x":9.75126, "y":5.67312, "heading":-0.06481, "vx":2.91006, "vy":0.44225, "omega":-0.57598, "ax":5.87185, "ay":1.10661, "alpha":2.3211, "fx":[92.85266,93.0537,102.95697,102.66012], "fy":[25.74083,12.76477,11.41492,23.8663]}, + {"t":1.36221, "x":9.82975, "y":5.68512, "heading":-0.07994, "vx":3.06435, "vy":0.47132, "omega":-0.51499, "ax":5.83476, "ay":1.1627, "alpha":2.87075, "fx":[90.81176,90.8066,102.62576,104.80634], "fy":[28.34463,12.84766,11.05079,25.28321]}, + {"t":1.38848, "x":9.91228, "y":5.69791, "heading":-0.09347, "vx":3.21766, "vy":0.50187, "omega":-0.43956, "ax":1.36849, "ay":1.5881, "alpha":2.63688, "fx":[16.76895,15.85862,28.92817,29.69243], "fy":[33.51877,21.0505,19.32396,31.99854]}, + {"t":1.41476, "x":9.9973, "y":5.71164, "heading":-0.10502, "vx":3.25362, "vy":0.5436, "omega":-0.37028, "ax":-0.14694, "ay":0.86279, "alpha":1.5063, "fx":[-5.58172,-6.28762,1.30464,0.76723], "fy":[18.68953,10.8938,10.18245,17.76357]}, + {"t":1.44103, "x":10.08274, "y":5.72622, "heading":-0.11475, "vx":3.24976, "vy":0.56627, "omega":-0.3307, "ax":-0.07752, "ay":0.44047, "alpha":0.78722, "fx":[-3.06253,-3.42568,0.47863,0.84054], "fy":[9.37692,5.68766,5.26728,9.03788]}, + {"t":1.46731, "x":10.1681, "y":5.74125, "heading":-0.12344, "vx":3.24772, "vy":0.57784, "omega":-0.31002, "ax":-0.01516, "ay":0.0844, "alpha":0.33015, "fx":[-0.97455,-1.08756,0.63104,0.42027], "fy":[2.26664,0.62285,0.51019,2.22767]}, + {"t":1.49358, "x":10.25343, "y":5.75647, "heading":-0.13159, "vx":3.24732, "vy":0.58006, "omega":-0.30134, "ax":0.05151, "ay":-0.28925, "alpha":0.08502, "fx":[0.63436,0.66833,1.08295,1.04906], "fy":[-4.62949,-5.19966,-4.89359,-4.56405]}, + {"t":1.51986, "x":10.33878, "y":5.77161, "heading":-0.1395, "vx":3.24868, "vy":0.57246, "omega":-0.29911, "ax":0.14027, "ay":-0.81237, "alpha":-0.19629, "fx":[2.60907,2.84249,2.02732,1.87394], "fy":[-14.43844,-13.12574,-12.89682,-13.70621]}, + {"t":1.54613, "x":10.42418, "y":5.78637, "heading":-0.14736, "vx":3.25236, "vy":0.55112, "omega":-0.30426, "ax":0.2627, "ay":-1.61369, "alpha":-0.44813, "fx":[5.28122,5.68482,3.47499,3.07508], "fy":[-28.14336,-26.60589,-25.18867,-27.66004]}, + {"t":1.57241, "x":10.50973, "y":5.80029, "heading":-0.15536, "vx":3.25926, "vy":0.50872, "omega":-0.31604, "ax":0.41184, "ay":-2.85224, "alpha":-1.21061, "fx":[8.96384,10.04915,4.44086,4.00659], "fy":[-51.98237,-44.92323,-43.91718,-49.35887]}, + {"t":1.59869, "x":10.59551, "y":5.81268, "heading":-0.16366, "vx":3.27009, "vy":0.43377, "omega":-0.34785, "ax":0.52196, "ay":-4.58895, "alpha":-2.16595, "fx":[13.28745,15.33159,4.01267,2.17136], "fy":[-82.12275,-74.16073,-69.52814,-80.17098]}, + {"t":1.62496, "x":10.68162, "y":5.82249, "heading":-0.1728, "vx":3.2838, "vy":0.31319, "omega":-0.40476, "ax":0.15874, "ay":-5.64429, "alpha":-3.83739, "fx":[10.13236,12.91213,-6.58847,-5.87179], "fy":[-107.10123,-86.78969,-82.37033,-100.08928]}, + {"t":1.65124, "x":10.76796, "y":5.82877, "heading":-0.18344, "vx":3.28797, "vy":0.16489, "omega":-0.50559, "ax":-5.52538, "ay":-2.18858, "alpha":-4.92297, "fx":[-81.7436,-80.14163,-103.10303,-103.43347], "fy":[-53.22363,-28.65065,-19.69908,-44.35691]}, + {"t":1.67751, "x":10.85244, "y":5.83235, "heading":-0.19672, "vx":3.14279, "vy":0.10738, "omega":-0.63494, "ax":-5.75714, "ay":-1.59382, "alpha":-5.73003, "fx":[-84.84898,-82.56526,-107.31284,-109.1481], "fy":[-47.2557,-16.05635,-7.1277,-35.83331]}, + {"t":1.70379, "x":10.93303, "y":5.83462, "heading":-0.21341, "vx":2.99152, "vy":0.0655, "omega":-0.7855, "ax":-5.81459, "ay":-1.40923, "alpha":-5.96262, "fx":[-85.91183,-83.18199,-108.56902,-110.04273], "fy":[-45.40392,-12.345,-2.75462,-33.46122]}, + {"t":1.73006, "x":11.00963, "y":5.83585, "heading":-0.23405, "vx":2.83873, "vy":0.02847, "omega":-0.94217, "ax":-5.84043, "ay":-1.31832, "alpha":-5.94374, "fx":[-86.86928,-83.31054,-108.31061,-110.93841], "fy":[-43.83276,-11.34517,-1.03395,-31.69136]}, + {"t":1.75634, "x":11.0822, "y":5.83615, "heading":-0.2588, "vx":2.68527, "vy":-0.00616, "omega":-1.09835, "ax":-5.85492, "ay":-1.26486, "alpha":-5.71862, "fx":[-88.01795,-84.135,-107.77687,-110.46494], "fy":[-42.75428,-10.64701,-0.78558,-30.1516]}, + {"t":1.78261, "x":11.15074, "y":5.83555, "heading":-0.28766, "vx":2.53143, "vy":-0.0394, "omega":-1.24861, "ax":-5.86437, "ay":-1.22869, "alpha":-5.17697, "fx":[-89.52679,-85.13491,-106.51109,-109.8524], "fy":[-39.88264,-12.09371,-1.5328,-28.41734]}, + {"t":1.80889, "x":11.21523, "y":5.83409, "heading":-0.32047, "vx":2.37734, "vy":-0.07168, "omega":-1.38464, "ax":-5.87082, "ay":-1.20358, "alpha":-4.56185, "fx":[-90.97057,-86.7484,-105.18273,-108.55337], "fy":[-37.73707,-12.48246,-3.44135,-26.59124]}, + {"t":1.83517, "x":11.27567, "y":5.83179, "heading":-0.35685, "vx":2.22308, "vy":-0.10331, "omega":-1.5045, "ax":-5.87557, "ay":-1.18477, "alpha":-3.57808, "fx":[-92.88383,-88.82507,-103.37658,-106.68614], "fy":[-33.12692,-14.86163,-6.15521,-24.85461]}, + {"t":1.86144, "x":11.33205, "y":5.82867, "heading":-0.39638, "vx":2.0687, "vy":-0.13444, "omega":-1.59852, "ax":-5.87926, "ay":-1.16994, "alpha":-2.5986, "fx":[-94.5196,-91.46648,-101.62435,-104.40701], "fy":[-29.74373,-15.47999,-9.93065,-22.85468]}, + {"t":1.88772, "x":11.38438, "y":5.82473, "heading":-0.43838, "vx":1.91422, "vy":-0.16518, "omega":-1.6668, "ax":-5.88215, "ay":-1.15817, "alpha":-1.20498, "fx":[-96.55451,-94.77805,-99.76421,-101.1139], "fy":[-23.47205,-17.82057,-14.59023,-21.34164]}, + {"t":1.91399, "x":11.43265, "y":5.81999, "heading":-0.48218, "vx":1.75966, "vy":-0.19561, "omega":-1.69846, "ax":-5.8845, "ay":-1.14856, "alpha":0.16447, "fx":[-98.20529,-98.53173,-97.93636,-97.69384], "fy":[-18.39999,-18.12354,-20.34876,-19.71127]}, + {"t":1.94027, "x":11.47685, "y":5.81445, "heading":-0.52681, "vx":1.60505, "vy":-0.22579, "omega":-1.69414, "ax":-5.88643, "ay":-1.14061, "alpha":1.92136, "fx":[-100.00509,-103.00124,-96.52395,-92.96569], "fy":[-10.62323,-19.76672,-26.80541,-18.85814]}, + {"t":1.96654, "x":11.51699, "y":5.80813, "heading":-0.57132, "vx":1.45038, "vy":-0.25576, "omega":-1.64365, "ax":-0.9632, "ay":-0.19637, "alpha":1.17287, "fx":[-16.61227,-20.04549,-15.50747,-12.05918], "fy":[0.77813,-3.70369,-7.33978,-2.82815]}, + {"t":1.97805, "x":11.53362, "y":5.80517, "heading":-0.59024, "vx":1.43929, "vy":-0.25802, "omega":-1.63015, "ax":-0.96834, "ay":-0.21594, "alpha":1.15623, "fx":[-16.54449,-20.0938,-15.73767,-12.19092], "fy":[0.42181,-3.99886,-7.59871,-3.22255]}, + {"t":1.98956, "x":11.55013, "y":5.80218, "heading":-0.60901, "vx":1.42814, "vy":-0.26051, "omega":-1.61684, "ax":-0.96537, "ay":-0.23124, "alpha":1.13955, "fx":[-16.51764,-19.95922,-15.67551,-12.21671], "fy":[0.10909,-4.16622,-7.83079,-3.53096]}, + {"t":2.00107, "x":11.5665, "y":5.79917, "heading":-0.62762, "vx":1.41703, "vy":-0.26317, "omega":-1.60373, "ax":-0.9626, "ay":-0.24448, "alpha":1.15959, "fx":[-16.35239,-20.0182,-15.75399,-12.06004], "fy":[-0.05149,-4.36414,-8.08403,-3.80194]}, + {"t":2.01258, "x":11.58275, "y":5.79613, "heading":-0.64608, "vx":1.40595, "vy":-0.26598, "omega":-1.59038, "ax":-0.96004, "ay":-0.25604, "alpha":1.16737, "fx":[-16.32532,-19.96013,-15.69099,-12.03691], "fy":[-0.19013,-4.48602,-8.35464,-4.04154]}, + {"t":2.0241, "x":11.59887, "y":5.79305, "heading":-0.66438, "vx":1.3949, "vy":-0.26893, "omega":-1.57694, "ax":-0.95766, "ay":-0.26622, "alpha":1.21829, "fx":[-16.17518,-20.13868,-15.75913,-11.78164], "fy":[-0.20236,-4.63276,-8.65602,-4.25966]}, + {"t":2.03561, "x":11.61486, "y":5.78993, "heading":-0.68253, "vx":1.38388, "vy":-0.27199, "omega":-1.56292, "ax":-0.95545, "ay":-0.27524, "alpha":1.25344, "fx":[-16.15452,-20.17462,-15.71093,-11.66753], "fy":[-0.20193,-4.71118,-8.98195,-4.45739]}, + {"t":2.04712, "x":11.63072, "y":5.78678, "heading":-0.70052, "vx":1.37288, "vy":-0.27516, "omega":-1.54849, "ax":-0.95341, "ay":-0.28329, "alpha":1.33661, "fx":[-16.01375,-20.47385,-15.78982,-11.29392], "fy":[-0.07992,-4.81798,-9.34589,-4.64571]}, + {"t":2.05863, "x":11.64646, "y":5.7836, "heading":-0.71835, "vx":1.36191, "vy":-0.27842, "omega":-1.53311, "ax":-0.95151, "ay":-0.29053, "alpha":1.39865, "fx":[-15.98467,-20.60489,-15.75269,-11.10269], "fy":[0.0497,-4.85717,-9.74138,-4.82284]}, + {"t":2.07014, "x":11.66208, "y":5.78037, "heading":-0.73599, "vx":1.35095, "vy":-0.28177, "omega":-1.51701, "ax":-0.94975, "ay":-0.29705, "alpha":1.51165, "fx":[-15.8354,-21.01283,-15.84784,-10.63145], "fy":[0.29366,-4.92332,-10.17903,-4.9983]}, + {"t":2.08165, "x":11.67756, "y":5.77711, "heading":-0.75346, "vx":1.34002, "vy":-0.28519, "omega":-1.49961, "ax":-0.94811, "ay":-0.30298, "alpha":1.60541, "fx":[-15.79559,-21.25738,-15.83324,-10.33207], "fy":[0.55399,-4.92579,-10.65706,-5.17295]}, + {"t":2.09316, "x":11.69293, "y":5.77381, "heading":-0.77072, "vx":1.32911, "vy":-0.28867, "omega":-1.48113, "ax":-0.94659, "ay":-0.30837, "alpha":1.7498, "fx":[-15.62939,-21.77025,-15.95652,-9.76031], "fy":[0.92574,-4.95058,-11.18276,-5.35397]}, + {"t":2.10467, "x":11.70816, "y":5.77047, "heading":-0.78776, "vx":1.31821, "vy":-0.29222, "omega":-1.46099, "ax":-0.94516, "ay":-0.31331, "alpha":1.87756, "fx":[-15.5672,-22.14277,-15.97005,-9.34141], "fy":[1.30872,-4.90818,-11.75263,-5.53855]}, + {"t":2.11618, "x":11.72327, "y":5.76708, "heading":-0.80458, "vx":1.30733, "vy":-0.29583, "omega":-1.43937, "ax":-0.94383, "ay":-0.31784, "alpha":2.05302, "fx":[-15.37128,-22.75174,-16.1263,-8.68322], "fy":[1.81132,-4.88961,-12.37682,-5.73772]}, + {"t":2.12769, "x":11.73826, "y":5.76366, "heading":-0.82115, "vx":1.29647, "vy":-0.29949, "omega":-1.41574, "ax":-0.94258, "ay":-0.32201, "alpha":2.22042, "fx":[-15.27924,-23.27589,-16.18143,-8.11272], "fy":[2.32196,-4.79885,-13.04954,-5.94476]}, + {"t":2.1392, "x":11.75312, "y":5.76019, "heading":-0.83745, "vx":1.28562, "vy":-0.30319, "omega":-1.39019, "ax":-0.94141, "ay":-0.32587, "alpha":2.42756, "fx":[-15.04716,-23.9773,-16.37895,-7.36772], "fy":[2.95153,-4.72663,-13.7815,-6.17192]}, + {"t":2.15071, "x":11.76785, "y":5.75668, "heading":-0.85345, "vx":1.27478, "vy":-0.30694, "omega":-1.36224, "ax":-0.9403, "ay":-0.32945, "alpha":2.64069, "fx":[-14.91122,-24.67559,-16.48661,-6.62427], "fy":[3.60602,-4.58328,-14.57267,-6.41709]}, + {"t":2.16222, "x":11.78247, "y":5.75312, "heading":-0.86913, "vx":1.26396, "vy":-0.31074, "omega":-1.33185, "ax":-0.93927, "ay":-0.33277, "alpha":2.88248, "fx":[-14.63719,-25.46553,-16.73134,-5.79446], "fy":[4.38494,-4.45605,-15.42936,-6.68831]}, + {"t":2.17373, "x":11.79695, "y":5.74952, "heading":-0.88446, "vx":1.25315, "vy":-0.31457, "omega":-1.29867, "ax":-0.93829, "ay":-0.33587, "alpha":3.14647, "fx":[-14.44249,-26.36488,-16.90556,-4.85043], "fy":[5.17995,-4.2477,-16.34844,-6.97914]}, + {"t":2.18524, "x":11.81131, "y":5.74588, "heading":-0.89941, "vx":1.24235, "vy":-0.31843, "omega":-1.26245, "ax":-0.93737, "ay":-0.33877, "alpha":3.42421, "fx":[-14.12208,-27.23982,-17.20139,-3.93849], "fy":[6.11274,-4.05646,-17.34151,-7.30312]}, + {"t":2.19675, "x":11.82555, "y":5.74219, "heading":-0.91394, "vx":1.23156, "vy":-0.32233, "omega":-1.22304, "ax":-0.93649, "ay":-0.34148, "alpha":3.74726, "fx":[-13.8527,-28.37088,-17.45854,-2.76145], "fy":[7.06784,-3.77593,-18.40676,-7.65411]}, + {"t":2.20826, "x":11.83967, "y":5.73846, "heading":-0.92801, "vx":1.22078, "vy":-0.32626, "omega":-1.1799, "ax":-0.93567, "ay":-0.34402, "alpha":4.06259, "fx":[-13.4805,-29.32552,-17.80431,-1.77809], "fy":[8.17002,-3.51559,-19.55175,-8.04118]}, + {"t":2.21977, "x":11.85366, "y":5.73468, "heading":-0.9416, "vx":1.21001, "vy":-0.33022, "omega":-1.13314, "ax":-0.93488, "ay":-0.34641, "alpha":4.45385, "fx":[-13.12274,-30.72505,-18.16519,-0.32317], "fy":[9.30121,-3.15733,-20.77942,-8.46219]}, + {"t":2.23128, "x":11.86752, "y":5.73086, "heading":-0.95464, "vx":1.19925, "vy":-0.33421, "omega":-1.08188, "ax":-0.93414, "ay":-0.34866, "alpha":4.81039, "fx":[-12.70055,-31.76023,-18.5619,0.73618], "fy":[10.59141,-2.81994,-22.09569,-8.92357]}, + {"t":2.24279, "x":11.88126, "y":5.72699, "heading":-0.96709, "vx":1.1885, "vy":-0.33822, "omega":-1.02651, "ax":-0.93343, "ay":-0.35078, "alpha":5.27841, "fx":[-12.2361,-33.46345,-19.04325,2.50345], "fy":[11.91586,-2.37774,-23.5043,-9.42305]}, + {"t":2.2543, "x":11.89488, "y":5.72307, "heading":-0.97891, "vx":1.17775, "vy":-0.34226, "omega":-0.96575, "ax":-0.93276, "ay":-0.35278, "alpha":5.67911, "fx":[-11.7655,-34.57928,-19.48561,3.63596], "fy":[13.41314,-1.95712,-25.01221,-9.96678]}, + {"t":2.26581, "x":11.90838, "y":5.71911, "heading":-0.99002, "vx":1.16702, "vy":-0.34632, "omega":-0.90038, "ax":-0.93212, "ay":-0.35468, "alpha":6.23438, "fx":[-11.18104,-36.62703,-20.10649,5.76289], "fy":[14.95465,-1.428,-26.62392,-10.55216]}, + {"t":2.27733, "x":11.92175, "y":5.7151, "heading":-1.00039, "vx":1.15629, "vy":-0.3504, "omega":-0.82862, "ax":-0.9315, "ay":-0.35648, "alpha":6.68271, "fx":[-10.66572,-37.82578,-20.58395,6.9646], "fy":[16.68494,-0.92189,-28.34771,-11.18472]}, + {"t":2.28884, "x":11.935, "y":5.71104, "heading":-1.00993, "vx":1.14557, "vy":-0.35451, "omega":-0.7517, "ax":-0.93092, "ay":-0.35819, "alpha":7.33628, "fx":[-9.95372,-40.26187,-21.36196,9.50563], "fy":[18.46749,-0.3045,-30.18698,-11.8591]}, + {"t":2.30035, "x":11.94812, "y":5.70694, "heading":-1.01858, "vx":1.13485, "vy":-0.35863, "omega":-0.66725, "ax":-0.93036, "ay":-0.35981, "alpha":7.83723, "fx":[-9.40575,-41.55414,-21.86303,10.78826], "fy":[20.45874,0.28445,-32.15289,-12.58158]}, + {"t":2.31186, "x":11.96112, "y":5.70278, "heading":-1.02626, "vx":1.12414, "vy":-0.36277, "omega":-0.57704, "ax":-0.92983, "ay":-0.36135, "alpha":8.59938, "fx":[-8.5625,-44.41911,-22.80616,13.7887], "fy":[22.51427,0.98063,-34.24703,-13.34195]}, + {"t":2.32337, "x":11.974, "y":5.69858, "heading":-1.0329, "vx":1.11344, "vy":-0.36693, "omega":-0.47806, "ax":-0.92932, "ay":-0.36282, "alpha":9.15838, "fx":[-7.99718,-45.82007,-23.31131,15.16362], "fy":[24.79884,1.64658,-36.48659,-14.15098]}, + {"t":2.33488, "x":11.98675, "y":5.69434, "heading":-1.0384, "vx":1.10274, "vy":-0.37111, "omega":-0.37264, "ax":-0.92883, "ay":-0.36422, "alpha":10.03945, "fx":[-7.03397,-49.15439,-24.41975,18.67583], "fy":[27.16137,2.40658,-38.86688,-14.9866]}, + {"t":2.34639, "x":11.99938, "y":5.69004, "heading":-1.04269, "vx":1.09205, "vy":-0.3753, "omega":-0.25709, "ax":-0.92835, "ay":-0.36556, "alpha":10.66309, "fx":[-6.47472,-50.68894,-24.90338,20.16613], "fy":[29.77672,3.13082,-41.41399,-15.86838]}, + {"t":2.3579, "x":12.01189, "y":5.6857, "heading":-1.04565, "vx":1.08136, "vy":-0.37951, "omega":-0.13435, "ax":-0.9279, "ay":-0.36684, "alpha":11.6718, "fx":[-5.42097,-54.526,-26.16073,24.23686], "fy":[32.48549,3.92532,-44.11467,-16.75618]}, + {"t":2.36941, "x":12.02428, "y":5.6813, "heading":-1.0472, "vx":1.07068, "vy":-0.38373, "omega":0.0, "ax":-0.93998, "ay":-0.337, "alpha":0.0, "fx":[-15.67026,-15.6644,-15.66762,-15.674], "fy":[-5.64517,-5.55452,-5.63323,-5.63757]}, + {"t":2.40453, "x":12.0613, "y":5.66762, "heading":-1.0472, "vx":1.03767, "vy":-0.39557, "omega":0.0, "ax":-0.9606, "ay":-0.27536, "alpha":0.0, "fx":[-16.07979,-15.99605,-16.04484,-15.93036], "fy":[-4.59157,-4.62995,-4.54472,-4.59456]}, + {"t":2.43965, "x":12.09715, "y":5.65356, "heading":-1.0472, "vx":1.00394, "vy":-0.40524, "omega":0.0, "ax":-0.97618, "ay":-0.21369, "alpha":0.0, "fx":[-16.27359,-16.26828,-16.27125,-16.27688], "fy":[-3.58643,-3.50649,-3.57602,-3.57981]}, + {"t":2.47477, "x":12.13181, "y":5.63919, "heading":-1.0472, "vx":0.96965, "vy":-0.41274, "omega":0.0, "ax":-0.98753, "ay":-0.15299, "alpha":0.0, "fx":[-16.53214,-16.45068,-16.49803,-16.36574], "fy":[-2.5673,-2.58389,-2.50079,-2.54923]}, + {"t":2.50989, "x":12.16525, "y":5.6246, "heading":-1.0472, "vx":0.93497, "vy":-0.41811, "omega":0.0, "ax":-0.9949, "ay":-0.09392, "alpha":0.0, "fx":[-16.58531,-16.58156,-16.58369,-16.58754], "fy":[-1.58215,-1.52734,-1.57508,-1.57767]}, + {"t":2.54501, "x":12.19747, "y":5.60986, "heading":-1.0472, "vx":0.90003, "vy":-0.42141, "omega":0.0, "ax":-0.99865, "ay":-0.03701, "alpha":0.0, "fx":[-16.71925,-16.64232,-16.68688,-16.53988], "fy":[-0.64967,-0.64319,-0.56473,-0.61024]}, + {"t":2.58013, "x":12.22847, "y":5.59504, "heading":-1.0472, "vx":0.86496, "vy":-0.42271, "omega":0.0, "ax":-0.99921, "ay":0.01732, "alpha":0.0, "fx":[-16.65668,-16.65479,-16.65588,-16.65778], "fy":[0.28064,0.30746,0.28405,0.2828]}, + {"t":2.61525, "x":12.25823, "y":5.5802, "heading":-1.0472, "vx":0.82987, "vy":-0.4221, "omega":0.0, "ax":-0.997, "ay":0.0688, "alpha":0.0, "fx":[-16.69189,-16.62127,-16.66198,-16.50307], "fy":[1.09872,1.12887,1.20075,1.15934]}, + {"t":2.65037, "x":12.28676, "y":5.56542, "heading":-1.0472, "vx":0.79485, "vy":-0.41969, "omega":0.0, "ax":-0.99249, "ay":0.11728, "alpha":0.0, "fx":[-16.54425,-16.54437,-16.5443,-16.5442], "fy":[1.95559,1.95349,1.95526,1.95538]}, + {"t":2.68549, "x":12.31406, "y":5.55076, "heading":-1.0472, "vx":0.76, "vy":-0.41557, "omega":0.0, "ax":-0.98608, "ay":0.16268, "alpha":0.0, "fx":[-16.5086,-16.44548,-16.48166,-16.31426], "fy":[2.64942,2.70209,2.76607,2.72954]}, + {"t":2.72061, "x":12.34014, "y":5.53626, "heading":-1.0472, "vx":0.72536, "vy":-0.40986, "omega":0.0, "ax":-0.97817, "ay":0.20503, "alpha":0.0, "fx":[-16.30519,-16.30728,-16.30603,-16.30407], "fy":[3.42674,3.3972,3.42292,3.42433]}, + {"t":2.75573, "x":12.36502, "y":5.52199, "heading":-1.0472, "vx":0.69101, "vy":-0.40266, "omega":0.0, "ax":-0.9691, "ay":0.24442, "alpha":0.0, "fx":[-16.22243,-16.16796,-16.19898,-16.0283], "fy":[4.00057,4.07266,4.12754,4.09655]}, + {"t":2.79085, "x":12.38869, "y":5.508, "heading":-1.0472, "vx":0.65698, "vy":-0.39407, "omega":0.0, "ax":-0.95916, "ay":0.28096, "alpha":0.0, "fx":[-15.98793,-15.99177,-15.98946,-15.98593], "fy":[4.69941,4.64655,4.69264,4.69514]}, + {"t":2.82597, "x":12.41117, "y":5.49434, "heading":-1.0472, "vx":0.62329, "vy":-0.3842, "omega":0.0, "ax":-0.94861, "ay":0.3148, "alpha":0.0, "fx":[-15.87691,-15.83083,-15.85685,-15.68695], "fy":[5.16579,5.25276,5.29882,5.27317]}, + {"t":2.86109, "x":12.43247, "y":5.48104, "heading":-1.0472, "vx":0.58998, "vy":-0.37315, "omega":0.0, "ax":-0.93765, "ay":0.34612, "alpha":0.0, "fx":[-15.62908,-15.63438,-15.63117,-15.62637], "fy":[5.79142,5.71954,5.78228,5.78565]}, + {"t":2.8962, "x":12.45261, "y":5.46815, "heading":-1.0472, "vx":0.55705, "vy":-0.36099, "omega":0.0, "ax":-0.92646, "ay":0.37509, "alpha":0.0, "fx":[-15.50333,-15.46517,-15.48651,-15.31985], "fy":[6.16506,6.26349,6.30128,6.2806]}, + {"t":2.93132, "x":12.47161, "y":5.4557, "heading":-1.0472, "vx":0.52451, "vy":-0.34782, "omega":0.0, "ax":-0.91518, "ay":0.40188, "alpha":0.0, "fx":[-15.25415,-15.26065,-15.25668,-15.25086], "fy":[6.72548,6.63832,6.71445,6.71853]}, + {"t":2.96644, "x":12.48946, "y":5.44373, "heading":-1.0472, "vx":0.49237, "vy":-0.33371, "omega":0.0, "ax":-0.90391, "ay":0.42667, "alpha":0.0, "fx":[-15.12235,-15.09163,-15.1086,-14.94816], "fy":[7.02185,7.12783,7.15788,7.1418]}, + {"t":3.00156, "x":12.5062, "y":5.43228, "heading":-1.0472, "vx":0.46062, "vy":-0.31872, "omega":0.0, "ax":-0.89273, "ay":0.44961, "alpha":0.0, "fx":[-14.87981,-14.88715,-14.88265,-14.87615], "fy":[7.52411,7.42666,7.51183,7.51638]}, + {"t":3.03668, "x":12.52182, "y":5.42136, "heading":-1.0472, "vx":0.42927, "vy":-0.30293, "omega":0.0, "ax":-0.88173, "ay":0.47086, "alpha":0.0, "fx":[-14.74777,-14.72345,-14.73669,-14.58389], "fy":[7.7577,7.86785,7.89127,7.8791]}, + {"t":3.0718, "x":12.53635, "y":5.41101, "heading":-1.0472, "vx":0.39831, "vy":-0.28639, "omega":0.0, "ax":-0.87093, "ay":0.49056, "alpha":0.0, "fx":[-14.51626,-14.52424,-14.51933,-14.51231], "fy":[8.20911,8.10388,8.19589,8.20079]}, + {"t":3.10692, "x":12.54981, "y":5.40126, "heading":-1.0472, "vx":0.36772, "vy":-0.26917, "omega":0.0, "ax":-0.86039, "ay":0.50885, "alpha":0.0, "fx":[-14.38714,-14.36904,-14.37867,-14.23434], "fy":[8.39104,8.50414,8.52118,8.51273]}, + {"t":3.14204, "x":12.56219, "y":5.39212, "heading":-1.0472, "vx":0.3375, "vy":-0.2513, "omega":0.0, "ax":-0.85012, "ay":0.52584, "alpha":0.0, "fx":[-14.1693,-14.17767,-14.1725,-14.1652], "fy":[8.79857,8.68893,8.78483,8.78992]}, + {"t":3.17716, "x":12.57352, "y":5.38362, "heading":-1.0472, "vx":0.30765, "vy":-0.23283, "omega":0.0, "ax":-0.84015, "ay":0.54166, "alpha":0.0, "fx":[-14.04538,-14.03207,-14.03893,-13.90334], "fy":[8.93965,9.05275,9.0649,9.05928]}, + {"t":3.21228, "x":12.5838, "y":5.37577, "heading":-1.0472, "vx":0.27814, "vy":-0.21381, "omega":0.0, "ax":-0.83048, "ay":0.55639, "alpha":0.0, "fx":[-13.84182,-13.85045,-13.84511,-13.83762], "fy":[9.30857,9.19603,9.2945,9.29972]}, + {"t":3.2474, "x":12.59306, "y":5.36861, "heading":-1.0472, "vx":0.24897, "vy":-0.19427, "omega":0.0, "ax":-0.82112, "ay":0.57013, "alpha":0.0, "fx":[-13.72369,-13.71507,-13.71925,-13.59283], "fy":[9.41647,9.52892,9.53631,9.53341]}, + {"t":3.28252, "x":12.6013, "y":5.36214, "heading":-1.0472, "vx":0.22014, "vy":-0.17424, "omega":0.0, "ax":-0.81207, "ay":0.58297, "alpha":0.0, "fx":[-13.53488,-13.54362,-13.5382,-13.53064], "fy":[9.75193,9.63836,9.73775,9.74302]}, + {"t":3.31764, "x":12.60853, "y":5.35638, "heading":-1.0472, "vx":0.19162, "vy":-0.15377, "omega":0.0, "ax":-0.80333, "ay":0.59497, "alpha":0.0, "fx":[-13.42314,-13.41826,-13.4203,-13.3026], "fy":[9.83343,9.94391,9.94755,9.94679]}, + {"t":3.35276, "x":12.61476, "y":5.35134, "heading":-1.0472, "vx":0.1634, "vy":-0.13287, "omega":0.0, "ax":-0.79489, "ay":0.60622, "alpha":0.0, "fx":[-13.24839,-13.25718,-13.25172,-13.24414], "fy":[10.13967,10.02572,10.12546,10.13074]}, + {"t":3.38788, "x":12.62001, "y":5.34705, "heading":-1.0472, "vx":0.13549, "vy":-0.11158, "omega":0.0, "ax":-0.78674, "ay":0.61677, "alpha":0.0, "fx":[-13.14299,-13.14147,-13.1416,-13.03224], "fy":[10.19965,10.30787,10.30813,10.30927]}, + {"t":3.423, "x":12.62428, "y":5.34351, "heading":-1.0472, "vx":0.10786, "vy":-0.08992, "omega":0.0, "ax":-0.77888, "ay":0.62667, "alpha":0.0, "fx":[-12.98164,-12.99038,-12.98494,-12.97744], "fy":[10.48031,10.36742,10.46625,10.47147]}, + {"t":3.45812, "x":12.62759, "y":5.34074, "heading":-1.0472, "vx":0.0805, "vy":-0.06791, "omega":0.0, "ax":-0.77131, "ay":0.63599, "alpha":0.0, "fx":[-12.88219,-12.8838,-12.88216,-12.78099], "fy":[10.52312,10.62876,10.62591,10.62879]}, + {"t":3.49324, "x":12.62994, "y":5.33875, "heading":-1.0472, "vx":0.05342, "vy":-0.04558, "omega":0.0, "ax":-0.764, "ay":0.64476, "alpha":0.0, "fx":[-12.73354,-12.74221,-12.73681,-12.72938], "fy":[10.78146,10.66965,10.76756,10.77273]}, + {"t":3.52836, "x":12.63135, "y":5.33754, "heading":-1.0472, "vx":0.02658, "vy":-0.02293, "omega":0.0, "ax":-0.75695, "ay":0.65303, "alpha":0.0, "fx":[-12.63389,-12.57397,-12.64633,-12.61793], "fy":[10.90429,10.89129,10.86652,10.88063]}, + {"t":3.56348, "x":12.63181, "y":5.33714, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/bargeToOppG.traj b/src/main/deploy/choreo/bargeToOppG.traj new file mode 100644 index 0000000..ee62507 --- /dev/null +++ b/src/main/deploy/choreo/bargeToOppG.traj @@ -0,0 +1,146 @@ +{ + "name":"bargeToOppG", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.7, "y":5.08871412277, "heading":0.0, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.160315799713134, "y":5.6167577, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.365684200286866, "y":5.6167577, "heading":0.0, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":10.99120807647705, "y":4.491031169891357, "heading":-0.5538539145382533, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":11.698677, "y":4.3309, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"barge.x", "val":7.7}, "y":{"exp":"barge.y", "val":5.08871412277}, "heading":{"exp":"barge.heading", "val":0.0}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.160315799713134 m", "val":8.160315799713134}, "y":{"exp":"5.6167577 m", "val":5.6167577}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.365684200286866 m", "val":9.365684200286866}, "y":{"exp":"5.6167577 m", "val":5.6167577}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"10.99120807647705 m", "val":10.99120807647705}, "y":{"exp":"4.491031169891357 m", "val":4.491031169891357}, "heading":{"exp":"-0.5538539145382533 rad", "val":-0.5538539145382533}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"FieldMaxX - G.x", "val":11.698677}, "y":{"exp":"FieldMaxY - G.y", "val":4.3309}, "heading":{"exp":"G.heading - 180 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.56807,1.17841,1.97478,3.18209], + "samples":[ + {"t":0.0, "x":7.7, "y":5.08871, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.23837, "ay":5.86759, "alpha":-0.0095, "fx":[20.65429,20.55758,20.68246,20.67806], "fy":[97.72849,98.07402,97.71269,97.72455]}, + {"t":0.04058, "x":7.70102, "y":5.09354, "heading":0.0, "vx":0.05025, "vy":0.23809, "omega":-0.00039, "ax":1.45228, "ay":5.81972, "alpha":-0.00801, "fx":[24.22707,24.1495,24.23019,24.2287], "fy":[96.95291,97.19072,96.95215,96.95165]}, + {"t":0.08115, "x":7.70425, "y":5.108, "heading":-0.00002, "vx":0.10918, "vy":0.47423, "omega":-0.00071, "ax":1.73908, "ay":5.7402, "alpha":0.01094, "fx":[28.95177,29.06856,29.04578,28.89266], "fy":[95.89539,95.61119,95.61409,95.62501]}, + {"t":0.12173, "x":7.71012, "y":5.13196, "heading":-0.00004, "vx":0.17974, "vy":0.70715, "omega":-0.00027, "ax":2.14054, "ay":5.60241, "alpha":-0.01808, "fx":[35.74339,35.59433,35.69036,35.69895], "fy":[93.28451,93.65806,93.32292,93.29257]}, + {"t":0.16231, "x":7.71917, "y":5.16527, "heading":-0.00006, "vx":0.2666, "vy":0.93447, "omega":-0.001, "ax":2.72991, "ay":5.33934, "alpha":0.02989, "fx":[45.34279,45.5447,45.47563,45.66198], "fy":[89.34049,88.91361,88.92894,88.83416]}, + {"t":0.20288, "x":7.73224, "y":5.20758, "heading":-0.0001, "vx":0.37737, "vy":1.15113, "omega":0.00021, "ax":3.62845, "ay":4.77319, "alpha":-0.03148, "fx":[60.60079,60.31412,60.49955,60.5234], "fy":[79.37463,80.0259,79.46832,79.39825]}, + {"t":0.24346, "x":7.75054, "y":5.25822, "heading":-0.00009, "vx":0.5246, "vy":1.3448, "omega":-0.00106, "ax":4.92179, "ay":3.42167, "alpha":0.04802, "fx":[81.72462,81.91649,81.82721,82.707], "fy":[57.5843,57.01077,57.04326,56.51205]}, + {"t":0.28404, "x":7.77587, "y":5.31561, "heading":-0.00013, "vx":0.72431, "vy":1.48364, "omega":0.00088, "ax":5.96682, "ay":0.56177, "alpha":-0.03647, "fx":[99.50514,99.48213,99.43022,99.43887], "fy":[9.21314,9.62541,9.39977,9.21968]}, + {"t":0.32461, "x":7.81018, "y":5.37627, "heading":-0.00009, "vx":0.96642, "vy":1.50644, "omega":-0.0006, "ax":5.38578, "ay":-2.63079, "alpha":0.01982, "fx":[89.48707,89.64643,89.5469,90.43293], "fy":[-44.409,-43.72184,-43.75602,-43.52912]}, + {"t":0.36519, "x":7.85382, "y":5.43523, "heading":-0.00012, "vx":1.18496, "vy":1.39969, "omega":0.00021, "ax":4.12773, "ay":-4.34825, "alpha":0.01016, "fx":[68.89719,68.73672,68.81816,68.77702], "fy":[-72.40306,-72.76168,-72.32312,-72.44512]}, + {"t":0.40576, "x":7.9053, "y":5.48844, "heading":-0.00011, "vx":1.35244, "vy":1.22325, "omega":0.00062, "ax":3.17605, "ay":-5.08639, "alpha":0.00638, "fx":[52.70435,52.91866,52.78228,53.36749], "fy":[-85.14105,-84.66987,-84.70433,-84.6356]}, + {"t":0.44634, "x":7.96279, "y":5.53389, "heading":-0.00009, "vx":1.48132, "vy":1.01687, "omega":0.00088, "ax":2.54546, "ay":-5.43029, "alpha":-0.00412, "fx":[42.5129,42.42376,42.41131,42.3783], "fy":[-90.48784,-90.64947,-90.42447,-90.51931]}, + {"t":0.48692, "x":8.025, "y":5.57068, "heading":-0.00005, "vx":1.5846, "vy":0.79652, "omega":0.00071, "ax":2.11779, "ay":-5.61144, "alpha":-0.00513, "fx":[35.18854,35.30814,35.16489,35.54839], "fy":[-93.74797,-93.44817,-93.4735,-93.49026]}, + {"t":0.52749, "x":8.09104, "y":5.59838, "heading":-0.00002, "vx":1.67054, "vy":0.56883, "omega":0.0005, "ax":1.81431, "ay":-5.71714, "alpha":-0.01242, "fx":[30.31994,30.28089,30.2006,30.17311], "fy":[-95.29306,-95.36944,-95.22714,-95.3184]}, + {"t":0.56807, "x":8.16032, "y":5.61676, "heading":0.0, "vx":1.74415, "vy":0.33685, "omega":0.0, "ax":1.79954, "ay":-5.71766, "alpha":0.0, "fx":[30.15816,29.99995,29.9406,29.89115], "fy":[-95.205,-95.6904,-95.09396,-95.25299]}, + {"t":0.60019, "x":8.21727, "y":5.62463, "heading":0.0, "vx":1.80196, "vy":0.15318, "omega":0.0, "ax":2.55568, "ay":-5.41934, "alpha":0.0, "fx":[42.17588,42.5791,42.66103,42.99162], "fy":[-91.06994,-90.13237,-90.09444,-90.05455]}, + {"t":0.63232, "x":8.27648, "y":5.62675, "heading":0.0, "vx":1.88406, "vy":-0.02091, "omega":0.0, "ax":3.56816, "ay":-0.98309, "alpha":0.0, "fx":[59.31146,59.22619,59.70836,59.6723], "fy":[-16.58306,-15.45272,-16.90109,-16.61377]}, + {"t":0.66444, "x":8.33884, "y":5.62557, "heading":0.0, "vx":1.99868, "vy":-0.05249, "omega":0.0, "ax":0.0107, "ay":0.46462, "alpha":0.0, "fx":[0.40053,0.39994,0.72309,-0.81031], "fy":[8.08794,7.52309,7.52299,7.84583]}, + {"t":0.69656, "x":8.40305, "y":5.62413, "heading":0.0, "vx":1.99902, "vy":-0.03756, "omega":0.0, "ax":0.00542, "ay":0.33712, "alpha":0.0, "fx":[0.20236,0.20208,-0.02151,-0.02162], "fy":[5.73157,5.0605,5.95484,5.73135]}, + {"t":0.72869, "x":8.46727, "y":5.6231, "heading":0.0, "vx":1.9992, "vy":-0.02673, "omega":0.0, "ax":0.00276, "ay":0.24171, "alpha":0.0, "fx":[0.01859,0.0195,0.35989,-0.21384], "fy":[3.60777,4.05623,4.05621,4.39671]}, + {"t":0.76081, "x":8.53149, "y":5.62236, "heading":0.0, "vx":1.99928, "vy":-0.01897, "omega":0.0, "ax":0.00152, "ay":0.19113, "alpha":0.0, "fx":[0.07477,0.0747,-0.02407,-0.0241], "fy":[3.23553,2.95259,3.32077,3.23547]}, + {"t":0.79293, "x":8.59571, "y":5.62185, "heading":0.0, "vx":1.99933, "vy":-0.01283, "omega":0.0, "ax":0.00084, "ay":0.1647, "alpha":0.0, "fx":[0.03531,0.03537,0.086,-0.10076], "fy":[2.75895,2.72419,2.72419,2.77486]}, + {"t":0.82506, "x":8.65994, "y":5.62152, "heading":0.0, "vx":1.99936, "vy":-0.00754, "omega":0.0, "ax":0.00039, "ay":0.15131, "alpha":0.0, "fx":[-0.0135,-0.0135,0.02638,0.02638], "fy":[2.5024,2.63479,2.44975,2.5024]}, + {"t":0.85718, "x":8.72416, "y":5.62136, "heading":0.0, "vx":1.99937, "vy":-0.00268, "omega":0.0, "ax":0.00002, "ay":0.14675, "alpha":0.0, "fx":[0.03936,0.03936,-0.00825,-0.06891], "fy":[2.61071,2.40723,2.40723,2.35962]}, + {"t":0.8893, "x":8.78839, "y":5.62135, "heading":0.0, "vx":1.99937, "vy":0.00204, "omega":0.0, "ax":-0.00033, "ay":0.15018, "alpha":0.0, "fx":[-0.02764,-0.02764,0.0165,0.0165], "fy":[2.48129,2.62654,2.42434,2.48129]}, + {"t":0.92143, "x":8.85261, "y":5.62149, "heading":0.0, "vx":1.99936, "vy":0.00686, "omega":0.0, "ax":-0.00077, "ay":0.16213, "alpha":0.0, "fx":[-0.0061,-0.00609,-0.00695,-0.03203], "fy":[2.72347,2.69587,2.69587,2.69507]}, + {"t":0.95355, "x":8.91684, "y":5.6218, "heading":0.0, "vx":1.99934, "vy":0.01207, "omega":0.0, "ax":-0.0014, "ay":0.18621, "alpha":0.0, "fx":[0.01577,0.01577,-0.06253,-0.06253], "fy":[3.14323,2.92123,3.20862,3.14322]}, + {"t":0.98567, "x":8.98106, "y":5.62228, "heading":0.0, "vx":1.99929, "vy":0.01805, "omega":0.0, "ax":-0.00254, "ay":0.23312, "alpha":0.0, "fx":[-0.12456,-0.12456,0.0531,0.0266], "fy":[3.46139,3.96819,3.96819,4.14617]}, + {"t":1.01779, "x":9.04529, "y":5.62298, "heading":0.0, "vx":1.99921, "vy":0.02554, "omega":0.0, "ax":-0.00492, "ay":0.3205, "alpha":0.0, "fx":[0.0262,0.02624,-0.1902,-0.19019], "fy":[5.45079,4.79675,5.672,5.4508]}, + {"t":1.04992, "x":9.1095, "y":5.62397, "heading":0.0, "vx":1.99905, "vy":0.03584, "omega":0.0, "ax":-0.00989, "ay":0.45783, "alpha":0.0, "fx":[-0.15634,-0.15641,-0.11555,-0.23103], "fy":[7.61641,7.62342,7.6234,7.66422]}, + {"t":1.08204, "x":9.17372, "y":5.62535, "heading":0.0, "vx":1.99874, "vy":0.05054, "omega":0.0, "ax":0.00882, "ay":-0.44168, "alpha":0.0, "fx":[-0.06324,-0.06436,0.35793,0.35772], "fy":[-7.57325,-6.341,-7.96269,-7.57371]}, + {"t":1.11416, "x":9.23793, "y":5.62675, "heading":0.0, "vx":1.99902, "vy":0.03636, "omega":0.0, "ax":-0.19142, "ay":-5.98061, "alpha":0.0, "fx":[-3.35949,-3.3835,-2.84619,-3.17458], "fy":[-100.48648,-99.50845,-99.51812,-99.2623]}, + {"t":1.14629, "x":9.30204, "y":5.62483, "heading":0.0, "vx":1.99287, "vy":-0.15576, "omega":0.0, "ax":-0.72701, "ay":-5.95044, "alpha":0.0, "fx":[-12.04575,-12.0091,-12.21441,-12.20646], "fy":[-99.10272,-99.5307,-99.03432,-99.09628]}, + {"t":1.17841, "x":9.36568, "y":5.61676, "heading":0.0, "vx":1.96952, "vy":-0.34691, "omega":0.0, "ax":2.67766, "ay":-5.36244, "alpha":-0.02617, "fx":[44.81164,44.67996,44.55153,44.49782], "fy":[-89.38031,-89.55575,-89.18921,-89.43217]}, + {"t":1.21304, "x":9.43548, "y":5.60153, "heading":0.0, "vx":2.06223, "vy":-0.53258, "omega":-0.00091, "ax":2.6287, "ay":-5.38961, "alpha":0.01589, "fx":[43.55464,43.71803,43.36029,44.644], "fy":[-90.15723,-89.7084,-89.76512,-89.73829]}, + {"t":1.24766, "x":9.50846, "y":5.57986, "heading":-0.00003, "vx":2.15325, "vy":-0.71919, "omega":-0.00036, "ax":2.56449, "ay":-5.41989, "alpha":-0.03732, "fx":[42.91258,42.82662,42.65229,42.60359], "fy":[-90.38451,-90.37613,-90.196,-90.43135]}, + {"t":1.28228, "x":9.58456, "y":5.55171, "heading":-0.00004, "vx":2.24204, "vy":-0.90686, "omega":-0.00165, "ax":2.47601, "ay":-5.46012, "alpha":0.02276, "fx":[40.91689,41.11734,40.77618,42.28498], "fy":[-91.44912,-90.84884,-90.89448,-90.87779]}, + {"t":1.31691, "x":9.66367, "y":5.51704, "heading":-0.0001, "vx":2.32777, "vy":-1.09591, "omega":-0.00086, "ax":2.3531, "ay":-5.51314, "alpha":-0.02109, "fx":[39.4115,39.24045,39.14485,39.10349], "fy":[-91.88324,-92.10544,-91.69364,-91.92303]}, + {"t":1.35153, "x":9.74568, "y":5.47579, "heading":-0.00013, "vx":2.40925, "vy":-1.2868, "omega":-0.00159, "ax":2.16005, "ay":-5.59, "alpha":0.03016, "fx":[35.53307,35.77033,35.46282,37.26152], "fy":[-93.79086,-92.9586,-92.99006,-92.99121]}, + {"t":1.38616, "x":9.83039, "y":5.42788, "heading":-0.00019, "vx":2.48404, "vy":-1.48035, "omega":-0.00055, "ax":1.83452, "ay":-5.70249, "alpha":0.00825, "fx":[30.77487,30.53462,30.51975,30.49319], "fy":[-94.93072,-95.59026,-94.75414,-94.95625]}, + {"t":1.42078, "x":9.9175, "y":5.37321, "heading":-0.0002, "vx":2.54756, "vy":-1.6778, "omega":-0.00026, "ax":1.14836, "ay":-5.8741, "alpha":-0.00814, "fx":[18.75164,18.90045,18.71376,20.20475], "fy":[-98.91,-97.5585,-97.56583,-97.63926]}, + {"t":1.45541, "x":10.0064, "y":5.31159, "heading":-0.00021, "vx":2.58732, "vy":-1.88119, "omega":-0.00054, "ax":-0.94187, "ay":-5.8973, "alpha":0.08662, "fx":[-15.64976,-15.52198,-15.8178,-15.81256], "fy":[-97.77079,-100.04384,-97.63964,-97.76609]}, + {"t":1.49003, "x":10.09542, "y":5.24292, "heading":-0.00023, "vx":2.55471, "vy":-2.08538, "omega":0.00246, "ax":-3.8746, "ay":-4.5085, "alpha":-0.27362, "fx":[-63.53579,-63.83021,-63.63741,-67.34767], "fy":[-76.84595,-74.71465,-74.73916,-74.31813]}, + {"t":1.52466, "x":10.18155, "y":5.16801, "heading":-0.00015, "vx":2.42055, "vy":-2.24149, "omega":-0.00702, "ax":-4.11701, "ay":-4.18145, "alpha":0.19317, "fx":[-68.6703,-68.49279,-68.67225,-68.67897], "fy":[-68.71169,-72.66407,-68.71746,-68.71765]}, + {"t":1.55928, "x":10.26289, "y":5.0879, "heading":-0.00039, "vx":2.278, "vy":-2.38627, "omega":-0.00033, "ax":0.02368, "ay":1.39339, "alpha":0.2427, "fx":[-0.19553,-0.20506,-0.0407,2.02034], "fy":[24.82518,22.63955,22.64161,22.80241]}, + {"t":1.59391, "x":10.34178, "y":5.00611, "heading":-0.0004, "vx":2.27882, "vy":-2.33802, "omega":0.00808, "ax":-2.8887, "ay":5.2266, "alpha":-0.16839, "fx":[-48.44592,-47.85568,-48.18704,-48.1242], "fy":[86.2789,89.84314,86.04291,86.33493]}, + {"t":1.62853, "x":10.41896, "y":4.92829, "heading":-0.00012, "vx":2.1788, "vy":-2.15705, "omega":0.00224, "ax":-3.03593, "ay":5.15916, "alpha":0.00085, "fx":[-49.8067,-50.49216,-50.23428,-51.89699], "fy":[87.4655,85.5139,85.56308,85.46042]}, + {"t":1.66316, "x":10.49258, "y":4.85669, "heading":-0.00004, "vx":2.07368, "vy":-1.97842, "omega":0.00227, "ax":-3.02069, "ay":5.17352, "alpha":-0.055, "fx":[-50.59405,-50.0858,-50.4001,-50.33355], "fy":[85.95648,87.19109,85.798,86.01489]}, + {"t":1.69778, "x":10.56257, "y":4.79129, "heading":0.00003, "vx":1.96909, "vy":-1.79929, "omega":0.00037, "ax":-3.03853, "ay":5.16576, "alpha":0.00892, "fx":[-50.16317,-50.65411,-50.40807,-51.37776], "fy":[86.99733,85.79679,85.85337,85.79514]}, + {"t":1.73241, "x":10.62892, "y":4.73209, "heading":0.00005, "vx":1.86388, "vy":-1.62042, "omega":0.00068, "ax":-3.04004, "ay":5.16647, "alpha":-0.02914, "fx":[-50.85215,-50.51298,-50.69587,-50.64305], "fy":[85.94766,86.69846,85.84369,86.00061]}, + {"t":1.76703, "x":10.69164, "y":4.67908, "heading":0.00007, "vx":1.75862, "vy":-1.44154, "omega":-0.00033, "ax":-3.04553, "ay":5.16431, "alpha":0.00356, "fx":[-50.39329,-50.79491,-50.57485,-51.30705], "fy":[86.68511,85.86765,85.92382,85.86945]}, + {"t":1.80165, "x":10.7507, "y":4.63226, "heading":0.00006, "vx":1.65317, "vy":-1.26272, "omega":-0.00021, "ax":-3.04712, "ay":5.16413, "alpha":-0.01181, "fx":[-50.91621,-50.71098,-50.79802,-50.75058], "fy":[85.98299,86.41623,85.90535,86.0299]}, + {"t":1.83628, "x":10.80612, "y":4.59164, "heading":0.00005, "vx":1.54767, "vy":-1.08392, "omega":-0.00062, "ax":-3.04969, "ay":5.16319, "alpha":0.00768, "fx":[-50.60185,-50.8659,-50.71005,-51.16942], "fy":[86.50695,85.90656,85.9484,85.90938]}, + {"t":1.8709, "x":10.85788, "y":4.5572, "heading":0.00003, "vx":1.44207, "vy":-0.90514, "omega":-0.00035, "ax":-3.05092, "ay":5.1629, "alpha":-0.00381, "fx":[-50.9448,-50.82486,-50.85213,-50.80775], "fy":[85.99563,86.28439,85.93371,86.03847]}, + {"t":1.90553, "x":10.90598, "y":4.52895, "heading":0.00002, "vx":1.33644, "vy":-0.72638, "omega":-0.00048, "ax":-3.05245, "ay":5.16235, "alpha":0.01201, "fx":[-50.72725,-50.92798,-50.78798,-51.08809], "fy":[86.39864,85.9196,85.96077,85.93664]}, + {"t":1.94015, "x":10.95042, "y":4.5069, "heading":0.0, "vx":1.23075, "vy":-0.54764, "omega":-0.00007, "ax":-3.05334, "ay":5.16211, "alpha":0.0019, "fx":[-50.96671,-50.89776,-50.88591,-50.84059], "fy":[86.00315,86.2022,85.94812,86.04619]}, + {"t":1.97478, "x":10.99121, "y":4.49103, "heading":0.0, "vx":1.12503, "vy":-0.3689, "omega":0.0, "ax":-0.57799, "ay":0.81058, "alpha":0.0, "fx":[-9.47828,-9.88767,-9.58638,-9.58689], "fy":[13.46405,13.55922,13.56097,13.46346]}, + {"t":2.00927, "x":11.02967, "y":4.47879, "heading":0.0, "vx":1.10509, "vy":-0.34094, "omega":0.0, "ax":-0.6879, "ay":0.7231, "alpha":0.0, "fx":[-11.50629,-11.50465,-11.42871,-11.42814], "fy":[12.01501,12.25794,11.92667,12.01551]}, + {"t":2.04377, "x":11.06738, "y":4.46746, "heading":0.0, "vx":1.08136, "vy":-0.316, "omega":0.0, "ax":-0.76256, "ay":0.64424, "alpha":0.0, "fx":[-12.60266,-12.88399,-12.67957,-12.68007], "fy":[10.70778,10.77017,10.77181,10.70719]}, + {"t":2.07826, "x":11.10423, "y":4.45694, "heading":0.0, "vx":1.05506, "vy":-0.29377, "omega":0.0, "ax":-0.8145, "ay":0.57753, "alpha":0.0, "fx":[-13.60464,-13.60358,-13.55059,-13.55021], "fy":[9.60019,9.76963,9.5385,9.60053]}, + {"t":2.11276, "x":11.14014, "y":4.44715, "heading":0.0, "vx":1.02696, "vy":-0.27385, "omega":0.0, "ax":-0.85142, "ay":0.52188, "alpha":0.0, "fx":[-14.0935,-14.34964,-14.16375,-14.16419], "fy":[8.67095,8.7276,8.72899,8.67045]}, + {"t":2.14725, "x":11.17506, "y":4.43802, "heading":0.0, "vx":0.99759, "vy":-0.25585, "omega":0.0, "ax":-0.87834, "ay":0.47547, "alpha":0.0, "fx":[-14.66104,-14.66038,-14.62234,-14.6221], "fy":[7.9065,8.0279,7.86228,7.90671]}, + {"t":2.18175, "x":11.20895, "y":4.42947, "heading":0.0, "vx":0.96729, "vy":-0.23945, "omega":0.0, "ax":-0.89844, "ay":0.43655, "alpha":0.0, "fx":[-14.88851,-15.11575,-14.9509,-14.95125], "fy":[7.25178,7.30207,7.30319,7.25137]}, + {"t":2.21624, "x":11.24178, "y":4.42147, "heading":0.0, "vx":0.9363, "vy":-0.22439, "omega":0.0, "ax":-0.9138, "ay":0.40365, "alpha":0.0, "fx":[-15.24703,-15.2466,-15.21858,-15.21842], "fy":[6.71438,6.8037,6.68184,6.71453]}, + {"t":2.25074, "x":11.27353, "y":4.41397, "heading":0.0, "vx":0.90478, "vy":-0.21047, "omega":0.0, "ax":-0.92578, "ay":0.37558, "alpha":0.0, "fx":[-15.35432,-15.55549,-15.40956,-15.40984], "fy":[6.23835,6.28294,6.28384,6.23802]}, + {"t":2.28523, "x":11.30419, "y":4.40694, "heading":0.0, "vx":0.87284, "vy":-0.19751, "omega":0.0, "ax":-0.93529, "ay":0.35143, "alpha":0.0, "fx":[-15.60156,-15.60127,-15.5803,-15.5802], "fy":[5.84755,5.91439,5.82315,5.84765]}, + {"t":2.31972, "x":11.33374, "y":4.40033, "heading":0.0, "vx":0.84058, "vy":-0.18539, "omega":0.0, "ax":-0.94296, "ay":0.33047, "alpha":0.0, "fx":[-15.64925,-15.82858,-15.69847,-15.69871], "fy":[5.48878,5.52861,5.52933,5.48851]}, + {"t":2.35422, "x":11.36218, "y":4.39413, "heading":0.0, "vx":0.80806, "vy":-0.17399, "omega":0.0, "ax":-0.94925, "ay":0.31214, "alpha":0.0, "fx":[-15.83169,-15.83148,-15.81541,-15.81533], "fy":[5.19504,5.24633,5.17626,5.19511]}, + {"t":2.38871, "x":11.38949, "y":4.38832, "heading":0.0, "vx":0.77531, "vy":-0.16322, "omega":0.0, "ax":-0.95446, "ay":0.29598, "alpha":0.0, "fx":[-15.84784,-16.00907,-15.89207,-15.89227], "fy":[4.91589,4.95176,4.95236,4.91567]}, + {"t":2.42321, "x":11.41566, "y":4.38286, "heading":0.0, "vx":0.74239, "vy":-0.15301, "omega":0.0, "ax":-0.95882, "ay":0.28166, "alpha":0.0, "fx":[-15.98944,-15.98929,-15.97696,-15.9769], "fy":[4.68883,4.72828,4.67429,4.68888]}, + {"t":2.4577, "x":11.4407, "y":4.37775, "heading":0.0, "vx":0.70931, "vy":-0.1433, "omega":0.0, "ax":-0.96253, "ay":0.26887, "alpha":0.0, "fx":[-15.98828,-16.13431,-16.02835,-16.02851], "fy":[4.46558,4.4981,4.4986,4.46539]}, + {"t":2.4922, "x":11.46459, "y":4.37297, "heading":0.0, "vx":0.67611, "vy":-0.13402, "omega":0.0, "ax":-0.96569, "ay":0.25739, "alpha":0.0, "fx":[-16.10248,-16.10236,-16.09289,-16.09285], "fy":[4.28583,4.31624,4.27455,4.28587]}, + {"t":2.52669, "x":11.48734, "y":4.3685, "heading":0.0, "vx":0.6428, "vy":-0.12514, "omega":0.0, "ax":-0.96842, "ay":0.24704, "alpha":0.0, "fx":[-16.09151,-16.22486,-16.12807,-16.12822], "fy":[4.10312,4.13287,4.13329,4.10296]}, + {"t":2.56119, "x":11.50894, "y":4.36433, "heading":0.0, "vx":0.60939, "vy":-0.11662, "omega":0.0, "ax":-0.9708, "ay":0.23766, "alpha":0.0, "fx":[-16.18639,-16.18631,-16.17911,-16.17908], "fy":[3.95802,3.98129,3.94928,3.95805]}, + {"t":2.59568, "x":11.52938, "y":4.36045, "heading":0.0, "vx":0.57591, "vy":-0.10842, "omega":0.0, "ax":-0.97287, "ay":0.22912, "alpha":0.0, "fx":[-16.16986,-16.29243,-16.20346,-16.20358], "fy":[3.80555,3.83293,3.83329,3.80541]}, + {"t":2.63018, "x":11.54867, "y":4.35685, "heading":0.0, "vx":0.54235, "vy":-0.10052, "omega":0.0, "ax":-0.9747, "ay":0.22131, "alpha":0.0, "fx":[-16.25054,-16.25048,-16.24512,-16.2451], "fy":[3.68647,3.70395,3.67982,3.6865]}, + {"t":2.66467, "x":11.5668, "y":4.35351, "heading":0.0, "vx":0.50873, "vy":-0.09289, "omega":0.0, "ax":-0.97632, "ay":0.21415, "alpha":0.0, "fx":[-16.2309,-16.34426,-16.26196,-16.26207], "fy":[3.55711,3.58246,3.58278,3.557]}, + {"t":2.69917, "x":11.58377, "y":4.35043, "heading":0.0, "vx":0.47505, "vy":-0.0855, "omega":0.0, "ax":-0.97776, "ay":0.20756, "alpha":0.0, "fx":[-16.30081,-16.30076,-16.29689,-16.29687], "fy":[3.45803,3.47085,3.45304,3.45805]}, + {"t":2.73366, "x":11.59957, "y":4.34761, "heading":0.0, "vx":0.44132, "vy":-0.07834, "omega":0.0, "ax":-0.97905, "ay":0.20148, "alpha":0.0, "fx":[-16.27953,-16.38491,-16.30839,-16.30849], "fy":[3.34672,3.37031,3.37059,3.34662]}, + {"t":2.76815, "x":11.61421, "y":4.34503, "heading":0.0, "vx":0.40755, "vy":-0.07139, "omega":0.0, "ax":-0.98021, "ay":0.19584, "alpha":0.0, "fx":[-16.34099,-16.34095,-16.33835,-16.33833], "fy":[3.26331,3.27216,3.25973,3.26332]}, + {"t":2.80265, "x":11.62769, "y":4.34268, "heading":0.0, "vx":0.37374, "vy":-0.06463, "omega":0.0, "ax":-0.98126, "ay":0.19061, "alpha":0.0, "fx":[-16.319,-16.41742,-16.34594,-16.34602], "fy":[3.16634,3.18839,3.18864,3.16625]}, + {"t":2.83714, "x":11.63999, "y":4.34056, "heading":0.0, "vx":0.33989, "vy":-0.05806, "omega":0.0, "ax":-0.98221, "ay":0.18574, "alpha":0.0, "fx":[-16.37369,-16.37366,-16.37212,-16.37211], "fy":[3.09542,3.10094,3.09302,3.09543]}, + {"t":2.87164, "x":11.65113, "y":4.33867, "heading":0.0, "vx":0.30601, "vy":-0.05165, "omega":0.0, "ax":-0.98307, "ay":0.18119, "alpha":0.0, "fx":[-16.35155,-16.44385,-16.3768,-16.37688], "fy":[3.01003,3.03074,3.03096,3.00995]}, + {"t":2.90613, "x":11.6611, "y":4.337, "heading":0.0, "vx":0.2721, "vy":-0.0454, "omega":0.0, "ax":-0.98385, "ay":0.17694, "alpha":0.0, "fx":[-16.40071,-16.40069,-16.40007,-16.40006], "fy":[2.94921,2.95188,2.94784,2.94922]}, + {"t":2.94063, "x":11.66991, "y":4.33554, "heading":0.0, "vx":0.23816, "vy":-0.0393, "omega":0.0, "ax":-0.98457, "ay":0.17296, "alpha":0.0, "fx":[-16.37876,-16.46566,-16.40253,-16.4026], "fy":[2.87331,2.89282,2.89303,2.87324]}, + {"t":2.97512, "x":11.67753, "y":4.33428, "heading":0.0, "vx":0.2042, "vy":-0.03333, "omega":0.0, "ax":-0.98524, "ay":0.16921, "alpha":0.0, "fx":[-16.42333,-16.42331,-16.42349,-16.42348], "fy":[2.82079,2.82096,2.82029,2.82079]}, + {"t":3.00962, "x":11.68399, "y":4.33323, "heading":0.0, "vx":0.17021, "vy":-0.02749, "omega":0.0, "ax":-0.98584, "ay":0.16569, "alpha":0.0, "fx":[-16.4018,-16.48387,-16.42424,-16.4243], "fy":[2.75275,2.77119,2.77138,2.75268]}, + {"t":3.04411, "x":11.68928, "y":4.33238, "heading":0.0, "vx":0.13621, "vy":-0.02178, "omega":0.0, "ax":-0.98641, "ay":0.16237, "alpha":0.0, "fx":[-16.44249,-16.44248,-16.44335,-16.44334], "fy":[2.70709,2.70511,2.70736,2.7071]}, + {"t":3.07861, "x":11.69339, "y":4.33173, "heading":0.0, "vx":0.10218, "vy":-0.01618, "omega":0.0, "ax":-0.98693, "ay":0.15924, "alpha":0.0, "fx":[-16.42151,-16.49926,-16.44275,-16.44281], "fy":[2.64565,2.66313,2.6633,2.64559]}, + {"t":3.1131, "x":11.69633, "y":4.33127, "heading":0.0, "vx":0.06814, "vy":-0.01068, "omega":0.0, "ax":-0.98741, "ay":0.15627, "alpha":0.0, "fx":[-16.45889,-16.45888,-16.46036,-16.46036], "fy":[2.60575,2.60186,2.6067,2.60575]}, + {"t":3.1476, "x":11.69809, "y":4.33099, "heading":0.0, "vx":0.03408, "vy":-0.00529, "omega":0.0, "ax":-0.98786, "ay":0.15347, "alpha":0.0, "fx":[-16.45857,-16.45863,-16.43867,-16.51251], "fy":[2.56374,2.54971,2.54976,2.5697]}, + {"t":3.18209, "x":11.69868, "y":4.3309, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/startBargeToOppE.traj b/src/main/deploy/choreo/startBargeToOppE.traj new file mode 100644 index 0000000..adf9f52 --- /dev/null +++ b/src/main/deploy/choreo/startBargeToOppE.traj @@ -0,0 +1,195 @@ +{ + "name":"startBargeToOppE", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.1008875, "y":4.0509, "heading":3.141592653589793, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.160315799713134, "y":5.6167577, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.365684200286866, "y":5.6167577, "heading":0.0, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":11.47006893157959, "y":5.786170482635498, "heading":0.0, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":12.055767059326172, "y":5.666640281677246, "heading":-0.9034901014083804, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":12.631814748155, "y":5.3371419902636, "heading":-1.047197551196598, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"startH.x", "val":7.1008875}, "y":{"exp":"startH.y", "val":4.0509}, "heading":{"exp":"startH.heading", "val":3.141592653589793}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.160315799713134 m", "val":8.160315799713134}, "y":{"exp":"5.6167577 m", "val":5.6167577}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.365684200286866 m", "val":9.365684200286866}, "y":{"exp":"5.6167577 m", "val":5.6167577}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"11.47006893157959 m", "val":11.47006893157959}, "y":{"exp":"5.786170482635498 m", "val":5.786170482635498}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"12.055767059326172 m", "val":12.055767059326172}, "y":{"exp":"5.666640281677246 m", "val":5.666640281677246}, "heading":{"exp":"-0.9034901014083804 rad", "val":-0.9034901014083804}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"FieldMaxX - E.x", "val":12.631814748155}, "y":{"exp":"FieldMaxY - E.y", "val":5.3371419902636}, "heading":{"exp":"E.heading - 180 deg", "val":-1.047197551196598}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.98505,1.58843,2.27824,2.58157,3.73527], + "samples":[ + {"t":0.0, "x":7.10089, "y":4.0509, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.27222, "ay":5.85861, "alpha":42.71769, "fx":[192.14015,113.93661,-45.26171,-175.98602], "fy":[11.54674,158.62131,189.15191,31.32061]}, + {"t":0.02662, "x":7.10134, "y":4.05298, "heading":3.14159, "vx":0.03387, "vy":0.15597, "omega":1.13727, "ax":1.30742, "ay":5.85321, "alpha":42.18818, "fx":[190.6473,112.85657,-45.96047,-170.36718], "fy":[11.62104,158.29802,187.88376,32.4778]}, + {"t":0.05325, "x":7.1027, "y":4.0592, "heading":-3.11131, "vx":0.06868, "vy":0.3118, "omega":2.26045, "ax":1.34522, "ay":5.84449, "alpha":41.16256, "fx":[188.22471,108.782,-48.94451,-158.36556], "fy":[16.8081,159.50166,185.35567,28.03411]}, + {"t":0.07987, "x":7.10501, "y":4.06958, "heading":-3.05113, "vx":0.10449, "vy":0.4674, "omega":3.35632, "ax":1.38977, "ay":5.8339, "alpha":39.30927, "fx":[183.23637,99.72337,-56.09853,-134.19385], "fy":[26.38208,162.49694,180.12301,19.99116]}, + {"t":0.10649, "x":7.10828, "y":4.08409, "heading":-2.96178, "vx":0.14149, "vy":0.62272, "omega":4.40286, "ax":1.44059, "ay":5.82138, "alpha":35.43199, "fx":[168.15366,84.5099,-64.97211,-91.63553], "fy":[40.67632,164.12341,168.99338,14.36536]}, + {"t":0.13312, "x":7.11256, "y":4.10273, "heading":-2.84456, "vx":0.17984, "vy":0.7777, "omega":5.34616, "ax":1.49331, "ay":5.80788, "alpha":25.0688, "fx":[126.38312,58.85903,-56.05122,-29.61978], "fy":[61.34175,150.66714,142.87985,32.36935]}, + {"t":0.15974, "x":7.11788, "y":4.12549, "heading":-2.70223, "vx":0.2196, "vy":0.93232, "omega":6.01357, "ax":1.55029, "ay":5.79269, "alpha":0.98145, "fx":[32.72311,26.37136,19.27117,25.00446], "fy":[94.84468,95.72694,98.90899,96.76471]}, + {"t":0.18636, "x":7.12427, "y":4.15237, "heading":-2.54213, "vx":0.26087, "vy":1.08654, "omega":6.0397, "ax":1.61471, "ay":5.77479, "alpha":-11.98574, "fx":[-20.08853,21.51858,73.75632,32.47926], "fy":[108.34531,60.89111,86.86145,128.95377]}, + {"t":0.21298, "x":7.13179, "y":4.18334, "heading":-2.38134, "vx":0.30386, "vy":1.24029, "omega":5.7206, "ax":1.68773, "ay":5.75355, "alpha":-10.42511, "fx":[-14.63925,31.36443,69.16061,26.64907], "fy":[103.52638,64.31793,91.78873,124.00241]}, + {"t":0.23961, "x":7.14048, "y":4.2184, "heading":-2.22904, "vx":0.3488, "vy":1.39346, "omega":5.44305, "ax":1.77267, "ay":5.72757, "alpha":-8.04914, "fx":[-4.19117,38.16145,60.62235,23.60547], "fy":[98.08326,71.66434,94.98241,117.17323]}, + {"t":0.26623, "x":7.15039, "y":4.25753, "heading":-2.08413, "vx":0.39599, "vy":1.54595, "omega":5.22876, "ax":1.87277, "ay":5.69517, "alpha":-6.75421, "fx":[2.67124,42.67351,56.87469,22.65328], "fy":[95.04198,76.04416,96.0969,112.55966]}, + {"t":0.29285, "x":7.1616, "y":4.3007, "heading":-1.94492, "vx":0.44585, "vy":1.69757, "omega":5.04894, "ax":1.99507, "ay":5.65292, "alpha":-10.71972, "fx":[-9.33118,58.77453,68.22102,15.36322], "fy":[90.015,62.82637,101.7564,122.32803]}, + {"t":0.31948, "x":7.17418, "y":4.3479, "heading":-1.8105, "vx":0.49896, "vy":1.84807, "omega":4.76355, "ax":2.15358, "ay":5.59374, "alpha":-18.18145, "fx":[-32.17412,87.80055,87.09938,0.87063], "fy":[77.52468,41.32918,113.94579,140.18021]}, + {"t":0.3461, "x":7.18822, "y":4.39908, "heading":-1.68368, "vx":0.5563, "vy":1.99699, "omega":4.27951, "ax":2.35803, "ay":5.50977, "alpha":-19.58924, "fx":[-28.34487,103.50642,87.20701,-5.13964], "fy":[66.68216,40.17711,119.51644,141.00523]}, + {"t":0.37272, "x":7.20387, "y":4.4542, "heading":-1.56975, "vx":0.61908, "vy":2.14368, "omega":3.75798, "ax":2.62087, "ay":5.38851, "alpha":-17.95907, "fx":[-10.55617,106.04474,82.44463,-3.17868], "fy":[60.894,44.91785,119.09211,134.39186]}, + {"t":0.39935, "x":7.22128, "y":4.51318, "heading":-1.4697, "vx":0.68885, "vy":2.28714, "omega":3.27986, "ax":2.96788, "ay":5.2038, "alpha":-16.27401, "fx":[10.09005,106.97071,79.37595,1.45597], "fy":[52.84734,49.87748,117.07443,127.18016]}, + {"t":0.42597, "x":7.24067, "y":4.57592, "heading":-1.38238, "vx":0.76786, "vy":2.42568, "omega":2.84659, "ax":3.44012, "ay":4.90201, "alpha":-14.84901, "fx":[28.32652,109.56334,80.6305,10.86035], "fy":[46.96809,48.71436,112.50096,118.67304]}, + {"t":0.45259, "x":7.26233, "y":4.64223, "heading":-1.30659, "vx":0.85945, "vy":2.55618, "omega":2.45127, "ax":4.094, "ay":4.36689, "alpha":-13.47863, "fx":[50.48334,113.13675,85.15565,24.20441], "fy":[34.1251,45.91918,104.58652,106.5447]}, + {"t":0.47921, "x":7.28666, "y":4.71183, "heading":-1.24133, "vx":0.96845, "vy":2.67244, "omega":2.09243, "ax":4.96106, "ay":3.34247, "alpha":-11.51425, "fx":[69.93727,118.42647,94.98248,47.44798], "fy":[19.35887,32.41259,87.81972,83.27851]}, + {"t":0.50584, "x":7.31421, "y":4.78417, "heading":-1.18563, "vx":1.10052, "vy":2.76143, "omega":1.78588, "ax":5.81115, "ay":1.40169, "alpha":-8.00195, "fx":[88.98075,118.9548,105.703,73.83773], "fy":[-5.8261,10.80392,49.93962,38.54455]}, + {"t":0.53246, "x":7.34556, "y":4.85818, "heading":-1.13808, "vx":1.25524, "vy":2.79875, "omega":1.57285, "ax":5.81698, "ay":-1.37216, "alpha":-2.43971, "fx":[92.92788,103.60127,99.78636,91.5498], "fy":[-32.55167,-25.84029,-13.67464,-19.42665]}, + {"t":0.55908, "x":7.38104, "y":4.93221, "heading":-1.09621, "vx":1.4101, "vy":2.76222, "omega":1.50789, "ax":4.75176, "ay":-3.63047, "alpha":3.02243, "fx":[84.58389,69.8375,74.39484,88.0217], "fy":[-49.2438,-59.97793,-70.87773,-61.97359]}, + {"t":0.58571, "x":7.42027, "y":5.00446, "heading":-1.05606, "vx":1.53661, "vy":2.66556, "omega":1.58836, "ax":3.55871, "ay":-4.811, "alpha":6.63944, "fx":[70.65898,35.96577,49.32873,81.33455], "fy":[-58.90161,-78.03114,-101.27077,-82.58503]}, + {"t":0.61233, "x":7.46244, "y":5.07372, "heading":-1.01378, "vx":1.63135, "vy":2.53748, "omega":1.76512, "ax":2.6661, "ay":-5.36111, "alpha":9.1586, "fx":[59.89545,9.43048,32.96352,75.48121], "fy":[-59.46615,-89.68704,-116.09917,-92.21624]}, + {"t":0.63895, "x":7.50682, "y":5.13937, "heading":-0.96678, "vx":1.70233, "vy":2.39475, "omega":2.00895, "ax":2.04529, "ay":-5.62977, "alpha":11.16771, "fx":[49.92208,-10.97166,22.91292,74.51269], "fy":[-59.59291,-94.09705,-125.11083,-96.5817]}, + {"t":0.66558, "x":7.55286, "y":5.20114, "heading":-0.9133, "vx":1.75678, "vy":2.24487, "omega":2.30627, "ax":1.60714, "ay":-5.77188, "alpha":12.97322, "fx":[41.98444,-26.67813,17.2739,74.58106], "fy":[-55.52259,-99.30168,-131.35564,-98.67802]}, + {"t":0.6922, "x":7.6002, "y":5.25885, "heading":-0.8519, "vx":1.79957, "vy":2.0912, "omega":2.65166, "ax":1.28652, "ay":-5.85296, "alpha":12.64558, "fx":[31.08797,-31.14484,15.4865,70.35276], "fy":[-59.2449,-101.25027,-131.49708,-98.27177]}, + {"t":0.71882, "x":7.64857, "y":5.31245, "heading":-0.7813, "vx":1.83382, "vy":1.93538, "omega":2.98832, "ax":1.04028, "ay":-5.90266, "alpha":6.24132, "fx":[19.75259,-9.51945,15.7528,43.37769], "fy":[-80.92594,-101.73914,-114.00607,-96.90681]}, + {"t":0.74544, "x":7.69776, "y":5.36189, "heading":-0.70175, "vx":1.86152, "vy":1.77823, "omega":3.15448, "ax":0.85702, "ay":-5.93278, "alpha":1.71968, "fx":[14.36907,6.21376,14.5019,22.05963], "fy":[-94.99302,-99.92534,-102.74168,-97.92645]}, + {"t":0.77207, "x":7.74762, "y":5.40713, "heading":-0.61776, "vx":1.88433, "vy":1.62028, "omega":3.20027, "ax":0.71241, "ay":-5.95247, "alpha":2.20174, "fx":[10.78854,2.91868,12.9489,20.84603], "fy":[-93.10534,-101.02317,-105.02077,-97.74995]}, + {"t":0.79869, "x":7.79804, "y":5.44816, "heading":-0.53256, "vx":1.9033, "vy":1.46181, "omega":3.25888, "ax":0.59373, "ay":-5.96596, "alpha":3.35582, "fx":[6.80221,-3.20224,13.0043,22.98436], "fy":[-90.20781,-102.4589,-108.62781,-96.50413]}, + {"t":0.82531, "x":7.84892, "y":5.48496, "heading":-0.4458, "vx":1.91911, "vy":1.30298, "omega":3.34823, "ax":0.49595, "ay":-5.97528, "alpha":4.72702, "fx":[2.39473,-8.9837,13.75743,25.90034], "fy":[-86.0982,-105.26998,-112.48282,-94.56923]}, + {"t":0.85194, "x":7.90019, "y":5.51753, "heading":-0.35666, "vx":1.93231, "vy":1.1439, "omega":3.47407, "ax":0.41695, "ay":-5.98165, "alpha":3.16437, "fx":[2.33769,-1.12863,10.90362,15.68859], "fy":[-88.29227,-104.40616,-111.17651,-94.96981]}, + {"t":0.87856, "x":7.95178, "y":5.54586, "heading":-0.26417, "vx":1.94341, "vy":0.98465, "omega":3.55832, "ax":0.3402, "ay":-5.98679, "alpha":-18.12702, "fx":[35.23758,82.94062,-43.89391,-51.60036], "fy":[-136.94717,-76.01645,-58.60205,-127.62193]}, + {"t":0.90518, "x":8.00364, "y":5.56996, "heading":-0.16944, "vx":1.95247, "vy":0.82526, "omega":3.07572, "ax":0.26533, "ay":-5.99082, "alpha":-33.90027, "fx":[66.98685,147.00421,-119.06818,-77.23088], "fy":[-165.35893,-51.0965,-24.88233,-158.11831]}, + {"t":0.93181, "x":8.05572, "y":5.58981, "heading":-0.08755, "vx":1.95953, "vy":0.66577, "omega":2.17319, "ax":0.1981, "ay":-5.99362, "alpha":-39.73021, "fx":[79.21285,172.96211,-159.98183,-78.98398], "fy":[-171.17137,-37.10501,-21.08662,-170.28015]}, + {"t":0.95843, "x":8.10796, "y":5.60541, "heading":-0.0297, "vx":1.9648, "vy":0.5062, "omega":1.11545, "ax":0.14186, "ay":-5.9954, "alpha":-41.89812, "fx":[83.82477,181.60506,-177.65099,-78.32006], "fy":[-172.64362,-28.25634,-24.02043,-174.84129]}, + {"t":0.98505, "x":8.16032, "y":5.61676, "heading":0.0, "vx":1.96858, "vy":0.34658, "omega":0.0, "ax":0.72971, "ay":-5.94905, "alpha":0.0, "fx":[12.23404,12.1979,12.11631,12.10743], "fy":[-99.1114,-99.39679,-99.04294,-99.12013]}, + {"t":1.01681, "x":8.2232, "y":5.62476, "heading":0.0, "vx":1.99175, "vy":0.15766, "omega":0.0, "ax":0.22965, "ay":-5.98592, "alpha":0.0, "fx":[3.7438,3.80802,4.03138,3.7295], "fy":[-100.10385,-99.7328,-99.72761,-99.56563]}, + {"t":1.04857, "x":8.28657, "y":5.62675, "heading":0.0, "vx":1.99905, "vy":-0.03243, "omega":0.0, "ax":-0.02163, "ay":-0.9745, "alpha":0.0, "fx":[-0.34817,-0.35105,-0.37087,-0.37196], "fy":[-16.23274,-16.33594,-16.17462,-16.23419]}, + {"t":1.08032, "x":8.35004, "y":5.62523, "heading":0.0, "vx":1.99836, "vy":-0.06338, "omega":0.0, "ax":0.01398, "ay":0.50493, "alpha":0.0, "fx":[0.21533,0.21525,0.07336,0.42847], "fy":[8.50487,8.43476,8.4346,8.29323]}, + {"t":1.11208, "x":8.41351, "y":5.62347, "heading":0.0, "vx":1.9988, "vy":-0.04734, "omega":0.0, "ax":0.0081, "ay":0.39415, "alpha":0.0, "fx":[0.14303,0.14287,0.12713,0.12707], "fy":[6.57819,6.53872,6.58589,6.57811]}, + {"t":1.14384, "x":8.47699, "y":5.62217, "heading":0.0, "vx":1.99906, "vy":-0.03483, "omega":0.0, "ax":0.00299, "ay":0.18755, "alpha":0.0, "fx":[0.04337,0.04367,0.10866,0.00341], "fy":[3.04232,3.1327,3.13269,3.19803]}, + {"t":1.17559, "x":8.54047, "y":5.62116, "heading":0.0, "vx":1.99916, "vy":-0.02887, "omega":0.0, "ax":0.00113, "ay":0.08184, "alpha":0.0, "fx":[0.03804,0.03799,-0.0004,-0.00042], "fy":[1.38338,1.27989,1.40999,1.38335]}, + {"t":1.20735, "x":8.60396, "y":5.62028, "heading":0.0, "vx":1.99919, "vy":-0.02627, "omega":0.0, "ax":0.00062, "ay":0.04882, "alpha":0.0, "fx":[0.01891,0.01893,0.07087,-0.0672], "fy":[0.78757,0.80532,0.80532,0.85725]}, + {"t":1.23911, "x":8.66745, "y":5.61947, "heading":0.0, "vx":1.99921, "vy":-0.02472, "omega":0.0, "ax":0.00053, "ay":0.04456, "alpha":0.0, "fx":[0.01227,0.01226,0.00557,0.00557], "fy":[0.74612,0.73097,0.74788,0.74612]}, + {"t":1.27086, "x":8.73094, "y":5.61871, "heading":0.0, "vx":1.99923, "vy":-0.02331, "omega":0.0, "ax":0.00051, "ay":0.04521, "alpha":0.0, "fx":[0.02651,0.02651,0.05399,-0.07297], "fy":[0.78014,0.73562,0.73562,0.76309]}, + {"t":1.30262, "x":8.79443, "y":5.61799, "heading":0.0, "vx":1.99925, "vy":-0.02187, "omega":0.0, "ax":0.00044, "ay":0.04146, "alpha":0.0, "fx":[0.00197,0.00197,0.0127,0.0127], "fy":[0.68573,0.71549,0.67744,0.68573]}, + {"t":1.33438, "x":8.85792, "y":5.61732, "heading":0.0, "vx":1.99926, "vy":-0.02055, "omega":0.0, "ax":0.00029, "ay":0.02934, "alpha":0.0, "fx":[0.03037,0.03037,0.05709,-0.09817], "fy":[0.53882,0.46368,0.46368,0.49037]}, + {"t":1.36613, "x":8.92141, "y":5.61668, "heading":0.0, "vx":1.99927, "vy":-0.01962, "omega":0.0, "ax":0.00002, "ay":0.00198, "alpha":0.0, "fx":[-0.00682,-0.00682,0.00746,0.00746], "fy":[0.02587,0.06136,0.01895,0.02587]}, + {"t":1.39789, "x":8.9849, "y":5.61606, "heading":0.0, "vx":1.99927, "vy":-0.01956, "omega":0.0, "ax":-0.00051, "ay":-0.04977, "alpha":0.0, "fx":[0.02211,0.02211,0.08152,-0.15949], "fy":[-0.79734,-0.86022,-0.86022,-0.80091]}, + {"t":1.42965, "x":9.04839, "y":5.61541, "heading":0.0, "vx":1.99925, "vy":-0.02114, "omega":0.0, "ax":-0.0014, "ay":-0.12101, "alpha":0.0, "fx":[-0.01429,-0.01429,-0.03225,-0.03225], "fy":[-2.00828,-2.06816,-1.98432,-2.00828]}, + {"t":1.4614, "x":9.11188, "y":5.61468, "heading":0.0, "vx":1.99921, "vy":-0.02498, "omega":0.0, "ax":-0.0018, "ay":-0.13268, "alpha":0.0, "fx":[-0.02786,-0.02787,0.07326,-0.13743], "fy":[-2.30648,-2.21382,-2.21381,-2.11271]}, + {"t":1.49316, "x":9.17536, "y":5.61382, "heading":0.0, "vx":1.99915, "vy":-0.0292, "omega":0.0, "ax":0.0028, "ay":0.21734, "alpha":0.0, "fx":[0.06796,0.06794,0.02545,0.02543], "fy":[3.64423,3.52712,3.67632,3.64422]}, + {"t":1.52492, "x":9.23885, "y":5.613, "heading":0.0, "vx":1.99924, "vy":-0.02229, "omega":0.0, "ax":-0.0042, "ay":1.71084, "alpha":0.0, "fx":[-0.02107,-0.02023,-0.1894,-0.0496], "fy":[28.82581,28.46948,28.46948,28.31086]}, + {"t":1.55667, "x":9.30234, "y":5.61316, "heading":0.0, "vx":1.99911, "vy":0.03204, "omega":0.0, "ax":-0.27548, "ay":5.12358, "alpha":0.0, "fx":[-4.81402,-4.81673,-4.36812,-4.36971], "fy":[85.18503,86.50551,84.75623,85.1838]}, + {"t":1.58843, "x":9.36568, "y":5.61676, "heading":0.0, "vx":1.99036, "vy":0.19475, "omega":0.0, "ax":5.9436, "ay":0.71084, "alpha":-21.44686, "fx":[142.36749,136.93349,55.13346,61.8737], "fy":[-41.33989,60.04867,91.41797,-62.72961]}, + {"t":1.61142, "x":9.41302, "y":5.62142, "heading":0.0, "vx":2.12702, "vy":0.21109, "omega":-0.49314, "ax":5.94963, "ay":0.71215, "alpha":-16.74772, "fx":[133.28241,128.81518,64.75888,69.85311], "fy":[-32.61606,51.23697,71.90309,-43.03913]}, + {"t":1.63442, "x":9.4635, "y":5.62647, "heading":-0.01134, "vx":2.26383, "vy":0.22746, "omega":-0.87823, "ax":5.9484, "ay":0.71287, "alpha":-11.83045, "fx":[123.53598,120.54765,74.62863,77.9153], "fy":[-21.27526,40.45129,52.77699,-24.42056]}, + {"t":1.65741, "x":9.51713, "y":5.63188, "heading":-0.03153, "vx":2.4006, "vy":0.24386, "omega":-1.15025, "ax":5.94677, "ay":0.71367, "alpha":-6.91165, "fx":[113.24951,111.84511,84.9424,86.48208], "fy":[-9.03598,29.03082,35.21401,-7.62279]}, + {"t":1.6804, "x":9.5739, "y":5.63768, "heading":-0.05798, "vx":2.53734, "vy":0.26027, "omega":-1.30917, "ax":5.94449, "ay":0.71473, "alpha":-2.23407, "fx":[103.47463,103.15694,94.6967,95.03901], "fy":[4.68188,17.44271,19.65694,5.87544]}, + {"t":1.7034, "x":9.63381, "y":5.64385, "heading":-0.08808, "vx":2.67402, "vy":0.2767, "omega":-1.36054, "ax":5.94105, "ay":0.71645, "alpha":1.59811, "fx":[95.6537,95.37249,102.09282,103.01878], "fy":[16.63029,8.21329,7.22506,15.70263]}, + {"t":1.72639, "x":9.69687, "y":5.6504, "heading":-0.11937, "vx":2.81063, "vy":0.29317, "omega":-1.3238, "ax":5.93531, "ay":0.71917, "alpha":4.43993, "fx":[90.26474,89.14528,107.80739,108.53733], "fy":[26.64765,1.66331,-2.14839,21.78997]}, + {"t":1.74938, "x":9.76306, "y":5.65734, "heading":-0.14981, "vx":2.9471, "vy":0.30971, "omega":-1.22171, "ax":5.92371, "ay":0.72485, "alpha":6.25706, "fx":[86.88008,84.41345,110.45258,113.2357], "fy":[33.22332,-2.25885,-7.59629,24.96352]}, + {"t":1.77238, "x":9.83239, "y":5.66465, "heading":-0.1779, "vx":3.08331, "vy":0.32638, "omega":-1.07784, "ax":5.88687, "ay":0.75494, "alpha":6.73077, "fx":[85.77131,81.88288,111.15596,113.7152], "fy":[35.31749,-2.01517,-8.78151,25.81706]}, + {"t":1.79537, "x":9.90484, "y":5.67235, "heading":-0.20268, "vx":3.21867, "vy":0.34373, "omega":-0.92307, "ax":2.58349, "ay":0.99551, "alpha":6.04103, "fx":[31.75722,26.63066,54.83621,59.03806], "fy":[35.39904,4.6569,-1.43057,27.75302]}, + {"t":1.81837, "x":9.97954, "y":5.68052, "heading":-0.2239, "vx":3.27807, "vy":0.36663, "omega":-0.78417, "ax":-0.05334, "ay":0.48304, "alpha":3.59698, "fx":[-7.49826,-11.22805,5.80719,9.36266], "fy":[18.64505,1.29161,-2.47252,14.74414]}, + {"t":1.84136, "x":10.0549, "y":5.68908, "heading":-0.24194, "vx":3.27684, "vy":0.37773, "omega":-0.70146, "ax":-0.03753, "ay":0.32294, "alpha":1.66815, "fx":[-3.39586,-5.38077,3.01508,3.25943], "fy":[10.63807,2.15853,0.1726,8.56384]}, + {"t":1.86435, "x":10.13023, "y":5.69785, "heading":-0.25806, "vx":3.27598, "vy":0.38516, "omega":-0.6631, "ax":-0.02745, "ay":0.23151, "alpha":0.68472, "fx":[-1.54851,-2.29758,0.63429,1.38163], "fy":[6.07025,2.62112,1.58434,5.16122]}, + {"t":1.88735, "x":10.20555, "y":5.70677, "heading":-0.27331, "vx":3.27535, "vy":0.39048, "omega":-0.64736, "ax":-0.01803, "ay":0.15091, "alpha":0.29987, "fx":[-0.65171,-1.10731,0.54237,0.01444], "fy":[3.69434,1.87547,1.42092,3.07176]}, + {"t":1.91034, "x":10.28086, "y":5.71578, "heading":-0.2882, "vx":3.27494, "vy":0.39395, "omega":-0.64046, "ax":-0.01423, "ay":0.11767, "alpha":0.12406, "fx":[-0.38206,-0.51512,-0.0925,0.04094], "fy":[2.43162,1.84931,1.3817,2.18347]}, + {"t":1.93333, "x":10.35616, "y":5.72487, "heading":-0.30292, "vx":3.27461, "vy":0.39666, "omega":-0.63761, "ax":-0.00916, "ay":0.07541, "alpha":0.08751, "fx":[-0.173,-0.33938,0.14337,-0.24146], "fy":[1.79385,1.02776,0.86235,1.34412]}, + {"t":1.95633, "x":10.43145, "y":5.73401, "heading":-0.31758, "vx":3.2744, "vy":0.39839, "omega":-0.6356, "ax":-0.0031, "ay":0.02566, "alpha":0.11, "fx":[-0.1941,-0.34811,0.09077,0.24506], "fy":[0.79719,0.46747,-0.11579,0.56176]}, + {"t":1.97932, "x":10.50674, "y":5.74318, "heading":-0.3322, "vx":3.27433, "vy":0.39898, "omega":-0.63307, "ax":0.00635, "ay":-0.05271, "alpha":0.0564, "fx":[0.05055,0.01637,0.17358,0.18303], "fy":[-0.49411,-1.03687,-1.07115,-0.91252]}, + {"t":2.00231, "x":10.58203, "y":5.75234, "heading":-0.34676, "vx":3.27447, "vy":0.39777, "omega":-0.63177, "ax":0.02467, "ay":-0.20351, "alpha":0.17688, "fx":[0.12995,-0.1321,0.69208,0.95509], "fy":[-2.88042,-3.42449,-4.09942,-3.16529]}, + {"t":2.02531, "x":10.65733, "y":5.76143, "heading":-0.36128, "vx":3.27504, "vy":0.39309, "omega":-0.62771, "ax":0.05108, "ay":-0.43195, "alpha":-0.02154, "fx":[0.74673,1.01112,0.68817,0.96006], "fy":[-7.23164,-7.26214,-7.00296,-7.30498]}, + {"t":2.0483, "x":10.73264, "y":5.77036, "heading":-0.37572, "vx":3.27622, "vy":0.38316, "omega":-0.6282, "ax":0.094, "ay":-0.82337, "alpha":0.23082, "fx":[1.16857,0.84227,1.9637,2.29328], "fy":[-13.07998,-14.0136,-14.46076,-13.34669]}, + {"t":2.07129, "x":10.808, "y":5.77895, "heading":-0.39016, "vx":3.27838, "vy":0.36422, "omega":-0.62289, "ax":0.14777, "ay":-1.39238, "alpha":-0.23827, "fx":[2.44519,3.26801,1.9442,2.19536], "fy":[-24.2773,-22.97809,-22.18052,-23.40523]}, + {"t":2.09429, "x":10.88342, "y":5.78696, "heading":-0.40448, "vx":3.28177, "vy":0.33221, "omega":-0.62837, "ax":0.21016, "ay":-2.25476, "alpha":-0.01256, "fx":[3.43051,3.59063,3.57392,3.41812], "fy":[-37.63028,-38.09062,-37.26527,-37.35723]}, + {"t":2.11728, "x":10.95894, "y":5.794, "heading":-0.41893, "vx":3.28661, "vy":0.28036, "omega":-0.62866, "ax":0.25182, "ay":-3.44237, "alpha":-1.20996, "fx":[5.19853,8.07432,2.37887,1.13939], "fy":[-62.08726,-55.92047,-53.18018,-58.34259]}, + {"t":2.14027, "x":11.03457, "y":5.79954, "heading":-0.43339, "vx":3.2924, "vy":0.20121, "omega":-0.65648, "ax":0.21515, "ay":-4.91111, "alpha":-1.94564, "fx":[5.96708,10.31444,1.11489,-3.05048], "fy":[-87.64753,-81.27104,-75.05553,-83.48921]}, + {"t":2.16327, "x":11.11033, "y":5.80286, "heading":-0.44848, "vx":3.29734, "vy":0.08829, "omega":-0.70122, "ax":0.02296, "ay":-5.64737, "alpha":-4.46176, "fx":[5.7147,15.26653,-7.07964,-12.37062], "fy":[-109.51337,-90.41136,-79.02558,-97.60565]}, + {"t":2.18626, "x":11.18616, "y":5.8034, "heading":-0.4646, "vx":3.29787, "vy":-0.04156, "omega":-0.80381, "ax":-3.22296, "ay":-4.85701, "alpha":-6.43161, "fx":[-44.61716,-33.21885,-64.44532,-72.61946], "fy":[-103.00341,-77.86445,-57.99639,-84.9921]}, + {"t":2.20925, "x":11.26113, "y":5.80116, "heading":-0.48309, "vx":3.22377, "vy":-0.15324, "omega":-0.9517, "ax":-5.53821, "ay":-2.17247, "alpha":-7.88055, "fx":[-82.30749,-68.48612,-103.73979,-114.74389], "fy":[-65.09617,-31.76748,-7.14961,-40.84314]}, + {"t":2.23225, "x":11.3338, "y":5.79706, "heading":-0.50497, "vx":3.09642, "vy":-0.2032, "omega":-1.1329, "ax":-5.77302, "ay":-1.52674, "alpha":-8.08789, "fx":[-87.16179,-72.86443,-107.30383,-117.60374], "fy":[-56.74313,-20.45264,5.79805,-30.40219]}, + {"t":2.25524, "x":11.40347, "y":5.79199, "heading":-0.53102, "vx":2.96368, "vy":-0.2383, "omega":-1.31887, "ax":-5.84183, "ay":-1.27952, "alpha":-7.33056, "fx":[-90.08565,-76.06561,-106.08078,-117.28984], "fy":[-49.9084,-17.12827,6.9461,-25.22547]}, + {"t":2.27824, "x":11.47007, "y":5.78617, "heading":-0.56134, "vx":2.82936, "vy":-0.26772, "omega":-1.48742, "ax":-5.84597, "ay":-1.13435, "alpha":-6.12029, "fx":[-92.22445,-79.28306,-103.85873,-114.43156], "fy":[-42.57992,-15.59272,4.61624,-22.07969]}, + {"t":2.2942, "x":11.51449, "y":5.78175, "heading":-0.58509, "vx":2.73602, "vy":-0.28583, "omega":-1.58513, "ax":-5.89047, "ay":-1.03517, "alpha":-5.97194, "fx":[-93.47608,-80.73253,-103.58017,-114.9762], "fy":[-40.62517,-14.62523,5.98512,-19.75792]}, + {"t":2.31017, "x":11.55743, "y":5.77706, "heading":-0.6104, "vx":2.64198, "vy":-0.30236, "omega":-1.68048, "ax":-5.90632, "ay":-0.95595, "alpha":-5.88529, "fx":[-94.44493,-81.39846,-103.53469,-114.44415], "fy":[-39.43897,-13.67132,7.4597,-18.09059]}, + {"t":2.32613, "x":11.59885, "y":5.77211, "heading":-0.63723, "vx":2.54769, "vy":-0.31762, "omega":-1.77444, "ax":-5.91783, "ay":-0.8947, "alpha":-5.27756, "fx":[-95.48485,-83.11417,-102.33312,-113.6577], "fy":[-35.60669,-13.44646,5.88252,-16.48653]}, + {"t":2.3421, "x":11.63877, "y":5.76692, "heading":-0.66556, "vx":2.45321, "vy":-0.33191, "omega":-1.85869, "ax":-5.92662, "ay":-0.84533, "alpha":-4.64896, "fx":[-96.49622,-85.25573,-101.77912,-111.64475], "fy":[-32.79583,-12.82159,4.4047,-15.15216]}, + {"t":2.35806, "x":11.67718, "y":5.76152, "heading":-0.69523, "vx":2.35859, "vy":-0.3454, "omega":-1.93291, "ax":-5.93346, "ay":-0.80521, "alpha":-3.50973, "fx":[-97.42527,-88.48876,-100.53513,-109.1824], "fy":[-27.07743,-13.02952,0.40786,-13.9906]}, + {"t":2.37403, "x":11.71408, "y":5.7559, "heading":-0.72609, "vx":2.26386, "vy":-0.35826, "omega":-1.98895, "ax":-5.93894, "ay":-0.77173, "alpha":-2.37961, "fx":[-98.25219,-91.92581,-99.94138,-105.87796], "fy":[-22.3394,-12.42778,-3.59254,-13.09805]}, + {"t":2.38999, "x":11.74947, "y":5.75008, "heading":-0.75784, "vx":2.16905, "vy":-0.37058, "omega":-2.02694, "ax":-5.94344, "ay":-0.74333, "alpha":-0.80038, "fx":[-98.96118,-96.1778,-99.24058,-101.91774], "fy":[-14.87828,-12.42263,-9.60233,-12.66085]}, + {"t":2.40596, "x":11.78334, "y":5.74407, "heading":-0.7902, "vx":2.07416, "vy":-0.38244, "omega":-2.03972, "ax":-5.9472, "ay":-0.71889, "alpha":0.82112, "fx":[-99.34036,-101.03144,-98.94393,-97.23201], "fy":[-8.28211,-11.29853,-15.79027,-12.56331]}, + {"t":2.42192, "x":11.8157, "y":5.73787, "heading":-0.82277, "vx":1.97921, "vy":-0.39392, "omega":-2.02661, "ax":-5.95036, "ay":-0.69776, "alpha":2.77614, "fx":[-99.38274,-106.36221,-98.92,-92.09382], "fy":[0.47655,-10.46806,-23.44499,-13.08855]}, + {"t":2.43789, "x":11.84653, "y":5.73149, "heading":-0.85512, "vx":1.88421, "vy":-0.40506, "omega":-1.98228, "ax":-5.95307, "ay":-0.67924, "alpha":4.80823, "fx":[-99.48369,-111.91558,-99.60166,-85.9384], "fy":[8.91044,-8.75057,-31.11313,-14.33744]}, + {"t":2.45385, "x":11.87586, "y":5.72494, "heading":-0.88677, "vx":1.78917, "vy":-0.41591, "omega":-1.90552, "ax":-5.95541, "ay":-0.66288, "alpha":7.04589, "fx":[-99.07721,-117.77883,-100.72942,-79.51005], "fy":[18.6755,-7.02494,-39.50406,-16.34632]}, + {"t":2.46982, "x":11.90366, "y":5.71822, "heading":-0.91719, "vx":1.69409, "vy":-0.42649, "omega":-1.79303, "ax":-5.95745, "ay":-0.64839, "alpha":9.32463, "fx":[-98.64052,-123.61028,-102.28587,-72.6947], "fy":[28.40178,-4.67884,-47.86846,-19.08803]}, + {"t":2.48578, "x":11.92995, "y":5.71133, "heading":-0.94582, "vx":1.59898, "vy":-0.43684, "omega":-1.64416, "ax":-5.95925, "ay":-0.63533, "alpha":11.6566, "fx":[-97.72884,-129.40113,-104.30028,-65.92126], "fy":[38.69282,-2.26372,-56.13287,-22.65891]}, + {"t":2.50175, "x":11.95472, "y":5.70427, "heading":-0.97207, "vx":1.50384, "vy":-0.44698, "omega":-1.45806, "ax":-5.96084, "ay":-0.62368, "alpha":13.99304, "fx":[-97.05496,-135.02682,-106.66734,-58.70809], "fy":[48.93278,0.39722,-63.93773,-26.97814]}, + {"t":2.51771, "x":11.97797, "y":5.69705, "heading":-0.99535, "vx":1.40868, "vy":-0.45694, "omega":-1.23466, "ax":-5.96226, "ay":-0.61305, "alpha":16.23002, "fx":[-95.9504,-140.31369,-109.41008,-51.8779], "fy":[59.03127,3.05884,-70.7514,-32.21604]}, + {"t":2.53368, "x":11.9997, "y":5.68968, "heading":-1.01506, "vx":1.31349, "vy":-0.46673, "omega":-0.97555, "ax":-5.96352, "ay":-0.60353, "alpha":18.40032, "fx":[-95.17939,-145.28615,-112.07183,-45.09887], "fy":[68.85765,5.67675,-77.21337,-37.5633]}, + {"t":2.54964, "x":12.01991, "y":5.68215, "heading":-1.03063, "vx":1.21828, "vy":-0.47636, "omega":-0.68179, "ax":-5.96467, "ay":-0.59475, "alpha":20.41481, "fx":[-94.09339,-149.81745,-114.68764,-39.11428], "fy":[78.27196,8.28044,-82.8548,-43.35409]}, + {"t":2.56561, "x":12.0386, "y":5.67447, "heading":-1.04152, "vx":1.12305, "vy":-0.48586, "omega":-0.35586, "ax":-5.9657, "ay":-0.58681, "alpha":22.29005, "fx":[-93.6368,-153.99544,-117.26942,-32.87969], "fy":[86.86864,10.50884,-87.71974,-48.78512]}, + {"t":2.58157, "x":12.05577, "y":5.66664, "heading":-1.0472, "vx":1.02781, "vy":-0.49523, "omega":0.0, "ax":-0.99523, "ay":-0.0231, "alpha":0.0, "fx":[-16.58854,-16.59575,-16.59159,-16.58438], "fy":[-0.35427,-0.45607,-0.36718,-0.36245]}, + {"t":2.61653, "x":12.09109, "y":5.64931, "heading":-1.0472, "vx":0.99302, "vy":-0.49603, "omega":0.0, "ax":-0.99382, "ay":0.09221, "alpha":0.0, "fx":[-16.49009,-16.57549,-16.52633,-16.67406], "fy":[1.56248,1.56971,1.48345,1.53303]}, + {"t":2.65149, "x":12.1252, "y":5.63203, "heading":-1.0472, "vx":0.95827, "vy":-0.49281, "omega":0.0, "ax":-0.98274, "ay":0.17586, "alpha":0.0, "fx":[-16.38125,-16.38406,-16.38244,-16.37962], "fy":[2.94159,2.90731,2.93781,2.9392]}, + {"t":2.68645, "x":12.1581, "y":5.61491, "heading":-1.0472, "vx":0.92391, "vy":-0.48666, "omega":0.0, "ax":-0.96977, "ay":0.23805, "alpha":0.0, "fx":[-16.10458,-16.16593,-16.13083,-16.26109], "fy":[4.00475,3.98537,3.92374,3.95882]}, + {"t":2.72141, "x":12.18981, "y":5.59804, "heading":-1.0472, "vx":0.89001, "vy":-0.47834, "omega":0.0, "ax":-0.95703, "ay":0.28556, "alpha":0.0, "fx":[-15.95308,-15.95381,-15.95342,-15.95258], "fy":[4.76149,4.75631,4.76143,4.76144]}, + {"t":2.75637, "x":12.22034, "y":5.58149, "heading":-1.0472, "vx":0.85655, "vy":-0.46836, "omega":0.0, "ax":-0.94527, "ay":0.32278, "alpha":0.0, "fx":[-15.70859,-15.75313,-15.72778,-15.83896], "fy":[5.41988,5.38888,5.34439,5.36948]}, + {"t":2.79133, "x":12.24971, "y":5.56531, "heading":-1.0472, "vx":0.82351, "vy":-0.45707, "omega":0.0, "ax":-0.93467, "ay":0.3526, "alpha":0.0, "fx":[-15.58059,-15.58014,-15.58046,-15.58071], "fy":[5.87436,5.88469,5.87621,5.87552]}, + {"t":2.82629, "x":12.27793, "y":5.54955, "heading":-1.0472, "vx":0.79083, "vy":-0.44475, "omega":0.0, "ax":-0.92521, "ay":0.37695, "alpha":0.0, "fx":[-15.38337,-15.41691,-15.39792,-15.49334], "fy":[6.32204,6.28679,6.25345,6.27209]}, + {"t":2.86126, "x":12.30501, "y":5.53423, "heading":-1.0472, "vx":0.75848, "vy":-0.43157, "omega":0.0, "ax":-0.9168, "ay":0.39717, "alpha":0.0, "fx":[-15.2829,-15.28175,-15.28248,-15.28339], "fy":[6.61464,6.63381,6.61754,6.61647]}, + {"t":2.89622, "x":12.33096, "y":5.51939, "heading":-1.0472, "vx":0.72643, "vy":-0.41768, "omega":0.0, "ax":-0.90931, "ay":0.4142, "alpha":0.0, "fx":[-15.12505,-15.15057,-15.1362,-15.21891], "fy":[6.94146,6.90435,6.87912,6.89309]}, + {"t":2.93118, "x":12.35581, "y":5.50504, "heading":-1.0472, "vx":0.69464, "vy":-0.4032, "omega":0.0, "ax":-0.90261, "ay":0.42873, "alpha":0.0, "fx":[-15.04644,-15.04481,-15.04583,-15.04719], "fy":[7.1389,7.16406,7.1425,7.14117]}, + {"t":2.96614, "x":12.37954, "y":5.4912, "heading":-1.0472, "vx":0.66309, "vy":-0.38821, "omega":0.0, "ax":-0.89661, "ay":0.44125, "alpha":0.0, "fx":[-14.91865,-14.93827,-14.92728,-14.99984], "fy":[7.39071,7.35306,7.33378,7.34435]}, + {"t":3.0011, "x":12.40217, "y":5.4779, "heading":-1.0472, "vx":0.63174, "vy":-0.37279, "omega":0.0, "ax":-0.8912, "ay":0.45216, "alpha":0.0, "fx":[-14.85639,-14.85449,-14.85567,-14.85729], "fy":[7.52859,7.55695,7.53253,7.53107]}, + {"t":3.03606, "x":12.42371, "y":5.46514, "heading":-1.0472, "vx":0.60058, "vy":-0.35698, "omega":0.0, "ax":-0.88632, "ay":0.46174, "alpha":0.0, "fx":[-14.75136,-14.76655,-14.7581,-14.82237], "fy":[7.73036,7.69301,7.67816,7.6862]}, + {"t":3.07102, "x":12.44417, "y":5.45295, "heading":-1.0472, "vx":0.5696, "vy":-0.34084, "omega":0.0, "ax":-0.8819, "ay":0.47021, "alpha":0.0, "fx":[-14.70126,-14.69915,-14.70045,-14.70228], "fy":[7.82878,7.85956,7.83298,7.83143]}, + {"t":3.10598, "x":12.46354, "y":5.44132, "heading":-1.0472, "vx":0.53877, "vy":-0.3244, "omega":0.0, "ax":-0.87786, "ay":0.47776, "alpha":0.0, "fx":[-14.61377,-14.62527,-14.61893,-14.67634], "fy":[7.99596,7.95883,7.94767,7.95361]}, + {"t":3.14094, "x":12.48184, "y":5.43027, "heading":-1.0472, "vx":0.50808, "vy":-0.30769, "omega":0.0, "ax":-0.87418, "ay":0.48452, "alpha":0.0, "fx":[-14.57267,-14.57041,-14.5718,-14.57378], "fy":[8.06687,8.09941,8.07125,8.06963]}, + {"t":3.1759, "x":12.49907, "y":5.41981, "heading":-1.0472, "vx":0.47751, "vy":-0.29076, "omega":0.0, "ax":-0.8708, "ay":0.49062, "alpha":0.0, "fx":[-14.49888,-14.50737,-14.50274,-14.55439], "fy":[8.20902,8.17223,8.16409,8.16833]}, + {"t":3.21086, "x":12.51523, "y":5.40994, "heading":-1.0472, "vx":0.44707, "vy":-0.2736, "omega":0.0, "ax":-0.86769, "ay":0.49614, "alpha":0.0, "fx":[-14.46457,-14.46217,-14.46364,-14.46577], "fy":[8.26006,8.29425,8.2646,8.26292]}, + {"t":3.24582, "x":12.53033, "y":5.40068, "heading":-1.0472, "vx":0.41674, "vy":-0.25626, "omega":0.0, "ax":-0.86483, "ay":0.50117, "alpha":0.0, "fx":[-14.40168,-14.40761,-14.40444,-14.4512], "fy":[8.38368,8.34717,8.34158,8.34437]}, + {"t":3.28078, "x":12.54437, "y":5.39203, "heading":-1.0472, "vx":0.3865, "vy":-0.23874, "omega":0.0, "ax":-0.86217, "ay":0.50576, "alpha":0.0, "fx":[-14.37254,-14.37006,-14.37158,-14.37378], "fy":[8.42009,8.45504,8.4247,8.42299]}, + {"t":3.31574, "x":12.55736, "y":5.38399, "heading":-1.0472, "vx":0.35636, "vy":-0.22106, "omega":0.0, "ax":-0.85971, "ay":0.50996, "alpha":0.0, "fx":[-14.31835,-14.3223,-14.32026,-14.36286], "fy":[8.52919,8.49327,8.48966,8.49134]}, + {"t":3.3507, "x":12.56929, "y":5.37657, "heading":-1.0472, "vx":0.3263, "vy":-0.20323, "omega":0.0, "ax":-0.85742, "ay":0.51384, "alpha":0.0, "fx":[-14.29332,-14.29076,-14.29232,-14.29462], "fy":[8.55455,8.59044,8.55925,8.55751]}, + {"t":3.38566, "x":12.58017, "y":5.36978, "heading":-1.0472, "vx":0.29633, "vy":-0.18526, "omega":0.0, "ax":-0.85528, "ay":0.51741, "alpha":0.0, "fx":[-14.24636,-14.2484,-14.24745,-14.28639], "fy":[8.65253,8.61683,8.61513,8.61573]}, + {"t":3.42062, "x":12.59001, "y":5.36362, "heading":-1.0472, "vx":0.26643, "vy":-0.16717, "omega":0.0, "ax":-0.85329, "ay":0.52073, "alpha":0.0, "fx":[-14.22445,-14.22183,-14.22343,-14.22579], "fy":[8.66917,8.70577,8.67394,8.67218]}, + {"t":3.45558, "x":12.5988, "y":5.3581, "heading":-1.0472, "vx":0.23659, "vy":-0.14897, "omega":0.0, "ax":-0.85142, "ay":0.5238, "alpha":0.0, "fx":[-14.18344,-14.18392,-14.18386,-14.21962], "fy":[8.75817,8.72285,8.7227,8.72242]}, + {"t":3.49054, "x":12.60656, "y":5.35321, "heading":-1.0472, "vx":0.20683, "vy":-0.13066, "omega":0.0, "ax":-0.84966, "ay":0.52667, "alpha":0.0, "fx":[-14.16405,-14.16139,-14.16301,-14.16541], "fy":[8.76808,8.80505,8.77287,8.7711]}, + {"t":3.5255, "x":12.61327, "y":5.34896, "heading":-1.0472, "vx":0.17712, "vy":-0.11224, "omega":0.0, "ax":-0.84801, "ay":0.52934, "alpha":0.0, "fx":[-14.12806,-14.12714,-14.12787,-14.16082], "fy":[8.84974,8.81472,8.81596,8.8149]}, + {"t":3.56046, "x":12.61894, "y":5.34536, "heading":-1.0472, "vx":0.14748, "vy":-0.09374, "omega":0.0, "ax":-0.84646, "ay":0.53184, "alpha":0.0, "fx":[-14.11068,-14.10796,-14.10961,-14.11207], "fy":[8.85417,8.89165,8.85901,8.85722]}, + {"t":3.59542, "x":12.62358, "y":5.34241, "heading":-1.0472, "vx":0.11789, "vy":-0.07515, "omega":0.0, "ax":-0.84499, "ay":0.53418, "alpha":0.0, "fx":[-14.07896,-14.07676,-14.07822,-14.10866], "fy":[8.92987,8.89508,8.89758,8.89581]}, + {"t":3.63038, "x":12.62719, "y":5.34011, "heading":-1.0472, "vx":0.08834, "vy":-0.05647, "omega":0.0, "ax":-0.84361, "ay":0.53639, "alpha":0.0, "fx":[-14.06317,-14.06042,-14.06208,-14.06458], "fy":[8.92983,8.9677,8.9347,8.9329]}, + {"t":3.66534, "x":12.62976, "y":5.33846, "heading":-1.0472, "vx":0.05885, "vy":-0.03772, "omega":0.0, "ax":-0.8423, "ay":0.53846, "alpha":0.0, "fx":[-14.04133,-14.03856,-14.04024,-14.04275], "fy":[8.96433,9.00235,8.96921,8.96741]}, + {"t":3.7003, "x":12.6313, "y":5.33747, "heading":-1.0472, "vx":0.0294, "vy":-0.01889, "omega":0.0, "ax":-0.84106, "ay":0.54041, "alpha":0.0, "fx":[-14.016,-14.03102,-14.01245,-14.02059], "fy":[9.00404,9.00708,9.01286,9.00956]}, + {"t":3.73527, "x":12.63181, "y":5.33714, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/startBargeToOppG.traj b/src/main/deploy/choreo/startBargeToOppG.traj new file mode 100644 index 0000000..7528044 --- /dev/null +++ b/src/main/deploy/choreo/startBargeToOppG.traj @@ -0,0 +1,167 @@ +{ + "name":"startBargeToOppG", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.1008875, "y":4.0509, "heading":3.141592653589793, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.160315799713134, "y":5.6167577, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.365684200286866, "y":5.6167577, "heading":0.0, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":10.99120807647705, "y":4.528883934020996, "heading":0.0, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":11.698677, "y":4.3309, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"startH.x", "val":7.1008875}, "y":{"exp":"startH.y", "val":4.0509}, "heading":{"exp":"startH.heading", "val":3.141592653589793}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.160315799713134 m", "val":8.160315799713134}, "y":{"exp":"5.6167577 m", "val":5.6167577}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.365684200286866 m", "val":9.365684200286866}, "y":{"exp":"5.6167577 m", "val":5.6167577}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"10.99120807647705 m", "val":10.99120807647705}, "y":{"exp":"4.528883934020996 m", "val":4.528883934020996}, "heading":{"exp":"G.heading - 180 deg", "val":0.0}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"FieldMaxX - G.x", "val":11.698677}, "y":{"exp":"FieldMaxY - G.y", "val":4.3309}, "heading":{"exp":"G.heading - 180 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.98505,1.58869,2.37066,3.58503], + "samples":[ + {"t":0.0, "x":7.10089, "y":4.0509, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.2722, "ay":5.85861, "alpha":42.72744, "fx":[192.19274,113.94704,-45.36251,-175.94919], "fy":[11.5168,158.64691,189.15786,31.31909]}, + {"t":0.02662, "x":7.10134, "y":4.05298, "heading":3.14159, "vx":0.03387, "vy":0.15597, "omega":1.13753, "ax":1.30744, "ay":5.8532, "alpha":42.19664, "fx":[190.7135,112.79904,-46.12277,-170.21212], "fy":[11.53122,158.37605,187.87992,32.49305]}, + {"t":0.05325, "x":7.1027, "y":4.0592, "heading":-3.11131, "vx":0.06868, "vy":0.3118, "omega":2.26093, "ax":1.34536, "ay":5.84446, "alpha":41.16775, "fx":[188.2905,108.63157,-49.19831,-158.0177], "fy":[16.75522,159.64997,185.33116,27.96096]}, + {"t":0.07987, "x":7.10501, "y":4.06958, "heading":-3.05112, "vx":0.1045, "vy":0.4674, "omega":3.35694, "ax":1.39015, "ay":5.83381, "alpha":39.29671, "fx":[183.18098,99.51814,-56.56959,-133.4369], "fy":[26.40569,162.65488,180.03394,19.8926]}, + {"t":0.10649, "x":7.10828, "y":4.08409, "heading":-2.96174, "vx":0.14151, "vy":0.62271, "omega":4.40314, "ax":1.44122, "ay":5.82122, "alpha":35.30421, "fx":[167.45291,83.73682,-66.27626,-88.81536], "fy":[40.95516,164.41773,168.43773,14.33736]}, + {"t":0.13311, "x":7.11256, "y":4.10273, "heading":-2.84452, "vx":0.17988, "vy":0.77769, "omega":5.34304, "ax":1.49357, "ay":5.80781, "alpha":24.90274, "fx":[125.4913,58.3284,-56.19322,-28.03835], "fy":[61.4127,150.60001,142.47394,32.76701]}, + {"t":0.15974, "x":7.11788, "y":4.12549, "heading":-2.70227, "vx":0.21964, "vy":0.93231, "omega":6.00602, "ax":1.55049, "ay":5.79263, "alpha":1.28813, "fx":[33.85106,26.75543,17.98284,24.79435], "fy":[94.56799,96.34267,99.49853,95.83242]}, + {"t":0.18636, "x":7.12428, "y":4.15236, "heading":-2.54237, "vx":0.26092, "vy":1.08653, "omega":6.04032, "ax":1.61487, "ay":5.77474, "alpha":-11.95766, "fx":[-19.96137,20.74751,73.73916,33.1512], "fy":[107.63365,61.36499,86.98627,129.0636]}, + {"t":0.21298, "x":7.13179, "y":4.18334, "heading":-2.38156, "vx":0.30391, "vy":1.24027, "omega":5.72197, "ax":1.68782, "ay":5.75352, "alpha":-10.45304, "fx":[-14.2632,30.59879,69.1451,27.06021], "fy":[103.6115,63.84383,91.90336,124.27491]}, + {"t":0.23961, "x":7.14048, "y":4.2184, "heading":-2.22923, "vx":0.34884, "vy":1.39345, "omega":5.44368, "ax":1.77269, "ay":5.72756, "alpha":-8.03863, "fx":[-3.93295,37.51143,60.73333,23.88757], "fy":[97.87815,71.72911,94.96128,117.33421]}, + {"t":0.26623, "x":7.1504, "y":4.25752, "heading":-2.0843, "vx":0.39604, "vy":1.54593, "omega":5.22967, "ax":1.87267, "ay":5.6952, "alpha":-6.79539, "fx":[2.13174,42.05627,57.63484,23.04296], "fy":[94.90336,76.47471,95.74899,112.61782]}, + {"t":0.29285, "x":7.16161, "y":4.3007, "heading":-1.94507, "vx":0.44589, "vy":1.69755, "omega":5.04875, "ax":1.99493, "ay":5.65297, "alpha":-10.75075, "fx":[-8.97101,57.6271,68.61228,15.74953], "fy":[90.16423,62.12363,101.91831,122.72294]}, + {"t":0.31948, "x":7.17419, "y":4.3479, "heading":-1.81066, "vx":0.49901, "vy":1.84805, "omega":4.76254, "ax":2.15338, "ay":5.59381, "alpha":-18.09961, "fx":[-31.18982,85.84757,87.45132,1.47448], "fy":[77.09331,41.20403,114.16029,140.52703]}, + {"t":0.3461, "x":7.18823, "y":4.39908, "heading":-1.68386, "vx":0.55634, "vy":1.99698, "omega":4.28067, "ax":2.35788, "ay":5.50983, "alpha":-19.59759, "fx":[-28.08992,102.52536,87.60661,-4.82326], "fy":[66.15862,40.11338,119.70628,141.40682]}, + {"t":0.37272, "x":7.20388, "y":4.4542, "heading":-1.5699, "vx":0.61911, "vy":2.14367, "omega":3.75892, "ax":2.62072, "ay":5.38859, "alpha":-17.96032, "fx":[-10.44596,104.99259,82.88574,-2.68806], "fy":[60.42564,44.7708,119.2635,134.84065]}, + {"t":0.39934, "x":7.22129, "y":4.51318, "heading":-1.46983, "vx":0.68888, "vy":2.28713, "omega":3.28077, "ax":2.96767, "ay":5.20392, "alpha":-16.29027, "fx":[9.75733,106.49019,79.53899,2.09229], "fy":[52.29688,49.90033,117.24641,127.54348]}, + {"t":0.42597, "x":7.24068, "y":4.57591, "heading":-1.38248, "vx":0.76789, "vy":2.42567, "omega":2.84707, "ax":3.43985, "ay":4.90219, "alpha":-14.84334, "fx":[27.15208,108.98798,81.24395,11.97885], "fy":[47.00889,48.41992,112.63435,118.80552]}, + {"t":0.45259, "x":7.26235, "y":4.64223, "heading":-1.30669, "vx":0.85947, "vy":2.55618, "omega":2.4519, "ax":4.09363, "ay":4.36723, "alpha":-13.48887, "fx":[49.4464,112.88996,85.76915,24.84956], "fy":[33.8863,46.22161,104.73823,106.35242]}, + {"t":0.47921, "x":7.28668, "y":4.71183, "heading":-1.24141, "vx":0.96845, "vy":2.67245, "omega":2.09278, "ax":4.96074, "ay":3.34293, "alpha":-11.51912, "fx":[68.33371,118.60647,95.96753,47.86523], "fy":[20.03137,32.53392,88.11055,82.22454]}, + {"t":0.50584, "x":7.31422, "y":4.78416, "heading":-1.18569, "vx":1.10052, "vy":2.76145, "omega":1.78611, "ax":5.81112, "ay":1.40178, "alpha":-7.98788, "fx":[88.31189,119.37797,106.35669,73.42754], "fy":[-5.1587,11.24509,49.44914,37.93244]}, + {"t":0.53246, "x":7.34558, "y":4.85818, "heading":-1.13814, "vx":1.25523, "vy":2.79877, "omega":1.57345, "ax":5.81696, "ay":-1.3722, "alpha":-2.43083, "fx":[92.76322,103.6965,99.70767,91.69659], "fy":[-32.47343,-25.92105,-13.77293,-19.32823]}, + {"t":0.55908, "x":7.38106, "y":4.9322, "heading":-1.09625, "vx":1.4101, "vy":2.76224, "omega":1.50873, "ax":4.75192, "ay":-3.63025, "alpha":2.98514, "fx":[84.15892,69.82618,74.73918,88.12433], "fy":[-49.39975,-59.91009,-70.52548,-62.2227]}, + {"t":0.58571, "x":7.42028, "y":5.00446, "heading":-1.05608, "vx":1.53661, "vy":2.66559, "omega":1.58821, "ax":3.5583, "ay":-4.8113, "alpha":6.66999, "fx":[70.17521,35.96722,49.62551,81.49246], "fy":[-59.03915,-77.20292,-101.55786,-83.00854]}, + {"t":0.61233, "x":7.46245, "y":5.07372, "heading":-1.0138, "vx":1.63134, "vy":2.5375, "omega":1.76578, "ax":2.66571, "ay":-5.3613, "alpha":9.16401, "fx":[59.20445,9.97297,33.14751,75.4194], "fy":[-59.15647,-89.25292,-116.48708,-92.58486]}, + {"t":0.63895, "x":7.50683, "y":5.13937, "heading":-0.96679, "vx":1.70231, "vy":2.39476, "omega":2.00975, "ax":2.0451, "ay":-5.62984, "alpha":11.16032, "fx":[49.204,-10.1528,23.08282,74.22913], "fy":[-59.22803,-93.45424,-125.71501,-96.98962]}, + {"t":0.66557, "x":7.55287, "y":5.20113, "heading":-0.91328, "vx":1.75675, "vy":2.24488, "omega":2.30688, "ax":1.60695, "ay":-5.77193, "alpha":12.98856, "fx":[41.25456,-26.26609,17.43487,74.72494], "fy":[-55.29311,-99.04493,-131.66586,-98.85742]}, + {"t":0.6922, "x":7.60021, "y":5.25885, "heading":-0.85187, "vx":1.79954, "vy":2.09121, "omega":2.65267, "ax":1.28644, "ay":-5.85297, "alpha":12.58906, "fx":[30.42818,-30.03358,15.69929,69.68357], "fy":[-58.6316,-101.0742,-131.90488,-98.65423]}, + {"t":0.71882, "x":7.64858, "y":5.31245, "heading":-0.78125, "vx":1.83378, "vy":1.93539, "omega":2.98783, "ax":1.04, "ay":-5.90271, "alpha":6.28654, "fx":[19.43848,-9.51311,15.91321,43.50652], "fy":[-80.68835,-101.79986,-114.26632,-96.82655]}, + {"t":0.74544, "x":7.69777, "y":5.36189, "heading":-0.7017, "vx":1.86147, "vy":1.77824, "omega":3.15519, "ax":0.85683, "ay":-5.93281, "alpha":1.72672, "fx":[14.38862,6.12957,14.65657,21.95694], "fy":[-94.92893,-99.95651,-102.77929,-97.92346]}, + {"t":0.77207, "x":7.74763, "y":5.40713, "heading":-0.6177, "vx":1.88428, "vy":1.62029, "omega":3.20116, "ax":0.7124, "ay":-5.95247, "alpha":2.18957, "fx":[10.75462,2.98174,12.99242,20.77298], "fy":[-93.09855,-101.0853,-104.95761,-97.75773]}, + {"t":0.79869, "x":7.79805, "y":5.44815, "heading":-0.53248, "vx":1.90325, "vy":1.46182, "omega":3.25946, "ax":0.59378, "ay":-5.96595, "alpha":3.36194, "fx":[6.6927,-3.00994,13.08816,22.82088], "fy":[-90.03786,-102.40264,-108.86284,-96.4949]}, + {"t":0.82531, "x":7.84893, "y":5.48496, "heading":-0.4457, "vx":1.91906, "vy":1.30299, "omega":3.34896, "ax":0.4961, "ay":-5.97527, "alpha":4.77231, "fx":[2.09306,-9.30809,14.07539,26.21841], "fy":[-86.13352,-105.15975,-112.40942,-94.71664]}, + {"t":0.85194, "x":7.90019, "y":5.51753, "heading":-0.35654, "vx":1.93227, "vy":1.14391, "omega":3.47601, "ax":0.41767, "ay":-5.9816, "alpha":2.92329, "fx":[2.64349,0.55051,10.15016,14.5054], "fy":[-88.2449,-104.08081,-111.15259,-95.36301]}, + {"t":0.87856, "x":7.95178, "y":5.54586, "heading":-0.264, "vx":1.94339, "vy":0.98466, "omega":3.55384, "ax":0.34045, "ay":-5.98677, "alpha":-18.05734, "fx":[35.22274,82.15979,-42.92446,-51.75724], "fy":[-137.0315,-75.71206,-58.79525,-127.64778]}, + {"t":0.90518, "x":8.00364, "y":5.56996, "heading":-0.16939, "vx":1.95245, "vy":0.82528, "omega":3.0731, "ax":0.26559, "ay":-5.9908, "alpha":-33.79601, "fx":[65.27532,147.23295,-116.32574,-78.47363], "fy":[-165.79821,-50.95632,-25.12022,-157.58051]}, + {"t":0.9318, "x":8.05572, "y":5.5898, "heading":-0.08757, "vx":1.95952, "vy":0.66578, "omega":2.17335, "ax":0.19829, "ay":-5.99361, "alpha":-39.71847, "fx":[79.23054,172.77518,-159.64279,-79.14111], "fy":[-171.22479,-36.91436,-21.21674,-170.28678]}, + {"t":0.95843, "x":8.10796, "y":5.60541, "heading":-0.02971, "vx":1.9648, "vy":0.50622, "omega":1.11593, "ax":0.14185, "ay":-5.9954, "alpha":-41.91593, "fx":[83.87258,181.67783,-177.65586,-78.43602], "fy":[-172.68444,-28.18119,-24.04351,-174.85248]}, + {"t":0.98505, "x":8.16032, "y":5.61676, "heading":0.0, "vx":1.96858, "vy":0.3466, "omega":0.0, "ax":0.72921, "ay":-5.94911, "alpha":0.0, "fx":[12.25618,12.19523,12.09331,12.0775], "fy":[-99.09119,-99.46552,-99.01248,-99.10622]}, + {"t":1.01682, "x":8.22323, "y":5.62477, "heading":0.0, "vx":1.99174, "vy":0.15759, "omega":0.0, "ax":0.22975, "ay":-5.98591, "alpha":0.0, "fx":[3.66418,3.77003,3.90519,3.98], "fy":[-100.19835,-99.67303,-99.66605,-99.59172]}, + {"t":1.04859, "x":8.28662, "y":5.62675, "heading":0.0, "vx":1.99904, "vy":-0.03258, "omega":0.0, "ax":-0.01566, "ay":-0.7747, "alpha":0.0, "fx":[-0.27997,-0.28491,-0.23879,-0.24069], "fy":[-12.934,-12.8583,-12.92666,-12.93637]}, + {"t":1.08036, "x":8.35012, "y":5.62533, "heading":0.0, "vx":1.99855, "vy":-0.05719, "omega":0.0, "ax":0.01409, "ay":0.58895, "alpha":0.0, "fx":[0.22265,0.22248,-0.01354,0.50818], "fy":[10.01585,9.83005,9.8298,9.5945]}, + {"t":1.11213, "x":8.41363, "y":5.62381, "heading":0.0, "vx":1.99899, "vy":-0.03848, "omega":0.0, "ax":0.00724, "ay":0.46598, "alpha":0.0, "fx":[0.1128,0.11259,0.12887,0.12879], "fy":[7.75969,7.81297,7.73858,7.75959]}, + {"t":1.1439, "x":8.47714, "y":5.62282, "heading":0.0, "vx":1.99922, "vy":-0.02368, "omega":0.0, "ax":0.00251, "ay":0.25622, "alpha":0.0, "fx":[0.04575,0.04619,0.13768,-0.06219], "fy":[4.19168,4.26704,4.26702,4.35882]}, + {"t":1.17567, "x":8.54066, "y":5.6222, "heading":0.0, "vx":1.9993, "vy":-0.01554, "omega":0.0, "ax":0.001, "ay":0.15327, "alpha":0.0, "fx":[0.05177,0.05171,-0.01826,-0.01829], "fy":[2.59,2.39182,2.64811,2.58996]}, + {"t":1.20744, "x":8.60417, "y":5.62178, "heading":0.0, "vx":1.99933, "vy":-0.01067, "omega":0.0, "ax":0.00056, "ay":0.12959, "alpha":0.0, "fx":[0.00085,0.00088,0.12383,-0.08836], "fy":[2.01206,2.16871,2.16871,2.29161]}, + {"t":1.23921, "x":8.66769, "y":5.62151, "heading":0.0, "vx":1.99935, "vy":-0.00655, "omega":0.0, "ax":0.0003, "ay":0.13523, "alpha":0.0, "fx":[0.04124,0.04124,-0.03132,-0.03132], "fy":[2.29045,2.0787,2.35707,2.29044]}, + {"t":1.27098, "x":8.73121, "y":5.62137, "heading":0.0, "vx":1.99936, "vy":-0.00225, "omega":0.0, "ax":0.0, "ay":0.14048, "alpha":0.0, "fx":[-0.01379,-0.01379,0.09489,-0.0672], "fy":[2.19161,2.35554,2.35554,2.4642]}, + {"t":1.30275, "x":8.79474, "y":5.62137, "heading":0.0, "vx":1.99936, "vy":0.00221, "omega":0.0, "ax":-0.0003, "ay":0.13587, "alpha":0.0, "fx":[0.02957,0.02957,-0.03945,-0.03945], "fy":[2.29946,2.09879,2.36211,2.29946]}, + {"t":1.33452, "x":8.85826, "y":5.6215, "heading":0.0, "vx":1.99935, "vy":0.00653, "omega":0.0, "ax":-0.00056, "ay":0.13102, "alpha":0.0, "fx":[-0.02039,-0.02039,0.07843,-0.07525], "fy":[2.05228,2.19501,2.19501,2.29378]}, + {"t":1.3663, "x":8.92178, "y":5.62178, "heading":0.0, "vx":1.99933, "vy":0.01069, "omega":0.0, "ax":-0.00103, "ay":0.15632, "alpha":0.0, "fx":[0.00936,0.00936,-0.04367,-0.04367], "fy":[2.63229,2.4859,2.67262,2.63229]}, + {"t":1.39807, "x":8.9853, "y":5.6222, "heading":0.0, "vx":1.9993, "vy":0.01565, "omega":0.0, "ax":-0.00254, "ay":0.2577, "alpha":0.0, "fx":[-0.02146,-0.02145,0.04568,-0.17236], "fy":[4.29159,4.27484,4.27484,4.34186]}, + {"t":1.42984, "x":9.04881, "y":5.62282, "heading":0.0, "vx":1.99922, "vy":0.02384, "omega":0.0, "ax":-0.00722, "ay":0.46297, "alpha":0.0, "fx":[-0.14724,-0.1472,-0.09365,-0.09363], "fy":[7.69072,7.85674,7.63187,7.69073]}, + {"t":1.46161, "x":9.11232, "y":5.62382, "heading":0.0, "vx":1.99899, "vy":0.03855, "omega":0.0, "ax":-0.01371, "ay":0.57477, "alpha":0.0, "fx":[-0.1235,-0.12359,-0.21549,-0.45152], "fy":[9.98801,9.4762,9.47616,9.38428]}, + {"t":1.49338, "x":9.17583, "y":5.62533, "heading":0.0, "vx":1.99856, "vy":0.05681, "omega":0.0, "ax":0.01633, "ay":-0.76584, "alpha":0.0, "fx":[0.23642,0.23533,0.30878,0.3084], "fy":[-12.8023,-12.62272,-12.83688,-12.80273]}, + {"t":1.52515, "x":9.23933, "y":5.62675, "heading":0.0, "vx":1.99907, "vy":0.03248, "omega":0.0, "ax":-0.19871, "ay":-5.9787, "alpha":0.0, "fx":[-3.41569,-3.41595,-3.37393,-3.04381], "fy":[-100.01686,-99.55931,-99.55799,-99.51433]}, + {"t":1.55692, "x":9.30274, "y":5.62476, "heading":0.0, "vx":1.99276, "vy":-0.15747, "omega":0.0, "ax":-0.7292, "ay":-5.94964, "alpha":0.0, "fx":[-12.10863,-12.10432,-12.20429,-12.20454], "fy":[-99.12989,-99.36386,-99.08952,-99.1276]}, + {"t":1.58869, "x":9.36568, "y":5.61676, "heading":0.0, "vx":1.96959, "vy":-0.34649, "omega":0.0, "ax":2.95454, "ay":-5.21457, "alpha":-0.00847, "fx":[49.35337,49.24097,49.21399,49.19473], "fy":[-86.90803,-87.05328,-86.80573,-86.9303]}, + {"t":1.62269, "x":9.43436, "y":5.60196, "heading":0.0, "vx":2.07005, "vy":-0.52378, "omega":-0.00029, "ax":2.90699, "ay":-5.24449, "alpha":0.01769, "fx":[48.24755,48.37333,48.24382,48.96762], "fy":[-87.57963,-87.3549,-87.36857,-87.38975]}, + {"t":1.65669, "x":9.50642, "y":5.58112, "heading":-0.00001, "vx":2.16888, "vy":-0.70209, "omega":0.00031, "ax":2.84293, "ay":-5.27885, "alpha":-0.00559, "fx":[47.47364,47.36026,47.36892,47.35857], "fy":[-87.99035,-88.08027,-87.91035,-88.00276]}, + {"t":1.69069, "x":9.5818, "y":5.5542, "heading":0.0, "vx":2.26554, "vy":-0.88156, "omega":0.00012, "ax":2.75537, "ay":-5.32423, "alpha":0.0197, "fx":[45.68055,45.84051,45.73928,46.46229], "fy":[-88.95239,-88.67364,-88.68259,-88.70052]}, + {"t":1.72468, "x":9.66042, "y":5.52115, "heading":0.00001, "vx":2.35922, "vy":-1.06258, "omega":0.00079, "ax":2.6299, "ay":-5.3861, "alpha":-0.00765, "fx":[43.87669,43.83804,43.82047,43.82171], "fy":[-89.8025,-89.77424,-89.75476,-89.803]}, + {"t":1.75868, "x":9.74215, "y":5.48191, "heading":0.00003, "vx":2.44863, "vy":-1.2457, "omega":0.00053, "ax":2.43318, "ay":-5.47595, "alpha":0.01612, "fx":[40.4673,40.47317,40.45412,40.84495], "fy":[-91.26504,-91.2726,-91.26811,-91.32022]}, + {"t":1.79268, "x":9.8268, "y":5.4364, "heading":0.00005, "vx":2.53136, "vy":-1.43188, "omega":0.00108, "ax":2.08658, "ay":-5.6141, "alpha":-0.02517, "fx":[34.74513,34.86956,34.75298,34.76147], "fy":[-93.68544,-93.29377,-93.67972,-93.67819]}, + {"t":1.82668, "x":9.91407, "y":5.38447, "heading":0.00009, "vx":2.6023, "vy":-1.62275, "omega":0.00023, "ax":1.32407, "ay":-5.83496, "alpha":-0.00008, "fx":[21.98128,21.98368,22.00311,22.31841], "fy":[-97.53689,-97.17839,-97.17463,-97.17423]}, + {"t":1.86068, "x":10.00331, "y":5.32593, "heading":0.00009, "vx":2.64731, "vy":-1.82113, "omega":0.00022, "ax":-1.17708, "ay":-5.84927, "alpha":0.06864, "fx":[-19.64709,-19.57684,-19.63034,-19.63138], "fy":[-97.16022,-98.5334,-97.16309,-97.16143]}, + {"t":1.89468, "x":10.09264, "y":5.26063, "heading":0.0001, "vx":2.6073, "vy":-2.02, "omega":0.00256, "ax":-3.76007, "ay":-4.59212, "alpha":-0.23506, "fx":[-61.78042,-61.96404,-61.75263,-65.21745], "fy":[-78.01626,-76.19845,-76.22119,-75.75775]}, + {"t":1.92868, "x":10.17911, "y":5.1893, "heading":0.00019, "vx":2.47946, "vy":-2.17613, "omega":-0.00544, "ax":-3.98117, "ay":-4.27121, "alpha":0.16431, "fx":[-66.3974,-66.2174,-66.4199,-66.42213], "fy":[-70.34137,-73.78383,-70.32807,-70.34279]}, + {"t":1.96268, "x":10.26111, "y":5.11284, "heading":0.0, "vx":2.3441, "vy":-2.32135, "omega":0.00015, "ax":0.11439, "ay":0.88178, "alpha":0.07678, "fx":[1.49087,1.49179,1.54812,3.09679], "fy":[14.51516,14.74044,14.74055,14.79945]}, + {"t":1.99668, "x":10.34087, "y":5.03443, "heading":0.00001, "vx":2.34799, "vy":-2.29137, "omega":0.00276, "ax":-3.13949, "ay":5.07763, "alpha":-0.14678, "fx":[-52.46344,-52.11876,-52.38498,-52.36813], "fy":[83.96311,86.74707,83.88066,83.97602]}, + {"t":2.03067, "x":10.41889, "y":4.95946, "heading":0.0001, "vx":2.24125, "vy":-2.11873, "omega":-0.00223, "ax":-3.24282, "ay":5.03076, "alpha":0.05366, "fx":[-53.93389,-54.14158,-54.13299,-54.01641], "fy":[84.68073,83.57771,83.58327,83.59976]}, + {"t":2.06467, "x":10.49321, "y":4.89033, "heading":0.00003, "vx":2.131, "vy":-1.94769, "omega":-0.0004, "ax":-3.26996, "ay":5.01911, "alpha":-0.00034, "fx":[-54.48493,-54.54879,-54.50473,-54.49591], "fy":[83.6548,83.69877,83.65351,83.65772]}, + {"t":2.09867, "x":10.56378, "y":4.82701, "heading":0.00001, "vx":2.01982, "vy":-1.77705, "omega":-0.00042, "ax":-3.28475, "ay":5.01238, "alpha":0.04953, "fx":[-55.20427,-54.93102,-54.91712,-53.96853], "fy":[83.0616,83.61915,83.63117,83.90394]}, + {"t":2.13267, "x":10.63055, "y":4.76949, "heading":0.0, "vx":1.90815, "vy":-1.60663, "omega":0.00127, "ax":-3.29244, "ay":5.00908, "alpha":-0.03225, "fx":[-54.93459,-54.75768,-54.92451,-54.91681], "fy":[83.3744,83.86277,83.37489,83.38395]}, + {"t":2.16667, "x":10.69352, "y":4.71776, "heading":0.00004, "vx":1.79621, "vy":-1.43633, "omega":0.00017, "ax":-3.29826, "ay":5.00641, "alpha":0.01185, "fx":[-54.69235,-55.0218,-55.00101,-55.20645], "fy":[84.0531,83.26969,83.27722,83.21805]}, + {"t":2.20067, "x":10.75268, "y":4.67182, "heading":0.00005, "vx":1.68407, "vy":-1.26611, "omega":0.00057, "ax":-3.3021, "ay":5.00471, "alpha":-0.03024, "fx":[-55.11021,-54.9102,-55.08475,-55.07238], "fy":[83.3063,83.78658,83.29181,83.31974]}, + {"t":2.23467, "x":10.80803, "y":4.63167, "heading":0.00007, "vx":1.5718, "vy":-1.09596, "omega":-0.00045, "ax":-3.30522, "ay":5.00327, "alpha":0.00835, "fx":[-54.90824,-55.12659,-55.08574,-55.26498], "fy":[83.8005,83.27637,83.28846,83.243]}, + {"t":2.26867, "x":10.85956, "y":4.5973, "heading":0.00005, "vx":1.45943, "vy":-0.92585, "omega":-0.00017, "ax":-3.30745, "ay":5.00227, "alpha":-0.01859, "fx":[-55.18957,-55.04704,-55.15659,-55.14145], "fy":[83.30245,83.64142,83.27981,83.3181]}, + {"t":2.30267, "x":10.90727, "y":4.56871, "heading":0.00005, "vx":1.34698, "vy":-0.75578, "omega":-0.0008, "ax":-3.30939, "ay":5.00137, "alpha":0.00651, "fx":[-55.02844,-55.18969,-55.13969,-55.30623], "fy":[83.66598,83.27468,83.28883,83.25227]}, + {"t":2.33666, "x":10.95115, "y":4.54591, "heading":0.00002, "vx":1.23446, "vy":-0.58574, "omega":-0.00058, "ax":-3.3109, "ay":5.00069, "alpha":-0.01114, "fx":[-55.23896,-55.13655,-55.2025,-55.18638], "fy":[83.29999,83.54695,83.27298,83.31631]}, + {"t":2.37066, "x":10.99121, "y":4.52888, "heading":0.0, "vx":1.12189, "vy":-0.41572, "omega":-0.00096, "ax":-0.61286, "ay":0.78449, "alpha":0.00207, "fx":[-10.25101,-10.25033,-10.18179,-10.18126], "fy":[13.05231,13.24164,12.96132,13.0527]}, + {"t":2.40536, "x":11.02976, "y":4.51493, "heading":-0.00003, "vx":1.10063, "vy":-0.3885, "omega":-0.00089, "ax":-0.70862, "ay":0.70282, "alpha":0.01516, "fx":[-11.79871,-11.80061,-11.73095,-11.91892], "fy":[11.9066,11.62902,11.62956,11.69779]}, + {"t":2.44006, "x":11.06753, "y":4.50188, "heading":-0.00006, "vx":1.07604, "vy":-0.36412, "omega":-0.00036, "ax":-0.77253, "ay":0.6323, "alpha":0.00135, "fx":[-12.89861,-12.89799,-12.85716,-12.85673], "fy":[10.52581,10.63859,10.46979,10.52617]}, + {"t":2.47475, "x":11.1044, "y":4.48962, "heading":-0.00008, "vx":1.04924, "vy":-0.34218, "omega":-0.00031, "ax":-0.81695, "ay":0.5741, "alpha":0.00653, "fx":[-13.59122,-13.59253,-13.54737,-13.74132], "fy":[9.70014,9.51182,9.51219,9.556]}, + {"t":2.50945, "x":11.14031, "y":4.4781, "heading":-0.00009, "vx":1.0209, "vy":-0.32226, "omega":-0.00009, "ax":-0.84877, "ay":0.52623, "alpha":0.00066, "fx":[-14.16262,-14.16221,-14.13488,-14.13459], "fy":[8.76121,8.84188,8.72333,8.76145]}, + {"t":2.54415, "x":11.17522, "y":4.46723, "heading":-0.00009, "vx":0.99145, "vy":-0.304, "omega":-0.00006, "ax":-0.87224, "ay":0.4866, "alpha":0.00279, "fx":[-14.51141,-14.51231,-14.48035,-14.65549], "fy":[8.20544,8.0697,8.06996,8.10077]}, + {"t":2.57884, "x":11.20909, "y":4.45698, "heading":-0.00009, "vx":0.96118, "vy":-0.28712, "omega":0.00003, "ax":-0.89003, "ay":0.4535, "alpha":0.00046, "fx":[-14.84669,-14.8464,-14.82624,-14.82605], "fy":[7.55167,7.6115,7.52381,7.55185]}, + {"t":2.61354, "x":11.24191, "y":4.44729, "heading":-0.00009, "vx":0.9303, "vy":-0.27138, "omega":0.00005, "ax":-0.90382, "ay":0.42557, "alpha":0.00093, "fx":[-15.03907,-15.03973,-15.01524,-15.17117], "fy":[7.16475,7.06246,7.06265,7.08618]}, + {"t":2.64823, "x":11.27364, "y":4.43813, "heading":-0.00009, "vx":0.89894, "vy":-0.25662, "omega":0.00008, "ax":-0.91475, "ay":0.40175, "alpha":0.00052, "fx":[-15.25664,-15.25644,-15.24035,-15.2402], "fy":[6.69124,6.73586,6.66922,6.69137]}, + {"t":2.68293, "x":11.30428, "y":4.42947, "heading":-0.00009, "vx":0.8672, "vy":-0.24268, "omega":0.0001, "ax":-0.92356, "ay":0.38124, "alpha":0.00001, "fx":[-15.37016,-15.37065,-15.35066,-15.48953], "fy":[6.41046,6.3301,6.33025,6.34945]}, + {"t":2.71763, "x":11.33381, "y":4.42127, "heading":-0.00008, "vx":0.83516, "vy":-0.22945, "omega":0.0001, "ax":-0.93077, "ay":0.36343, "alpha":0.00066, "fx":[-15.52249,-15.52235,-15.50876,-15.50865], "fy":[6.05444,6.08744,6.03606,6.05455]}, + {"t":2.75232, "x":11.36223, "y":4.41353, "heading":-0.00008, "vx":0.80287, "vy":-0.21684, "omega":0.00012, "ax":-0.93677, "ay":0.34783, "alpha":-0.0005, "fx":[-15.59269,-15.59307,-15.57595,-15.70037], "fy":[5.84225,5.7778,5.77792,5.79437]}, + {"t":2.78702, "x":11.38952, "y":4.40622, "heading":-0.00008, "vx":0.77036, "vy":-0.20477, "omega":0.0001, "ax":-0.94181, "ay":0.33406, "alpha":0.00073, "fx":[-15.70565,-15.70554,-15.69365,-15.69356], "fy":[5.56619,5.59202,5.55019,5.56627]}, + {"t":2.82172, "x":11.41568, "y":4.39931, "heading":-0.00007, "vx":0.73769, "vy":-0.19318, "omega":0.00013, "ax":-0.9461, "ay":0.32184, "alpha":-0.00072, "fx":[-15.7504,-15.75071,-15.73557,-15.84773], "fy":[5.40147,5.34779,5.34789,5.36246]}, + {"t":2.85641, "x":11.44071, "y":4.39281, "heading":-0.00007, "vx":0.70486, "vy":-0.18202, "omega":0.00011, "ax":-0.94979, "ay":0.31092, "alpha":0.00084, "fx":[-15.83791,-15.83783,-15.82719,-15.82711], "fy":[5.18152,5.20088,5.16724,5.18159]}, + {"t":2.89111, "x":11.46459, "y":4.38668, "heading":-0.00006, "vx":0.6719, "vy":-0.17123, "omega":0.00013, "ax":-0.95298, "ay":0.3011, "alpha":-0.00086, "fx":[-15.86684,-15.8671,-15.85342,-15.95549], "fy":[5.04957,5.00457,5.00466,5.01785]}, + {"t":2.9258, "x":11.48733, "y":4.38092, "heading":-0.00006, "vx":0.63884, "vy":-0.16078, "omega":0.0001, "ax":-0.95577, "ay":0.29223, "alpha":0.0009, "fx":[-15.93705,-15.93698,-15.92736,-15.92729], "fy":[4.87088,4.8857,4.85789,4.87094]}, + {"t":2.9605, "x":11.50892, "y":4.37551, "heading":-0.00006, "vx":0.60568, "vy":-0.15064, "omega":0.00014, "ax":-0.95822, "ay":0.28418, "alpha":-0.00089, "fx":[-15.95582,-15.95603,-15.9435,-16.03675], "fy":[4.76332,4.72443,4.7245,4.73661]}, + {"t":2.9952, "x":11.52936, "y":4.37046, "heading":-0.00005, "vx":0.57243, "vy":-0.14078, "omega":0.0001, "ax":-0.96039, "ay":0.27685, "alpha":0.00094, "fx":[-16.01363,-16.01358,-16.00481,-16.00475], "fy":[4.61509,4.62647,4.60315,4.61515]}, + {"t":3.02989, "x":11.54864, "y":4.36574, "heading":-0.00005, "vx":0.53911, "vy":-0.13118, "omega":0.00014, "ax":-0.96232, "ay":0.27014, "alpha":-0.00092, "fx":[-16.0256,-16.02578,-16.0142,-16.1], "fy":[4.52556,4.49184,4.49191,4.50313]}, + {"t":3.06459, "x":11.56677, "y":4.36135, "heading":-0.00004, "vx":0.50572, "vy":-0.1218, "omega":0.00011, "ax":-0.96405, "ay":0.26398, "alpha":0.00095, "fx":[-16.07428,-16.07423,-16.06622,-16.06616], "fy":[4.40097,4.40967,4.3899,4.40102]}, + {"t":3.09929, "x":11.58374, "y":4.35728, "heading":-0.00004, "vx":0.47227, "vy":-0.11265, "omega":0.00014, "ax":-0.9656, "ay":0.2583, "alpha":-0.00095, "fx":[-16.08154,-16.0817,-16.07094,-16.15047], "fy":[4.325,4.29581,4.29587,4.30631]}, + {"t":3.13398, "x":11.59954, "y":4.35353, "heading":-0.00003, "vx":0.43877, "vy":-0.10368, "omega":0.00011, "ax":-0.96701, "ay":0.25305, "alpha":0.00096, "fx":[-16.1233,-16.12327,-16.11594,-16.1159], "fy":[4.21924,4.22559,4.20898,4.21928]}, + {"t":3.16868, "x":11.61418, "y":4.35009, "heading":-0.00003, "vx":0.40522, "vy":-0.0949, "omega":0.00014, "ax":-0.96829, "ay":0.24819, "alpha":-0.00097, "fx":[-16.12726,-16.1274,-16.11746,-16.1914], "fy":[4.15399,4.12833,4.12838,4.13804]}, + {"t":3.20338, "x":11.62766, "y":4.34694, "heading":-0.00003, "vx":0.37162, "vy":-0.08629, "omega":0.00011, "ax":-0.96945, "ay":0.24367, "alpha":0.00092, "fx":[-16.1636,-16.16357,-16.15696,-16.15692], "fy":[4.06295,4.06779,4.05354,4.06298]}, + {"t":3.23807, "x":11.63997, "y":4.3441, "heading":-0.00002, "vx":0.33799, "vy":-0.07784, "omega":0.00014, "ax":-0.97051, "ay":0.23945, "alpha":-0.00103, "fx":[-16.16514,-16.16526,-16.15624,-16.22531], "fy":[4.00613,3.98379,3.98383,3.99259]}, + {"t":3.27277, "x":11.65111, "y":4.34154, "heading":-0.00002, "vx":0.30431, "vy":-0.06953, "omega":0.0001, "ax":-0.97149, "ay":0.23552, "alpha":0.00084, "fx":[-16.19717,-16.19714,-16.19137,-16.19133], "fy":[3.92717,3.93082,3.91881,3.9272]}, + {"t":3.30746, "x":11.66109, "y":4.33927, "heading":-0.00001, "vx":0.27061, "vy":-0.06136, "omega":0.00013, "ax":-0.97239, "ay":0.23184, "alpha":-0.00116, "fx":[-16.19684,-16.19695,-16.18917,-16.25391], "fy":[3.87705,3.85791,3.85795,3.86549]}, + {"t":3.34216, "x":11.66989, "y":4.33728, "heading":-0.00001, "vx":0.23687, "vy":-0.05331, "omega":0.00009, "ax":-0.97322, "ay":0.22838, "alpha":0.00065, "fx":[-16.22536,-16.22533,-16.22073,-16.22071], "fy":[3.80787,3.81122,3.80105,3.80789]}, + {"t":3.37686, "x":11.67752, "y":4.33557, "heading":-0.00001, "vx":0.2031, "vy":-0.04539, "omega":0.00011, "ax":-0.97398, "ay":0.22513, "alpha":-0.00146, "fx":[-16.22343,-16.22352,-16.21767,-16.27866], "fy":[3.76293,3.74765,3.74767,3.75328]}, + {"t":3.41155, "x":11.68398, "y":4.33413, "heading":0.0, "vx":0.16931, "vy":-0.03758, "omega":0.00006, "ax":-0.97469, "ay":0.22208, "alpha":0.00028, "fx":[-16.24906,-16.24902,-16.24634,-16.24632], "fy":[3.70194,3.70613,3.69765,3.70195]}, + {"t":3.44625, "x":11.68927, "y":4.33296, "heading":0.0, "vx":0.13549, "vy":-0.02987, "omega":0.00007, "ax":-0.97536, "ay":0.21919, "alpha":-0.00205, "fx":[-16.24555,-16.24561,-16.24307,-16.30065], "fy":[3.66109,3.65068,3.65068,3.65297]}, + {"t":3.48095, "x":11.69339, "y":4.33205, "heading":0.0, "vx":0.10165, "vy":-0.02227, "omega":0.0, "ax":-0.97597, "ay":0.21647, "alpha":-0.00045, "fx":[-16.26871,-16.26866,-16.26934,-16.26934], "fy":[3.6066,3.61387,3.60673,3.60659]}, + {"t":3.51564, "x":11.69632, "y":4.33141, "heading":0.0, "vx":0.06778, "vy":-0.01476, "omega":-0.00001, "ax":-0.97655, "ay":0.21389, "alpha":-0.00099, "fx":[-16.27711,-16.27705,-16.28016,-16.28018], "fy":[3.56222,3.57198,3.56551,3.5622]}, + {"t":3.55034, "x":11.69809, "y":4.33103, "heading":0.0, "vx":0.0339, "vy":-0.00734, "omega":-0.00005, "ax":-0.97709, "ay":0.21145, "alpha":0.00141, "fx":[-16.27089,-16.32395,-16.27781,-16.27785], "fy":[3.52184,3.52762,3.5278,3.5218]}, + {"t":3.58503, "x":11.69868, "y":4.3309, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/startHToH.traj b/src/main/deploy/choreo/startHToH.traj index 40489af..e11befb 100644 --- a/src/main/deploy/choreo/startHToH.traj +++ b/src/main/deploy/choreo/startHToH.traj @@ -3,25 +3,25 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":4.0509, "heading":3.141592653589793, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.1008875, "y":4.0509, "heading":3.141592653589793, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":5.827323, "y":4.0509, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":3.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"startH.x", "val":7.1008875}, "y":{"exp":"startH.y", "val":4.0509}, "heading":{"exp":"startH.heading", "val":3.141592653589793}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"startH.x", "val":7.1008875}, "y":{"exp":"startH.y", "val":4.0509}, "heading":{"exp":"startH.heading", "val":3.141592653589793}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"H.x", "val":5.827323}, "y":{"exp":"H.y", "val":4.0509}, "heading":{"exp":"H.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"maxAccel", "val":3.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", @@ -30,42 +30,36 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.60818], + "waypoints":[0.0,1.30427], "samples":[ - {"t":0.0, "x":7.10089, "y":4.0509, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.99863, "ay":0.0, "alpha":0.0002, "fx":[-49.96561,-50.01021,-49.98348,-49.98348], "fy":[-0.00318,0.00317,0.00317,-0.00317]}, - {"t":0.04873, "x":7.09733, "y":4.0509, "heading":3.14159, "vx":-0.14613, "vy":0.0, "omega":0.00001, "ax":-2.99918, "ay":0.0, "alpha":0.00039, "fx":[-49.97443,-50.01945,-49.99274,-49.99274], "fy":[-0.004,0.00399,0.00399,-0.004]}, - {"t":0.09747, "x":7.08664, "y":4.0509, "heading":-3.14159, "vx":-0.29229, "vy":0.0, "omega":0.00003, "ax":-2.99897, "ay":0.0, "alpha":0.00356, "fx":[-49.98718,-49.98718,-49.99565,-49.99565], "fy":[-0.01312,0.01319,0.01306,-0.01311]}, - {"t":0.1462, "x":7.06884, "y":4.0509, "heading":-3.14159, "vx":-0.43844, "vy":0.0, "omega":0.0002, "ax":-2.99863, "ay":0.0, "alpha":0.00174, "fx":[-49.95623,-50.0184,-49.98412,-49.98412], "fy":[-0.01009,0.01006,0.01007,-0.01009]}, - {"t":0.19493, "x":7.04391, "y":4.0509, "heading":-3.14158, "vx":-0.58457, "vy":0.0, "omega":0.00029, "ax":-2.99795, "ay":0.0, "alpha":0.00714, "fx":[-49.96167,-49.96167,-49.987,-49.987], "fy":[-0.02211,0.02224,0.02204,-0.02211]}, - {"t":0.24366, "x":7.01186, "y":4.0509, "heading":-3.14157, "vx":-0.73067, "vy":0.0, "omega":0.00064, "ax":-2.99591, "ay":0.0, "alpha":0.01773, "fx":[-49.94883,-49.80898,-50.00186,-50.00185], "fy":[-0.02493,0.0249,0.02483,-0.02493]}, - {"t":0.2924, "x":6.9727, "y":4.0509, "heading":-3.14154, "vx":-0.87666, "vy":0.0, "omega":0.0015, "ax":-2.5267, "ay":0.00001, "alpha":0.0176, "fx":[-42.08049,-42.08052,-42.15715,-42.15713], "fy":[-0.04731,0.04763,0.04736,-0.04733]}, - {"t":0.34113, "x":6.92698, "y":4.0509, "heading":-3.14146, "vx":-0.9998, "vy":0.0, "omega":0.00236, "ax":-0.00004, "ay":-0.00001, "alpha":-0.10502, "fx":[0.72127,-1.87954,0.57785,0.57792], "fy":[-0.06698,0.06681,0.06677,-0.06705]}, - {"t":0.38986, "x":6.87825, "y":4.0509, "heading":-3.14135, "vx":-0.9998, "vy":0.0, "omega":-0.00276, "ax":0.0, "ay":0.00001, "alpha":0.01934, "fx":[0.04539,0.04531,-0.04542,-0.04533], "fy":[-0.04863,0.05029,0.04761,-0.04872]}, - {"t":0.43859, "x":6.82953, "y":4.0509, "heading":-3.14148, "vx":-0.9998, "vy":0.0, "omega":-0.00182, "ax":0.0, "ay":-0.00001, "alpha":0.02201, "fx":[-0.02104,0.1813,-0.08014,-0.0801], "fy":[-0.02714,0.027,0.02697,-0.02718]}, - {"t":0.48733, "x":6.78081, "y":4.0509, "heading":-3.14157, "vx":-0.9998, "vy":0.0, "omega":-0.00075, "ax":0.0, "ay":0.0, "alpha":0.00761, "fx":[0.01724,0.01723,-0.01724,-0.01723], "fy":[-0.0198,0.02037,0.01941,-0.01982]}, - {"t":0.53606, "x":6.73209, "y":4.0509, "heading":3.14158, "vx":-0.9998, "vy":0.0, "omega":-0.00038, "ax":0.0, "ay":0.0, "alpha":0.00593, "fx":[0.00715,0.02936,-0.01826,-0.01825], "fy":[-0.01066,0.01059,0.01058,-0.01067]}, - {"t":0.58479, "x":6.68336, "y":4.0509, "heading":3.14156, "vx":-0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":0.00305, "fx":[0.00648,0.00647,-0.00648,-0.00647], "fy":[-0.00833,0.00871,0.0081,-0.00833]}, - {"t":0.63352, "x":6.63464, "y":4.0509, "heading":3.14156, "vx":-0.9998, "vy":0.0, "omega":0.00006, "ax":0.0, "ay":0.0, "alpha":0.00189, "fx":[0.00478,0.00613,-0.00546,-0.00545], "fy":[-0.00378,0.00373,0.00372,-0.00378]}, - {"t":0.68226, "x":6.58592, "y":4.0509, "heading":3.14156, "vx":-0.9998, "vy":0.0, "omega":0.00015, "ax":0.0, "ay":0.0, "alpha":0.00103, "fx":[0.00198,0.00198,-0.00198,-0.00198], "fy":[-0.00302,0.00323,0.00289,-0.00302]}, - {"t":0.73099, "x":6.53719, "y":4.0509, "heading":3.14157, "vx":-0.9998, "vy":0.0, "omega":0.0002, "ax":0.0, "ay":0.0, "alpha":0.00045, "fx":[0.00136,0.00143,-0.0014,-0.00139], "fy":[-0.00081,0.00079,0.00079,-0.00081]}, - {"t":0.77972, "x":6.48847, "y":4.0509, "heading":3.14158, "vx":-0.9998, "vy":0.0, "omega":0.00023, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.0001,-0.0001,0.0001,0.0001], "fy":[-0.00012,0.00019,0.00006,-0.00012]}, - {"t":0.82846, "x":6.43975, "y":4.0509, "heading":3.14159, "vx":-0.9998, "vy":0.0, "omega":0.00023, "ax":0.0, "ay":0.0, "alpha":-0.00044, "fx":[-0.00115,-0.00119,0.00117,0.00117], "fy":[0.00097,-0.00095,-0.00095,0.00096]}, - {"t":0.87719, "x":6.39103, "y":4.0509, "heading":-3.14159, "vx":-0.9998, "vy":0.0, "omega":0.00021, "ax":0.0, "ay":0.0, "alpha":-0.00096, "fx":[-0.00203,-0.00203,0.00203,0.00203], "fy":[0.00262,-0.0027,-0.0026,0.00262]}, - {"t":0.92592, "x":6.3423, "y":4.0509, "heading":-3.14158, "vx":-0.9998, "vy":0.0, "omega":0.00016, "ax":0.0, "ay":0.0, "alpha":-0.00173, "fx":[-0.00424,-0.00544,0.00484,0.00484], "fy":[0.00361,-0.00357,-0.00357,0.00361]}, - {"t":0.97465, "x":6.29358, "y":4.0509, "heading":-3.14157, "vx":-0.9998, "vy":0.0, "omega":0.00007, "ax":0.0, "ay":0.0, "alpha":-0.00274, "fx":[-0.00597,-0.00597,0.00597,0.00597], "fy":[0.00734,-0.00757,-0.00722,0.00734]}, - {"t":1.02339, "x":6.24486, "y":4.0509, "heading":-3.14157, "vx":-0.9998, "vy":0.0, "omega":-0.00006, "ax":0.0, "ay":0.0, "alpha":-0.00528, "fx":[-0.00629,-0.02598,0.01613,0.01614], "fy":[0.00964,-0.00956,-0.00956,0.00963]}, - {"t":1.07212, "x":6.19613, "y":4.0509, "heading":-3.14157, "vx":-0.9998, "vy":0.0, "omega":-0.00032, "ax":0.0, "ay":0.0, "alpha":-0.00674, "fx":[-0.01537,-0.01537,0.01536,0.01537], "fy":[0.01741,-0.01781,-0.01714,0.0174]}, - {"t":1.12085, "x":6.14741, "y":4.0509, "heading":-3.14158, "vx":-0.9998, "vy":0.0, "omega":-0.00064, "ax":0.0, "ay":0.0, "alpha":-0.01922, "fx":[0.01777,-0.15708,0.06963,0.06965], "fy":[0.02404,-0.02387,-0.02389,0.02403]}, - {"t":1.16958, "x":6.09869, "y":4.0509, "heading":3.14157, "vx":-0.9998, "vy":0.0, "omega":-0.00158, "ax":0.0, "ay":-0.00001, "alpha":-0.01696, "fx":[-0.03986,-0.03989,0.03988,0.03991], "fy":[0.04261,-0.04388,-0.0418,0.04258]}, - {"t":1.21832, "x":6.04997, "y":4.0509, "heading":3.14149, "vx":-0.9998, "vy":0.0, "omega":-0.00241, "ax":0.00004, "ay":0.00001, "alpha":0.09404, "fx":[-0.64123,1.67552,-0.51597,-0.51592], "fy":[0.0587,-0.05844,-0.05851,0.05865]}, - {"t":1.26705, "x":6.00124, "y":4.0509, "heading":3.14138, "vx":-0.9998, "vy":0.0, "omega":0.00218, "ax":2.52601, "ay":0.0, "alpha":-0.01507, "fx":[42.07471,42.07466,42.1402,42.14024], "fy":[0.04058,-0.04076,-0.04066,0.04052]}, - {"t":1.31578, "x":5.95552, "y":4.0509, "heading":3.14148, "vx":-0.8767, "vy":0.0, "omega":0.00144, "ax":2.99591, "ay":0.0, "alpha":-0.01983, "fx":[49.97336,49.75432,50.01688,50.01689], "fy":[0.02011,-0.02006,-0.02002,0.02008]}, - {"t":1.36451, "x":5.91635, "y":4.0509, "heading":3.14155, "vx":-0.7307, "vy":0.0, "omega":0.00047, "ax":2.99795, "ay":0.0, "alpha":-0.00598, "fx":[49.96394,49.96394,49.98472,49.98472], "fy":[0.01873,-0.01883,-0.01867,0.01872]}, - {"t":1.41325, "x":5.8843, "y":4.0509, "heading":3.14157, "vx":-0.5846, "vy":0.0, "omega":0.00018, "ax":2.99863, "ay":0.0, "alpha":-0.00021, "fx":[49.9526,50.03485,49.9777,49.9777], "fy":[0.00907,-0.00904,-0.00905,0.00907]}, - {"t":1.46198, "x":5.85938, "y":4.0509, "heading":3.14158, "vx":-0.43847, "vy":0.0, "omega":0.00017, "ax":2.99897, "ay":0.0, "alpha":-0.00339, "fx":[49.98705,49.98706,49.99578,49.99577], "fy":[0.01213,-0.0122,-0.01208,0.01213]}, - {"t":1.51071, "x":5.84157, "y":4.0509, "heading":3.14159, "vx":-0.29232, "vy":0.0, "omega":0.00001, "ax":2.99918, "ay":0.0, "alpha":0.00012, "fx":[49.97187,50.02812,49.98968,49.98968], "fy":[0.00459,-0.00458,-0.00458,0.00459]}, - {"t":1.55945, "x":5.83088, "y":4.0509, "heading":3.14159, "vx":-0.14616, "vy":0.0, "omega":0.00001, "ax":2.99931, "ay":0.0, "alpha":-0.00026, "fx":[49.97734,50.0221,49.99453,49.99452], "fy":[0.00389,-0.00388,-0.00388,0.00389]}, - {"t":1.60818, "x":5.82732, "y":4.0509, "heading":-3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.10089, "y":4.0509, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.99871, "ay":0.0, "alpha":-0.00759, "fx":[-49.8693,-50.24484,-49.91712,-49.91712], "fy":[-0.03299,0.03297,0.03301,-0.03299]}, + {"t":0.04831, "x":7.09739, "y":4.0509, "heading":3.14159, "vx":-0.14486, "vy":0.0, "omega":-0.00037, "ax":-2.9993, "ay":0.0, "alpha":-0.00055, "fx":[-49.9227,-50.13482,-49.9651,-49.9651], "fy":[-0.02916,0.02915,0.02916,-0.02915]}, + {"t":0.09661, "x":7.08689, "y":4.0509, "heading":3.14157, "vx":-0.28974, "vy":0.0, "omega":-0.00039, "ax":-2.99924, "ay":0.0, "alpha":0.01197, "fx":[-49.97457,-49.97457,-50.01718,-50.01718], "fy":[-0.03698,0.03696,0.03699,-0.03697]}, + {"t":0.14492, "x":7.0694, "y":4.0509, "heading":3.14156, "vx":-0.43462, "vy":0.0, "omega":0.00018, "ax":-2.99916, "ay":0.0, "alpha":-0.00413, "fx":[-49.91075,-50.17034,-49.94866,-49.94866], "fy":[-0.02584,0.02583,0.02585,-0.02584]}, + {"t":0.19323, "x":7.0449, "y":4.0509, "heading":3.14156, "vx":-0.5795, "vy":0.0, "omega":-0.00001, "ax":-2.99907, "ay":0.0, "alpha":0.01172, "fx":[-49.97159,-49.97159,-50.01451,-50.01451], "fy":[-0.03563,0.03561,0.03564,-0.03562]}, + {"t":0.24153, "x":7.01341, "y":4.0509, "heading":3.14156, "vx":-0.72438, "vy":0.0, "omega":0.00055, "ax":-2.99895, "ay":0.0, "alpha":-0.0066, "fx":[-49.88696,-50.21702,-49.93024,-49.93024], "fy":[-0.02874,0.02872,0.02874,-0.02873]}, + {"t":0.28984, "x":6.97492, "y":4.0509, "heading":3.14159, "vx":-0.86925, "vy":0.0, "omega":0.00023, "ax":-2.9988, "ay":0.0, "alpha":0.01359, "fx":[-49.96262,-49.96263,-50.01462,-50.01462], "fy":[-0.04021,0.0402,0.04022,-0.0402]}, + {"t":0.33814, "x":6.92943, "y":4.0509, "heading":-3.14158, "vx":-1.01411, "vy":0.0, "omega":0.00089, "ax":-2.9986, "ay":0.0, "alpha":-0.00984, "fx":[-49.84592,-50.29233,-49.90148,-49.90147], "fy":[-0.03592,0.0359,0.03593,-0.03592]}, + {"t":0.38645, "x":6.87694, "y":4.0509, "heading":-3.14154, "vx":-1.15896, "vy":0.0, "omega":0.00041, "ax":-2.99833, "ay":0.0, "alpha":0.01663, "fx":[-49.94734,-49.94735,-50.01395,-50.01394], "fy":[-0.0477,0.04771,0.04771,-0.0477]}, + {"t":0.43476, "x":6.81746, "y":4.0509, "heading":-3.14152, "vx":-1.3038, "vy":0.0, "omega":0.00122, "ax":-2.99791, "ay":0.0, "alpha":-0.01662, "fx":[-49.74441,-50.45513,-49.84757,-49.84756], "fy":[-0.04514,0.04511,0.04517,-0.04515]}, + {"t":0.48306, "x":6.75098, "y":4.0509, "heading":-3.14146, "vx":-1.44861, "vy":0.0, "omega":0.00041, "ax":-2.99721, "ay":0.0, "alpha":0.0193, "fx":[-49.92144,-49.92146,-50.00262,-50.00261], "fy":[-0.05342,0.05347,0.05338,-0.05343]}, + {"t":0.53137, "x":6.67751, "y":4.0509, "heading":-3.14144, "vx":-1.5934, "vy":0.0, "omega":0.00135, "ax":-2.99581, "ay":0.0, "alpha":-0.07863, "fx":[-49.392,-51.35536,-49.50387,-49.50384], "fy":[-0.05188,0.05179,0.05197,-0.0519]}, + {"t":0.57968, "x":6.59704, "y":4.0509, "heading":-3.14138, "vx":-1.73812, "vy":0.0, "omega":-0.00245, "ax":-2.99162, "ay":0.0, "alpha":0.01148, "fx":[-49.84367,-49.84369,-49.89402,-49.894], "fy":[-0.03076,0.03091,0.03064,-0.03077]}, + {"t":0.62798, "x":6.50959, "y":4.0509, "heading":-3.14149, "vx":-1.88263, "vy":0.0, "omega":-0.0019, "ax":-0.00065, "ay":0.0, "alpha":-0.00734, "fx":[0.0404,-0.14404,0.03017,0.03017], "fy":[-0.00526,0.00527,0.00527,-0.00526]}, + {"t":0.67629, "x":6.41864, "y":4.0509, "heading":-3.14159, "vx":-1.88266, "vy":0.0, "omega":-0.00225, "ax":2.99162, "ay":0.0, "alpha":-0.00848, "fx":[49.85091,49.85091,49.88695,49.88695], "fy":[0.02329,-0.02322,-0.02338,0.02328]}, + {"t":0.72459, "x":6.33119, "y":4.0509, "heading":3.14149, "vx":-1.73815, "vy":0.0, "omega":-0.00266, "ax":2.99581, "ay":0.0, "alpha":0.07713, "fx":[49.41678,51.30379,49.51725,49.51726], "fy":[0.04593,-0.04581,-0.04601,0.04591]}, + {"t":0.7729, "x":6.25072, "y":4.0509, "heading":3.14136, "vx":-1.59343, "vy":0.0, "omega":0.00107, "ax":2.99721, "ay":0.0, "alpha":-0.01768, "fx":[49.92515,49.92513,49.99892,49.99894], "fy":[0.04923,-0.04915,-0.04928,0.0492]}, + {"t":0.82121, "x":6.17725, "y":4.0509, "heading":3.14141, "vx":-1.44865, "vy":0.0, "omega":0.00021, "ax":2.99791, "ay":0.0, "alpha":0.01941, "fx":[49.73857,50.48317,49.83646,49.83647], "fy":[0.0427,-0.04263,-0.04272,0.04267]}, + {"t":0.86951, "x":6.11076, "y":4.0509, "heading":3.14142, "vx":-1.30383, "vy":0.0, "omega":0.00115, "ax":2.99833, "ay":0.0, "alpha":-0.01586, "fx":[49.9489,49.94888,50.01239,50.01241], "fy":[0.04547,-0.04542,-0.04552,0.04545]}, + {"t":0.91782, "x":6.05128, "y":4.0509, "heading":3.14148, "vx":-1.15899, "vy":0.0, "omega":0.00038, "ax":2.9986, "ay":0.0, "alpha":0.01059, "fx":[49.84653,50.29656,49.89905,49.89906], "fy":[0.0347,-0.03466,-0.03471,0.03468]}, + {"t":0.96613, "x":5.99879, "y":4.0509, "heading":3.1415, "vx":-1.01414, "vy":0.0, "omega":0.00089, "ax":2.9988, "ay":0.0, "alpha":-0.01304, "fx":[49.96349,49.96348,50.01376,50.01376], "fy":[0.03838,-0.03834,-0.03841,0.03836]}, + {"t":1.01443, "x":5.9533, "y":4.0509, "heading":3.14154, "vx":-0.86928, "vy":0.0, "omega":0.00026, "ax":2.99895, "ay":0.0, "alpha":0.00725, "fx":[49.8875,50.22056,49.9282,49.9282], "fy":[0.02761,-0.02758,-0.02762,0.0276]}, + {"t":1.06274, "x":5.91481, "y":4.0509, "heading":3.14155, "vx":-0.72441, "vy":0.0, "omega":0.00062, "ax":2.99907, "ay":0.0, "alpha":-0.01093, "fx":[49.97276,49.97276,50.01334,50.01335], "fy":[0.03296,-0.03293,-0.033,0.03296]}, + {"t":1.11105, "x":5.88331, "y":4.0509, "heading":3.14158, "vx":-0.57953, "vy":0.0, "omega":0.00009, "ax":2.99916, "ay":0.0, "alpha":0.00505, "fx":[49.91251,50.1732,49.94636,49.94636], "fy":[0.02367,-0.02365,-0.02367,0.02366]}, + {"t":1.15935, "x":5.85882, "y":4.0509, "heading":3.14159, "vx":-0.43466, "vy":0.0, "omega":0.00033, "ax":2.99924, "ay":0.0, "alpha":-0.01037, "fx":[49.97725,49.97725,50.0145,50.0145], "fy":[0.03188,-0.03186,-0.03191,0.03188]}, + {"t":1.20766, "x":5.84132, "y":4.0509, "heading":-3.14158, "vx":-0.28977, "vy":0.0, "omega":-0.00017, "ax":2.9993, "ay":0.0, "alpha":0.0025, "fx":[49.92642,50.14027,49.96052,49.96052], "fy":[0.02423,-0.02422,-0.02423,0.02422]}, + {"t":1.25596, "x":5.83082, "y":4.0509, "heading":-3.14159, "vx":-0.14489, "vy":0.0, "omega":-0.00005, "ax":2.99936, "ay":0.0, "alpha":0.00102, "fx":[49.9309,50.12623,49.96709,49.96709], "fy":[0.02576,-0.02575,-0.02577,0.02576]}, + {"t":1.30427, "x":5.82732, "y":4.0509, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/startToJSlow.traj b/src/main/deploy/choreo/startToJSlow.traj index 7390bf3..81a7bc1 100644 --- a/src/main/deploy/choreo/startToJSlow.traj +++ b/src/main/deploy/choreo/startToJSlow.traj @@ -3,9 +3,9 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.7971601486206055, "y":5.54304313659668, "heading":4.1887902047863905, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.079583864905389, "y":5.098261807735684, "heading":4.1887902047863905, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":7.1, "y":5.07, "heading":3.14159, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.7971601486206055, "y":5.54304313659668, "heading":-2.205689634858387, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.1366723649053885, "y":5.197141990263579, "heading":4.1887902047863905, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -18,7 +18,7 @@ "params":{ "waypoints":[ {"x":{"exp":"start.x", "val":7.1}, "y":{"exp":"start.y", "val":5.07}, "heading":{"exp":"start.heading", "val":3.14159}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.7971601486206055 m", "val":5.7971601486206055}, "y":{"exp":"5.54304313659668 m", "val":5.54304313659668}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.7971601486206055 m", "val":5.7971601486206055}, "y":{"exp":"5.54304313659668 m", "val":5.54304313659668}, "heading":{"exp":"-2.205689634858387 rad", "val":-2.205689634858387}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"J.x", "val":5.1366723649053885}, "y":{"exp":"J.y", "val":5.197141990263579}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -34,75 +34,76 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.92438,2.23311], + "waypoints":[0.0,0.92046,2.14877], "samples":[ - {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.08201, "ay":2.88181, "alpha":11.64697, "fx":[-44.00122,-38.23394,-90.14853,-99.79718], "fy":[19.97364,83.40552,72.26972,16.50461]}, - {"t":0.03188, "x":7.09881, "y":5.07714, "heading":3.14159, "vx":-0.13011, "vy":0.09186, "omega":0.37125, "ax":-4.09552, "ay":2.86522, "alpha":9.49084, "fx":[-48.88679,-43.69643,-86.44441,-94.05354], "fy":[25.38459,76.36435,67.65272,21.64572]}, - {"t":0.06375, "x":7.09259, "y":5.08153, "heading":-3.12976, "vx":-0.26066, "vy":0.18319, "omega":0.67377, "ax":-4.10937, "ay":2.8451, "alpha":7.65504, "fx":[-52.80443,-49.00577,-83.25869,-88.93608], "fy":[29.40111,70.58788,63.64125,26.07529]}, - {"t":0.09563, "x":7.08219, "y":5.08881, "heading":-3.10828, "vx":-0.39165, "vy":0.27388, "omega":0.91778, "ax":-4.12524, "ay":2.82179, "alpha":5.92347, "fx":[-56.20235,-53.78802,-80.68315,-84.38932], "fy":[34.00375,64.79007,59.42225,29.93544]}, - {"t":0.1275, "x":7.06761, "y":5.09897, "heading":-3.07903, "vx":-0.52314, "vy":0.36382, "omega":1.10659, "ax":-4.14357, "ay":2.7945, "alpha":4.41795, "fx":[-59.61533,-58.25799,-78.56065,-79.85113], "fy":[36.67262,60.32934,55.52993,33.80026]}, - {"t":0.15938, "x":7.04883, "y":5.11199, "heading":-3.04376, "vx":-0.65522, "vy":0.4529, "omega":1.24741, "ax":-4.16492, "ay":2.76222, "alpha":3.09499, "fx":[-62.43887,-62.04336,-76.12843,-77.0982], "fy":[39.85144,55.47063,52.29008,36.5675]}, - {"t":0.19125, "x":7.02583, "y":5.12783, "heading":-3.00399, "vx":-0.78797, "vy":0.54094, "omega":1.34607, "ax":-4.19052, "ay":2.72279, "alpha":1.93791, "fx":[-65.69179,-65.51065,-74.53251,-73.68089], "fy":[41.01555,51.86776,49.12112,39.54614]}, - {"t":0.22313, "x":6.99858, "y":5.14646, "heading":-2.96109, "vx":-0.92155, "vy":0.62773, "omega":1.40784, "ax":-4.22077, "ay":2.67513, "alpha":0.90335, "fx":[-68.14734,-68.40152,-72.43913,-72.44461], "fy":[43.06551,47.24391,46.49201,41.57078]}, - {"t":0.255, "x":6.96706, "y":5.16782, "heading":-2.91621, "vx":-1.05609, "vy":0.713, "omega":1.43663, "ax":-4.25833, "ay":2.61423, "alpha":0.07514, "fx":[-71.26263,-70.94599,-71.46604,-70.26254], "fy":[42.55831,44.22603,43.90431,43.62301]}, - {"t":0.28688, "x":6.93124, "y":5.19188, "heading":-2.87042, "vx":-1.19182, "vy":0.79633, "omega":1.43903, "ax":-4.30529, "ay":2.53523, "alpha":-0.67923, "fx":[-73.54063,-73.14308,-69.89147,-70.49321], "fy":[43.31618,39.76039,41.58476,44.38305]}, - {"t":0.31875, "x":6.89106, "y":5.21855, "heading":-2.82455, "vx":-1.32905, "vy":0.87714, "omega":1.41738, "ax":-4.36512, "ay":2.4295, "alpha":-1.21373, "fx":[-76.34247,-75.09107,-69.82216,-69.80231], "fy":[41.63393,36.62709,38.93114,44.802]}, - {"t":0.35063, "x":6.84648, "y":5.24774, "heading":-2.77937, "vx":-1.46819, "vy":0.95458, "omega":1.37869, "ax":-4.44386, "ay":2.28042, "alpha":-1.64668, "fx":[-78.71517,-76.90182,-69.32418,-71.36673], "fy":[39.98918,32.34419,36.12265,43.59799]}, - {"t":0.3825, "x":6.79742, "y":5.27933, "heading":-2.73543, "vx":-1.60984, "vy":1.02727, "omega":1.3262, "ax":-4.55497, "ay":2.04642, "alpha":-1.86191, "fx":[-81.20635,-78.7198,-70.6626,-73.12757], "fy":[36.22308,27.56037,31.96544,40.70251]}, - {"t":0.41438, "x":6.7438, "y":5.31311, "heading":-2.69315, "vx":-1.75503, "vy":1.0925, "omega":1.26685, "ax":-4.70588, "ay":1.6643, "alpha":-1.93676, "fx":[-84.17147,-81.08277,-72.60198,-75.92276], "fy":[29.61214,21.0902,25.66839,34.60193]}, - {"t":0.44625, "x":6.68546, "y":5.34878, "heading":-2.65277, "vx":-1.90503, "vy":1.14555, "omega":1.20512, "ax":-4.90712, "ay":0.89369, "alpha":-1.7778, "fx":[-87.02781,-83.82125,-76.83171,-79.51686], "fy":[16.90124,8.41202,12.96854,21.30762]}, - {"t":0.47813, "x":6.62225, "y":5.38575, "heading":-2.61436, "vx":-2.06145, "vy":1.17404, "omega":1.14845, "ax":-4.88588, "ay":-0.96661, "alpha":-1.29954, "fx":[-85.53254,-82.38364,-77.3009,-80.56426], "fy":[-14.70875,-20.56982,-17.63444,-11.5385]}, - {"t":0.51, "x":6.55406, "y":5.42268, "heading":-2.57775, "vx":-2.21719, "vy":1.14323, "omega":1.10703, "ax":-2.59397, "ay":-4.24486, "alpha":-0.44097, "fx":[-44.72744,-43.33204,-41.40991,-43.49155], "fy":[-70.89139,-72.01052,-71.03859,-69.09836]}, - {"t":0.54188, "x":6.48207, "y":5.45697, "heading":-2.54246, "vx":-2.29987, "vy":1.00792, "omega":1.09297, "ax":0.79394, "ay":-4.91937, "alpha":0.36382, "fx":[14.6747,13.58372,11.77445,12.90528], "fy":[-82.11103,-81.07253,-81.75314,-83.07712]}, - {"t":0.57375, "x":6.40916, "y":5.4866, "heading":-2.50763, "vx":-2.27456, "vy":0.85112, "omega":1.10457, "ax":2.15156, "ay":-4.5013, "alpha":0.9186, "fx":[39.32765,36.57657,32.5579,34.99992], "fy":[-74.54092,-72.3317,-75.06343,-78.20214]}, - {"t":0.60563, "x":6.33775, "y":5.51144, "heading":-2.47242, "vx":-2.20598, "vy":0.70764, "omega":1.13385, "ax":2.72374, "ay":-4.18366, "alpha":1.19662, "fx":[49.69645,46.42901,41.03441,44.4539], "fy":[-69.78953,-65.86075,-69.74849,-73.55979]}, - {"t":0.6375, "x":6.26882, "y":5.53187, "heading":-2.43628, "vx":-2.11916, "vy":0.57428, "omega":1.17199, "ax":3.02725, "ay":-3.97181, "alpha":1.16472, "fx":[54.62298,51.19782,46.39858,49.63212], "fy":[-65.74744,-62.43123,-66.42176,-70.23193]}, - {"t":0.66938, "x":6.20281, "y":5.54816, "heading":-2.39892, "vx":-2.02267, "vy":0.44768, "omega":1.20912, "ax":3.20296, "ay":-3.833, "alpha":1.06451, "fx":[56.88457,54.15993,49.72114,52.80136], "fy":[-63.92578,-59.88472,-64.40471,-67.36173]}, - {"t":0.70125, "x":6.13996, "y":5.56048, "heading":-2.36038, "vx":-1.92057, "vy":0.3255, "omega":1.24305, "ax":3.32478, "ay":-3.72886, "alpha":0.60888, "fx":[57.81006,55.72212,53.55905,54.599], "fy":[-61.19988,-60.39602,-62.50369,-64.53376]}, - {"t":0.73313, "x":6.08043, "y":5.56896, "heading":-2.32076, "vx":-1.81459, "vy":0.20664, "omega":1.26246, "ax":3.40787, "ay":-3.65386, "alpha":0.14477, "fx":[57.02823,56.9816,56.41034,56.8102], "fy":[-61.25668,-59.91879,-61.15058,-61.30633]}, - {"t":0.765, "x":6.02432, "y":5.57369, "heading":-2.28051, "vx":-1.70597, "vy":0.09018, "omega":1.26707, "ax":3.47106, "ay":-3.59449, "alpha":-0.60572, "fx":[55.92984,57.79511,60.37447,57.344], "fy":[-59.5819,-62.26017,-59.45114,-58.3806]}, - {"t":0.79688, "x":5.97171, "y":5.57474, "heading":-2.24013, "vx":-1.59533, "vy":-0.0244, "omega":1.24776, "ax":3.51909, "ay":-3.54796, "alpha":-1.44538, "fx":[53.31922,58.66775,63.77133,58.88784], "fy":[-60.86964,-63.49806,-57.98934,-54.21405]}, - {"t":0.82875, "x":5.92264, "y":5.57216, "heading":-2.20035, "vx":-1.48315, "vy":-0.13749, "omega":1.20169, "ax":3.55775, "ay":-3.50958, "alpha":-2.51861, "fx":[50.71353,59.47273,68.21029,58.82758], "fy":[-60.67482,-67.03363,-56.06994,-50.23349]}, - {"t":0.86063, "x":5.87718, "y":5.56599, "heading":-2.16205, "vx":-1.36975, "vy":-0.24936, "omega":1.12141, "ax":3.58892, "ay":-3.47803, "alpha":-3.77529, "fx":[46.52059,60.50487,72.75158,59.52498], "fy":[-62.78396,-70.06865,-53.80097,-45.25461]}, - {"t":0.89251, "x":5.83534, "y":5.55628, "heading":-2.1263, "vx":-1.25535, "vy":-0.36022, "omega":1.00107, "ax":3.61494, "ay":-3.45124, "alpha":-5.17339, "fx":[42.18668,62.19611,78.13816,58.51632], "fy":[-64.52394,-73.76342,-51.37574,-40.45923]}, - {"t":0.92438, "x":5.79716, "y":5.54304, "heading":-2.0944, "vx":-1.14013, "vy":-0.47023, "omega":0.83617, "ax":0.77937, "ay":-0.62074, "alpha":-5.87121, "fx":[-6.26542,17.68621,32.47509,8.07069], "fy":[-15.84369,-30.00584,-5.06322,9.52307]}, - {"t":0.95975, "x":5.75732, "y":5.52602, "heading":-2.06482, "vx":-1.11256, "vy":-0.49219, "omega":0.6285, "ax":0.87423, "ay":-0.48184, "alpha":-4.60238, "fx":[-0.79559,19.10995,29.86735,10.11043], "fy":[-12.66818,-22.99902,-3.56037,7.09953]}, - {"t":0.99512, "x":5.71851, "y":5.50831, "heading":-2.04259, "vx":-1.08164, "vy":-0.50923, "omega":0.46571, "ax":0.93865, "ay":-0.33994, "alpha":-3.60846, "fx":[3.98912,19.0585,27.48211,12.05775], "fy":[-9.59234,-17.61648,-1.81413,6.35634]}, - {"t":1.03049, "x":5.68084, "y":5.49009, "heading":-2.02612, "vx":-1.04844, "vy":-0.52125, "omega":0.33807, "ax":0.9767, "ay":-0.20716, "alpha":-2.84151, "fx":[6.98436,19.41919,25.53932,13.18139], "fy":[-6.6027,-12.71073,-0.32344,5.8239]}, - {"t":1.06586, "x":5.64437, "y":5.47152, "heading":-2.01416, "vx":-1.01389, "vy":-0.52858, "omega":0.23757, "ax":0.99452, "ay":-0.08956, "alpha":-2.22251, "fx":[9.46612,18.80345,23.86869,14.17447], "fy":[-4.08087,-8.78864,1.07155,5.82601]}, - {"t":1.10124, "x":5.60913, "y":5.45277, "heading":-2.00575, "vx":-0.97871, "vy":-0.53175, "omega":0.15895, "ax":0.9986, "ay":0.01112, "alpha":-1.76267, "fx":[10.98402,18.69656,22.29085,14.61346], "fy":[-1.8454,-5.61519,2.26509,5.93681]}, - {"t":1.13661, "x":5.57514, "y":5.43397, "heading":-2.00013, "vx":-0.94339, "vy":-0.53136, "omega":0.09661, "ax":0.99417, "ay":0.09585, "alpha":-1.37319, "fx":[12.19272,17.95082,21.11888,15.02663], "fy":[-0.04613,-2.87309,3.23374,6.07634]}, - {"t":1.17198, "x":5.54239, "y":5.41523, "heading":-1.99672, "vx":-0.90823, "vy":-0.52797, "omega":0.04803, "ax":0.98487, "ay":0.16668, "alpha":-1.09922, "fx":[12.94832,17.71729,19.87855,15.12511], "fy":[1.50151,-0.90145,4.13104,6.38267]}, - {"t":1.20735, "x":5.51088, "y":5.39666, "heading":-1.99502, "vx":-0.87339, "vy":-0.52207, "omega":0.00915, "ax":0.97308, "ay":0.22592, "alpha":-0.85098, "fx":[13.50573,17.03044,19.09299,15.25411], "fy":[2.7423,1.01698,4.78658,6.51805]}, - {"t":1.24272, "x":5.4806, "y":5.37834, "heading":-1.99469, "vx":-0.83897, "vy":-0.51408, "omega":-0.02095, "ax":0.96026, "ay":0.27568, "alpha":-0.68912, "fx":[13.87254,16.81711,18.13789,15.20042], "fy":[3.80276,2.23392,5.46799,6.87684]}, - {"t":1.27809, "x":5.45152, "y":5.36033, "heading":-1.99543, "vx":-0.80501, "vy":-0.50433, "omega":-0.04532, "ax":0.94724, "ay":0.31773, "alpha":-0.52862, "fx":[14.10024,16.23163,17.62873,15.1998], "fy":[4.66655,3.60188,5.92477,6.99273]}, - {"t":1.31346, "x":5.42364, "y":5.34269, "heading":-1.99704, "vx":-0.7715, "vy":-0.49309, "omega":-0.06402, "ax":0.93453, "ay":0.35355, "alpha":-0.43526, "fx":[14.26131,16.07773,16.89334,15.08053], "fy":[5.40916,4.35057,6.45944,7.35467]}, - {"t":1.34883, "x":5.39694, "y":5.32547, "heading":-1.9993, "vx":-0.73845, "vy":-0.48059, "omega":-0.07941, "ax":0.92238, "ay":0.38428, "alpha":-0.32864, "fx":[14.32448,15.58283,16.5693,15.02603], "fy":[6.02124,5.35977,6.78895,7.4529]}, - {"t":1.3842, "x":5.37139, "y":5.30871, "heading":-2.00211, "vx":-0.70582, "vy":-0.46699, "omega":-0.09104, "ax":0.91091, "ay":0.41085, "alpha":-0.27723, "fx":[14.37126,15.49008,15.99654,14.87995], "fy":[6.55954,5.81895,7.22041,7.79547]}, - {"t":1.41957, "x":5.347, "y":5.29245, "heading":-2.00533, "vx":-0.6736, "vy":-0.45246, "omega":-0.10084, "ax":0.90017, "ay":0.43398, "alpha":-0.20352, "fx":[14.35851,15.06088,15.79196,14.80994], "fy":[7.00146,6.58976,7.46598,7.88001]}, - {"t":1.45495, "x":5.32374, "y":5.27671, "heading":-2.0089, "vx":-0.64176, "vy":-0.43711, "omega":-0.10804, "ax":0.89014, "ay":0.45428, "alpha":-0.17778, "fx":[14.33809,15.02346,15.33748,14.65386], "fy":[7.40789,6.86965,7.82108,8.19157]}, - {"t":1.49032, "x":5.30159, "y":5.26154, "heading":-2.01272, "vx":-0.61028, "vy":-0.42104, "omega":-0.11433, "ax":0.88081, "ay":0.47218, "alpha":-0.1241, "fx":[14.29639,14.64323,15.20534,14.58582], "fy":[7.73211,7.47811,8.00891,8.26523]}, - {"t":1.52569, "x":5.28056, "y":5.24694, "heading":-2.01676, "vx":-0.57912, "vy":-0.40434, "omega":-0.11872, "ax":0.87213, "ay":0.48809, "alpha":-0.11353, "fx":[14.23665,14.64775,14.8387,14.42918], "fy":[8.05208,7.64898,8.30446,8.53922]}, - {"t":1.56106, "x":5.26062, "y":5.23294, "heading":-2.02096, "vx":-0.54827, "vy":-0.38708, "omega":-0.12274, "ax":0.86407, "ay":0.50229, "alpha":-0.07179, "fx":[14.1922,14.30407,14.74689,14.3712], "fy":[8.29338,8.14296,8.45124,8.60401]}, - {"t":1.59643, "x":5.24177, "y":5.21957, "heading":-2.0253, "vx":-0.51771, "vy":-0.36931, "omega":-0.12528, "ax":0.85656, "ay":0.51503, "alpha":-0.06943, "fx":[14.11018,14.33864,14.44599,14.21906], "fy":[8.55562,8.25111,8.69822,8.83652]}, - {"t":1.6318, "x":5.22399, "y":5.20682, "heading":-2.02973, "vx":-0.48741, "vy":-0.35109, "omega":-0.12773, "ax":0.84957, "ay":0.52653, "alpha":-0.03442, "fx":[14.07798,14.02361,14.37191,14.17428], "fy":[8.73785,8.66181,8.81496,8.89336]}, - {"t":1.66717, "x":5.20728, "y":5.19474, "heading":-2.03425, "vx":-0.45736, "vy":-0.33247, "omega":-0.12895, "ax":0.84305, "ay":0.53694, "alpha":-0.03552, "fx":[13.98563,14.07743,14.12018,14.02993], "fy":[8.96165,8.73683,9.02157,9.08234]}, - {"t":1.70254, "x":5.19163, "y":5.18331, "heading":-2.03881, "vx":-0.42754, "vy":-0.31348, "omega":-0.13021, "ax":0.83697, "ay":0.54642, "alpha":-0.00321, "fx":[13.97623,13.78273,14.0482,14.00026], "fy":[9.10108,9.08834,9.11479,9.12988]}, - {"t":1.73791, "x":5.17703, "y":5.17257, "heading":-2.04342, "vx":-0.39794, "vy":-0.29415, "omega":-0.13032, "ax":0.83128, "ay":0.55507, "alpha":-0.00397, "fx":[13.88241,13.84958,13.83081,13.8652], "fy":[9.29872,9.15405,9.28596,9.27222]}, - {"t":1.77328, "x":5.16348, "y":5.16251, "heading":-2.04803, "vx":-0.36853, "vy":-0.27452, "omega":-0.13046, "ax":0.82595, "ay":0.563, "alpha":0.02881, "fx":[13.90374,13.5704,13.74747,13.85109], "fy":[9.40832,9.4633,9.36033,9.30766]}, - {"t":1.80866, "x":5.15096, "y":5.15315, "heading":-2.05264, "vx":-0.33932, "vy":-0.2546, "omega":-0.12944, "ax":0.82095, "ay":0.57029, "alpha":0.03177, "fx":[13.81825,13.64308,13.55067,13.72746], "fy":[9.58948,9.53636,9.50011,9.39977]}, - {"t":1.84403, "x":5.13947, "y":5.1445, "heading":-2.05722, "vx":-0.31028, "vy":-0.23443, "omega":-0.12832, "ax":0.81626, "ay":0.57701, "alpha":0.06861, "fx":[13.87904,13.37415,13.44344,13.72983], "fy":[9.67884,9.82222,9.55706,9.41595]}, - {"t":1.8794, "x":5.12901, "y":5.13657, "heading":-2.06176, "vx":-0.28141, "vy":-0.21402, "omega":-0.12589, "ax":0.81184, "ay":0.58323, "alpha":0.07917, "fx":[13.81387,13.44684,13.25127,13.62003], "fy":[9.85083,9.91977,9.6673,9.45093]}, - {"t":1.91477, "x":5.11956, "y":5.12937, "heading":-2.06621, "vx":-0.25269, "vy":-0.19339, "omega":-0.12309, "ax":0.80768, "ay":0.589, "alpha":0.12439, "fx":[13.92536,13.18319,13.10569,13.64037], "fy":[9.92905,10.20182,9.7066,9.43607]}, - {"t":1.95014, "x":5.11113, "y":5.12289, "heading":-2.07057, "vx":-0.22413, "vy":-0.17256, "omega":-0.11869, "ax":0.80376, "ay":0.59437, "alpha":0.14767, "fx":[13.89785,13.24921,12.89758,13.54824], "fy":[10.0986,10.34301,9.78731,9.40237]}, - {"t":1.98551, "x":5.1037, "y":5.11716, "heading":-2.07477, "vx":-0.1957, "vy":-0.15153, "omega":-0.11347, "ax":0.80005, "ay":0.59937, "alpha":0.20732, "fx":[14.07635,12.98533,12.69454,13.58939], "fy":[10.17509,10.64665,9.80619,9.3368]}, - {"t":2.02088, "x":5.09728, "y":5.11218, "heading":-2.07878, "vx":-0.1674, "vy":-0.13033, "omega":-0.10613, "ax":0.79654, "ay":0.60404, "alpha":0.25058, "fx":[14.11295,13.03598,12.44153,13.52122], "fy":[10.35004,10.85476,9.85547,9.21598]}, - {"t":2.05625, "x":5.09186, "y":5.10794, "heading":-2.08253, "vx":-0.13922, "vy":-0.10897, "omega":-0.09727, "ax":0.79322, "ay":0.60841, "alpha":0.3338, "fx":[14.38356,12.76403,12.15388,13.58861], "fy":[10.43559,11.2174,9.84735,9.06757]}, - {"t":2.09162, "x":5.08743, "y":5.10447, "heading":-2.08597, "vx":-0.11117, "vy":-0.08745, "omega":-0.08546, "ax":0.79006, "ay":0.61252, "alpha":0.40817, "fx":[14.52494,12.78769,11.8128,13.55446], "fy":[10.62566,11.52714,9.85942,8.82928]}, - {"t":2.12699, "x":5.08399, "y":5.10176, "heading":-2.089, "vx":-0.08322, "vy":-0.06578, "omega":-0.07103, "ax":0.78707, "ay":0.61637, "alpha":0.5288, "fx":[14.92696,12.49567,11.39972,13.65793], "fy":[10.7355,12.00248,9.81297,8.54767]}, - {"t":2.16237, "x":5.08154, "y":5.09982, "heading":-2.09151, "vx":-0.05538, "vy":-0.04398, "omega":-0.05232, "ax":0.78422, "ay":0.62, "alpha":0.65147, "fx":[15.23621,12.47502,10.90479,13.67449], "fy":[10.95536,12.46564,9.77641,8.14329]}, - {"t":2.19774, "x":5.08007, "y":5.09865, "heading":-2.09336, "vx":-0.02764, "vy":-0.02205, "omega":-0.02928, "ax":0.78151, "ay":0.62343, "alpha":0.82778, "fx":[15.83191,12.31196,10.30288,13.66312], "fy":[11.03432,13.1995,9.69064,7.64455]}, - {"t":2.23311, "x":5.07958, "y":5.09826, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.1, "y":5.07, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.16085, "ay":2.76674, "alpha":10.4878, "fx":[-48.05099,-42.60453,-89.20785,-97.57417], "fy":[20.68796,78.03689,68.30258,17.45358]}, + {"t":0.03174, "x":7.0979, "y":5.07139, "heading":3.14159, "vx":-0.13207, "vy":0.08782, "omega":0.33288, "ax":-4.17205, "ay":2.75257, "alpha":8.5305, "fx":[-52.36607,-47.56022,-85.82869,-92.42953], "fy":[25.65221,71.68791,64.0103,22.18586]}, + {"t":0.06348, "x":7.09161, "y":5.07557, "heading":-3.13103, "vx":-0.26449, "vy":0.17518, "omega":0.60364, "ax":-4.18314, "ay":2.73547, "alpha":6.878, "fx":[-55.82949,-52.26817,-82.90831,-87.91794], "fy":[29.30276,66.46412,60.36912,26.25969]}, + {"t":0.09522, "x":7.08111, "y":5.08251, "heading":-3.11187, "vx":-0.39726, "vy":0.26201, "omega":0.82195, "ax":-4.19587, "ay":2.71565, "alpha":5.32283, "fx":[-58.83799,-56.52106,-80.55495,-83.85826], "fy":[33.41277,61.26293,56.58056,29.81814]}, + {"t":0.12696, "x":7.06639, "y":5.09219, "heading":-3.08578, "vx":-0.53043, "vy":0.3482, "omega":0.99089, "ax":-4.21052, "ay":2.69256, "alpha":3.9756, "fx":[-61.84322,-60.44655,-78.62494,-79.83498], "fy":[35.82787,57.26251,53.09389,33.35055]}, + {"t":0.1587, "x":7.04743, "y":5.1046, "heading":-3.05433, "vx":-0.66408, "vy":0.43366, "omega":1.11708, "ax":-4.22746, "ay":2.66553, "alpha":2.79787, "fx":[-64.28617,-63.77215,-76.42496,-77.39546], "fy":[38.69916,52.95219,50.21265,35.86862]}, + {"t":0.19044, "x":7.02422, "y":5.1197, "heading":-3.01887, "vx":-0.79826, "vy":0.51827, "omega":1.20588, "ax":-4.24795, "ay":2.63231, "alpha":1.76634, "fx":[-67.1068,-66.80438,-74.97008,-74.36371], "fy":[39.79449,49.75836,47.38555,38.5793]}, + {"t":0.22218, "x":6.99675, "y":5.13748, "heading":-2.9806, "vx":-0.93309, "vy":0.60182, "omega":1.26195, "ax":-4.2718, "ay":2.59289, "alpha":0.8405, "fx":[-69.19752,-69.36369,-73.10437,-73.16951], "fy":[41.73676,45.67683,45.04033,40.435]}, + {"t":0.25392, "x":6.96498, "y":5.15789, "heading":-2.94055, "vx":-1.06867, "vy":0.68411, "omega":1.28862, "ax":-4.30212, "ay":2.54156, "alpha":0.08898, "fx":[-71.93703,-71.6415,-72.22651,-71.05228], "fy":[41.35906,43.03314,42.71442,42.35973]}, + {"t":0.28566, "x":6.92889, "y":5.18088, "heading":-2.89965, "vx":-1.20522, "vy":0.76478, "omega":1.29145, "ax":-4.33804, "ay":2.47885, "alpha":-0.5827, "fx":[-73.77541,-73.53676,-70.75476,-71.18519], "fy":[42.29045,39.15362,40.70703,43.13409]}, + {"t":0.3174, "x":6.88845, "y":5.2064, "heading":-2.85865, "vx":-1.34291, "vy":0.84346, "omega":1.27295, "ax":-4.38624, "ay":2.39129, "alpha":-1.07791, "fx":[-76.23638,-75.28099,-70.62223,-70.32638], "fy":[40.94301,36.47826,38.35986,43.66558]}, + {"t":0.34914, "x":6.84362, "y":5.23438, "heading":-2.81825, "vx":-1.48213, "vy":0.91936, "omega":1.23874, "ax":-4.44789, "ay":2.27277, "alpha":-1.47013, "fx":[-78.14699,-76.8182,-70.0419,-71.56948], "fy":[39.84767,32.83014,36.03074,42.83516]}, + {"t":0.38088, "x":6.79434, "y":5.26471, "heading":-2.77893, "vx":-1.6233, "vy":0.9915, "omega":1.19208, "ax":-4.53826, "ay":2.0836, "alpha":-1.68786, "fx":[-80.33189,-78.41597,-71.06907,-72.78534], "fy":[36.86139,28.87907,32.55005,40.6401]}, + {"t":0.41262, "x":6.74053, "y":5.29723, "heading":-2.7411, "vx":-1.76735, "vy":1.05763, "omega":1.13851, "ax":-4.65789, "ay":1.79503, "alpha":-1.76559, "fx":[-82.72585,-80.32324,-72.47028,-75.05971], "fy":[31.88266,23.92177,27.77286,36.1116]}, + {"t":0.44436, "x":6.68209, "y":5.3317, "heading":-2.70496, "vx":-1.91519, "vy":1.11461, "omega":1.08247, "ax":-4.84234, "ay":1.19805, "alpha":-1.67019, "fx":[-85.50167,-82.95275,-76.23457,-78.18897], "fy":[22.13758,13.94808,17.90547,25.89224]}, + {"t":0.4761, "x":6.61886, "y":5.36768, "heading":-2.6706, "vx":-2.06889, "vy":1.15263, "omega":1.02945, "ax":-4.97547, "ay":-0.23479, "alpha":-1.26104, "fx":[-86.75644,-84.15863,-79.07656,-81.76331], "fy":[-2.41179,-8.25122,-5.53423,0.54197]}, + {"t":0.50784, "x":6.55069, "y":5.40415, "heading":-2.63793, "vx":-2.22681, "vy":1.14518, "omega":0.98943, "ax":-3.25906, "ay":-3.75203, "alpha":-0.48924, "fx":[-55.82359,-54.4005,-52.19375,-54.8899], "fy":[-62.61268,-63.99229,-62.99747,-60.57552]}, + {"t":0.53958, "x":6.47837, "y":5.4386, "heading":-2.60652, "vx":-2.33025, "vy":1.02609, "omega":0.9739, "ax":0.82226, "ay":-4.91094, "alpha":0.26049, "fx":[14.85486,14.04784,12.55285,13.37106], "fy":[-81.88709,-81.47924,-81.48158,-82.60441]}, + {"t":0.57132, "x":6.40482, "y":5.4687, "heading":-2.57561, "vx":-2.30415, "vy":0.87022, "omega":0.98217, "ax":2.38463, "ay":-4.38058, "alpha":0.84956, "fx":[42.86392,40.62779,36.76885,38.74191], "fy":[-72.9006,-70.45248,-72.82481,-75.91089]}, + {"t":0.60306, "x":6.33289, "y":5.49411, "heading":-2.54444, "vx":-2.22846, "vy":0.73118, "omega":1.00913, "ax":2.96377, "ay":-4.01624, "alpha":1.13666, "fx":[53.30002,50.7103,45.414,48.19431], "fy":[-67.2644,-63.15364,-66.74732,-70.62992]}, + {"t":0.6348, "x":6.26365, "y":5.5153, "heading":-2.51241, "vx":-2.13439, "vy":0.6037, "omega":1.04521, "ax":3.25142, "ay":-3.78987, "alpha":1.11544, "fx":[58.039,55.19407,50.37283,53.19249], "fy":[-63.09925,-59.47211,-63.08593,-67.0442]}, + {"t":0.66654, "x":6.19754, "y":5.53255, "heading":-2.47923, "vx":-2.03119, "vy":0.48341, "omega":1.08061, "ax":3.41411, "ay":-3.64575, "alpha":1.03061, "fx":[60.2255,57.92836,53.43971,56.05289], "fy":[-61.001,-56.90758,-60.98891,-64.19406]}, + {"t":0.69828, "x":6.13479, "y":5.54606, "heading":-2.44494, "vx":-1.92283, "vy":0.3677, "omega":1.11333, "ax":3.52603, "ay":-3.53884, "alpha":0.63893, "fx":[61.24319,59.25831,56.83235,57.77505], "fy":[-58.0612,-57.13946,-59.22567,-61.53693]}, + {"t":0.73002, "x":6.07553, "y":5.55594, "heading":-2.4096, "vx":-1.81091, "vy":0.25537, "omega":1.13361, "ax":3.5979, "ay":-3.46664, "alpha":0.21411, "fx":[60.47548,60.20648,59.32405,59.89514], "fy":[-58.06671,-56.64407,-58.01155,-58.42676]}, + {"t":0.76176, "x":6.01987, "y":5.5623, "heading":-2.37362, "vx":-1.69672, "vy":0.14534, "omega":1.1404, "ax":3.65391, "ay":-3.40824, "alpha":-0.45332, "fx":[59.56265,60.72415,62.74722,60.60171], "fy":[-56.25546,-58.63341,-56.78701,-55.57924]}, + {"t":0.7935, "x":5.96786, "y":5.5652, "heading":-2.33742, "vx":-1.58074, "vy":0.03717, "omega":1.12601, "ax":3.69497, "ay":-3.36423, "alpha":-1.1891, "fx":[57.23828,61.19047,65.77993,62.16449], "fy":[-57.059,-59.81143,-55.46941,-51.98073]}, + {"t":0.82524, "x":5.91954, "y":5.56469, "heading":-2.30168, "vx":-1.46346, "vy":-0.06961, "omega":1.08827, "ax":3.72854, "ay":-3.32742, "alpha":-2.16394, "fx":[54.93798,61.49334,69.6502,62.53052], "fy":[-56.47039,-63.17674,-54.11238,-48.10642]}, + {"t":0.85698, "x":5.87497, "y":5.5608, "heading":-2.26714, "vx":-1.34512, "vy":-0.17523, "omega":1.01959, "ax":3.75457, "ay":-3.29838, "alpha":-3.24774, "fx":[51.12498,62.12332,73.57825,63.52126], "fy":[-58.10693,-65.67532,-52.56761,-43.57975]}, + {"t":0.88872, "x":5.83417, "y":5.55358, "heading":-2.23478, "vx":-1.22595, "vy":-0.27992, "omega":0.9165, "ax":3.77631, "ay":-3.27378, "alpha":-4.52838, "fx":[47.26537,62.92981,78.38172,63.21986], "fy":[-58.9617,-69.69422,-50.69509,-38.93849]}, + {"t":0.92046, "x":5.79716, "y":5.54304, "heading":-2.20569, "vx":-1.10609, "vy":-0.38383, "omega":0.77277, "ax":0.80598, "ay":-0.58505, "alpha":-4.84811, "fx":[-2.80682,15.42347,29.9064,11.21844], "fy":[-12.42224,-26.39532,-7.20602,7.01353]}, + {"t":0.95278, "x":5.76183, "y":5.53033, "heading":-2.18071, "vx":-1.08004, "vy":-0.40274, "omega":0.61606, "ax":0.88993, "ay":-0.45182, "alpha":-3.87545, "fx":[1.53823,17.15236,28.05461,12.59381], "fy":[-9.94229,-20.50133,-5.24442,5.5617]}, + {"t":0.98511, "x":5.72738, "y":5.51708, "heading":-2.1608, "vx":-1.05127, "vy":-0.41734, "omega":0.49079, "ax":0.94488, "ay":-0.32181, "alpha":-3.10527, "fx":[5.43234,17.44694,26.24111,13.88242], "fy":[-7.49902,-15.97175,-3.2723,5.28518]}, + {"t":1.01743, "x":5.69389, "y":5.50342, "heading":-2.14493, "vx":-1.02073, "vy":-0.42774, "omega":0.39042, "ax":0.97729, "ay":-0.20382, "alpha":-2.49295, "fx":[7.86514,18.07072,24.67947,14.54831], "fy":[-5.18586,-11.78743,-1.61817,5.00123]}, + {"t":1.04975, "x":5.66141, "y":5.48949, "heading":-2.13231, "vx":-0.98914, "vy":-0.43433, "omega":0.30984, "ax":0.99334, "ay":-0.10087, "alpha":-1.9905, "fx":[9.9962,17.73898,23.30754,15.19138], "fy":[-3.21545,-8.44274,-0.16064,5.09304]}, + {"t":1.08208, "x":5.62996, "y":5.47539, "heading":-2.1223, "vx":-0.95703, "vy":-0.43759, "omega":0.2455, "ax":0.99849, "ay":-0.0131, "alpha":-1.60657, "fx":[11.29217,17.90819,21.97965,15.39745], "fy":[-1.46028,-5.67079,1.06368,5.19367]}, + {"t":1.1144, "x":5.59954, "y":5.46124, "heading":-2.11436, "vx":-0.92475, "vy":-0.43802, "omega":0.19357, "ax":0.99683, "ay":0.06104, "alpha":-1.27728, "fx":[12.41917,17.40273,20.97736,15.66717], "fy":[-0.03186,-3.29043,2.06236,5.32976]}, + {"t":1.14673, "x":5.57017, "y":5.44712, "heading":-2.10811, "vx":-0.89253, "vy":-0.43604, "omega":0.15228, "ax":0.99114, "ay":0.12347, "alpha":-1.03689, "fx":[13.11788,17.3822,19.91806,15.66884], "fy":[1.22503,-1.50309,2.9499,5.56102]}, + {"t":1.17905, "x":5.54184, "y":5.43309, "heading":-2.10318, "vx":-0.8605, "vy":-0.43205, "omega":0.11876, "ax":0.98322, "ay":0.17622, "alpha":-0.81912, "fx":[13.7045,16.86656,19.22769,15.7606], "fy":[2.23971,0.19544,3.63334,5.68121]}, + {"t":1.21137, "x":5.51454, "y":5.41921, "heading":-2.09934, "vx":-0.82871, "vy":-0.42636, "omega":0.09229, "ax":0.97422, "ay":0.22098, "alpha":-0.66967, "fx":[14.0766,16.81071,18.39959,15.67238], "fy":[3.13636,1.34634,4.29269,5.95886]}, + {"t":1.2437, "x":5.48826, "y":5.40555, "heading":-2.09636, "vx":-0.79722, "vy":-0.41921, "omega":0.07064, "ax":0.96482, "ay":0.25922, "alpha":-0.52438, "fx":[14.3671,16.34553,17.94013,15.67987], "fy":[3.86596,2.58014,4.77525,6.06322]}, + {"t":1.27602, "x":5.463, "y":5.39213, "heading":-2.09408, "vx":-0.76604, "vy":-0.41084, "omega":0.05369, "ax":0.95544, "ay":0.29212, "alpha":-0.4333, "fx":[14.55589,16.29879,17.29598,15.55649], "fy":[4.51934,3.31915,5.28517,6.35467]}, + {"t":1.30835, "x":5.43873, "y":5.379, "heading":-2.09234, "vx":-0.73515, "vy":-0.40139, "omega":0.03968, "ax":0.94632, "ay":0.32064, "alpha":-0.33412, "fx":[14.68784,15.87909,17.00125,15.53038], "fy":[5.05224,4.24416,5.63654,6.4465]}, + {"t":1.34067, "x":5.41546, "y":5.3662, "heading":-2.09106, "vx":-0.70456, "vy":-0.39103, "omega":0.02888, "ax":0.93757, "ay":0.34551, "alpha":-0.28069, "fx":[14.76432,15.8679,16.49254,15.39093], "fy":[5.54134,4.71995,6.04402,6.73257]}, + {"t":1.37299, "x":5.39318, "y":5.35374, "heading":-2.09013, "vx":-0.67426, "vy":-0.37986, "omega":0.01981, "ax":0.92928, "ay":0.36736, "alpha":-0.21143, "fx":[14.81108,15.49131,16.30731,15.35292], "fy":[5.93797,5.43218,6.30841,6.81613]}, + {"t":1.40532, "x":5.37187, "y":5.34165, "heading":-2.08949, "vx":-0.64422, "vy":-0.36799, "omega":0.01298, "ax":0.92146, "ay":0.38666, "alpha":-0.18245, "fx":[14.81874,15.51185,15.90106,15.20939], "fy":[6.3164,5.73547,6.64352,7.08667]}, + {"t":1.43764, "x":5.35153, "y":5.32996, "heading":-2.08907, "vx":-0.61444, "vy":-0.35549, "omega":0.00708, "ax":0.91411, "ay":0.40383, "alpha":-0.13232, "fx":[14.82249,15.17181,15.7862,15.1703], "fy":[6.61527,6.301,6.84701,7.16331]}, + {"t":1.46997, "x":5.33214, "y":5.31868, "heading":-2.08884, "vx":-0.58489, "vy":-0.34243, "omega":0.0028, "ax":0.90721, "ay":0.41917, "alpha":-0.11871, "fx":[14.78674,15.21768,15.45812,15.02842], "fy":[6.9163,6.49589,7.12682,7.41057]}, + {"t":1.50229, "x":5.31371, "y":5.30783, "heading":-2.08875, "vx":-0.55556, "vy":-0.32888, "omega":-0.00104, "ax":0.90075, "ay":0.43296, "alpha":-0.08123, "fx":[14.77264,14.90633,15.38702,14.99402], "fy":[7.14524,6.95227,7.28802,7.48311]}, + {"t":1.53461, "x":5.29623, "y":5.29742, "heading":-2.08878, "vx":-0.52645, "vy":-0.31489, "omega":-0.00366, "ax":0.89469, "ay":0.4454, "alpha":-0.07751, "fx":[14.70853,14.9726,15.11898,14.85611], "fy":[7.39251,7.07601,7.52535,7.70432]}, + {"t":1.56694, "x":5.27968, "y":5.28748, "heading":-2.0889, "vx":-0.49753, "vy":-0.30049, "omega":-0.00617, "ax":0.88901, "ay":0.45668, "alpha":-0.04806, "fx":[14.69113,14.6846,15.07322,14.8288], "fy":[7.56899,7.453,7.65507,7.77323]}, + {"t":1.59926, "x":5.26406, "y":5.278, "heading":-2.0891, "vx":-0.46879, "vy":-0.28573, "omega":-0.00772, "ax":0.88369, "ay":0.46694, "alpha":-0.05036, "fx":[14.60833,14.76586,14.85246,14.69615], "fy":[7.7777,7.53253,7.85803,7.96665]}, + {"t":1.63158, "x":5.24937, "y":5.26901, "heading":-2.08935, "vx":-0.44023, "vy":-0.27064, "omega":-0.00935, "ax":0.87869, "ay":0.47633, "alpha":-0.0261, "fx":[14.59602,14.49772,14.81926,14.67659], "fy":[7.91474,7.84826,7.96441,8.03307]}, + {"t":1.66391, "x":5.2356, "y":5.26051, "heading":-2.08965, "vx":-0.41182, "vy":-0.25524, "omega":-0.01019, "ax":0.874, "ay":0.48493, "alpha":-0.0318, "fx":[14.50107,14.58891,14.63655,14.54995], "fy":[8.09522,7.90205,8.13873,8.19811]}, + {"t":1.69623, "x":5.22274, "y":5.25251, "heading":-2.08998, "vx":-0.38357, "vy":-0.23957, "omega":-0.01122, "ax":0.86958, "ay":0.49284, "alpha":-0.01078, "fx":[14.49928,14.33654,14.60757,14.53855], "fy":[8.20225,8.16911,8.22757,8.26291]}, + {"t":1.72856, "x":5.2108, "y":5.24503, "heading":-2.09034, "vx":-0.35546, "vy":-0.22364, "omega":-0.01157, "ax":0.86542, "ay":0.50015, "alpha":-0.01815, "fx":[14.39624,14.4351,14.4554,14.41779], "fy":[8.36204,8.209,8.37783,8.39997]}, + {"t":1.76088, "x":5.19976, "y":5.23806, "heading":-2.09072, "vx":-0.32749, "vy":-0.20747, "omega":-0.01215, "ax":0.8615, "ay":0.50691, "alpha":0.00104, "fx":[14.40773,14.19677,14.42439,14.41404], "fy":[8.44573,8.43758,8.45295,8.46326]}, + {"t":1.7932, "x":5.18962, "y":5.23162, "heading":-2.09111, "vx":-0.29964, "vy":-0.19108, "omega":-0.01212, "ax":0.85779, "ay":0.51318, "alpha":-0.00668, "fx":[14.30016,14.29916,14.29717,14.29944], "fy":[8.59,8.47242,8.58242,8.57291]}, + {"t":1.82553, "x":5.18039, "y":5.22571, "heading":-2.0915, "vx":-0.27192, "vy":-0.1745, "omega":-0.01234, "ax":0.85429, "ay":0.51901, "alpha":0.0117, "fx":[14.32681,14.07239,14.2602,14.30307], "fy":[8.65562,8.66993,8.64671,8.63452]}, + {"t":1.85785, "x":5.17204, "y":5.22034, "heading":-2.0919, "vx":-0.2443, "vy":-0.15772, "omega":-0.01196, "ax":0.85098, "ay":0.52445, "alpha":0.00463, "fx":[14.21744,14.17689,14.15267,14.19449], "fy":[8.78801,8.70602,8.75793,8.71761]}, + {"t":1.89018, "x":5.16459, "y":5.21552, "heading":-2.09229, "vx":-0.2168, "vy":-0.14077, "omega":-0.01181, "ax":0.84784, "ay":0.52954, "alpha":0.02296, "fx":[14.25971,13.96058,14.10704,14.20477], "fy":[8.83982,8.8786,8.81344,8.77675]}, + {"t":1.9225, "x":5.15803, "y":5.21124, "heading":-2.09267, "vx":-0.18939, "vy":-0.12365, "omega":-0.01107, "ax":0.84486, "ay":0.5343, "alpha":0.01723, "fx":[14.15167,14.06487,14.01441,14.10249], "fy":[8.96324,8.91883,8.90895,8.83512]}, + {"t":1.95482, "x":5.15234, "y":5.20753, "heading":-2.09303, "vx":-0.16208, "vy":-0.10638, "omega":-0.01051, "ax":0.84203, "ay":0.53877, "alpha":0.0362, "fx":[14.20904,13.8575,13.95943,14.1188], "fy":[9.00445,9.07334,8.95657,8.88972]}, + {"t":1.98715, "x":5.14755, "y":5.20437, "heading":-2.09336, "vx":-0.13486, "vy":-0.08896, "omega":-0.00934, "ax":0.83934, "ay":0.54297, "alpha":0.03234, "fx":[14.10568,13.96033,13.87635,14.023], "fy":[9.12042,9.11923,9.03846,8.92607]}, + {"t":2.01947, "x":5.14362, "y":5.20178, "heading":-2.09367, "vx":-0.10773, "vy":-0.07141, "omega":-0.00829, "ax":0.83678, "ay":0.54693, "alpha":0.05232, "fx":[14.17539,13.76184,13.81336,14.04403], "fy":[9.15422,9.2616,9.07877,8.97336]}, + {"t":2.0518, "x":5.14058, "y":5.19975, "heading":-2.09394, "vx":-0.08068, "vy":-0.05373, "omega":-0.0066, "ax":0.83434, "ay":0.55066, "alpha":0.05051, "fx":[14.08108,13.86122,13.7342,13.9554], "fy":[9.26337,9.31079,9.14966,8.99299]}, + {"t":2.08412, "x":5.13841, "y":5.19831, "heading":-2.09415, "vx":-0.05372, "vy":-0.03593, "omega":-0.00497, "ax":0.83201, "ay":0.55418, "alpha":0.07151, "fx":[14.15709,13.67216,13.66829,13.97917], "fy":[9.29259,9.44804,9.18242,9.02887]}, + {"t":2.11644, "x":5.13711, "y":5.19743, "heading":-2.09431, "vx":-0.02682, "vy":-0.01802, "omega":-0.00266, "ax":0.82979, "ay":0.55752, "alpha":0.08228, "fx":[14.15298,13.62944,13.5961,13.95001], "fy":[9.35844,9.54188,9.22783,9.04624]}, + {"t":2.14877, "x":5.13667, "y":5.19714, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3066a66..f4c1c9e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,7 @@ import static edu.wpi.first.units.Units.Rotations; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -26,7 +27,7 @@ import frc.robot.commands.algae.ProcessorAlgaeCommand; import frc.robot.commands.auton.NonProcessorShallowAutonCommand; import frc.robot.commands.auton.ProcessorShallowAutonCommand; -import frc.robot.commands.auton.TestAutonCommand; +import frc.robot.commands.auton.StealOneAlgeaAutonCommand; import frc.robot.commands.auton.ToggleVirtualSwitchCommand; import frc.robot.commands.biscuit.HoldBiscuitCommand; import frc.robot.commands.biscuit.JogBiscuitCommand; @@ -158,7 +159,7 @@ public class RobotContainer { private NonProcessorShallowAutonCommand nonProcessorShallowAutonCommand; private ProcessorShallowAutonCommand processorShallowAutonCommand; - private TestAutonCommand testAutonCommand; + private StealOneAlgeaAutonCommand testAutonCommand; private Alliance alliance = Alliance.Blue; private SuppliedValueWidget allianceColor; @@ -240,7 +241,22 @@ public RobotContainer() { visionSubsystem, pathHandler); - testAutonCommand = new TestAutonCommand(driveSubsystem, robotStateSubsystem, elevatorSubsystem); + testAutonCommand = + new StealOneAlgeaAutonCommand( + driveSubsystem, + robotStateSubsystem, + algaeSubsystem, + biscuitSubsystem, + coralSubsystem, + elevatorSubsystem, + tagAlignSubsystem, + visionSubsystem, + "startBargeToG", + "GToBarge", + "bargeToOppE", + "OppEToOppbarge", + ScoringLevel.L3, + new Pose2d(7.1, 3.7209, Rotation2d.fromRadians(3.14159))); configureTelemetry(); configureDriverBindings(); diff --git a/src/main/java/frc/robot/commands/auton/DriveAlgaeWaitAutonServoCommand.java b/src/main/java/frc/robot/commands/auton/DriveAlgaeWaitAutonServoCommand.java new file mode 100644 index 0000000..20eaecd --- /dev/null +++ b/src/main/java/frc/robot/commands/auton/DriveAlgaeWaitAutonServoCommand.java @@ -0,0 +1,217 @@ +package frc.robot.commands.auton; + +import choreo.Choreo; +import choreo.trajectory.SwerveSample; +import choreo.trajectory.Trajectory; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.commands.drive.DriveAutonCommand; +import frc.robot.constants.AutonConstants; +import frc.robot.constants.PathHandlerConstants; +import frc.robot.subsystems.biscuit.BiscuitSubsystem; +import frc.robot.subsystems.drive.DriveSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem.ElevatorStates; +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.tagAlign.TagAlignSubsystem; +import frc.robot.subsystems.tagAlign.TagAlignSubsystem.TagAlignStates; +import frc.robot.subsystems.vision.VisionSubsystem; +import java.util.Optional; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +public class DriveAlgaeWaitAutonServoCommand extends Command implements AutoCommandInterface { + private final DriveSubsystem driveSubsystem; + private final TagAlignSubsystem tagAlignSubsystem; + private final ElevatorSubsystem elevatorSubsystem; + private final RobotStateSubsystem robotStateSubsystem; + private final VisionSubsystem visionSubsystem; + + private Trajectory trajectory; + private final Timer timer = new Timer(); + private static final Logger logger = LoggerFactory.getLogger(DriveAutonCommand.class); + private boolean pathExists = false; + private String trajectoryName; + private boolean mirrorTrajectory = false; + private boolean mirrorToProcessor = false; + private boolean isServoing = false; + + private boolean resetOdometry; + private boolean firstPath; + private boolean lastPath; + private boolean hasStaged = false; + private boolean hasPreppedAlgae = false; + + private double yOffset; + private RobotStateSubsystem.ScoringLevel algaeLevel; + private double raiseElevAfterTime = 0.0; + + private SwerveSample desiredState; + private Pose2d initialPose = new Pose2d(); + private Pose2d finalPose = new Pose2d(); + + public DriveAlgaeWaitAutonServoCommand( + DriveSubsystem driveSubsystem, + TagAlignSubsystem tagAlignSubsystem, + ElevatorSubsystem elevatorSubsystem, + BiscuitSubsystem biscuitSubsystem, + RobotStateSubsystem robotStateSubsystem, + VisionSubsystem visionSubsystem, + String trajectoryName, + boolean firstPath, + boolean lastPath, + boolean resetOdometry, + double yOffset, + RobotStateSubsystem.ScoringLevel algaeLevel, + double raiseElevAfterTime) { + + addRequirements(driveSubsystem, elevatorSubsystem, biscuitSubsystem); + this.driveSubsystem = driveSubsystem; + this.tagAlignSubsystem = tagAlignSubsystem; + this.elevatorSubsystem = elevatorSubsystem; + this.robotStateSubsystem = robotStateSubsystem; + this.visionSubsystem = visionSubsystem; + + this.resetOdometry = resetOdometry; + this.firstPath = firstPath; + this.lastPath = lastPath; + this.trajectoryName = trajectoryName; + this.yOffset = yOffset; + this.algaeLevel = algaeLevel; + this.raiseElevAfterTime = raiseElevAfterTime; + + Optional> tempTrajectory = Choreo.loadTrajectory(trajectoryName); + if (tempTrajectory.isPresent()) { + trajectory = tempTrajectory.get(); + pathExists = true; + } else { + logger.error("Trajectory {} not found", trajectoryName); + pathExists = false; + } + org.littletonrobotics.junction.Logger.recordOutput("Auto/mirrorToProcessor", mirrorToProcessor); + org.littletonrobotics.junction.Logger.recordOutput("Auto/mirrorTrajectory", mirrorTrajectory); + timer.start(); + } + + @Override + public void reassignAlliance() { + mirrorTrajectory = driveSubsystem.shouldFlip(); + if (pathExists) { + initialPose = trajectory.getInitialPose(mirrorTrajectory).get(); + finalPose = trajectory.getFinalPose(mirrorTrajectory).get(); + + if (resetOdometry) { + driveSubsystem.prepForAuto(initialPose, initialPose.getRotation().getDegrees()); + driveSubsystem.resetHolonomicController(initialPose.getRotation().getRadians()); + } + } + } + + @Override + public void initialize() { + if (firstPath) { + elevatorSubsystem.zero(); + } else { + robotStateSubsystem.clearCoral(); + } + + visionSubsystem.setIsAuto(false); + robotStateSubsystem.setAutoAlgaeLevel(algaeLevel); + + isServoing = false; + hasStaged = false; + hasPreppedAlgae = false; + + if (pathExists) { + if (resetOdometry) { + driveSubsystem.resetOdometry(initialPose); + } + driveSubsystem.setEnableHolo(true); + driveSubsystem.setAutoDebugMsg("Initialize " + trajectoryName); + + driveSubsystem.grapherTrajectoryActive(true); + timer.reset(); + + desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get(); + + driveSubsystem.calculateController(desiredState); + } + } + + @Override + public void execute() { + + if (elevatorSubsystem.getState() == ElevatorStates.ZEROED + && tagAlignSubsystem.getCurRadius() <= 3 + && !hasPreppedAlgae + && timer.get() > raiseElevAfterTime) { + hasPreppedAlgae = true; + robotStateSubsystem.toReefAlignAlgaeAuto(); + } + + if (pathExists) { + if (!isServoing) { + desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get(); + driveSubsystem.calculateController(desiredState); + + if (shouldTransitionToServoing()) { + isServoing = true; + + visionSubsystem.setIsAuto(true); + + tagAlignSubsystem.startAuto( + robotStateSubsystem.getAllianceColor(), + robotStateSubsystem.getCoralLevel(), + yOffset, + true, + true); + } + } + } + + if (robotStateSubsystem.getState() == RobotStateSubsystem.RobotStates.REEF_ALIGN_CORAL) { + robotStateSubsystem.toPlaceCoralAuto(); + } + } + + private boolean shouldTransitionToServoing() { + return tagAlignSubsystem.getCurRadius() < PathHandlerConstants.kServoRadius; + } + + @Override + public boolean isFinished() { + if (!pathExists) { + return true; + } + return ((timer.hasElapsed(trajectory.getTotalTime() + AutonConstants.kAutoTimeout) + || tagAlignSubsystem.getState() == TagAlignStates.DONE && isServoing)) + && robotStateSubsystem.hasAlgae(); + // || (FastMath.sqrt( + // FastMath.pow(driveSubsystem.getPoseMeters().getX() - finalPose.getX(), 2) + // + FastMath.pow( + // (driveSubsystem.getPoseMeters().getY() - finalPose.getY()), 2)) + // < AutonConstants.kMaxPathErrorMeters) + // && driveSubsystem.getHolonomicControllerOmegaErrorRadians() + // < AutonConstants.kMaxOmegaErrorRadians); + } + + @Override + public void end(boolean interrupted) { + driveSubsystem.setEnableHolo(false); + + if (!interrupted && !lastPath) { + driveSubsystem.calculateController( + trajectory.sampleAt(trajectory.getTotalTime(), mirrorTrajectory).get()); + } else { + driveSubsystem.drive(0, 0, 0); + } + + tagAlignSubsystem.terminate(); + visionSubsystem.setIsAuto(true); + + driveSubsystem.grapherTrajectoryActive(false); + logger.info("End Trajectory {}: {}", trajectoryName, timer.get()); + driveSubsystem.setAutoDebugMsg("End " + trajectoryName); + } +} diff --git a/src/main/java/frc/robot/commands/auton/StealAlgaeImmediately.java b/src/main/java/frc/robot/commands/auton/StealAlgaeImmediately.java new file mode 100644 index 0000000..64e5a39 --- /dev/null +++ b/src/main/java/frc/robot/commands/auton/StealAlgaeImmediately.java @@ -0,0 +1,101 @@ +package frc.robot.commands.auton; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.drive.PrepOdomForAutoCommand; +import frc.robot.commands.robotState.AutoScoreAlgaeCommand; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.biscuit.BiscuitSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.drive.DriveSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; +import frc.robot.subsystems.tagAlign.TagAlignSubsystem; +import frc.robot.subsystems.vision.VisionSubsystem; + +public class StealAlgaeImmediately extends SequentialCommandGroup implements AutoCommandInterface { + + private DriveSubsystem driveSubsystem; + private DriveAlgaeWaitAutonServoCommand firstPath; + private DriveBargeAutonCommand secondPath; + private CoralSubsystem coralSubsystem; + private RobotStateSubsystem robotStateSubsystem; + private VisionSubsystem visionSubsystem; + + public StealAlgaeImmediately( + DriveSubsystem driveSubsystem, + RobotStateSubsystem robotStateSubsystem, + AlgaeSubsystem algaeSubsystem, + BiscuitSubsystem biscuitSubsystem, + CoralSubsystem coralSubsystem, + ElevatorSubsystem elevatorSubsystem, + TagAlignSubsystem tagAlignSubsystem, + VisionSubsystem visionSubsystem, + String firstPathName, + String secondPathName, + ScoringLevel OppAlgeaLevel, + Pose2d startPose) { + addRequirements( + driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem); + this.driveSubsystem = driveSubsystem; + this.coralSubsystem = coralSubsystem; + this.robotStateSubsystem = robotStateSubsystem; + this.visionSubsystem = visionSubsystem; + + firstPath = + new DriveAlgaeWaitAutonServoCommand( + driveSubsystem, + tagAlignSubsystem, + elevatorSubsystem, + biscuitSubsystem, + robotStateSubsystem, + visionSubsystem, + firstPathName, + true, + true, + true, + 0.0, + OppAlgeaLevel, + 2.0); + + secondPath = + new DriveBargeAutonCommand( + driveSubsystem, + tagAlignSubsystem, + elevatorSubsystem, + biscuitSubsystem, + robotStateSubsystem, + visionSubsystem, + secondPathName, + true, + false); + + addCommands( + new SequentialCommandGroup( + new PrepOdomForAutoCommand( + robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(180.0), startPose), + // new SetGyroOffsetCommand(driveSubsystem, Rotation2d.fromDegrees(180)), + firstPath, + secondPath, + new AutoScoreAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem))); + } + + @Override + public void reassignAlliance() { + firstPath.reassignAlliance(); + secondPath.reassignAlliance(); + driveSubsystem.teleResetGyro(); + coralSubsystem.setAutoPreload(); + robotStateSubsystem.setIsAutoPlacing(false); + robotStateSubsystem.setScoringLevel(ScoringLevel.L4); + robotStateSubsystem.setGetAlgaeOnCycle(false); + // robotStateSubsystem.setIsAuto(true); + robotStateSubsystem.setScoreSide(ScoreSide.RIGHT); + visionSubsystem.setVisionUpdating(true); + // pathHandler.reassignAlliance(); + } +} diff --git a/src/main/java/frc/robot/commands/auton/StealOneAlgeaAutonCommand.java b/src/main/java/frc/robot/commands/auton/StealOneAlgeaAutonCommand.java new file mode 100644 index 0000000..e23f57e --- /dev/null +++ b/src/main/java/frc/robot/commands/auton/StealOneAlgeaAutonCommand.java @@ -0,0 +1,140 @@ +package frc.robot.commands.auton; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.drive.PrepOdomForAutoCommand; +import frc.robot.commands.robotState.AutoScoreAlgaeCommand; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.biscuit.BiscuitSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.drive.DriveSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; +import frc.robot.subsystems.tagAlign.TagAlignSubsystem; +import frc.robot.subsystems.vision.VisionSubsystem; + +public class StealOneAlgeaAutonCommand extends SequentialCommandGroup + implements AutoCommandInterface { + + private DriveSubsystem driveSubsystem; + private DriveAlgaeAutonServoCommand firstPath; + private DriveBargeAutonCommand secondPath; + private DriveAlgaeWaitAutonServoCommand thirdPath; + private DriveBargeAutonCommand fourthPath; + private CoralSubsystem coralSubsystem; + private RobotStateSubsystem robotStateSubsystem; + private VisionSubsystem visionSubsystem; + + public StealOneAlgeaAutonCommand( + DriveSubsystem driveSubsystem, + RobotStateSubsystem robotStateSubsystem, + AlgaeSubsystem algaeSubsystem, + BiscuitSubsystem biscuitSubsystem, + CoralSubsystem coralSubsystem, + ElevatorSubsystem elevatorSubsystem, + TagAlignSubsystem tagAlignSubsystem, + VisionSubsystem visionSubsystem, + String firstPathName, + String secondPathName, + String thirdPathName, + String fourthPathName, + ScoringLevel OppAlgeaHeight, + Pose2d startPose) { + addRequirements( + driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem); + this.driveSubsystem = driveSubsystem; + this.coralSubsystem = coralSubsystem; + this.robotStateSubsystem = robotStateSubsystem; + this.visionSubsystem = visionSubsystem; + + firstPath = + new DriveAlgaeAutonServoCommand( + driveSubsystem, + tagAlignSubsystem, + elevatorSubsystem, + biscuitSubsystem, + robotStateSubsystem, + visionSubsystem, + firstPathName, + true, + true, + true, + 0.0, + ScoringLevel.L2); + + secondPath = + new DriveBargeAutonCommand( + driveSubsystem, + tagAlignSubsystem, + elevatorSubsystem, + biscuitSubsystem, + robotStateSubsystem, + visionSubsystem, + secondPathName, + true, + false); + + thirdPath = + new DriveAlgaeWaitAutonServoCommand( + driveSubsystem, + tagAlignSubsystem, + elevatorSubsystem, + biscuitSubsystem, + robotStateSubsystem, + visionSubsystem, + thirdPathName, + false, + true, + false, + 0.0, + OppAlgeaHeight, + 1.5); + + fourthPath = + new DriveBargeAutonCommand( + driveSubsystem, + tagAlignSubsystem, + elevatorSubsystem, + biscuitSubsystem, + robotStateSubsystem, + visionSubsystem, + fourthPathName, + true, + false); + + addCommands( + new SequentialCommandGroup( + new PrepOdomForAutoCommand( + robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(180.0), startPose), + // new SetGyroOffsetCommand(driveSubsystem, Rotation2d.fromDegrees(180)), + firstPath, + secondPath, + new AutoScoreAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem), + new WaitForElevBelowBarge(elevatorSubsystem), + thirdPath, + fourthPath, + new AutoScoreAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem))); + } + + @Override + public void reassignAlliance() { + firstPath.reassignAlliance(); + secondPath.reassignAlliance(); + thirdPath.reassignAlliance(); + fourthPath.reassignAlliance(); + driveSubsystem.teleResetGyro(); + coralSubsystem.setAutoPreload(); + robotStateSubsystem.setIsAutoPlacing(false); + robotStateSubsystem.setScoringLevel(ScoringLevel.L4); + robotStateSubsystem.setGetAlgaeOnCycle(false); + // robotStateSubsystem.setIsAuto(true); + robotStateSubsystem.setScoreSide(ScoreSide.RIGHT); + visionSubsystem.setVisionUpdating(true); + // pathHandler.reassignAlliance(); + } +} diff --git a/src/main/java/frc/robot/commands/auton/StealOneAlgeaNoSuperCycleAutonCommand.java b/src/main/java/frc/robot/commands/auton/StealOneAlgeaNoSuperCycleAutonCommand.java new file mode 100644 index 0000000..d18382b --- /dev/null +++ b/src/main/java/frc/robot/commands/auton/StealOneAlgeaNoSuperCycleAutonCommand.java @@ -0,0 +1,121 @@ +package frc.robot.commands.auton; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.drive.PrepOdomForAutoCommand; +import frc.robot.commands.robotState.AutoScoreAlgaeCommand; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.biscuit.BiscuitSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.drive.DriveSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; +import frc.robot.subsystems.tagAlign.TagAlignSubsystem; +import frc.robot.subsystems.vision.VisionSubsystem; + +public class StealOneAlgeaNoSuperCycleAutonCommand extends SequentialCommandGroup + implements AutoCommandInterface { + + private DriveSubsystem driveSubsystem; + private DriveAutonServoCommand firstPath; + private DriveAlgaeWaitAutonServoCommand secondPath; + private DriveBargeAutonCommand thirdPath; + private CoralSubsystem coralSubsystem; + private RobotStateSubsystem robotStateSubsystem; + private VisionSubsystem visionSubsystem; + + public StealOneAlgeaNoSuperCycleAutonCommand( + DriveSubsystem driveSubsystem, + RobotStateSubsystem robotStateSubsystem, + AlgaeSubsystem algaeSubsystem, + BiscuitSubsystem biscuitSubsystem, + CoralSubsystem coralSubsystem, + ElevatorSubsystem elevatorSubsystem, + TagAlignSubsystem tagAlignSubsystem, + VisionSubsystem visionSubsystem, + String firstPathName, + String secondPathName, + String thirdPathName, + ScoringLevel OppAlgeaHeight, + Pose2d startPose) { + addRequirements( + driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem); + this.driveSubsystem = driveSubsystem; + this.coralSubsystem = coralSubsystem; + this.robotStateSubsystem = robotStateSubsystem; + this.visionSubsystem = visionSubsystem; + + firstPath = + new DriveAutonServoCommand( + driveSubsystem, + tagAlignSubsystem, + elevatorSubsystem, + biscuitSubsystem, + robotStateSubsystem, + firstPathName, + true, + true, + false, + false, + 0.0); + + secondPath = + new DriveAlgaeWaitAutonServoCommand( + driveSubsystem, + tagAlignSubsystem, + elevatorSubsystem, + biscuitSubsystem, + robotStateSubsystem, + visionSubsystem, + secondPathName, + false, + true, + false, + 0.0, + OppAlgeaHeight, + 1.5); + + thirdPath = + new DriveBargeAutonCommand( + driveSubsystem, + tagAlignSubsystem, + elevatorSubsystem, + biscuitSubsystem, + robotStateSubsystem, + visionSubsystem, + thirdPathName, + true, + false); + + addCommands( + new SequentialCommandGroup( + new PrepOdomForAutoCommand( + robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(180.0), startPose), + // new SetGyroOffsetCommand(driveSubsystem, Rotation2d.fromDegrees(180)), + firstPath, + new WaitForElevBelowBarge(elevatorSubsystem), + secondPath, + thirdPath, + new AutoScoreAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem))); + } + + @Override + public void reassignAlliance() { + firstPath.reassignAlliance(); + secondPath.reassignAlliance(); + thirdPath.reassignAlliance(); + driveSubsystem.teleResetGyro(); + coralSubsystem.setAutoPreload(); + robotStateSubsystem.setIsAutoPlacing(false); + robotStateSubsystem.setScoringLevel(ScoringLevel.L4); + robotStateSubsystem.setGetAlgaeOnCycle(false); + // robotStateSubsystem.setIsAuto(true); + robotStateSubsystem.setScoreSide(ScoreSide.RIGHT); + visionSubsystem.setVisionUpdating(true); + // pathHandler.reassignAlliance(); + } +} diff --git a/src/main/java/frc/robot/commands/auton/WaitForElevBelowBarge.java b/src/main/java/frc/robot/commands/auton/WaitForElevBelowBarge.java new file mode 100644 index 0000000..8ce504e --- /dev/null +++ b/src/main/java/frc/robot/commands/auton/WaitForElevBelowBarge.java @@ -0,0 +1,21 @@ +package frc.robot.commands.auton; + +import static edu.wpi.first.units.Units.Rotations; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.ElevatorConstants; +import frc.robot.subsystems.elevator.ElevatorSubsystem; + +public class WaitForElevBelowBarge extends Command { + private ElevatorSubsystem elevatorSubsystem; + + public WaitForElevBelowBarge(ElevatorSubsystem elevatorSubsystem) { + this.elevatorSubsystem = elevatorSubsystem; + } + + @Override + public boolean isFinished() { + return elevatorSubsystem.getPosition().in(Rotations) + < ElevatorConstants.kUnderBargeSafeThreshold.in(Rotations); + } +} diff --git a/src/main/java/frc/robot/commands/auton/AutoScoreAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/AutoScoreAlgaeCommand.java similarity index 100% rename from src/main/java/frc/robot/commands/auton/AutoScoreAlgaeCommand.java rename to src/main/java/frc/robot/commands/robotState/AutoScoreAlgaeCommand.java diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 8072310..cdecc67 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -73,6 +73,7 @@ public class ElevatorConstants { // Min elevator height before biscuit movement public static final Angle kBiscuitSafeThreshold = Rotations.of(5); + public static final Angle kUnderBargeSafeThreshold = Rotations.of(2.3676757); public static final Angle kProtectAlgaeSetpoint = kBiscuitSafeThreshold; // FIXME use better setpoint diff --git a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java index ef745cb..1ae8145 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java @@ -13,6 +13,9 @@ import frc.robot.commands.auton.NonProcessorShallowSlowAutonCommand; import frc.robot.commands.auton.ProcessorShallowAutonCommand; import frc.robot.commands.auton.ProcessorShallowSlowAutonCommand; +import frc.robot.commands.auton.StealAlgaeImmediately; +import frc.robot.commands.auton.StealOneAlgeaAutonCommand; +import frc.robot.commands.auton.StealOneAlgeaNoSuperCycleAutonCommand; import frc.robot.constants.AutonConstants; import frc.robot.constants.PathHandlerConstants; import frc.robot.constants.RobotConstants; @@ -404,6 +407,57 @@ private AutoCommandInterface getAutoCommand(int switchPos) { AutonConstants.kMiddleBargeStart); } + case 0x14 -> { + return new StealOneAlgeaAutonCommand( + driveSubsystem, + robotStateSubsystem, + algaeSubsystem, + biscuitSubsystem, + coralSubsystem, + elevatorSubsystem, + tagAlignSubsystem, + visionSubsystem, + "startBargeToG", + "GToBarge", + "bargeToOppE", + "OppEToOppbarge", + ScoringLevel.L3, + new Pose2d(7.1, 3.7209, Rotation2d.fromRadians(3.14159))); + } + + case 0x15 -> { + return new StealAlgaeImmediately( + driveSubsystem, + robotStateSubsystem, + algaeSubsystem, + biscuitSubsystem, + coralSubsystem, + elevatorSubsystem, + tagAlignSubsystem, + visionSubsystem, + "startBargeToOppE", + "OppEToOppbarge", + ScoringLevel.L3, + new Pose2d(7.1, 3.7209, Rotation2d.fromRadians(3.14159))); + } + + case 0x16 -> { + return new StealOneAlgeaNoSuperCycleAutonCommand( + driveSubsystem, + robotStateSubsystem, + algaeSubsystem, + biscuitSubsystem, + coralSubsystem, + elevatorSubsystem, + tagAlignSubsystem, + visionSubsystem, + "startHToH", + "HToOppE", + "OppEToOppbarge", + ScoringLevel.L3, + new Pose2d(7.1008875, 4.0509, Rotation2d.fromDegrees(180.0))); + } + case 0x20 -> { return new ProcessorShallowSlowAutonCommand( driveSubsystem,