diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/deploy/choreo/E_rightStation.traj b/src/main/deploy/choreo/E_rightStation.traj new file mode 100644 index 0000000..234f3ff --- /dev/null +++ b/src/main/deploy/choreo/E_rightStation.traj @@ -0,0 +1,87 @@ +{ + "name":"E_rightStation", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":5.000988006591797, "y":2.773613691329956, "heading":2.0852470875746736, "intervals":52, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.3086029589176178, "y":0.4841791093349457, "heading":0.8139619227060371, "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}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"5.000988006591797 m", "val":5.000988006591797}, "y":{"exp":"2.773613691329956 m", "val":2.773613691329956}, "heading":{"exp":"2.0852470875746736 rad", "val":2.0852470875746736}, "intervals":52, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.3086029589176178 m", "val":0.3086029589176178}, "y":{"exp":"0.4841791093349457 m", "val":0.4841791093349457}, "heading":{"exp":"0.8139619227060371 rad", "val":0.8139619227060371}, "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}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.33399], + "samples":[ + {"t":0.0, "x":5.00099, "y":2.77361, "heading":2.08525, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-10.56352, "ay":-5.15399, "alpha":-2.87458, "fx":[-138.96052,-138.80031,-136.58525,-136.68004], "fy":[-64.728,-65.08992,-69.61716,-69.41289]}, + {"t":0.02565, "x":4.99751, "y":2.77192, "heading":2.08525, "vx":-0.27099, "vy":-0.13222, "omega":-0.07374, "ax":-10.56284, "ay":-5.15365, "alpha":-2.87289, "fx":[-138.95069,-138.7909,-136.57725,-136.67169], "fy":[-64.72524,-65.08702,-69.61135,-69.40702]}, + {"t":0.05131, "x":4.98708, "y":2.76683, "heading":2.08336, "vx":-0.54197, "vy":-0.26443, "omega":-0.14744, "ax":-10.5621, "ay":-5.15329, "alpha":-2.87117, "fx":[-138.93815,-138.78289,-136.5706,-136.66028], "fy":[-64.72624,-65.07915,-69.60106,-69.40532]}, + {"t":0.07696, "x":4.96971, "y":2.75835, "heading":2.07957, "vx":-0.81292, "vy":-0.39663, "omega":-0.2211, "ax":-10.56129, "ay":-5.1529, "alpha":-2.86942, "fx":[-138.92278,-138.77616,-136.56522,-136.6457], "fy":[-64.73099,-65.06632,-69.58624,-69.40771]}, + {"t":0.10261, "x":4.94538, "y":2.74648, "heading":2.0739, "vx":-1.08386, "vy":-0.52882, "omega":-0.29471, "ax":-10.56041, "ay":-5.15247, "alpha":-2.86762, "fx":[-138.90444,-138.77057,-136.56101,-136.62789], "fy":[-64.73947,-65.04853,-69.56678,-69.41405]}, + {"t":0.12827, "x":4.9141, "y":2.73122, "heading":2.06634, "vx":-1.35477, "vy":-0.661, "omega":-0.36828, "ax":-10.55944, "ay":-5.152, "alpha":-2.86578, "fx":[-138.88294,-138.76593,-136.55785,-136.60673], "fy":[-64.75168,-65.02579,-69.54255,-69.4242]}, + {"t":0.15392, "x":4.87587, "y":2.71257, "heading":2.05689, "vx":-1.62566, "vy":-0.79317, "omega":-0.44179, "ax":-10.55838, "ay":-5.15148, "alpha":-2.86388, "fx":[-138.85806,-138.76201,-136.55564,-136.58212], "fy":[-64.76765,-64.99813,-69.51337,-69.43793]}, + {"t":0.17958, "x":4.83069, "y":2.69052, "heading":2.04556, "vx":-1.89652, "vy":-0.92532, "omega":-0.51526, "ax":-10.55719, "ay":-5.1509, "alpha":-2.86193, "fx":[-138.82952,-138.75854,-136.55421,-136.55391], "fy":[-64.78739,-64.96557,-69.47904,-69.45499]}, + {"t":0.20523, "x":4.77856, "y":2.66509, "heading":2.03234, "vx":-2.16735, "vy":-1.05746, "omega":-0.58868, "ax":-10.55588, "ay":-5.15026, "alpha":-2.85992, "fx":[-138.79698,-138.75519,-136.55338,-136.52194], "fy":[-64.81093,-64.92816,-69.4393,-69.47509]}, + {"t":0.23088, "x":4.71949, "y":2.63627, "heading":2.01724, "vx":-2.43814, "vy":-1.18958, "omega":-0.66205, "ax":-10.5544, "ay":-5.14954, "alpha":-2.85784, "fx":[-138.76003,-138.75155,-136.5529,-136.486], "fy":[-64.83828,-64.88592,-69.39386,-69.49784]}, + {"t":0.25654, "x":4.65347, "y":2.60406, "heading":2.00026, "vx":-2.7089, "vy":-1.32169, "omega":-0.73536, "ax":-10.55273, "ay":-5.14872, "alpha":-2.85568, "fx":[-138.71816,-138.74711,-136.55247,-136.44579], "fy":[-64.86942,-64.83889,-69.34235,-69.52281]}, + {"t":0.28219, "x":4.5805, "y":2.56846, "heading":1.98139, "vx":-2.97962, "vy":-1.45377, "omega":-0.80862, "ax":-10.55084, "ay":-5.1478, "alpha":-2.85344, "fx":[-138.67075,-138.74125,-136.55166,-136.40094], "fy":[-64.90432,-64.78707,-69.28434,-69.54947]}, + {"t":0.30784, "x":4.50059, "y":2.52947, "heading":1.96065, "vx":-3.25029, "vy":-1.58583, "omega":-0.88182, "ax":-10.54866, "ay":-5.14674, "alpha":-2.8511, "fx":[-138.61699,-138.73315,-136.54993,-136.35094], "fy":[-64.94286,-64.73042,-69.21933,-69.57717]}, + {"t":0.3335, "x":4.41374, "y":2.48709, "heading":1.93802, "vx":-3.5209, "vy":-1.71786, "omega":-0.95496, "ax":-10.54613, "ay":-5.1455, "alpha":-2.84866, "fx":[-138.55588,-138.7218,-136.5465,-136.29508], "fy":[-64.98485,-64.66884,-69.14668,-69.60512]}, + {"t":0.35915, "x":4.31995, "y":2.44133, "heading":1.91353, "vx":-3.79144, "vy":-1.84986, "omega":-1.02804, "ax":-10.54317, "ay":-5.14406, "alpha":-2.84611, "fx":[-138.48608,-138.70583,-136.54032,-136.23239], "fy":[-65.02995,-64.60213,-69.06563,-69.63234]}, + {"t":0.3848, "x":4.21921, "y":2.39218, "heading":1.88715, "vx":-4.06191, "vy":-1.98182, "omega":-1.10105, "ax":-10.53964, "ay":-5.14233, "alpha":-2.84344, "fx":[-138.40579,-138.68344,-136.5299,-136.16144], "fy":[-65.07758,-64.52991,-68.97517,-69.65757]}, + {"t":0.41046, "x":4.11154, "y":2.33965, "heading":1.85891, "vx":-4.33229, "vy":-2.11374, "omega":-1.174, "ax":-10.53537, "ay":-5.14025, "alpha":-2.84063, "fx":[-138.3125,-138.65209,-136.51306,-136.08016], "fy":[-65.12686,-64.45155,-68.87397,-69.67917]}, + {"t":0.43611, "x":3.99694, "y":2.28373, "heading":1.82879, "vx":-4.60256, "vy":-2.24561, "omega":-1.24687, "ax":-10.5301, "ay":-5.13768, "alpha":-2.83766, "fx":[-138.2026,-138.60814,-136.48658,-135.98543], "fy":[-65.17633,-64.36592,-68.76019,-69.69489]}, + {"t":0.46176, "x":3.8754, "y":2.22443, "heading":1.7968, "vx":-4.8727, "vy":-2.37741, "omega":-1.31967, "ax":-10.52342, "ay":-5.13442, "alpha":-2.83453, "fx":[-138.07062,-138.54611,-136.44545,-135.87233], "fy":[-65.22366,-64.27113,-68.63111,-69.70152]}, + {"t":0.48742, "x":3.74693, "y":2.16176, "heading":1.76295, "vx":-5.14266, "vy":-2.50913, "omega":-1.39238, "ax":-10.5147, "ay":-5.13016, "alpha":-2.83121, "fx":[-137.90779,-138.45728,-136.38154,-135.73278], "fy":[-65.26491,-64.16384,-68.48249,-69.69414]}, + {"t":0.51307, "x":3.61155, "y":2.0957, "heading":1.72723, "vx":-5.4124, "vy":-2.64073, "omega":-1.46501, "ax":-10.50281, "ay":-5.12436, "alpha":-2.82768, "fx":[-137.69913,-138.32678,-136.28074,-135.55269], "fy":[-65.29309,-64.03794,-68.30714,-69.66466]}, + {"t":0.53873, "x":3.46924, "y":2.02627, "heading":1.68965, "vx":-5.68184, "vy":-2.77219, "omega":-1.53755, "ax":-10.48566, "ay":-5.116, "alpha":-2.82389, "fx":[-137.41648,-138.12682,-136.11639,-135.3052], "fy":[-65.2949,-63.88144,-68.09173,-69.59837]}, + {"t":0.56438, "x":3.32003, "y":1.95347, "heading":1.6502, "vx":-5.95083, "vy":-2.90344, "omega":-1.61, "ax":-10.45877, "ay":-5.10288, "alpha":-2.81977, "fx":[-136.99981,-137.79831,-135.8315,-134.93257], "fy":[-65.24164,-63.66803,-67.80789,-69.46451]}, + {"t":0.59003, "x":3.16393, "y":1.87731, "heading":1.6089, "vx":-6.21914, "vy":-3.03434, "omega":-1.68233, "ax":-10.41054, "ay":-5.07935, "alpha":-2.81514, "fx":[-136.29399,-137.18895,-135.27852,-134.28489], "fy":[-65.05891,-63.32856,-67.38248,-69.18463]}, + {"t":0.61569, "x":3.00096, "y":1.79779, "heading":1.56574, "vx":-6.4862, "vy":-3.16465, "omega":-1.75455, "ax":-10.29888, "ay":-5.02487, "alpha":-2.80924, "fx":[-134.73625,-135.74667,-133.92164,-132.81704], "fy":[-64.47647,-62.61018,-66.55014,-68.47592]}, + {"t":0.64134, "x":2.83118, "y":1.71495, "heading":1.52073, "vx":-6.75041, "vy":-3.29355, "omega":-1.82662, "ax":-9.75895, "ay":-4.76144, "alpha":-2.79161, "fx":[-127.45393,-128.68825,-127.11297,-125.80233], "fy":[-61.15422,-59.31449,-63.00897,-64.89382]}, + {"t":0.66699, "x":2.6548, "y":1.6289, "heading":1.47387, "vx":-7.00076, "vy":-3.4157, "omega":-1.89823, "ax":9.75895, "ay":4.76143, "alpha":2.80263, "fx":[127.38606,128.70083,127.17982,125.7907], "fy":[61.28544,59.25997,62.883,64.94253]}, + {"t":0.69265, "x":2.47841, "y":1.54284, "heading":1.42518, "vx":-6.75041, "vy":-3.29355, "omega":-1.82634, "ax":10.29888, "ay":5.02486, "alpha":2.81256, "fx":[134.52546,135.79285,134.1287,132.77451], "fy":[64.90669,62.48026,66.13913,68.58642]}, + {"t":0.7183, "x":2.30863, "y":1.46, "heading":1.37832, "vx":-6.4862, "vy":-3.16465, "omega":-1.75418, "ax":10.41054, "ay":5.07934, "alpha":2.81671, "fx":[135.94583,137.27043,135.62045,134.20958], "fy":[65.77515,63.1221,66.69866,69.35857]}, + {"t":0.74395, "x":2.14566, "y":1.38049, "heading":1.33332, "vx":-6.21914, "vy":-3.03434, "omega":-1.68193, "ax":10.45877, "ay":5.10288, "alpha":2.82047, "fx":[136.52015,137.91359,136.30253,134.82589], "fy":[66.23154,63.38865,66.8629,69.69891]}, + {"t":0.76961, "x":1.98956, "y":1.30432, "heading":1.29018, "vx":-5.95083, "vy":-2.90344, "omega":-1.60957, "ax":10.48566, "ay":5.116, "alpha":2.82403, "fx":[136.81138,138.274,136.71056,135.16893], "fy":[66.54575,63.53344,66.8975,69.88973]}, + {"t":0.79526, "x":1.84035, "y":1.23152, "heading":1.24888, "vx":-5.68184, "vy":-2.77219, "omega":-1.53712, "ax":10.50281, "ay":5.12436, "alpha":2.82743, "fx":[136.97478,138.50388,136.99195,135.38873], "fy":[66.79187,63.62577,66.87589,70.00932]}, + {"t":0.82092, "x":1.69804, "y":1.16209, "heading":1.20945, "vx":-5.4124, "vy":-2.64073, "omega":-1.46459, "ax":10.5147, "ay":5.13016, "alpha":2.83069, "fx":[137.07048,138.66226,137.20362,135.54304], "fy":[66.99839,63.69202,66.82657,70.0884]}, + {"t":0.84657, "x":1.56266, "y":1.09604, "heading":1.17188, "vx":-5.14266, "vy":-2.50913, "omega":-1.39197, "ax":10.52342, "ay":5.13442, "alpha":2.83381, "fx":[137.12669,138.77694,137.37218,135.65871], "fy":[67.17854,63.74416,66.76301,70.14174]}, + {"t":0.87222, "x":1.43419, "y":1.03336, "heading":1.13617, "vx":-4.8727, "vy":-2.37741, "omega":-1.31927, "ax":10.5301, "ay":5.13768, "alpha":2.8368, "fx":[137.1584,138.86282,137.51173,135.74982], "fy":[67.33928,63.78824,66.69238,70.17747]}, + {"t":0.89788, "x":1.31265, "y":0.97406, "heading":1.10233, "vx":-4.60256, "vy":-2.24561, "omega":-1.2465, "ax":10.53537, "ay":5.14025, "alpha":2.83968, "fx":[137.17436,138.92864,137.63043,135.8244], "fy":[67.48462,63.82749,66.6189,70.20057]}, + {"t":0.92353, "x":1.19805, "y":0.91814, "heading":1.07035, "vx":-4.33229, "vy":-2.11374, "omega":-1.17365, "ax":10.53964, "ay":5.14234, "alpha":2.84244, "fx":[137.17996,138.97995,137.73334,135.88733], "fy":[67.61705,63.86372,66.54518,70.21434]}, + {"t":0.94918, "x":1.09038, "y":0.86561, "heading":1.04024, "vx":-4.06191, "vy":-1.98183, "omega":-1.10073, "ax":10.54317, "ay":5.14406, "alpha":2.84509, "fx":[137.17875,139.02042,137.82379,135.94168], "fy":[67.73818,63.89789,66.4729,70.22112]}, + {"t":0.97484, "x":0.98964, "y":0.81646, "heading":1.012, "vx":-3.79144, "vy":-1.84986, "omega":-1.02775, "ax":10.54613, "ay":5.1455, "alpha":2.84764, "fx":[137.17313,139.05266,137.90402,135.98946], "fy":[67.84915,63.93052,66.40323,70.22265]}, + {"t":1.00049, "x":0.89585, "y":0.7707, "heading":0.98564, "vx":-3.5209, "vy":-1.71786, "omega":-0.95469, "ax":10.54866, "ay":5.14674, "alpha":2.8501, "fx":[137.16478,139.07853,137.97566,136.03205], "fy":[67.95078,63.96182,66.33695,70.22028]}, + {"t":1.02614, "x":0.809, "y":0.72832, "heading":0.96115, "vx":-3.25029, "vy":-1.58583, "omega":-0.88158, "ax":10.55084, "ay":5.1478, "alpha":2.85247, "fx":[137.15491,139.09947,138.03986,136.07036], "fy":[68.04369,63.99184,66.27461,70.21511]}, + {"t":1.0518, "x":0.72909, "y":0.68934, "heading":0.93853, "vx":-2.97962, "vy":-1.45377, "omega":-0.8084, "ax":10.55273, "ay":5.14872, "alpha":2.85475, "fx":[137.14441,139.11656,138.09753,136.10504], "fy":[68.12837,64.02053,66.2166,70.20803]}, + {"t":1.07745, "x":0.65612, "y":0.65374, "heading":0.91779, "vx":-2.7089, "vy":-1.32169, "omega":-0.73517, "ax":10.5544, "ay":5.14954, "alpha":2.85695, "fx":[137.13394,139.13067,138.14934,136.13655], "fy":[68.20521,64.04776,66.16319,70.19978]}, + {"t":1.1031, "x":0.5901, "y":0.62152, "heading":0.89893, "vx":-2.43814, "vy":-1.18958, "omega":-0.66188, "ax":10.55588, "ay":5.15026, "alpha":2.85907, "fx":[137.12399,139.14249,138.19582,136.16519], "fy":[68.27456,64.07338,66.11458,70.191]}, + {"t":1.12876, "x":0.53103, "y":0.5927, "heading":0.88195, "vx":-2.16735, "vy":-1.05746, "omega":-0.58853, "ax":10.55719, "ay":5.1509, "alpha":2.86113, "fx":[137.11496,139.1526,138.23742,136.19121], "fy":[68.33669,64.09723,66.0709,70.18223]}, + {"t":1.15441, "x":0.4789, "y":0.56727, "heading":0.86686, "vx":-1.89652, "vy":-0.92532, "omega":-0.51513, "ax":10.55838, "ay":5.15148, "alpha":2.86312, "fx":[137.10713,139.16146,138.27448,136.21477], "fy":[68.39184,64.11911,66.03224,70.17393]}, + {"t":1.18007, "x":0.43372, "y":0.54523, "heading":0.85364, "vx":-1.62566, "vy":-0.79317, "omega":-0.44168, "ax":10.55944, "ay":5.152, "alpha":2.86504, "fx":[137.10074,139.16946,138.30729,136.23598], "fy":[68.44022,64.13889,65.99866,70.16649]}, + {"t":1.20572, "x":0.39549, "y":0.52657, "heading":0.84231, "vx":-1.35477, "vy":-0.661, "omega":-0.36819, "ax":10.56041, "ay":5.15247, "alpha":2.86691, "fx":[137.09597,139.17693,138.33608,136.25494], "fy":[68.48201,64.15639,65.97021,70.16027]}, + {"t":1.23137, "x":0.36422, "y":0.51131, "heading":0.83286, "vx":-1.08386, "vy":-0.52882, "omega":-0.29464, "ax":10.56129, "ay":5.1529, "alpha":2.86873, "fx":[137.09296,139.18414,138.36105,136.27173], "fy":[68.51737,64.17149,65.94692,70.15553]}, + {"t":1.25703, "x":0.33989, "y":0.49944, "heading":0.82531, "vx":-0.81292, "vy":-0.39663, "omega":-0.22105, "ax":10.5621, "ay":5.15329, "alpha":2.87049, "fx":[137.09186,139.19131,138.38238,136.28638], "fy":[68.54642,64.18407,65.92881,70.15252]}, + {"t":1.28268, "x":0.32251, "y":0.49096, "heading":0.81963, "vx":-0.54197, "vy":-0.26443, "omega":-0.14741, "ax":10.56284, "ay":5.15365, "alpha":2.87221, "fx":[137.09274,139.19864,138.40019,136.29897], "fy":[68.56927,64.19405,65.9159,70.15144]}, + {"t":1.30833, "x":0.31208, "y":0.48588, "heading":0.81585, "vx":-0.27099, "vy":-0.13222, "omega":-0.07373, "ax":10.56352, "ay":5.15399, "alpha":2.87389, "fx":[137.09572,139.20628,138.4146,136.30953], "fy":[68.58601,64.20136,65.90822,70.15243]}, + {"t":1.33399, "x":0.3086, "y":0.48418, "heading":0.81396, "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/J_leftStation.traj b/src/main/deploy/choreo/J_leftStation.traj new file mode 100644 index 0000000..fdefc43 --- /dev/null +++ b/src/main/deploy/choreo/J_leftStation.traj @@ -0,0 +1,90 @@ +{ + "name":"J_leftStation", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":4.923254489898682, "y":5.280681133270264, "heading":-2.034444356476718, "intervals":55, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.37427788972854614, "y":7.690096378326416, "heading":2.2035452332948458, "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}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.923254489898682 m", "val":4.923254489898682}, "y":{"exp":"5.280681133270264 m", "val":5.280681133270264}, "heading":{"exp":"-2.034444356476718 rad", "val":-2.034444356476718}, "intervals":55, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.37427788972854614 m", "val":0.37427788972854614}, "y":{"exp":"7.690096378326416 m", "val":7.690096378326416}, "heading":{"exp":"2.2035452332948458 rad", "val":2.2035452332948458}, "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}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.32504], + "samples":[ + {"t":0.0, "x":4.92325, "y":5.28068, "heading":-2.03444, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-10.38332, "ay":5.49963, "alpha":-4.69083, "fx":[-137.36487,-137.36657,-133.33686,-133.55786], "fy":[68.038,68.00271,75.59984,75.2374]}, + {"t":0.02409, "x":4.92024, "y":5.28228, "heading":-2.03444, "vx":-0.25015, "vy":0.1325, "omega":-0.11301, "ax":-10.38265, "ay":5.49928, "alpha":-4.68813, "fx":[-137.35515,-137.35631,-133.32926,-133.55052], "fy":[68.03579,68.0004,75.59255,75.23071]}, + {"t":0.04818, "x":4.9112, "y":5.28707, "heading":-2.03717, "vx":-0.50029, "vy":0.26498, "omega":-0.22595, "ax":-10.38192, "ay":5.49889, "alpha":-4.68538, "fx":[-137.3398,-137.35048,-133.32641,-133.53683], "fy":[68.04326,67.98735,75.57517,75.2337]}, + {"t":0.07227, "x":4.89614, "y":5.29504, "heading":-2.04261, "vx":-0.7504, "vy":0.39746, "omega":-0.33883, "ax":-10.38114, "ay":5.49848, "alpha":-4.68258, "fx":[-137.31869,-137.34893,-133.32828,-133.51674], "fy":[68.06043,67.96363,75.54757,75.24622]}, + {"t":0.09637, "x":4.87504, "y":5.30622, "heading":-2.05077, "vx":-1.0005, "vy":0.52993, "omega":-0.45164, "ax":-10.38029, "ay":5.49803, "alpha":-4.67973, "fx":[-137.29166,-137.35145,-133.33482,-133.49026], "fy":[68.08736,67.9294,75.50953,75.26802]}, + {"t":0.12046, "x":4.84793, "y":5.32058, "heading":-2.06165, "vx":-1.25058, "vy":0.66238, "omega":-0.56439, "ax":-10.37936, "ay":5.49754, "alpha":-4.6768, "fx":[-137.25849,-137.35775,-133.34606,-133.45739], "fy":[68.12421,67.88492,75.46073,75.29879]}, + {"t":0.14455, "x":4.81479, "y":5.33813, "heading":-2.07525, "vx":-1.50063, "vy":0.79483, "omega":-0.67706, "ax":-10.37834, "ay":5.497, "alpha":-4.67378, "fx":[-137.21889,-137.36748,-133.36201,-133.41817], "fy":[68.17115,67.83051,75.40075,75.33811]}, + {"t":0.16864, "x":4.77562, "y":5.35888, "heading":-2.09156, "vx":-1.75067, "vy":0.92726, "omega":-0.78966, "ax":-10.37722, "ay":5.4964, "alpha":-4.67067, "fx":[-137.17252,-137.38019,-133.38271,-133.37266], "fy":[68.22842,67.7666,75.32908,75.38549]}, + {"t":0.19273, "x":4.73044, "y":5.38281, "heading":-2.11059, "vx":-2.00067, "vy":1.05968, "omega":-0.90218, "ax":-10.37598, "ay":5.49575, "alpha":-4.66745, "fx":[-137.11897,-137.39537,-133.40821,-133.3209], "fy":[68.29631,67.69369,75.2451,75.44029]}, + {"t":0.21682, "x":4.67923, "y":5.40993, "heading":-2.13232, "vx":-2.25064, "vy":1.19208, "omega":-1.01463, "ax":-10.3746, "ay":5.49502, "alpha":-4.66409, "fx":[-137.05775,-137.41239,-133.43851,-133.26298], "fy":[68.3751,67.61238,75.14814,75.50177]}, + {"t":0.24092, "x":4.62199, "y":5.44025, "heading":-2.15677, "vx":-2.50059, "vy":1.32446, "omega":-1.12699, "ax":-10.37306, "ay":5.49421, "alpha":-4.66059, "fx":[-136.98831,-137.43048,-133.4736,-133.19895], "fy":[68.46509,67.52335,75.03739,75.56908]}, + {"t":0.26501, "x":4.55874, "y":5.47375, "heading":-2.18392, "vx":-2.75049, "vy":1.45683, "omega":-1.23927, "ax":-10.37133, "ay":5.49329, "alpha":-4.65691, "fx":[-136.90998,-137.44878,-133.5134,-133.12886], "fy":[68.56658,67.42738,74.91198,75.64117]}, + {"t":0.2891, "x":4.48947, "y":5.51044, "heading":-2.21377, "vx":-3.00035, "vy":1.58917, "omega":-1.35147, "ax":-10.36937, "ay":5.49225, "alpha":-4.65304, "fx":[-136.82199,-137.46622,-133.55772,-133.05272], "fy":[68.67981,67.3253,74.77096,75.71685]}, + {"t":0.31319, "x":4.41417, "y":5.55032, "heading":-2.24633, "vx":-3.25017, "vy":1.72149, "omega":-1.46357, "ax":-10.36713, "ay":5.49106, "alpha":-4.64896, "fx":[-136.72341,-137.48153,-133.60622,-132.97046], "fy":[68.80496,67.21803,74.61327,75.79474]}, + {"t":0.33728, "x":4.33286, "y":5.59339, "heading":-2.28159, "vx":-3.49993, "vy":1.85377, "omega":-1.57557, "ax":-10.36454, "ay":5.48969, "alpha":-4.64466, "fx":[-136.61314,-137.4932,-133.65837,-132.88192], "fy":[68.94208,67.10653,74.43775,75.87317]}, + {"t":0.36137, "x":4.24554, "y":5.63964, "heading":-2.31955, "vx":-3.74963, "vy":1.98603, "omega":-1.68746, "ax":-10.36152, "ay":5.48809, "alpha":-4.6401, "fx":[-136.48979,-137.49934,-133.71329,-132.7867], "fy":[69.09107,66.99176,74.2431,75.95023]}, + {"t":0.38547, "x":4.15219, "y":5.68908, "heading":-2.3602, "vx":-3.99925, "vy":2.11825, "omega":-1.79925, "ax":-10.35795, "ay":5.48621, "alpha":-4.63529, "fx":[-136.35163,-137.4976,-133.76965,-132.68411], "fy":[69.25154,66.8746,74.02786,76.02359]}, + {"t":0.40956, "x":4.05284, "y":5.74171, "heading":-2.40355, "vx":-4.24879, "vy":2.25042, "omega":-1.91092, "ax":-10.35367, "ay":5.48394, "alpha":-4.63022, "fx":[-136.19632,-137.48495,-133.82544,-132.57296], "fy":[69.42273,66.75582,73.7903,76.09047]}, + {"t":0.43365, "x":3.94748, "y":5.79751, "heading":-2.44959, "vx":-4.49823, "vy":2.38253, "omega":-2.02247, "ax":-10.34844, "ay":5.48117, "alpha":-4.62488, "fx":[-136.02069,-137.45731,-133.87758,-132.4512], "fy":[69.60332,66.63584,73.52828,76.14736]}, + {"t":0.45774, "x":3.8361, "y":5.8565, "heading":-2.49831, "vx":-4.74754, "vy":2.51459, "omega":-2.13389, "ax":-10.3419, "ay":5.4777, "alpha":-4.61929, "fx":[-135.82011,-137.40902,-133.9213,-132.31536], "fy":[69.79104,66.51444,73.23896,76.18973]}, + {"t":0.48183, "x":3.71873, "y":5.91867, "heading":-2.54972, "vx":-4.99669, "vy":2.64655, "omega":-2.24518, "ax":-10.3335, "ay":5.47325, "alpha":-4.61345, "fx":[-135.58747,-137.33166,-133.94899,-132.15946], "fy":[69.98216,66.39026,72.91821,76.2114]}, + {"t":0.50592, "x":3.59535, "y":5.98402, "heading":-2.60381, "vx":-5.24564, "vy":2.77841, "omega":-2.35633, "ax":-10.32231, "ay":5.46732, "alpha":-4.6074, "fx":[-135.31109,-137.21187,-133.94788,-131.97288], "fy":[70.17027,66.25968,72.5595,76.20329]}, + {"t":0.53002, "x":3.46598, "y":6.05254, "heading":-2.66058, "vx":-5.49432, "vy":2.91013, "omega":-2.46733, "ax":-10.30665, "ay":5.45903, "alpha":-4.60114, "fx":[-134.97012,-137.0265,-133.89501,-131.73556], "fy":[70.34382,66.11443,72.15129,76.15065]}, + {"t":0.55411, "x":3.33062, "y":6.12424, "heading":-2.72002, "vx":-5.74263, "vy":3.04165, "omega":-2.57817, "ax":-10.28321, "ay":5.44662, "alpha":-4.59469, "fx":[-134.52298,-136.73045,-133.74478,-131.4063], "fy":[70.47988,65.93573,71.67054,76.02633]}, + {"t":0.5782, "x":3.18929, "y":6.1991, "heading":-2.78213, "vx":-5.99037, "vy":3.17286, "omega":-2.68887, "ax":-10.24426, "ay":5.42598, "alpha":-4.58802, "fx":[-133.87302,-136.22049,-133.39167,-130.88743], "fy":[70.52567,65.67678,71.06336,75.77034]}, + {"t":0.60229, "x":3.04199, "y":6.27711, "heading":-2.84691, "vx":-6.23717, "vy":3.30358, "omega":-2.7994, "ax":-10.16679, "ay":5.38495, "alpha":-4.58089, "fx":[-132.73197,-135.19157,-132.52274,-129.88539], "fy":[70.32531,65.19327,70.1679,75.20923]}, + {"t":0.62638, "x":2.88878, "y":6.35826, "heading":-2.91435, "vx":-6.4821, "vy":3.43332, "omega":-2.90976, "ax":-9.93796, "ay":5.26374, "alpha":-4.571, "fx":[-129.68303,-132.15167,-129.60681,-126.95378], "fy":[69.11733,63.76736,68.18623,73.50242]}, + {"t":0.65047, "x":2.72973, "y":6.4425, "heading":-2.98446, "vx":-6.72152, "vy":3.56013, "omega":-3.01988, "ax":0.0, "ay":-0.00001, "alpha":-0.00246, "fx":[-0.00128,-0.00094,0.00124,0.00089], "fy":[0.00072,-0.00145,-0.0011,0.00107]}, + {"t":0.67457, "x":2.5678, "y":6.52827, "heading":-3.05721, "vx":-6.72152, "vy":3.56013, "omega":-3.01994, "ax":9.93796, "ay":-5.26374, "alpha":4.57184, "fx":[129.32181,132.12199,129.98711,126.96435], "fy":[-69.78118,-63.90914,-67.47084,-73.41211]}, + {"t":0.69866, "x":2.40875, "y":6.61251, "heading":-3.12996, "vx":-6.4821, "vy":3.43332, "omega":-2.9098, "ax":10.16679, "ay":-5.38494, "alpha":4.58301, "fx":[132.00513,135.09811,133.28792,129.9405], "fy":[-71.67042,-65.46792,-68.71617,-75.04091]}, + {"t":0.72275, "x":2.25554, "y":6.69367, "heading":3.08312, "vx":-6.23717, "vy":3.30358, "omega":-2.79939, "ax":10.24426, "ay":-5.42597, "alpha":4.59117, "fx":[132.79658,136.06363,134.52401,130.98839], "fy":[-72.52228,-66.08128,-68.90879,-75.52332]}, + {"t":0.74684, "x":2.10825, "y":6.77168, "heading":3.01568, "vx":-5.99037, "vy":3.17286, "omega":-2.68878, "ax":10.28321, "ay":-5.4466, "alpha":4.5986, "fx":[133.11425,136.51159,135.22507,131.5536], "fy":[-73.09578,-66.46623,-68.84991,-75.69997]}, + {"t":0.77093, "x":1.96691, "y":6.84654, "heading":2.9509, "vx":-5.74263, "vy":3.04165, "omega":-2.57799, "ax":10.30665, "ay":-5.45902, "alpha":4.60554, "fx":[133.24729,136.74747,135.70305,131.92939], "fy":[-73.5452,-66.76634,-68.7034,-75.74458]}, + {"t":0.79502, "x":1.83156, "y":6.91823, "heading":2.88879, "vx":-5.49432, "vy":2.91013, "omega":-2.46704, "ax":10.32231, "ay":-5.46731, "alpha":4.61202, "fx":[133.29291,136.87482,136.06285,132.21313], "fy":[-73.92234,-67.02785,-68.5244,-75.71745]}, + {"t":0.81912, "x":1.70218, "y":6.98676, "heading":2.82936, "vx":-5.24564, "vy":2.77841, "omega":-2.35593, "ax":10.3335, "ay":-5.47324, "alpha":4.61804, "fx":[133.29297,136.93893,136.34986,132.44581], "fy":[-74.24965,-67.26915,-68.33647,-75.64608]}, + {"t":0.84321, "x":1.57881, "y":7.0521, "heading":2.7726, "vx":-4.99669, "vy":2.64655, "omega":-2.24467, "ax":10.3419, "ay":-5.47769, "alpha":4.6236, "fx":[133.26836,136.96312,136.58706,132.64723], "fy":[-74.53864,-67.49825,-68.15109,-75.54556]}, + {"t":0.8673, "x":1.46143, "y":7.11427, "heading":2.71852, "vx":-4.74754, "vy":2.51459, "omega":-2.13328, "ax":10.34844, "ay":-5.48116, "alpha":4.62874, "fx":[133.2306,136.96083,136.78753,132.82782], "fy":[-74.79602,-67.71867,-67.97423,-75.42531]}, + {"t":0.89139, "x":1.35006, "y":7.17326, "heading":2.66713, "vx":-4.49823, "vy":2.38254, "omega":-2.02177, "ax":10.35367, "ay":-5.48393, "alpha":4.63347, "fx":[133.18644,136.94048,136.95938,132.99335], "fy":[-75.02618,-67.93177,-67.80904,-75.29187]}, + {"t":0.91548, "x":1.24469, "y":7.22907, "heading":2.61842, "vx":-4.24879, "vy":2.25042, "omega":-1.91014, "ax":10.35795, "ay":-5.4862, "alpha":4.63784, "fx":[133.14003,136.90777,137.10805,133.14712], "fy":[-75.23225,-68.13786,-67.65706,-75.15006]}, + {"t":0.93957, "x":1.14534, "y":7.2817, "heading":2.5724, "vx":-3.99925, "vy":2.11825, "omega":-1.79841, "ax":10.36152, "ay":-5.48809, "alpha":4.6419, "fx":[133.09394,136.86672,137.23741,133.29104], "fy":[-75.41669,-68.33667,-67.51887,-75.00367]}, + {"t":0.96366, "x":1.052, "y":7.33114, "heading":2.52907, "vx":-3.74963, "vy":1.98603, "omega":-1.68657, "ax":10.36454, "ay":-5.48969, "alpha":4.64568, "fx":[133.0498,136.8203,137.35034,133.42617], "fy":[-75.5815,-68.52769,-67.39446,-74.85575]}, + {"t":0.98776, "x":0.96467, "y":7.37739, "heading":2.48844, "vx":-3.49993, "vy":1.85378, "omega":-1.57465, "ax":10.36713, "ay":-5.49106, "alpha":4.64923, "fx":[133.00861,136.77077,137.44913,133.55312], "fy":[-75.72845,-68.71027,-67.28342,-74.70884]}, + {"t":1.01185, "x":0.88336, "y":7.42046, "heading":2.45051, "vx":-3.25017, "vy":1.72149, "omega":-1.46264, "ax":10.36937, "ay":-5.49225, "alpha":4.65259, "fx":[132.97092,136.71993,137.53562,133.67217], "fy":[-75.85909,-68.88375,-67.18512,-74.56505]}, + {"t":1.03594, "x":0.80807, "y":7.46034, "heading":2.41527, "vx":-3.00035, "vy":1.58917, "omega":-1.35056, "ax":10.37133, "ay":-5.49329, "alpha":4.65579, "fx":[132.93704,136.66918,137.61135,133.78346], "fy":[-75.97485,-69.04747,-67.09875,-74.42619]}, + {"t":1.06003, "x":0.73879, "y":7.49703, "heading":2.38273, "vx":-2.75049, "vy":1.45683, "omega":-1.23839, "ax":10.37306, "ay":-5.49421, "alpha":4.65886, "fx":[132.90705,136.61968,137.67762,133.887], "fy":[-76.07705,-69.20085,-67.02347,-74.29378]}, + {"t":1.08412, "x":0.67554, "y":7.53053, "heading":2.3529, "vx":-2.50059, "vy":1.32446, "omega":-1.12615, "ax":10.3746, "ay":-5.49503, "alpha":4.66184, "fx":[132.88093,136.57238,137.73557,133.98276], "fy":[-76.16691,-69.34332,-66.95834,-74.16913]}, + {"t":1.10821, "x":0.61831, "y":7.56084, "heading":2.32577, "vx":-2.25064, "vy":1.19208, "omega":-1.01384, "ax":10.37598, "ay":-5.49576, "alpha":4.66475, "fx":[132.85856,136.52805,137.78618,134.07068], "fy":[-76.24553,-69.47442,-66.90248,-74.05332]}, + {"t":1.13231, "x":0.5671, "y":7.58797, "heading":2.30134, "vx":-2.00067, "vy":1.05968, "omega":-0.90146, "ax":10.37722, "ay":-5.49641, "alpha":4.66759, "fx":[132.83976,136.48735,137.8303,134.1507], "fy":[-76.31396,-69.59373,-66.85502,-73.9473]}, + {"t":1.1564, "x":0.52191, "y":7.6119, "heading":2.27962, "vx":-1.75067, "vy":0.92726, "omega":-0.78901, "ax":10.37834, "ay":-5.49701, "alpha":4.67039, "fx":[132.82433,136.4508,137.86869,134.22276], "fy":[-76.37312,-69.70088,-66.81511,-73.85187]}, + {"t":1.18049, "x":0.48274, "y":7.63265, "heading":2.26062, "vx":-1.50064, "vy":0.79483, "omega":-0.67649, "ax":10.37936, "ay":-5.49755, "alpha":4.67316, "fx":[132.81206,136.41887,137.902,134.28678], "fy":[-76.42385,-69.79559,-66.78202,-73.76767]}, + {"t":1.20458, "x":0.4496, "y":7.6502, "heading":2.24432, "vx":-1.25058, "vy":0.66238, "omega":-0.56391, "ax":10.38029, "ay":-5.49804, "alpha":4.67591, "fx":[132.80276,136.39192,137.93079,134.34274], "fy":[-76.46691,-69.87759,-66.75505,-73.69527]}, + {"t":1.22867, "x":0.42249, "y":7.66456, "heading":2.23073, "vx":-1.0005, "vy":0.52993, "omega":-0.45126, "ax":10.38114, "ay":-5.49849, "alpha":4.67864, "fx":[132.79623,136.37028,137.95556,134.39059], "fy":[-76.50294,-69.9467,-66.73361,-73.63511]}, + {"t":1.25276, "x":0.4014, "y":7.67573, "heading":2.21986, "vx":-0.7504, "vy":0.39746, "omega":-0.33854, "ax":10.38192, "ay":-5.4989, "alpha":4.68136, "fx":[132.79233,136.35418,137.97671,134.43031], "fy":[-76.53248,-70.00274,-66.7172,-73.58759]}, + {"t":1.27686, "x":0.38633, "y":7.68371, "heading":2.2117, "vx":-0.50029, "vy":0.26498, "omega":-0.22576, "ax":10.38265, "ay":-5.49929, "alpha":4.68408, "fx":[132.79092,136.34386,137.99457,134.4619], "fy":[-76.55597,-70.0456,-66.70543,-73.55299]}, + {"t":1.30095, "x":0.37729, "y":7.6885, "heading":2.20627, "vx":-0.25015, "vy":0.1325, "omega":-0.11291, "ax":10.38332, "ay":-5.49964, "alpha":4.68679, "fx":[132.79193,136.33947,138.00941,134.48536], "fy":[-76.57377,-70.07517,-66.69799,-73.53154]}, + {"t":1.32504, "x":0.37428, "y":7.6901, "heading":2.20355, "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/example/part1.traj b/src/main/deploy/choreo/example/part1.traj deleted file mode 100644 index a3349b7..0000000 --- a/src/main/deploy/choreo/example/part1.traj +++ /dev/null @@ -1,282 +0,0 @@ -{ - "name":"part1", - "version":"v2025.0.0", - "snapshot":{ - "waypoints":[ - {"x":1.1957025527954102, "y":1.3570677042007446, "heading":3.141592653589793, "intervals":248, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":4.9078826904296875, "y":3.760383367538452, "heading":0.8090155801802582, "intervals":247, "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":16.54, "h":8.21}}, "enabled":true}], - "targetDt":0.01 - }, - "params":{ - "waypoints":[ - {"x":{"exp":"1.1957025527954102 m", "val":1.1957025527954102}, "y":{"exp":"1.3570677042007446 m", "val":1.3570677042007446}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":248, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"4.9078826904296875 m", "val":4.9078826904296875}, "y":{"exp":"3.760383367538452 m", "val":3.760383367538452}, "heading":{"exp":"0.8090155801802582 rad", "val":0.8090155801802582}, "intervals":247, "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":"16.54 m", "val":16.54}, "h":{"exp":"8.21 m", "val":8.21}}}, "enabled":true}], - "targetDt":{ - "exp":"0.01 s", - "val":0.01 - } - }, - "trajectory":{ - "waypoints":[0.0,1.7853677967348196], - "samples":[ - {"t":0.0, "x":1.1957025527954102, "y":1.3570677042007446, "heading":3.141592653589793, "vx":0.0, "vy":0.0, "omega":0.0, "ax":5.106416746744897, "ay":3.172042833041066, "alpha":-8.02352603829853, "fx":[44.522184148382514,104.61901335765268,110.3393545317284,87.95419911679254], "fy":[101.57974345362996,34.1487435492215,12.187556891062634,67.90612006317765]}, - {"t":0.007199063696511402, "x":1.195834876695402, "y":1.3571499021684053, "heading":3.141592653589793, "vx":0.03676141942074899, "vy":0.022835738403125264, "omega":-0.05776187502032877, "ax":5.107013627954697, "ay":3.1749835994639946, "alpha":-8.003319818064618, "fx":[44.65254354815599,104.54736777368876,110.31980086909932,87.9556500779964], "fy":[101.51297805206394,34.31880122230134,12.29609428157894,67.8943767828564]}, - {"t":0.014398127393022804, "x":1.1962318638625158, "y":1.357396572276229, "heading":3.141176822172292, "vx":0.0735271358273466, "vy":0.045692647571045686, "omega":-0.1153782841741279, "ax":5.107705580086173, "ay":3.178351393049003, "alpha":-7.9800060720953265, "fx":[44.79835037911511,104.45573408587252,110.30068128745728,87.96767614758181], "fy":[101.43906052360296,34.54660591870077,12.397083770564665,67.86864094701643]}, - {"t":0.021597191089534205, "x":1.1968935476946232, "y":1.3578078779995657, "heading":3.1403462065553285, "vx":0.11029783364141366, "vy":0.0685738016995013, "omega":-0.17282685618568985, "ax":5.1084891243921104, "ay":3.182138021705288, "alpha":-7.953633098683377, "fx":[44.95962187718874,104.34398180814732,110.28196275664996,87.99018691585026], "fy":[101.35789451308096,34.831250189322596,12.49088896953737,67.828995367921]}, - {"t":0.028796254786045607, "x":1.1978199664266458, "y":1.3583840047328115, "heading":3.13910201500918, "vx":0.1470741722408482, "vy":0.09148221600884852, "omega":-0.2300855674817927, "ax":5.109359256108743, "ay":3.186335257967981, "alpha":-7.924261006162816, "fx":[45.13637757530929,104.21189754441043,110.2636048092903,88.02307619496017], "fy":[101.26937287174884,35.171794197123226,12.577907804408644,67.77552931815795]}, - {"t":0.03599531848255701, "x":1.199011162910719, "y":1.3591251593639275, "heading":3.1374456143532305, "vx":0.18385677497393513, "vy":0.11442084648940018, "omega":-0.28713282721294026, "ax":5.110309568591665, "ay":3.1909347045818564, "alpha":-7.8919615207263085, "fx":[45.32863952493761,104.05919333416132,110.24555943176422,88.06622200681248], "fy":[101.17337734329077,35.567255194945616,12.658573862802903,67.70833887394082]}, - {"t":0.04319438217906841, "x":1.2004671843204844, "y":1.3600315698436354, "heading":3.135378526840765, "vx":0.22064621906711815, "vy":0.13739258867909382, "omega":-0.3439475608910659, "ax":5.111332387297922, "ay":3.1959276446986857, "alpha":-7.856817814047956, "fx":[45.53643249238126,103.88551568026924,110.22777095024216,88.11948658894062], "fy":[101.06977823597016,36.016595848622295,12.733357836757229,67.62752728475952]}, - {"t":0.05039344587557981, "x":1.202188081786202, "y":1.361103484741937, "heading":3.1329024264416505, "vx":0.25744302649731754, "vy":0.16040027536272142, "omega":-0.4005092927862825, "ax":5.112418915311175, "ay":3.2013048793181675, "alpha":-7.818924325013477, "fx":[45.75978413815724,103.69045537458445,110.2101759056262,88.18271641595592], "fy":[100.95843407540696,36.51871157160267,12.802769076130296,67.53320537223378]}, - {"t":0.057592509572091215, "x":1.2041739099679196, "y":1.3623411727838577, "heading":3.1300191345318376, "vx":0.2942476559118924, "vy":0.18344667310088572, "omega":-0.45679822704025685, "ax":5.113559392096277, "ay":3.2070565549620067, "alpha":-7.778386547457829, "fx":[45.99872518890858,103.47355823644942,110.19270291037064,88.25574223377774], "fy":[100.83919123164736,37.072417060496434,12.86735726994148,67.4254919613025]}, - {"t":0.06479157326860262, "x":1.2064247265745898, "y":1.3637449223557312, "heading":3.126730614998922, "vx":0.3310604956914876, "vy":0.2065344775183717, "omega":-0.512795327251493, "ax":5.114743267090739, "ay":3.21317198493239, "alpha":-7.735320753727778, "fx":[46.253289612491606,103.23433687426169,110.17527247901832,88.33837910341309], "fy":[100.71188351330646,37.676432243976514,12.92771427480693,67.3045143473731]}, - {"t":0.07199063696511401, "x":1.2089405918377425, "y":1.3653150409729398, "heading":3.1230389687747646, "vx":0.36788185826257663, "vy":0.22966630730574605, "omega":-0.5684823940705259, "ax":5.115959389579653, "ay":3.219639467890124, "alpha":-7.6898536221258205, "fx":[46.52351480804537,102.97228356949432,110.15779682313094,88.43042645080754], "fy":[100.5763317205122,38.32936788290607,12.984476111171228,67.17040880328351]}, - {"t":0.07918970066162541, "x":1.2117215679491335, "y":1.367051854699766, "heading":3.118946427809506, "vx":0.4047119757769261, "vy":0.2528446969148892, "omega":-0.6238421401130586, "ax":5.1171962160103215, "ay":3.226446107961525, "alpha":-7.642121734871647, "fx":[46.80944182332643,102.68688436490788,110.14017959976212,88.53166811827663], "fy":[100.43234314752428,39.02971108788458,13.038325151521876,67.02332113120087]}, - {"t":0.0863887643581368, "x":1.2147677184726642, "y":1.3689557075119982, "heading":3.114455348506264, "vx":0.44155099728353153, "vy":0.2760720879594656, "omega":-0.6788582612588937, "ax":5.11844203552547, "ay":3.2335776410264163, "alpha":-7.592270912386265, "fx":[47.11111561274176,102.37763441351942,110.12231560116392,88.64187241281817], "fy":[100.2797110245808,39.77581105022869,13.08999252617816,66.86340726483951]}, - {"t":0.0935878280546482, "x":1.2180791077417816, "y":1.371026960593128, "heading":3.109568204642558, "vx":0.4783989875241808, "vy":0.2993508193648299, "omega":-0.733515503158333, "ax":5.119685213981338, "ay":3.2410182722942142, "alpha":-7.540455349615978, "fx":[47.42858534950281,102.04405461343472,110.10409037140772,88.76079214521758], "fy":[100.118213887585,40.56586530808835,13.140260776568596,66.69083392924378]}, - {"t":0.1007868917511596, "x":1.2216558002545357, "y":1.3732659915554246, "heading":3.1042875798129423, "vx":0.51525592748572, "vy":0.32268311634863334, "omega":-0.7877997215209185, "ax":5.120914456117867, "ay":3.2487505306320292, "alpha":-7.486836520943441, "fx":[47.76190480604287,101.68570951468112,110.08537973359635,88.88816465334436], "fy":[99.9476148627808,41.39790689222714,13.189966788060053,66.50577936615292]}, - {"t":0.107985955447671, "x":1.225497860079401, "y":1.3756731935779067, "heading":3.0986161594376194, "vx":0.5521217168396986, "vy":0.3460710783527286, "omega":-0.8416979345205581, "ax":5.122119084787535, "ay":3.2567551454994876, "alpha":-7.431581821938991, "fx":[48.11113281676175,101.30222643668078,110.06604920874148,89.02371180146726], "fy":[99.76766085230103,42.26979271702951,13.240005038975324,66.30843413521524]}, - {"t":0.1151850191441824, "x":1.229605350285903, "y":1.3782489744542468, "heading":3.092556722393784, "vx":0.5889961783922006, "vy":0.3695166660891208, "omega":-0.8951983654225332, "ax":5.123289335308142, "ay":3.265010952557925, "alpha":-7.374862919213589, "fx":[48.476333837072566,100.89331568175096,110.04595330454768,89.16713994635056], "fy":[99.578081604998,43.17919359325357,13.291331205766038,66.09900200298841]}, - {"t":0.12238408284069378, "x":1.2339783324149007, "y":1.3809937555449705, "heading":3.0861121323400944, "vx":0.6258790646527416, "vy":0.3930216879063927, "omega":-0.9482904733309916, "ax":5.1244166620854035, "ay":3.273494834082312, "alpha":-7.31685378387597, "fx":[48.85757861292113,100.4587916721438,110.02493464892432,89.31813985943184], "fy":[99.37858865514674,44.12358623826613,13.344966168742294,65.87770093396773]}, - {"t":0.12958314653720518, "x":1.23861686600411, "y":1.3839079706299635, "heading":3.0792853288197897, "vx":0.6627700666105587, "vy":0.4165877857271523, "omega":-1.0009649697791752, "ax":5.125494053663116, "ay":3.282181700172889, "alpha":-7.25772838973456, "fx":[49.254944974306305,99.99859477755962,110.00282293903408,89.47638659239398], "fy":[99.16887410993962,45.10024764444499,13.402000467405612,65.64476420101728]}, - {"t":0.1367822102337166, "x":1.243521008184969, "y":1.3869920646592584, "heading":3.072079318244373, "vx":0.6996688247789699, "vy":0.4402164208502212, "omega":-1.0532138187488536, "ax":5.126516351235577, "ay":3.2910445166751576, "alpha":-7.197658063880135, "fx":[49.66851876616987,99.51281352995348,109.97943367202176,89.64153927195957], "fy":[98.9486092645325,46.10625215553035,13.46359925888427,65.40044163518127]}, - {"t":0.143981273930228, "x":1.248690813367266, "y":1.3902464924023257, "heading":3.064497164877154, "vx":0.736574942532722, "vy":0.4639088599538203, "omega":-1.1050302176164355, "ax":5.127480564751214, "ay":3.3000543848934973, "alpha":-7.136808485799559, "fx":[50.09839492952637,99.00170686955715,109.95456661792502,89.81324080715767], "fy":[98.71744302110028,47.13847153712386,13.53100783859996,65.14500103908595]}, - {"t":0.1511803376267394, "x":1.2541263330279775, "y":1.3936717169986748, "heading":3.056541981953963, "vx":0.7734880017209903, "vy":0.4876661616726204, "omega":-1.1564085564955098, "ax":5.128386179749177, "ay":3.309180677469849, "alpha":-7.075336340002381, "fx":[50.54467874636235,98.46572600627267,109.92800398889518,89.99111749062102], "fy":[98.47500008512732,48.19357827563768,13.605557785659192,64.87872979133897]}, - {"t":0.15837940132325082, "x":1.259827615620455, "y":1.3972682084153385, "heading":3.0482169230965614, "vx":0.8104075804893133, "vy":0.5114891641529907, "omega":-1.2073443534814288, "ax":5.129235447028704, "ay":3.3183912341034203, "alpha":-7.013385636540855, "fx":[51.007487259741026,97.90553542364457,109.8995082506982,90.17477847178009], "fy":[98.22087890990146,49.26805228082767,13.6886738034518,64.6019366754484]}, - {"t":0.16557846501976223, "x":1.265794706619417, "y":1.4010364418198402, "heading":3.0395251741922253, "vx":0.847333273186877, "vy":0.5353784740172464, "omega":-1.2578341634070844, "ax":5.130033646859724, "ay":3.3276526188906854, "alpha":-6.95108372451592, "fx":[51.48695088344388,97.3220325339475,109.86881951353408,90.36381507790132], "fy":[97.95464935492798,50.35819103722492,13.78188132808945,64.31495397065757]}, - {"t":0.17277752871627364, "x":1.2720276487161055, "y":1.404976895880384, "heading":3.03046994593021, "vx":0.8842647121758668, "vy":0.5593344571805033, "omega":-1.307875457899658, "ax":5.130789318703672, "ay":3.3369304400584605, "alpha":-6.8885370330191735, "fx":[51.983215213413025,96.7163654601183,109.83565242800452,90.55779995468676], "fy":[97.67585002064168,51.460123171187846,13.886814983527236,64.01813984933221]}, - {"t":0.17997659241278505, "x":1.2785264821764002, "y":1.4090900510082158, "heading":3.021054467201686, "vx":0.9212015912945948, "vy":0.5833572319693121, "omega":-1.357466474776141, "ax":5.131514447231207, "ay":3.3461897311278683, "alpha":-6.825826583046993, "fx":[52.49644305615984,96.08994842207402,109.79969249875602,90.75628599433657], "fy":[97.38398521521412,52.56982527636164,14.005227967811456,63.71188113240566]}, - {"t":0.18717565610929648, "x":1.2852912453726637, "y":1.4133763875603276, "heading":3.011281979583894, "vx":0.9581436906597808, "vy":0.607446664984314, "omega":-1.4066060351288368, "ax":5.132224595660887, "ay":3.355395390380877, "alpha":-6.763003322272624, "fx":[53.02681669027755,95.4444742396967,109.76059171304472,90.958805014698], "fy":[97.07852150273668,53.683141691124064,14.13900245557812,63.396596462051725]}, - {"t":0.19437471980580787, "x":1.2923219754976003, "y":1.4178363840237598, "heading":3.001155733141104, "vx":0.995090902428746, "vy":0.6316023701266468, "omega":-1.4552933268255956, "ax":5.132938977608875, "ay":3.364512674119678, "alpha":-6.7000833397987725, "fx":[53.574540380615105,94.78192349737364,109.71796336247166,91.16486614738756], "fy":[96.75888377315624,54.79580682115818,14.290161107432278,63.07273996060057]}, - {"t":0.20157378350231928, "x":1.2996187094654352, "y":1.4224705152056816, "heading":2.990678983784179, "vx":1.0320432570788585, "vy":0.6558237111753543, "omega":-1.5035276535607416, "ax":5.133680459220748, "ay":3.373507737489895, "alpha":-6.637043021732513, "fx":[54.13984316613876,94.10456998227733,109.6713759139285,91.3739538857495], "fy":[96.42445076507086,55.90346947949507,14.460879778157487,62.740805456483464]}, - {"t":0.20877284719883069, "x":1.307181485002337, "y":1.4272792504560352, "heading":2.979854992436729, "vx":1.0690009497023245, "vy":0.6801098082582182, "omega":-1.5513081490306806, "ax":5.13447548477964, "ay":3.382348214143391, "alpha":-6.573814209436151, "fx":[54.722981951795525,93.41498212826704,109.62034576001496,91.58552573713682], "fy":[96.07454995720772,57.00171855645439,14.653501510299416,62.40133136882373]}, - {"t":0.2159719108953421, "x":1.3150103419242147, "y":1.4322630519517896, "heading":2.9686870262589395, "vx":1.105964365765429, "vy":0.7044595484956181, "omega":-1.5986334562534432, "ax":5.135353919298545, "ay":3.3910038259703996, "alpha":-6.510279418860067, "fx":[55.32424493605597,92.71602026142271,109.56432864687802,91.7990094121556], "fy":[95.70845173114662,58.086109319305166,14.870551903841214,62.05490636085416]}, - {"t":0.2231709745918535, "x":1.3231053235958723, "y":1.4374223730736178, "heading":2.957178362179997, "vx":1.1429341057345888, "vy":0.7288716010338929, "omega":-1.645501372471904, "ax":5.1363488036977945, "ay":3.399447011431808, "alpha":-6.446267174685683, "fx":[55.943955419090706,92.01082961743708,109.50270954185284,92.01379947401148], "fy":[95.32536268518338,59.152189484191545,15.11475593405926,61.702175887281456]}, - {"t":0.23037003828836491, "x":1.331466478561109, "y":1.442757656907084, "heading":2.945332292986875, "vx":1.1799110079399096, "vy":0.7533444366021058, "omega":-1.6919084604671968, "ax":5.137496018624893, "ay":3.40765356150986, "alpha":-6.381547503261803, "fx":[56.582476042725496,91.3028291880853,109.43479065840512,92.22925335382855], "fy":[94.92441795677856,60.19552423640078,15.389056286022797,61.34384978642758]}, - {"t":0.23756910198487632, "x":1.3400938623286989, "y":1.4482693349010982, "heading":2.933152136211305, "vx":1.2168961690185638, "vy":0.7778763516470594, "omega":-1.737849627425492, "ax":5.138833854823165, "ay":3.4156032504907845, "alpha":-6.315827617433434, "fx":[57.240213530902686,90.5956966283431,109.35977730621612,92.4446866213594], "fy":[94.50467237743462,61.21171929407642,15.696633242247508,60.980711091714255]}, - {"t":0.24476816568138773, "x":1.3489875392944155, "y":1.4539578257164685, "heading":2.9206412460485103, "vx":1.253890961265225, "vy":0.8024654970093539, "omega":-1.783317672739581, "ax":5.1404024883100945, "ay":3.423280449368953, "alpha":-6.24874781215776, "fx":[57.91762401128301,89.89334955389468,109.27676117379366,92.65936737499958], "fy":[94.0650902473764,62.196441175217466,16.04092613514537,60.61362627285004]}, - {"t":0.2519672293778991, "x":1.3581475847743611, "y":1.4598235342967876, "heading":2.907803028531344, "vx":1.290897046204275, "vy":0.8271099110153832, "omega":-1.828302806262741, "ax":5.142243359234514, "ay":3.4306747093270236, "alpha":-6.1798775749235535, "fx":[58.61521902031808,89.19992367633957,109.18470058282443,92.8725095853096], "fy":[93.6045334675951,63.14543386548982,16.42565632496781,60.24355715735314]}, - {"t":0.2591662930744105, "x":1.3675740871199067, "y":1.4658668511926736, "heading":2.894640960172548, "vx":1.3279163836903671, "vy":0.8518075567698393, "omega":-1.872792138561258, "ax":5.144398453666005, "ay":3.437781302078612, "alpha":-6.108711899411273, "fx":[59.33357231917749,88.51974833956575,109.08239717554382,93.0832651891176], "fy":[93.12174770468803,64.05453112306529,16.854851593480888,59.871574831494335]}, - {"t":0.26636535677092193, "x":1.3772671498795377, "y":1.4720881521684783, "heading":2.8811586102767195, "vx":1.3649512358385434, "vy":0.8765563633381792, "omega":-1.9167691446287571, "ax":5.146909486566216, "ay":3.4446017042863515, "alpha":-6.03466777546301, "fx":[60.07332767440245,87.85732006747861,108.96846840713977,93.29071467903708], "fy":[92.6153461879158,64.91966378174656,17.332871756315793,59.498875887014634]}, - {"t":0.27356442046743334, "x":1.3872268939678198, "y":1.4784877981180309, "heading":2.8673596671130297, "vx":1.4020041650725124, "vy":0.9013542704164484, "omega":-1.9602131023316, "ax":5.14981698265033, "ay":3.451144012940829, "alpha":-5.957080815341673, "fx":[60.83520779028395,87.21727475620747,108.8413151173698,93.49385587013056], "fy":[92.08379064090582,65.73686153817556,17.864435169163457,59.126801457926376]}, - {"t":0.28076348416394475, "x":1.3974534597964992, "y":1.4850661353127186, "heading":2.853247968130608, "vx":1.439078025555988, "vy":0.9261992759914436, "omega":-2.003098506566511, "ax":5.153159250675849, "ay":3.457423276068514, "alpha":-5.875201964762424, "fx":[61.620024627215145,86.604359204636,108.69908335136054,93.69159044201064], "fy":[91.52536872190886,66.50224874559817,18.454645623184906,58.756859592070334]}, - {"t":0.28796254786045616, "x":1.4079470093173316, "y":1.4918234960012533, "heading":2.838827534391449, "vx":1.4761759472398703, "vy":0.9510894863816618, "omega":-2.045394459740705, "ax":5.156971242802331, "ay":3.4634617223482613, "alpha":-5.788194240127392, "fx":[62.4286913854696,86.02340258671553,108.5396194898576,93.8827077446404], "fy":[90.93816718966058,67.21203393618973,19.109018899675903,58.390751631108266]}, - {"t":0.2951616115569676, "x":1.4187077279205142, "y":1.4987601993756297, "heading":2.824102609391284, "vx":1.513301311697882, "vy":0.9760231679312762, "omega":-2.0870640387631623, "ax":5.161283287498054, "ay":3.4692888688772587, "alpha":-5.695129429743568, "fx":[63.262236492535614,85.47928844619952,108.36041764389668,94.06586521001324], "fy":[90.32003980702976,67.86249284455711,19.833507946173302,58.03040343953811]}, - {"t":0.302360675253479, "x":1.429735826126317, "y":1.5058765529120277, "heading":2.8090777024375297, "vx":1.5504577188403204, "vy":1.0009987994799217, "omega":-2.128063638287763, "ax":5.166119680071142, "ay":3.474941480828361, "alpha":-5.5949846978493385, "fx":[64.12182000996084,84.97692769520886,108.15855818014892,94.23956452274793], "fy":[89.66856872404392,68.44994480596426,20.634525224044385,57.67800253098431]}, - {"t":0.3095597389499904, "x":1.441031541000566, "y":1.5131728540884024, "heading":2.7937576367552666, "vx":1.5876489434809535, "vy":1.0260151245420546, "omega":-2.168342289508587, "ax":5.171497109559511, "ay":3.480463349217142, "alpha":-5.4866390360626385, "fx":[65.0087529526603,84.52123297251559,107.93063618686703,94.40212244394428], "fy":[88.98101774021596,68.97072247917879,21.51896025277701,57.33604241825937]}, - {"t":0.3167588026465018, "x":1.4525951372166774, "y":1.5206493924719584, "heading":2.778147602497255, "vx":1.624878880578997, "vy":1.051071201886442, "omega":-2.207840953408968, "ax":5.17742289493756, "ay":3.4859048424475807, "alpha":-5.368869515455766, "fx":[65.92452012023195,84.11709455571508,107.67267869667278,94.55163483842848], "fy":[88.2542753867611,69.42113490888298,22.494189696532416,57.00737586986475]}, - {"t":0.3239578663430132, "x":1.4644269076777865, "y":1.528306452159125, "heading":2.7622532148418975, "vx":1.662151477783429, "vy":1.0761664528872, "omega":-2.246491787028993, "ax":5.183892998365594, "ay":3.491322171704273, "alpha":-5.240347302407662, "fx":[66.87080715890885,83.76935789695175,107.38004959871898,94.68593198867885], "fy":[87.48478516632785,69.79742390812204,23.56807644873994,56.69527922134335]}, - {"t":0.3311569300395246, "x":1.4765271736017709, "y":1.5361443145373823, "heading":2.746080577373386, "vx":1.6994706536745623, "vy":1.1013007035863416, "omega":-2.2842173810508672, "ax":5.19088977550939, "ay":3.4967762936325677, "alpha":-5.099633408727377, "fx":[67.84953269872958,83.48280257565284,107.04734144712864,94.80252263079974], "fy":[86.66845949359087,70.09571384177946,24.748953105299297,56.4035305176222]}, - {"t":0.33835599373603603, "x":1.488896283959395, "y":1.544163261321461, "heading":2.729636350950522, "vx":1.7368401998100242, "vy":1.1264742188566534, "omega":-2.320929966789153, "ax":5.198379418010661, "ay":3.5023313480633664, "alpha":-4.945174141761229, "fx":[68.86288657085859,83.2621223081337,106.66825391393289,94.89852326327625], "fy":[85.80057279855939,70.31195479146457,26.04558383707709,56.13650507690254]}, - {"t":0.34555505743254744, "x":1.5015346141410049, "y":1.552363577795001, "heading":2.712927828284465, "vx":1.774263664358917, "vy":1.1516877253176503, "omega":-2.3565305904260336, "ax":5.2063090355780455, "ay":3.5080525010791974, "alpha":-4.7752961910995655, "fx":[69.91337526135054,83.11190533007436,106.23545953921082,94.97056802940328], "fy":[84.87562780700183,70.4418591437699,27.467097081029664,55.89929317553973]}, - {"t":0.35275412112905885, "x":1.5144425637098804, "y":1.560745556161286, "heading":2.6959630144612103, "vx":1.811744214729766, "vy":1.1769424187236257, "omega":-2.3909082518754676, "ax":5.2146033190856285, "ay":3.5140030272047933, "alpha":-4.588201213043713, "fx":[71.00387590495527,83.03661420839879,105.74045888817145,95.01469271556194], "fy":[83.88718703452173,70.4808315842321,29.022879544535613,55.69784603126002]}, - {"t":0.35995318482557026, "x":1.5276205530801732, "y":1.5693094988715617, "heading":2.6787507136634443, "vx":1.8492844761759033, "vy":1.202239950346207, "omega":-2.42393900466058, "ax":5.223160716350763, "ay":3.5202404253752664, "alpha":-4.381959611688249, "fx":[72.13770021240971,83.04056373316268,105.17342941119496,95.02618387629867], "fy":[82.82765882336008,70.42389255904938,30.72241995864669,55.53915828631014]}, - {"t":0.36715224852208167, "x":1.5410690189338525, "y":1.578055721754667, "heading":2.6613006223724347, "vx":1.8868863428700284, "vy":1.227582385395518, "omega":-2.4554850110206647, "ax":5.231849040138755, "ay":3.5268113154405727, "alpha":-4.154502911358955, "fx":[73.31866971635188,83.127895150321,104.52307552040249,94.99938045273905], "fy":[81.68802348320591,70.26559529980965,32.57508877568444,55.43149790832602]}, - {"t":0.3743513122185931, "x":1.5547884081637555, "y":1.5869845567150935, "heading":2.643623429372268, "vx":1.9245507573605192, "vy":1.2529721247009518, "omega":-2.48539354210688, "ax":5.24050040108299, "ay":3.5337448167747234, "alpha":-3.9036135697320207, "fx":[74.55120340495836,83.30254439033598,103.77649188212123,94.927409859562], "fy":[80.45747989058893,69.99993678668866,34.589838080522355,55.38469820460943]}, - {"t":0.3815503759151045, "x":1.5687791700976248, "y":1.596096353695634, "heading":2.6257309229517425, "vx":1.962277453549509, "vy":1.27841177872413, "omega":-2.5134959048419474, "ax":5.2489053092075, "ay":3.5410440626005606, "alpha":-3.626910151657287, "fx":[75.8404180159523,83.56820128051896,102.91905804037106,94.80183252950953], "fy":[79.12298555398434,69.62026340124734,36.7748045836924,55.41053175548842]}, - {"t":0.3887494396116159, "x":1.583041746718979, "y":1.6053914815131527, "heading":2.6076361058318644, "vx":2.000064657207451, "vy":1.303903980482945, "omega":-2.5396062620452517, "ax":5.256805686720443, "ay":3.5486754655308053, "alpha":-3.32182424455563, "fx":[77.19223941355241,83.92825584157261,101.93439044159572,94.61215681362974], "fy":[77.66865283707673,69.119172739975,39.136798529711236,55.523193108882694]}, - {"t":0.3959485033081273, "x":1.5975765605509091, "y":1.6148703270694182, "heading":2.589353318587342, "vx":2.037908736186335, "vy":1.3294511211975486, "omega":-2.563520286370424, "ax":5.263886336076762, "ay":3.556555325988016, "alpha":-2.9855630126276353, "fx":[78.61351972634877,84.38572659554522,100.8043878258015,94.34516764105568], "fy":[76.07494942869127,68.48841430899748,41.68066359054402,55.73992657442121]}, - {"t":0.4031475670046387, "x":1.612383999800645, "y":1.6245332923118174, "heading":2.570898372758462, "vx":2.075803789210948, "vy":1.3550549895295032, "omega":-2.585013544668279, "ax":5.2697640236810575, "ay":3.56453337388019, "alpha":-2.6150473713475715, "fx":[80.1121475198806,84.94316426811166,99.50941754896743,93.98398358937432], "fy":[74.31763060103196,67.71879389567017,44.4084985611159,56.08184809254364]}, - {"t":0.4103466307011502, "x":1.627464400260929, "y":1.6343807881704373, "heading":2.5522886955940502, "vx":2.113741156083012, "vy":1.3807162923364074, "omega":-2.603839437264005, "ax":5.273972621265032, "ay":3.5723728518893423, "alpha":-2.2068097180203603, "fx":[81.69712408207971,85.60252222538031,98.02870301292442,93.50671176882328], "fy":[72.36630211005301,66.8000898145278,47.31874218234668,56.57502615489436]}, - {"t":0.41754569439766154, "x":1.642818023300283, "y":1.6444132245289238, "heading":2.5335434896296984, "vx":2.151708820917156, "vy":1.406434032044847, "omega":-2.619726400990114, "ax":5.275941433078356, "ay":3.579726763012638, "alpha":-1.756826029088972, "fx":[83.37855190789334,86.36498223291176,96.34098474067127,92.88449791020474], "fy":[70.18247356099675,65.72099482220528,50.40513859251461,57.25190498238295]}, - {"t":0.42474475809417295, "x":1.658445028995516, "y":1.6546309950975056, "heading":2.514683912401538, "vx":2.1896906593529506, "vy":1.4322047130278817, "omega":-2.6323739034772147, "ax":5.274961483032121, "ay":3.586109851594113, "alpha":-1.2602437348850846, "fx":[85.16743026263916,87.23072017975164,94.42553806384464,92.0786536058527], "fy":[67.71691207661905,64.46910700693554,53.6556225701892,58.15316834599446]}, - {"t":0.4319438217906843, "x":1.6743454429712592, "y":1.665034455846615, "heading":2.495733284997371, "vx":2.227665443065943, "vy":1.4580213462721947, "omega":-2.641446478397782, "ax":5.270130439433453, "ay":3.5908655759664017, "alpha":-0.7109479350467482, "fx":[87.07505597782028,88.19859093295048,92.26363824509131,91.03635827890214], "fy":[64.9060507200043,63.03100885756134,57.05118722561002,59.33013723992661]}, - {"t":0.43914288548719577, "x":1.6905191146457292, "y":1.675623895419197, "heading":2.4767173435484597, "vx":2.265605447788348, "vy":1.483872216279187, "omega":-2.6465646378670873, "ax":5.260259923248579, "ay":3.5931264216545986, "alpha":-0.10088413532183564, "fx":[89.11164511769853,89.26570278640179,89.84057160469386,89.68414530155698], "fy":[61.66717183726748,61.39250207166611,60.564814247190846,60.84772124006487]}, - {"t":0.4463419491837072, "x":1.7069656630535992, "y":1.68639949563745, "heading":2.45766455614352, "vx":2.303474394036021, "vy":1.5097393622582964, "omega":-2.6472909091832366, "ax":5.2437178954835755, "ay":3.5917636336529006, "alpha":0.5809691797937347, "fx":[91.28345107399836,90.42683791629032,87.14831232660111,87.91796285668127], "fy":[57.89215806271162,59.53911697444922,64.16053948617602,62.78767233692774]}, - {"t":0.4535410128802186, "x":1.7236844037597752, "y":1.697361279772975, "heading":2.4386065402651145, "vx":2.341224253172144, "vy":1.535596697439777, "omega":-2.643108475052191, "ax":5.218163356067048, "ay":3.5853174908277707, "alpha":1.350086578583692, "fx":[93.58703945683662,91.67364950698816,84.18905630270748,85.5881172923087], "fy":[53.440036513209925,57.457114197552194,67.79266141884725,65.25108655044389]}, - {"t":0.46074007657673, "x":1.7406742459050064, "y":1.7085090454709244, "heading":2.4195786339964247, "vx":2.378790143551272, "vy":1.5614076264284626, "omega":-2.6333891157771623, "ax":5.180117405425816, "ay":3.571889482383564, "alpha":2.2283410055355137, "fx":[95.99835277039467,92.99351535402418,80.98001845782959,82.4773730385535], "fy":[48.13003975925512,55.13539614611517,71.40491326882285,68.35692317967191]}, - {"t":0.4679391402732414, "x":1.7579335413933184, "y":1.7198422777272682, "heading":2.400620698014245, "vx":2.41608213870834, "vy":1.587121886329041, "omega":-2.6173471469407636, "ax":5.124340810850572, "ay":3.5489623180717356, "alpha":3.245903322279656, "fx":[98.45284481190814,94.36782013909348,77.56047179918056,78.27314721203267], "fy":[41.74134690552876,52.56917260248968,74.92898914357544,72.22782568263396]}, - {"t":0.4751382039697528, "x":1.7754598589767916, "y":1.7313600344609965, "heading":2.381778249187536, "vx":2.4529725946082865, "vy":1.6126710921133582, "omega":-2.5939796821709544, "ax":5.043201440778478, "ay":3.5131097678843477, "alpha":4.440795456830741, "fx":[100.81274707144335,95.7692055640339,74.00338971647982,72.5483117345616], "fy":[34.03765515987915,49.767170305756075,78.28090506826432,76.94223731882212]}, - {"t":0.4823372676662642, "x":1.7932496507165658, "y":1.7430607924981407, "heading":2.3631040242281314, "vx":2.4892789230147887, "vy":1.637962193105194, "omega":-2.562010112814051, "ax":4.927091739442584, "ay":3.459691941791851, "alpha":5.848379572208808, "fx":[102.82543243827496,97.15688245739588,70.43695884562639,64.81440915388042], "fy":[24.863179634851097,46.766187037991216,81.35196685219057,82.41214657705729]}, - {"t":0.4895363313627756, "x":1.811297805246356, "y":1.7549422385523146, "heading":2.3446599502348766, "vx":2.524749370285591, "vy":1.6628687357644607, "omega":-2.519907255752343, "ax":4.769443651641332, "ay":3.3834631876688683, "alpha":7.461007839292969, "fx":[104.11483447405668,98.46881220720142,67.08448555714666,54.83935519101212], "fy":[14.403484407687406,43.65972971873427,83.98957969161077,88.15416909733908]}, - {"t":0.49673539505928704, "x":1.8295972286096536, "y":1.7670010130580995, "heading":2.326518977391414, "vx":2.559084898930679, "vy":1.6872265027672904, "omega":-2.4661949850771023, "ax":4.582312277281416, "ay":3.283439231883081, "alpha":9.135776331365095, "fx":[104.3395939655318,99.61376613382082,64.31448449089112,43.50743829958246], "fy":[3.6641958228149822,40.64082878984899,85.97440848496623,93.12201434349376]}, - {"t":0.5039344587557985, "x":1.8481389864469395, "y":1.779232548733365, "heading":2.308764682605827, "vx":2.5920732568921343, "vy":1.7108641909412412, "omega":-2.4004259493505233, "ax":4.411876895906919, "ay":3.1731218685288907, "alpha":10.542815065137578, "fx":[103.6056370470367,100.4915669953221,62.57007622144833,33.511774340592254], "fy":[-5.399942550818262,37.989142207532225,87.07458492911523,96.23179571089796]}, - {"t":0.5111335224523098, "x":1.8669138130382443, "y":1.7916313949490177, "heading":2.2914838632976937, "vx":2.623834639686935, "vy":1.7337076973895738, "omega":-2.324527552156058, "ax":4.302843791374827, "ay":3.0741167763596304, "alpha":11.43607961787517, "fx":[102.51701686798012,101.08204212165909,61.95933121400069,27.202176756784105], "fy":[-11.475950234632458,35.848065356664335,87.2969979310499,97.49027408377694]}, - {"t":0.5183325861488212, "x":1.8859144664442948, "y":1.8041921274780428, "heading":2.274749441385427, "vx":2.6548110862171814, "vy":1.755838459873101, "omega":-2.242198486548599, "ax":4.248065161281089, "ay":2.990813955998283, "alpha":11.923609585962035, "fx":[101.45949695774654,101.47417707766328,62.0832634110459,24.01655421653248], "fy":[-15.261860509165,34.07031803942095,86.95896329493351,97.72413775436104]}, - {"t":0.5255316498453325, "x":1.905136701769176, "y":1.8169100221282757, "heading":2.258607711660542, "vx":2.6853931779001745, "vy":1.7773695200467476, "omega":-2.156359661646924, "ax":4.2189141637187415, "ay":2.9150923425792263, "alpha":12.219992214981378, "fx":[100.43331447758968,101.75890641243176,62.515541906841015,22.342328355300307], "fy":[-18.004828692578567,32.448473746841316,86.3647521710179,97.53114944062386]}, - {"t":0.5327307135418439, "x":1.924578344122704, "y":1.8297809580583673, "heading":2.243083941103758, "vx":2.7157654096949, "vy":1.798355455502188, "omega":-2.0683871593204, "ax":4.1971242029989515, "ay":2.840527499143566, "alpha":12.45073971815986, "fx":[99.35500053642492,101.98017766437972,63.03463130688907,21.197717655704597], "fy":[-20.42313163812582,30.866333764425896,85.66843448928044,97.15460344242491]}, - {"t":0.5399297772383552, "x":1.9442380734586309, "y":1.8428010408564297, "heading":2.2281934901947644, "vx":2.7459807741744595, "vy":1.8188045939002149, "omega":-1.978753491020683, "ax":4.174355031285051, "ay":2.764218210366834, "alpha":12.66890245294028, "fx":[98.16397868997456,102.15168017613853,63.54790810341725,20.1547718097712], "fy":[-22.812636923804178,29.27740701010607,84.93301737012665,96.676455929189]}, - {"t":0.5471288409348666, "x":1.9641147351046144, "y":1.8559663608819896, "heading":2.213948317773212, "vx":2.7760322219365334, "vy":1.8387043768677025, "omega":-1.8875492552970763, "ax":4.146119614101985, "ay":2.684978107203362, "alpha":12.899013438671998, "fx":[96.81427847439188,102.27456244364978,64.00796438039147,19.00042801116752], "fy":[-25.297712943552767,27.665339619228067,84.18979712262993,96.1254136583678]}, - {"t":0.5543279046313779, "x":1.9842070073655296, "y":1.8692728873433588, "heading":2.200359730454026, "vx":2.805880401131809, "vy":1.8580337052851976, "omega":-1.7946884359299204, "ax":4.108251716099294, "ay":2.6021375270113927, "alpha":13.16056182717134, "fx":[95.2534634248568,102.34531603261784,64.37190037957522,17.550065032257006], "fy":[-27.96283096835676,26.02407392323607,83.46785222714254,95.51736400943363]}, - {"t":0.5615269683278893, "x":2.004513177289045, "y":1.8827164202018016, "heading":2.187439654088374, "vx":2.83545596691731, "vy":1.8767666590892351, "omega":-1.6999447130542376, "ax":4.055613899299726, "ay":2.515240444605161, "alpha":13.475292157474332, "fx":[93.41061915821506,102.35722813403062,64.58691714380679,15.58456362219319], "fy":[-30.888885373077898,24.354614368679258,82.80489738036655,94.86345478227838]}, - {"t":0.5687260320244006, "x":2.025030899577129, "y":1.896292561001296, "heading":2.1752016438185486, "vx":2.8646525897068256, "vy":1.8948740352619895, "omega":-1.6029352264834795, "ax":3.9841172086003502, "ay":2.425176482012565, "alpha":13.853285653979803, "fx":[91.22450853736903,102.29549562999058,64.60412915464853,12.950641729014444], "fy":[-34.06610016634207,22.68516121857258,82.23531078762016,94.1518603818006]}, - {"t":0.575925095720912, "x":2.0457569575001293, "y":1.9099967241044409, "heading":2.1636620110217124, "vx":2.8933345032659066, "vy":1.9123330352312793, "omega":-1.5032045406545114, "ax":3.8980370170597856, "ay":2.337375322800256, "alpha":14.250274152401298, "fx":[88.7632886656818,102.12730209492427,64.43033749048116,9.897049086294528], "fy":[-37.1536577989405,21.118447609858062,81.7452443165894,93.3223077097655]}, - {"t":0.5831241594174233, "x":2.066687267727477, "y":1.9238243004462583, "heading":2.152840345784655, "vx":2.921396720043079, "vy":1.929159949062772, "omega":-1.4006159093386243, "ax":3.814402580091031, "ay":2.262680181251442, "alpha":14.551556290503532, "fx":[86.32425395948196,101.80517587024087,64.15855317067941,7.239602965238621], "fy":[-39.4285989659945,19.830868198671023,81.2438704521448,92.30403021005934]}, - {"t":0.5903232231139347, "x":2.087817432400038, "y":1.9377710792180112, "heading":2.1427572226389793, "vx":2.948856847181292, "vy":1.945449127812435, "omega":-1.295858328719918, "ax":3.748339097587909, "ay":2.2085606001175093, "alpha":14.66676421030158, "fx":[84.19384780691534,101.29752201369018,63.87800768953573,5.6633247156428705], "fy":[-40.378884211326515,18.930105862975427,80.63548329293978,91.08123058979976]}, - {"t":0.5975222868104461, "x":2.109143572356845, "y":1.951833722410421, "heading":2.13342825598887, "vx":2.975841379100951, "vy":1.9613486962502864, "omega":-1.1902713589482434, "ax":3.6980915990131273, "ay":2.1717573240541364, "alpha":14.608607748884504, "fx":[82.34093159793753,100.60142543703324,63.58364659946071,5.087916296586627], "fy":[-40.14094513990124,18.35859121544702,79.89419931866244,89.65203735817785]}, - {"t":0.6047213505069574, "x":2.1306626736013143, "y":1.9660098739159355, "heading":2.1248594166596684, "vx":3.0024641760777806, "vy":1.9769833155595171, "omega":-1.085103061246674, "ax":3.6527273960314943, "ay":2.143807701812154, "alpha":14.43744198367208, "fx":[80.53283432950647,99.71729891097618,63.21686196533988,5.060396273655559], "fy":[-39.1349424776582,17.987106911245863,79.02987876664903,87.9801792431475]}, - {"t":0.6119204142034688, "x":2.152372258522656, "y":1.980297855775926, "heading":2.1170476906044744, "vx":3.0287603932678033, "vy":1.9924167237579344, "omega":-0.9811669967915312, "ax":3.602670805940917, "ay":2.1174577239669508, "alpha":14.197965345774994, "fx":[78.54411210494739,98.62727980295976,62.72164219140532,5.228564280170134], "fy":[-37.679951045614544,17.710231406436613,78.03794469313996,86.00117505438419]}, - {"t":0.6191194778999801, "x":2.1742698544571395, "y":1.9946962609107897, "heading":2.109984206897657, "vx":3.054696249877334, "vy":2.0076604367874427, "omega":-0.8789549399064356, "ax":3.5415324387787472, "ay":2.0876738819986516, "alpha":13.908230549477665, "fx":[76.2032746879847,97.28886517928906,62.052332316894145,5.41734166646188], "fy":[-35.93160042731979,17.46073263913216,76.88578477336014,83.62802460325778]}, - {"t":0.6263185415964915, "x":2.1963525799810326, "y":2.009203634710311, "heading":2.1036565542989076, "vx":3.0801919674873637, "vy":2.022689734041494, "omega":-0.7788287022749802, "ax":3.464406349375105, "ay":2.050493783972834, "alpha":13.567745284111586, "fx":[73.36341658719167,95.63094366443362,61.16117895937361,5.558703787416363], "fy":[-33.93744860151095,17.19313890555485,75.5123552530443,80.74520471428767]}, - {"t":0.6335176052930028, "x":2.2186168522116536, "y":2.023818241920566, "heading":2.0980497168625587, "vx":3.1051324494671135, "vy":2.0374513694016154, "omega":-0.6811536397566188, "ax":3.3661863669953087, "ay":2.001864979854047, "alpha":13.163681904514767, "fx":[69.8707045284593,93.54429134648204,59.98398129344877,5.6324906416736775], "fy":[-31.684522566391195,16.868064597093188,73.82340201142206,77.19765805267586]}, - {"t":0.6407166689895142, "x":2.241058127360821, "y":2.0385378589532457, "heading":2.0931460484228404, "vx":3.1293658395374413, "vy":2.0518629229034, "omega":-0.5863874552454026, "ax":3.2403323431639985, "ay":1.9367289559021748, "alpha":12.672239024201726, "fx":[65.54535418441893,90.86274024545132,58.42418358060538,5.636226058036075], "fy":[-29.123573666938817,16.441136899587764,71.67738892929057,72.7778694113544]}, - {"t":0.6479157326860255, "x":2.263670598940765, "y":2.0533595377908873, "heading":2.0889246077816934, "vx":3.152693198473745, "vy":2.065805558019818, "omega":-0.49515919933275687, "ax":3.0777821194694845, "ay":1.848147080255495, "alpha":12.056352919149596, "fx":[60.177392233454334,87.33055747237206,56.330280778900665,5.570542402340924], "fy":[-26.182792753380184,15.853185243177515,68.85916037830991,67.21625926814329]}, - {"t":0.6551147963825369, "x":2.2864467934575066, "y":2.0682792951017386, "heading":2.085359925165783, "vx":3.1748503479957897, "vy":2.079110486571099, "omega":-0.4083647467201777, "ax":2.8658686757154856, "ay":1.7263863962454369, "alpha":11.26095514400886, "fx":[53.55026512021362,82.55061726020294,53.46044700267223,5.429095325893485], "fy":[-22.787518665073726,15.018418669791366,65.03747743030179,60.192977116289605]}, - {"t":0.6623138600790482, "x":2.309377007337026, "y":2.0832916802245607, "heading":2.082420081342735, "vx":3.1954819191381025, "vy":2.0915388522024605, "omega":-0.32729641335490006, "ax":2.5876997151485397, "ay":1.5584118602182022, "alpha":10.20834899321342, "fx":[45.526986849012026,75.91754208751267,49.432987715822854,5.186610344035066], "fy":[-18.90827909499585,13.8096792603464,59.71001367384104,51.42114552768086]}, - {"t":0.6695129237755596, "x":2.3324485409470226, "y":2.09838918517554, "heading":2.0800638536153535, "vx":3.2141109342149017, "vy":2.10275795844957, "omega":-0.2538058587165383, "ax":2.224227288980249, "ay":1.3298113228574295, "alpha":8.80416406884431, "fx":[36.24735058488498,66.60287071724242,43.697576834499166,4.7860809774573205], "fy":[-14.66105992715373,12.051670889537382,52.19049358929202,40.89773588648482]}, - {"t":0.676711987472071, "x":2.3556447672680223, "y":2.113561533402066, "heading":2.0782366890719053, "vx":3.2301232881437896, "vy":2.112331354867163, "omega":-0.19042412079039073, "ax":1.7662229891685175, "ay":1.03466107898593, "alpha":6.9887539935217164, "fx":[26.407392850977462,53.89521044103628,35.71798405858604,4.15120339021504], "fy":[-10.428934863603086,9.582395099613365,41.88654889737378,29.35714651121371]}, - {"t":0.6839110511685823, "x":2.3789443991608183, "y":2.1287951528144644, "heading":2.0768658136969833, "vx":3.2428384399450563, "vy":2.119779945879084, "omega":-0.1401116356317789, "ax":1.2478746310513291, "ay":0.6992061430801976, "alpha":4.89110420487139, "fx":[17.31811289516684,38.49359137563193,25.81158526670788,3.28067216671055], "fy":[-6.813033131880598,6.5212077413420335,29.368296488749586,18.49671463553486]}, - {"t":0.6911101148650937, "x":2.4023221361460587, "y":2.144073702377353, "heading":2.0758571411074476, "vx":3.2518219688992556, "vy":2.1248135754401103, "omega":-0.10490026491463444, "ax":0.7722031418341744, "ay":0.394804872367738, "alpha":2.950736375248133, "fx":[10.298380918880328,23.756729154207665,16.12451008769635,2.3601978231170455], "fy":[-4.244890962846406,3.5615456744941967,17.41702032181074,10.128396628265934]}, - {"t":0.698309178561605, "x":2.4257522199299357, "y":2.1593806013310917, "heading":2.0751019574185463, "vx":3.2573811085039663, "vy":2.1276558008639785, "omega":-0.08365772579760943, "ax":0.4343946564195741, "ay":0.18316131109414643, "alpha":1.5804012191835477, "fx":[5.765260047205512,13.12563121531548,9.038627630307534,1.6261963652750204], "fy":[-2.6821162227034354,1.4435290179209448,8.87528494977158,4.825388233736088]}, - {"t":0.7055082422581164, "x":2.4492135705951315, "y":2.174702477272266, "heading":2.074499700121824, "vx":3.2605083433049553, "vy":2.128974390809282, "omega":-0.07228031675466244, "ax":0.2369844372565268, "ay":0.06360225632228621, "alpha":0.7915471962898979, "fx":[3.1998104048989706,6.913244346094138,4.8654985676664975,1.1455965635860397], "fy":[-1.80902613815947,0.24620931587643835,3.9693386572861233,1.9209028923829008]}, - {"t":0.7127073059546277, "x":2.4726923188807053, "y":2.1900307476616887, "heading":2.0739793495175034, "vx":3.262214409363847, "vy":2.129432267503788, "omega":-0.06658191806977633, "ax":0.1325866559453279, "ay":0.004032670777608589, "alpha":0.383555184582078, "fx":[1.848829647247742,3.653552986829855,2.6614304403921043,0.8572312506226792], "fy":[-1.329668455256673,-0.33644863595216995,1.4679238491252733,0.4725715464003533]}, - {"t":0.7199063696511391, "x":2.496180643957756, "y":2.2053607706924976, "heading":2.0735000220482833, "vx":3.2631689091453047, "vy":2.1294612989575827, "omega":-0.06382067986484279, "ax":0.0793933282518158, "ay":-0.023067777021238963, "alpha":0.18255007761949496, "fx":[1.1561585939868508,2.0162768432647225,1.5442619941193785,0.6851337572184105], "fy":[-1.0573031564541924,-0.5853254189848401,0.27359757137563,-0.20047414339089567]}, - {"t":0.7271054333476504, "x":2.51967446212705, "y":2.2206903004616674, "heading":2.0730405729087815, "vx":3.263740466772467, "vy":2.12929523256147, "omega":-0.06250649022825701, "ax":0.05235207721305281, "ay":-0.03400417192775523, "alpha":0.0858814862645253, "fx":[0.7989528647042635,1.2037706004065931,0.9817366839972976,0.5775152675155882], "fy":[-0.8911442613769627,-0.6692724165538412,-0.265134735430324,-0.48805352682856706]}, - {"t":0.7343044970441618, "x":2.543171694249166, "y":2.2360183513106393, "heading":2.072590584704183, "vx":3.264117352710969, "vy":2.129050434361815, "omega":-0.06188822393828771, "ax":0.03825303608852343, "ay":-0.03730676671845904, "alpha":0.03998238492302549, "fx":[0.6080511583232394,0.796525873268164,0.6931267798053007,0.504988983466626], "fy":[-0.7801809872518245,-0.676918211379335,-0.4887396663741723,-0.5924708449241117]}, - {"t":0.7415035607406731, "x":2.5666712742450546, "y":2.2513445542607853, "heading":2.072145047437987, "vx":3.264392738754355, "vy":2.1287818605718978, "omega":-0.06160038820248854, "ax":0.030501370426052957, "ay":-0.037193691000965166, "alpha":0.01832538930758833, "fx":[0.4992135538747032,0.5857020103050946,0.5382864691472051,0.4520763016431877], "fy":[-0.6993472738811269,-0.6520445129135423,-0.5657590730288838,-0.6134653077027666]}, - {"t":0.7487026244371845, "x":2.59017263589169, "y":2.26666882666127, "heading":2.071701582319588, "vx":3.2646123200628834, "vy":2.128514100821273, "omega":-0.06146846255759999, "ax":0.025891950058102613, "ay":-0.03568290317843133, "alpha":0.008115373212038871, "fx":[0.4315990618449681,0.4700908219007183,0.4490820387483728,0.41088672612240096], "fy":[-0.6364034727753276,-0.6154949820572007,-0.5772459595804621,-0.59867947876479]}, - {"t":0.7559016881336958, "x":2.6136754588730473, "y":2.281991210591691, "heading":2.071259066942309, "vx":3.264798717860579, "vy":2.1282572173284153, "omega":-0.06141003946892572, "ax":0.02289049731733152, "ay":-0.033713345855427984, "alpha":0.0032865273946469627, "fx":[0.3856375416070644,0.401447151911259,0.3929304718648144,0.377428073913919], "fy":[-0.5852749388280467,-0.5768480249933239,-0.56132594240915,-0.570368560848468]}, - {"t":0.7631007518302072, "x":2.6371795459666014, "y":2.2973117962391334, "heading":2.070816972156567, "vx":3.264963508008811, "vy":2.1280145128041794, "omega":-0.06138637954887149, "ax":0.020765817092197835, "ay":-0.031709717808255086, "alpha":0.0009924616228369435, "fx":[0.35193669890249357,0.3569463474416202,0.3543599113047434,0.3496394708266217], "fy":[-0.5428295615511587,-0.5403203945015946,-0.5356065240192922,-0.5387364278295987]}, - {"t":0.7702998155267186, "x":2.66068476433754, "y":2.3126306865617794, "heading":2.0703750477000966, "vx":3.265113002448768, "vy":2.127786232525879, "omega":-0.0613792347544325, "ax":0.01916638639608163, "ay":-0.02985738544267857, "alpha":-0.0001000225755939402, "fx":[0.325923345828535,0.32575043371758866,0.32598117734152693,0.32640403757251313], "fy":[-0.5073567134674463,-0.5076506203723582,-0.5080945615079071,-0.5083604383944994]}, - {"t":0.7774988792232299, "x":2.6841910174820116, "y":2.3279479814801296, "heading":2.0699331746794565, "vx":3.265250982485265, "vy":2.127571287306266, "omega":-0.06137995482332544, "ax":0.017919243356820408, "ay":-0.028232297827323324, "alpha":-0.0006185212507780944, "fx":[0.3053061972088801,0.30263416044516733,0.30419506792095957,0.30706938384903165], "fy":[-0.4778511353990139,-0.4794614849221993,-0.4823622912540197,-0.48121832073098286]}, - {"t":0.7846979429197413, "x":2.7076982316360145, "y":2.3432637711054687, "heading":2.0694912964749945, "vx":3.2653799842595843, "vy":2.127368041195908, "omega":-0.06138440759720756, "ax":0.016936268253142643, "ay":-0.02686139426572686, "alpha":-0.0008609097233535218, "fx":[0.2888908506478203,0.2850166114403017,0.2871933884304821,0.2912234578662056], "fy":[-0.45367765565660045,-0.4558920347529032,-0.4599483234404767,-0.4581005091243381]}, - {"t":0.7918970066162526, "x":2.731206349009919, "y":2.358578133073693, "heading":2.0690493862147297, "vx":3.2655019095335196, "vy":2.127174664307612, "omega":-0.06139060534114305, "ax":0.016170659987105967, "ay":-0.025750358770242208, "alpha":-0.000969461110206825, "fx":[0.2760256445665321,0.2715853998794843,0.2740330980856533,0.278589055670665], "fy":[-0.43440983150912016,-0.4368857132914183,-0.44146484599913266,-0.4392645486419962]}, - {"t":0.799096070312764, "x":2.754715324292232, "y":2.373891131699931, "heading":2.0686074313365115, "vx":3.265618323144781, "vy":2.126989285834617, "omega":-0.06139758455342682, "ax":0.015596388505596977, "ay":-0.02489669239973541, "alpha":-0.001012737710897812, "fx":[0.26633807084940253,0.26164886106914864,0.2642003278213273,0.268973164114295], "fy":[-0.41975328731145223,-0.4223259245287366,-0.42711789872252226,-0.4247453460508347]}, - {"t":0.8062951340092753, "x":2.7782251227623016, "y":2.389202817896012, "heading":2.0681654262144993, "vx":3.2657306025390684, "vy":2.1268100529601988, "omega":-0.06140487531671552, "ax":0.015198558523353936, "ay":-0.024295771233000765, "alpha":-0.0010239668773613455, "fx":[0.25961001888764923,0.25483270520716,0.2574066304594301,0.26224317262453267], "fy":[-0.409507990171413,-0.4120979826969424,-0.4169496373135427,-0.4145008580012978]}, - {"t":0.8134941977057867, "x":2.801735719229811, "y":2.4045132293550395, "heading":2.067723368605818, "vx":3.265840017929974, "vy":2.1266351461555364, "omega":-0.06141224691948883, "ax":0.01496868900120875, "ay":-0.023943517780586933, "alpha":-0.0010193818029276894, "fx":[0.25571562977882,0.25093331986304623,0.2534899466887928,0.25831357164702246], "fy":[-0.4035459447729991,-0.4061149788125352,-0.4109501699745107,-0.4084784528749896]}, - {"t":0.820693261402298, "x":2.8252470974290205, "y":2.419822390776874, "heading":2.0672812579284985, "vx":3.2659477784755464, "vy":2.1264627752459155, "omega":-0.06141958551401923, "ax":0.014902503437930785, "ay":-0.023837903351324404, "alpha":-0.0010070717116315972, "fx":[0.25459233727948044,0.2498481407385741,0.2523680769283228,0.2571407230552485], "fy":[-0.4018040322327756,-0.4043337069217649,-0.40911500367637,-0.4066509187128165]}, - {"t":0.8278923250988094, "x":2.8487592496881784, "y":2.435130314026365, "heading":2.06683909442017, "vx":3.2660550625470335, "vy":2.126291164661298, "omega":-0.061426835487418265, "ax":0.014998929010719673, "ay":-0.02397964747209425, "alpha":-0.0009911495393039411, "fx":[0.2562277599075083,0.2515435802305517,0.2540175489703302,0.2587210745067235], "fy":[-0.40428036907513615,-0.40676208791023005,-0.4114719322445414,-0.4090333800648535]}, - {"t":0.8350913887953207, "x":2.872272176790901, "y":2.450436998167274, "heading":2.066396878718821, "vx":3.266163040792361, "vy":2.1261185336517263, "omega":-0.06143397083608451, "ax":0.01525930688398555, "ay":-0.024372147498046674, "alpha":-0.000973897533023604, "fx":[0.26064838162467213,0.2560338283425432,0.25845803214424673,0.26308553399818596], "fy":[-0.4110251732915755,-0.4134555752991826,-0.4180875620524182,-0.4156847112011079]}, - {"t":0.8422904524918321, "x":2.895785887983129, "y":2.4657424293555947, "heading":2.0659546116496426, "vx":3.2662728935145835, "vy":2.1259430770094667, "omega":-0.06144098198645864, "ax":0.015687545507117113, "ay":-0.02502214255751263, "alpha":-0.0009564883377352816, "fx":[0.2679228318974021,0.2633808512639188,0.2657542452650536,0.2703047134820411], "fy":[-0.42214935847366386,-0.4245277148385022,-0.4290812135183016,-0.4267196549405343]}, - {"t":0.8494895161883435, "x":2.919300401109159, "y":2.4810465805768813, "heading":2.0655122941067456, "vx":3.2663858291539314, "vy":2.1257629410113723, "omega":-0.06144786780692699, "ax":0.0162897913649718, "ay":-0.025939771863616137, "alpha":-0.0009399482067258268, "fx":[0.27815739346251384,0.27368527345206195,0.2760092857097216,0.28048680818216676], "fy":[-0.4378220621405334,-0.440150003499837,-0.4446293958480889,-0.4423109280430844]}, - {"t":0.8566885798848548, "x":2.9428157428722046, "y":2.4963494112088775, "heading":2.065069926992389, "vx":3.2665031003995706, "vy":2.125576198941453, "omega":-0.061454634553938625, "ax":0.017074557102531737, "ay":-0.027138501048414653, "alpha":-0.0009248311803740516, "fx":[0.2914978436653644,0.28708973237238716,0.289367159558931,0.29377858782897315], "fy":[-0.45827057698771934,-0.4605510566553395,-0.4649634062878285,-0.4626875113887959]}, - {"t":0.8638876435813662, "x":2.966331949214254, "y":2.5116508664098376, "heading":2.0646275111637893, "vx":3.2666260212237415, "vy":2.1253808271437777, "omega":-0.06146129247251465, "ax":0.01805274048300536, "ay":-0.02863554154150552, "alpha":-0.000911699902022486, "fx":[0.3081303818737923,0.3037774419152455,0.3060125310313795,0.3103674462817844], "fy":[-0.4837857842723794,-0.4860231678728571,-0.49037821949420635,-0.4881423014672983]}, - {"t":0.8710867072778775, "x":2.989849065819066, "y":2.5269508763235837, "heading":2.06418504740441, "vx":3.266755984052375, "vy":2.1251746780562364, "omega":-0.0614678558581814, "ax":0.019237904245789215, "ay":-0.030452136670691052, "alpha":-0.0009010307595516764, "fx":[0.3282863022683308,0.32397728270989085,0.3261755829280033,0.33048581919586384], "fy":[-0.5147273783932808,-0.516927318777969,-0.5212370351916747,-0.5190367942404749]}, - {"t":0.8782857709743889, "x":3.013367148746015, "y":2.542249355083017, "heading":2.0637425363947988, "vx":3.266894478950428, "vy":2.124955451184649, "omega":-0.061474342436011915, "ax":0.020646229343649725, "ay":-0.03261349326681849, "alpha":-0.0008931063903466288, "fx":[0.35224114509385357,0.34796339782895974,0.35013129293951206,0.3544099790701178], "fy":[-0.5515231230980084,-0.5536921040438021,-0.5579697329344344,-0.555799795655041]}, - {"t":0.8854848346709002, "x":3.03688626520085, "y":2.5575461996064446, "heading":2.0632999786879007, "vx":3.267043112470566, "vy":2.1247206645692556, "omega":-0.06148077196580377, "ax":0.02229686936199697, "ay":-0.03514958018064488, "alpha":-0.0008884761331710893, "fx":[0.3803212260757364,0.37605953245181334,0.3782049696797291,0.38246774441601017], "fy":[-0.5946807675371991,-0.596826844573538,-0.6010881465892087,-0.5989414480966148]}, - {"t":0.8926838983674116, "x":3.0604064944513265, "y":2.572841288167796, "heading":2.062857374694409, "vx":3.2672036290533355, "vy":2.12446762050263, "omega":-0.061487168162079275, "ax":0.024212179088839488, "ay":-0.03809529137456562, "alpha":-0.0008875533662247157, "fx":[0.41290713040461513,0.40864438848026385,0.4107765445030613,0.4150408909777311], "fy":[-0.6447923026457456,-0.6469247787402448,-0.6511873761970837,-0.6490555674813921]}, - {"t":0.8998829620639229, "x":3.0839279289028236, "y":2.5881344787158165, "heading":2.0624147246542917, "vx":3.2673779340728273, "vy":2.1241933700734865, "omega":-0.06149355771529677, "ax":0.026417948722923825, "ay":-0.04149070336044894, "alpha":-0.0008905639437550328, "fx":[0.4504376131515654,0.4461557790658302,0.4482846452674927,0.45256895828053534], "fy":[-0.7025385244568654,-0.7046675436495458,-0.708950001274637,-0.7068239011539016]}, - {"t":0.9070820257604344, "x":3.1074506753459383, "y":2.603425606931338, "heading":2.061972028615374, "vx":3.2675681185684153, "vy":2.1238946758571817, "omega":-0.061499968941853646, "ax":0.028943705495925044, "ay":-0.0453821202033672, "alpha":-0.0008982050060042717, "fx":[0.4934155100621888,0.489093371281639,0.4912309538288653,0.4955567606991067], "fy":[-0.7687044363713904,-0.7708421939290596,-0.7751662435306702,-0.7730346449694115]}, - {"t":0.9142810894569456, "x":3.130974856389941, "y":2.618714483988878, "heading":2.0615292864216284, "vx":3.2677764861478935, "vy":2.123567967083155, "omega":-0.06150643517690437, "ax":0.03182324510954246, "ay":-0.049822185320861447, "alpha":-0.0009107696512506712, "fx":[0.5424157866472856,0.5380308893051691,0.5401902432315121,0.544580256365274], "fy":[-0.8441843278217552,-0.84634411505704,-0.8507327136434919,-0.8485833112180263]}, - {"t":0.921480153153457, "x":3.1545006121036763, "y":2.634000893992586, "heading":2.0610864976770444, "vx":3.2680055837164663, "vy":2.1232092939975304, "omega":-0.06151299186563656, "ax":0.03509483017987218, "ay":-0.05487090407017766, "alpha":-0.0009289816306076066, "fx":[0.5980898830659659,0.5936164587498169,0.595812791059839,0.6002929465297403], "fy":[-0.9299961585233308,-0.9321934794026028,-0.9366728380388818,-0.9344910372203642]}, - {"t":0.9286792168499683, "x":3.178028101882832, "y":2.649284591057148, "heading":2.060643661730441, "vx":3.26825823363435, "vy":2.122814274864044, "omega":-0.06151967966356817, "ax":0.038801851856980187, "ay":-0.0605962655253585, "alpha":-0.000953449558931928, "fx":[0.661176371560781,0.6565858247505567,0.6588362378008792,0.6634351575172648], "fy":[-1.0272941342959905,-1.02954646050372,-1.034145473434357,-1.0319144856854312]}, - {"t":0.9358782805464796, "x":3.201557506565852, "y":2.6645652959910313, "heading":2.060200777637954, "vx":3.268537570637411, "vy":2.122378038488756, "omega":-0.06152654360767432, "ax":0.04299321632654791, "ay":-0.06707524805014856, "alpha":-0.000985172921228242, "fx":[0.7325081535390447,0.7277670907618332,0.7300915453103479,0.7348424435110078], "fy":[-1.137384006576747,-1.1397117660325116,-1.1444638640674074,-1.1421634730340475]}, - {"t":0.9430773442429912, "x":3.225089030825663, "y":2.67984269253991, "heading":2.059757844131496, "vx":3.2688470815402635, "vy":2.1218951595055833, "omega":-0.06153363593028631, "ax":0.04772395825319336, "ay":-0.07439463956836993, "alpha":-0.001025292606742276, "fx":[0.8130226457261794,0.8080920388549883,0.8105136823574933,0.815455132538394], "fy":[-1.2617379685501078,-1.2641649027032706,-1.2691087143286626,-1.2667145459848628]}, - {"t":0.9502764079395024, "x":3.2486229058631206, "y":2.6951164231429425, "heading":2.059314859566956, "vx":3.269190649355578, "vy":2.121359587756652, "omega":-0.061541017077069805, "ax":0.05305583294609387, "ay":-0.08265252422694797, "alpha":-0.0010758564720302942, "fx":[0.903772810403628,0.8986034356836086,0.9011507344959412,0.906331170668242], "fy":[-1.4020172255583063,-1.4045725730880445,-1.409756268462631,-1.4072370854785796]}, - {"t":0.9574754716360138, "x":3.2721593924334145, "y":2.710386084142137, "heading":2.05887182186507, "vx":3.2695726016764284, "vy":2.120764566970064, "omega":-0.06154876223634025, "ax":0.05905797326103793, "ay":-0.09195947915539213, "alpha":-0.0011400652529664742, "fx":[1.005938625130631,1.0004653102631569,1.0031747562939897,1.0086582171428458], "fy":[-1.5600914998117,-1.562812780542227,-1.568299578105734,-1.5656138556493244]}, - {"t":0.9646745353325252, "x":3.295698784237812, "y":2.7256512203752528, "heading":2.0584287284052896, "vx":3.2699977637877216, "vy":2.1201025448221267, "omega":-0.06155696963871454, "ax":0.06580725474547268, "ay":-0.1024402007255029, "alpha":-0.0012239320403557511, "fx":[1.120835218353112,1.1149631895886274,1.1178860040958518,1.123765884441314], "fy":[-1.7380607176261769,-1.7410005542328857,-1.7468821650350772,-1.7439705776593473]}, - {"t":0.9718735990290364, "x":3.319241411707209, "y":2.740911319079104, "heading":2.057985575859896, "vx":3.2704715144063266, "vy":2.11936507129202, "omega":-0.06156578080343328, "ax":0.07338843109467616, "ay":-0.11423557450546162, "alpha":-0.0013392889537316409, "fx":[1.2499184527875251,1.2434955678106647,1.246711841876414,1.2531389961477737], "fy":[-1.9382792763120211,-1.941519709953002,-1.947944053310829,-1.9447147071607351]}, - {"t":0.9790726627255478, "x":3.3427876461904726, "y":2.7561658030074616, "heading":2.057542359882367, "vx":3.270999842396364, "vy":2.118542682114748, "omega":-0.06157542242991924, "ax":0.08189370595288045, "ay":-0.12750558808874685, "alpha":-0.0015099702939498191, "fx":[1.3947843124253003,1.3875443144413324,1.3911926150033858,1.3984327838175032], "fy":[-2.16338299681263,-2.1670653519525285,-2.174293567971679,-2.170592366675931]}, - {"t":0.9862717264220592, "x":3.3663379045399795, "y":2.771414022634449, "heading":2.0570990744941544, "vx":3.2715894004018624, "vy":2.1176247612644357, "omega":-0.06158629280224524, "ax":0.09142042048424376, "ay":-0.14243309151387756, "alpha":-0.0017847372298463043, "fx":[1.557143443640567,1.5485886721309947,1.5529281507421238,1.561480512563017], "fy":[-2.416305796378514,-2.420693191017801,-2.4292178489946754,-2.4247676955399986]}, - {"t":0.9934707901185704, "x":3.3898926540233427, "y":2.7866552472705037, "heading":2.056655710849439, "vx":3.27224754183209, "vy":2.116599376366136, "omega":-0.06159914123924446, "ax":0.10206526513046234, "ay":-0.1592305597639352, "alpha":-0.0022651638492754336, "fx":[1.73875316833807,1.727905943709807,1.7334484872864144,1.744296226446424], "fy":[-2.7002929011896626,-2.7059032191521384,-2.7166985303507833,-2.7109703962699165]}, - {"t":1.000669853815082, "x":3.4134524173514, "y":2.801888654818215, "heading":2.0562122547080075, "vx":3.2729823161769653, "vy":2.1154530654239645, "omega":-0.06161544829807846, "ax":0.11391100007892484, "ay":-0.17815181069299427, "alpha":-0.00316364962036595, "fx":[1.941255911437404,1.9261355239332016,1.9339234371777392,1.9490592016821109], "fy":[-3.0188888065641297,-3.0267743118069457,-3.041821046681468,-3.033761139751449]}, - {"t":1.0078689175115934, "x":3.437017777328367, "y":2.817113319689162, "heading":2.0557686811710205, "vx":3.273802368722267, "vy":2.114170539191137, "omega":-0.061638223613208955, "ax":0.12699666654756755, "ay":-0.1995142257315095, "alpha":-0.004926900933485075, "fx":[2.1658122222179395,2.142324479382661,2.1545164927187987,2.178054649892232], "fy":[-3.3758668229295297,-3.3882050333672167,-3.4116003640054733,-3.399047354438341]}, - {"t":1.0150679812081047, "x":3.460589380008108, "y":2.83232819800227, "heading":2.055324943673089, "vx":3.2747166258139875, "vy":2.112734223571736, "omega":-0.061673692686855554, "ax":0.1412531410662234, "ay":-0.2237438606268176, "alpha":-0.008498396470726734, "fx":[2.412306620511777,2.371878351926343,2.392988843339801,2.433528238147969], "fy":[-3.7750462581167743,-3.796390258478221,-3.8367010760083216,-3.815138609596868]}, - {"t":1.022267044904616, "x":3.484167933914604, "y":2.84753210831894, "heading":2.0548809508310377, "vx":3.275733516173855, "vy":2.11112347726738, "omega":-0.06173487318436656, "ax":0.15636479589774455, "ay":-0.2514673660432058, "alpha":-0.01587494571936408, "fx":[2.677644585386785,2.6021846953350205,2.6417031459825973,2.7173493266692317], "fy":[-4.2198467989309725,-4.259769296357162,-4.335021053615662,-4.294914632275492]}, - {"t":1.0294661086011274, "x":3.5077542000718003, "y":2.862723704363989, "heading":2.054436517546687, "vx":3.276859196299415, "vy":2.109313147681641, "omega":-0.06184915792977896, "ax":0.17147693355366472, "ay":-0.28370164480883797, "alpha":-0.03127772190111945, "fx":[2.9521545096270563,2.8033748383827986,2.881283938267936,3.0302810173631056], "fy":[-4.712294589891175,-4.790955815511433,-4.939159339612206,-4.860325471246038]}, - {"t":1.0366651722976388, "x":3.5313489616766596, "y":2.8779014324358223, "heading":2.053991261519175, "vx":3.2780936696665504, "vy":2.107270761469857, "omega":-0.06207432824202692, "ax":0.1845899369515588, "ay":-0.3222396859209733, "alpha":-0.06357339328781454, "fx":[3.212131568039534,2.90919731429175,3.067487402354528,3.3704717623154066], "fy":[-5.250864843739469,-5.410613884680109,-5.711709017928118,-5.5516316803947925]}, - {"t":1.04386423599415, "x":3.5549529501345747, "y":2.8930634585929815, "heading":2.053544384476243, "vx":3.2794225443803997, "vy":2.104950937445368, "omega":-0.06253199714970928, "ax":0.19133819054899665, "ay":-0.3704155934932026, "alpha":-0.1311549451873774, "fx":[3.404784110293304,2.7784132688565726,3.1047590438788024,3.7304750753659706], "fy":[-5.825923720413732,-6.1550495771286515,-6.775747100166378,-6.445932642921988]}, - {"t":1.0510632996906617, "x":3.578566680115443, "y":2.90820753579445, "heading":2.053094212645692, "vx":3.280800000201737, "vy":2.1022842919936298, "omega":-0.06347618995422565, "ax":0.1826244567974332, "ay":-0.434545970229564, "alpha":-0.2713986883109342, "fx":[3.4190985056630514,2.119634934581574,2.795124548408613,4.091701038153311], "fy":[-6.410377044662174,-7.09028009303368,-8.372789352894364,-7.692563985966388]}, - {"t":1.0582623633871728, "x":3.6021901006872685, "y":2.923330753818391, "heading":2.0526372435109996, "vx":3.2821147252987624, "vy":2.099155967874885, "omega":-0.06543000639852563, "ax":0.14039491443058577, "ay":-0.5263424400417974, "alpha":-0.5574614300780503, "fx":[3.0336970126412424,0.3555245109882249,1.747880735941794,4.415207036306227], "fy":[-6.941452087569547,-8.334691393507986,-10.961204960534769,-9.57438877990896]}, - {"t":1.0654614270836842, "x":3.625821891743741, "y":2.9384290720920343, "heading":2.0521662087272734, "vx":3.2831254372304146, "vy":2.095366795122847, "omega":-0.06944320674200574, "ax":0.03163158412811126, "ay":-0.6656328444142888, "alpha":-1.1213600831272736, "fx":[1.8388192266221055,-3.5747790543599995,-0.7424105952442585,4.630547204710808], "fy":[-7.288181674044704,-10.07946180852057,-15.33146266989859,-12.589790764693914]}, - {"t":1.0726604907801955, "x":3.649458140567433, "y":2.953496502401347, "heading":2.051666282658648, "vx":3.283353155019374, "vy":2.090574861877419, "omega":-0.07751594940716423, "ax":-0.19324542340975115, "ay":-0.8777995451290431, "alpha":-2.1547284462689964, "fx":[-0.8239000464115359,-11.272570905827394,-5.688655579077063,4.636929091903618], "fy":[-7.206355549815005,-12.53367680311529,-22.513177780723105,-17.471266275347293]}, - {"t":1.079859554476707, "x":3.673090201449841, "y":2.9685239373473187, "heading":2.0511082404013705, "vx":3.281961968907188, "vy":2.084255527039266, "omega":-0.09302797674053974, "ax":-0.573844306205498, "ay":-1.1708183353037838, "alpha":-3.7849049387967577, "fx":[-5.731026185043532,-23.94659555998522,-13.71919825866006,4.353110174275185], "fy":[-6.323265290548419,-15.615277388283536,-32.8475390959627,-24.875057757690048]}, - {"t":1.0870586181732182, "x":3.6967023845373688, "y":2.9834982859274533, "heading":2.0504385260712574, "vx":3.2778308271949346, "vy":2.075826731266371, "omega":-0.12027574848017808, "ax":-1.075423226816349, "ay":-1.4991069860144686, "alpha":-5.808600170932874, "fx":[-13.259526294856917,-40.04098962963082,-23.683528004841,3.813478398627444], "fy":[-4.2662371824893315,-18.57657811433028,-44.55651385072547,-34.59819445293386]}, - {"t":1.0942576818697296, "x":3.7202718297280657, "y":2.998403448041084, "heading":2.0495726532968033, "vx":3.270088786884376, "vy":2.065034564586168, "omega":-0.16209223109829068, "ax":-1.5905508835709834, "ay":-1.7877880945612346, "alpha":-7.751105448877941, "fx":[-22.84483186652325,-55.40472595840043,-33.11724298817428,3.1475390804145067], "fy":[-0.9782548282324092,-20.593344634147368,-54.82794796799941,-45.23950840009298]}, - {"t":1.101456745566241, "x":3.7437721908410104, "y":3.013223435991011, "heading":2.0484057410004173, "vx":3.2586383097610057, "vy":2.052164164217557, "omega":-0.21789293294313936, "ax":-2.040816526334402, "ay":-2.0012848342684464, "alpha":-9.318208653816551, "fx":[-33.192123006554084,-67.45114499043265,-40.62906607839411,2.417513338102616], "fy":[3.109246447649734,-21.61878915557961,-62.45551024744119,-55.200076697761226]}, - {"t":1.1086558092627523, "x":3.767178451389545, "y":3.0279452367125614, "heading":2.04683711589714, "vx":3.243946341595031, "vy":2.0377567872207965, "omega":-0.28497531057934833, "ax":-2.408375143997901, "ay":-2.1471338630191084, "alpha":-10.4879994003726, "fx":[-43.041756320828675,-76.1673755933687,-46.24906519495647,1.5951086968889867], "fy":[7.343440886513587,-22.018748988607808,-67.7557417218745,-63.657480821145185]}, - {"t":1.1158548729592637, "x":3.7904694188817496, "y":3.042559538385748, "heading":2.044785560484346, "vx":3.2266082955282953, "vy":2.022299433775985, "omega":-0.3604790863116039, "ax":-2.7030461411105153, "ay":-2.2453550484553206, "alpha":-11.338305435770906, "fx":[-51.67948545007929,-82.41093528467242,-50.46563926871433,0.6438941986150659], "fy":[11.22336624672218,-22.08615787321607,-71.39372341659556,-70.51487264495759]}, - {"t":1.123053936655775, "x":3.813627932790062, "y":3.057060016355883, "heading":2.0421904485807287, "vx":3.2071488941838315, "vy":2.0061349797608723, "omega":-0.44210426935422, "ax":-2.9404874666018026, "ay":-2.3127060573237865, "alpha":-11.950486665070184, "fx":[-58.92814710657021,-86.96700565780117,-53.72433467885619,-0.4479143964535409], "fy":[14.519943054820706,-21.97656676408876,-73.9042664641878,-75.99298307477198]}, - {"t":1.1302530003522864, "x":3.8366402043500223, "y":3.071442380107804, "heading":2.039007711785148, "vx":3.1859801376129715, "vy":1.9894856615428904, "omega":-0.5281365840603703, "ax":-3.1349829797325746, "ay":-2.360195952439528, "alpha":-12.38751356217277, "fx":[-64.90516584675166,-90.38242794037838,-56.338254626282655,-1.6748055395713433], "fy":[17.197875418450696,-21.76480453513181,-75.64571273305324,-80.37238950998358]}, - {"t":1.1374520640487975, "x":3.8594950406704376, "y":3.0857036537394156, "heading":2.0352056228760396, "vx":3.1634111954543975, "vy":1.97249446054503, "omega":-0.617315083235851, "ax":-3.29775385737896, "ay":-2.394562548873403, "alpha":-12.694582941338346, "fx":[-69.82379560424674,-93.01396346292384,-58.513383295258,-3.024255814346107], "fy":[19.311691752080765,-21.48741604947346,-76.84890881381834,-83.89866213729812]}, - {"t":1.144651127745309, "x":3.8821831838147705, "y":3.099841716082247, "heading":2.030761532271008, "vx":3.1396704553797097, "vy":1.9552558522304095, "omega":-0.7087041944311929, "ax":-3.4370825558213216, "ay":-2.419909889529791, "alpha":-12.903663228089425, "fx":[-73.89624068140321,-95.093378770122,-60.38404624951402,-4.48149765605833], "fy":[20.942499606343112,-21.163355321739846,-77.66363470360673,-86.76340887773493]}, - {"t":1.1518501914418204, "x":3.904696805398448, "y":3.113855019753578, "heading":2.0256595256333125, "vx":3.1149266791301837, "vy":1.9378347667958669, "omega":-0.8015984879285407, "ax":-3.55891998833574, "ay":-2.4387907988200457, "alpha":-13.037572999044638, "fx":[-77.30133203223242,-96.77376964943002,-62.0387211380485,-6.031020002726166], "fy":[22.17023618458788,-20.803195563394382,-78.18939227675251,-89.11018310008767]}, - {"t":1.1590492551383318, "x":3.9270291377558615, "y":3.127742418655309, "heading":2.019888767059688, "vx":3.0893057874433674, "vy":1.9202777564926956, "omega":-0.8954568063965803, "ax":-3.6674995259648697, "ay":-2.452839860828251, "alpha":-13.112928611952071, "fx":[-80.18026842457631,-98.15865788679896,-63.53658791685727,-7.656956065209686], "fy":[23.064495935144706,-20.413248191102458,-78.49421640993523,-91.04544818964048]}, - {"t":1.1662483188348431, "x":3.9491742100323735, "y":3.1415030594645645, "heading":2.013442316472964, "vx":3.0629032247490207, "vy":1.902619606097251, "omega":-0.9898576147218302, "ax":-3.76582676606962, "ay":-2.463135137217611, "alpha":-13.142135020830036, "fx":[-82.64165711519406,-99.31978929116168,-64.91785535434344,-9.34324141394401], "fy":[23.683129200653152,-19.997472978152494,-78.62602220558618,-92.64852969503616]}, - {"t":1.1734473825313545, "x":3.9711266605990527, "y":3.155136311340196, "heading":2.006316268454105, "vx":3.035792797990058, "vy":1.884887339351306, "omega":-1.084468681844939, "ax":-3.856038087701288, "ay":-2.4704076740815744, "alpha":-13.13470268231698, "fx":[-84.76833251128102,-100.30808150982354,-66.2102302357357,-11.073773994764071], "fy":[24.073532821385875,-19.55840643141096,-78.61953147827961,-93.97930567462264]}, - {"t":1.180646446227866, "x":3.992881603807308, "y":3.168641719042909, "heading":1.9985091093366316, "vx":3.0080329341805223, "vy":1.867102717149242, "omega":-1.179026243089678, "ax":-3.939654713367685, "ay":-2.4751658282250544, "alpha":-13.098117656842318, "fx":[-86.6234751093587,-101.16047702001109,-67.43303490464251,-12.832610728705502], "fy":[24.274579439893532,-19.097633045415176,-78.50058772564637,-95.08380879397424]}, - {"t":1.1878455099243772, "x":4.014434535208509, "y":3.1820189708182895, "heading":1.9900212243127704, "vx":2.979671108956727, "vy":1.8492838406924217, "omega":-1.2733204264056863, "ax":-4.017761016284943, "ay":-2.4777710060596485, "alpha":-13.038421025727812, "fx":[-88.25547461839025,-101.90432579870816,-68.5998788654756,-14.604181937970267], "fy":[24.318449985896866,-18.61603578629821,-78.28890701769737,-95.99821062528336]}, - {"t":1.1950445736208886, "x":4.035781264034514, "y":3.195267875858409, "heading":1.9808545094570071, "vx":2.9507469914831312, "vy":1.831446209394429, "omega":-1.3671848498718346, "ax":-4.091129897945522, "ay":-2.478484962145786, "alpha":-12.960598963865708, "fx":[-89.7016160866603,-102.56025061868536,-69.72042658717528,-16.37350266552415], "fy":[24.232155689910343,-18.11393654514383,-77.99987256248222,-96.75162678064444]}, - {"t":1.2022436373174, "x":4.056917865069625, "y":3.2083883481536906, "heading":1.9710120586378743, "vx":2.921294686757119, "vy":1.813603438281096, "omega":-1.4604890273576436, "ax":-4.160311564995764, "ay":-2.4774999123743533, "alpha":-12.868848813488846, "fx":[-90.99082300889935,-103.14406048046712,-70.80158983998632,-18.126364076372816], "fy":[24.038734111886537,-17.59117984172637,-77.64573078882073,-97.36808202064071]}, - {"t":1.2094427010139113, "x":4.077840644364545, "y":3.2213803947290547, "heading":1.9604979051018707, "vx":2.8913443388033815, "vy":1.7957677586038114, "omega":-1.5531326896667248, "ax":-4.225696993571987, "ay":-2.4749579807874027, "alpha":-12.76676281902817, "fx":[-92.1456894048985,-103.66805389893253,-71.84834693483923,-19.849496893758563], "fy":[23.758162133556365,-17.047185987036716,-77.23640377796745,-97.86788079191804]}, - {"t":1.2166417647104226, "x":4.098546114847263, "y":3.2342441069800834, "heading":1.949316803939826, "vx":2.8609232769845003, "vy":1.7779503784539337, "omega":-1.645041428399162, "ax":-4.287563675497973, "ay":-2.4709639418345133, "alpha":-12.65745654569448, "fx":[-93.18398869542948,-104.14192249419736,-72.86431469467212,-21.53069947995651], "fy":[23.408038562784196,-16.480988373981223,-76.7800509169997,-98.26855785599211]}, - {"t":1.223840828406934, "x":4.1190309790008754, "y":3.2469796542750737, "heading":1.9374740459133803, "vx":2.8300568329817413, "vy":1.7601617516448842, "omega":-1.7361632643074418, "ax":-4.346108881691994, "ay":-2.4655936833868695, "alpha":-12.543659312255134, "fx":[-94.1198008396058,-104.57338742373122,-73.85215451406698,-23.15893141130424], "fy":[23.00408400630276,-15.891262298232702,-76.28346227353263,-98.58553178020938]}, - {"t":1.231039892103445, "x":4.1392921165609335, "y":3.259587279273491, "heading":1.924975295986088, "vx":2.7987689183104667, "vy":1.742411785668466, "omega":-1.8264658666837048, "ax":-4.401474158764081, "ay":-2.458900066424438, "alpha":-12.427776927215906, "fx":[-94.96436294560768,-104.968653186173,-74.8138656758305,-24.72438246752216], "fy":[22.56048943584053,-15.276353026252483,-75.75233616187381,-98.83254655610692]}, - {"t":1.238238955799957, "x":4.159326575735575, "y":3.2720672945896627, "heading":1.911826451872328, "vx":2.7670824254829753, "vy":1.7247100074669204, "omega":-1.915934224388767, "ax":-4.453764155476816, "ay":-2.450917959072578, "alpha":-12.311928542006394, "fx":[-95.72673087688554,-105.332732943781,-75.75100343887136,-26.218548546028657], "fy":[22.09009214505025,-14.63431669394569,-75.19147254225147,-99.02195576854707]}, - {"t":1.245438019496468, "x":4.179131566825801, "y":3.2844200805194363, "heading":1.8980335193526272, "vx":2.735019493638458, "vy":1.707065692964633, "omega":-2.0045685821895676, "ax":-4.503063843856548, "ay":-2.4416720306024224, "alpha":-12.197940755750732, "fx":[-96.4143546884876,-105.66967698627946,-76.66485555018599,-27.634422954477248], "fy":[21.60420471570136,-13.963018267513798,-74.60489354250436,-99.16486337423297]}, - {"t":1.2526370831929794, "x":4.198704457311786, "y":3.2966460834973663, "heading":1.8836025024454193, "vx":2.702601650197077, "vy":1.6894879404903358, "omega":-2.09238233465649, "ax":-4.549462664670731, "ay":-2.431195581492572, "alpha":-12.087237624654842, "fx":[-97.03374110797984,-105.98271101356312,-77.55662833855223,-28.967152384081572], "fy":[21.111590263012022,-13.260443297408628,-73.99586520014851,-99.27104662686644]}, - {"t":1.2598361468894907, "x":4.218042767333269, "y":3.308745814594531, "heading":1.868539308740772, "vx":2.669849778689212, "vy":1.6719856086404938, "omega":-2.1793991282312493, "ax":-4.593109433932001, "ay":-2.419587252347828, "alpha":-11.980418004441388, "fx":[-97.59167729517397,-106.27424930144124,-78.42775639837586,-30.216226075995102], "fy":[20.614811667778497,-12.525714349245897,-73.36670566973929,-99.34833908092922]}, - {"t":1.267035210586002, "x":4.2371441635155485, "y":3.320719846099513, "heading":1.8528496755965136, "vx":2.636783691309288, "vy":1.654566845891575, "omega":-2.2656469205560548, "ax":-4.634372786536395, "ay":-2.407200992187844, "alpha":-11.875808143333316, "fx":[-98.09856750562447,-106.54564325412092,-79.28064323671174,-31.39256635982492], "fy":[20.097550901453086,-11.762377356826477,-72.71801231431385,-99.40036169723813]}, - {"t":1.2742342742825137, "x":4.25600644556064, "y":3.332568799790318, "heading":1.836539139101626, "vx":2.6034205464256335, "vy":1.6372372526185093, "omega":-2.3511416198274597, "ax":-4.6743355358242695, "ay":-2.395260630029349, "alpha":-11.764796687090932, "fx":[-98.57842529844515,-106.79621893581728,-80.12075035456986,-32.541045491630314], "fy":[19.483392372992057,-10.989079793686011,-72.04614820682066,-99.41895626389122]}, - {"t":1.2814333379790248, "x":4.274627508635523, "y":3.344293306049014, "heading":1.8196131208209692, "vx":2.569769707164368, "vy":1.619993618773182, "omega":-2.4358371405543338, "ax":-4.716224183753619, "ay":-2.3877064954173304, "alpha":-11.617972287628213, "fx":[-99.0968292237309,-107.02038520479066,-80.96246923742922,-33.806812078067175], "fy":[18.512505112214495,-10.272785180352477,-71.33577704268775,-99.36076010728542]}, - {"t":1.2886324016755362, "x":4.293005231703737, "y":3.3558938700415455, "heading":1.8020773740917904, "vx":2.535817308858498, "vy":1.6028043676240986, "omega":-2.5194756630772734, "ax":-4.768882735023157, "ay":-2.396060001385358, "alpha":-11.34923568638015, "fx":[-99.8202742944901,-107.19998856701812,-81.84338411248827,-35.605676330688816], "fy":[16.416539497023855,-9.819823873034698,-70.54088895428237,-99.08100687329492]}, - {"t":1.2958314653720475, "x":4.311137164739219, "y":3.367370471053595, "heading":1.7839395083114868, "vx":2.5014858182878728, "vy":1.5855549790534622, "omega":-2.601179533690245, "ax":-4.850369170539893, "ay":-2.4463567529939967, "alpha":-10.764721402147108, "fx":[-101.00462540695234,-107.28795563522284,-82.84415535096156,-38.87683072288183], "fy":[11.447458354328091,-10.141374856357755,-69.555356101141,-98.19804101523704]}, - {"t":1.3030305290685589, "x":4.329019831608174, "y":3.3787215892658455, "heading":1.7652134511623891, "vx":2.466567701677561, "vy":1.5679435009642675, "omega":-2.678675448739501, "ax":-4.973353792731343, "ay":-2.564871278830611, "alpha":-9.658848728174087, "fx":[-102.43259468025306,-107.19266489859336,-84.05608953795024,-44.699950937228216], "fy":[1.7959084011949198,-11.986962428501151,-68.24484786620309,-96.07500442294683]}, - {"t":1.3102295927650702, "x":4.3466479337991135, "y":3.389942850227932, "heading":1.7459294959846323, "vx":2.4307642109384013, "vy":1.549478829254613, "omega":-2.748210115968594, "ax":-5.113037268678676, "ay":-2.727484523795688, "alpha":-8.161863400928945, "fx":[-102.95805295379029,-106.80870035069093,-85.44963550496477,-52.66881508029716], "fy":[-10.996210764452975,-15.573187105126705,-66.61758422140457,-92.38794330203687]}, - {"t":1.3174286564615816, "x":4.364014664725567, "y":3.4010269690031034, "heading":1.7261449563083775, "vx":2.393955129958546, "vy":1.529843494436559, "omega":-2.806967890474107, "ax":-5.234328413178327, "ay":-2.8791491650161953, "alpha":-6.628397523173735, "fx":[-102.09906124284296,-106.07454725205044,-86.8858039869672,-61.07830206192388], "fy":[-23.15905077002508,-20.358545256043435,-64.8367173781719,-87.53970059724215]}, - {"t":1.324627720158093, "x":4.381113261684589, "y":3.41196580162708, "heading":1.7059374156707925, "vx":2.356272866303616, "vy":1.50911631620585, "omega":-2.8546861464492332, "ax":-5.3269730791160415, "ay":-2.9956331579226663, "alpha":-5.243859547129822, "fx":[-100.4688806926498,-104.97522413058236,-88.25625574125917,-68.7407910178751], "fy":[-32.85734891954559,-25.751528578179872,-63.048920800338074,-82.16165326484543]}, - {"t":1.3318267838546043, "x":4.397938180902101, "y":3.4227523994948412, "heading":1.6853863482689555, "vx":2.3179236477974583, "vy":1.487550562290583, "omega":-2.8924370253445804, "ax":-5.394066312249011, "ay":-3.0793724392136115, "alpha":-4.034578532549569, "fx":[-98.7019634091346,-103.52620345630832,-89.50937654477781,-75.2685549663076], "fy":[-40.08644119481416,-31.366046454248004,-61.33931368669728,-76.72517508657802]}, - {"t":1.3390258475511156, "x":4.4144852830485455, "y":3.4333815741688136, "heading":1.6645635098853522, "vx":2.2790914208323714, "vy":1.4653819639554024, "omega":-2.921482213188982, "ax":-5.44070875762855, "ay":-3.1393056914113844, "alpha":-2.976678116826748, "fx":[-97.1101372844149,-101.76426929579878,-90.62601947878991,-80.67917091886973], "fy":[-45.3812666159048,-36.96242108738362,-59.7540516563593,-71.49702694861905]}, - {"t":1.346224911247627, "x":4.4307516208618205, "y":3.443849602625419, "heading":1.6435315733443796, "vx":2.2399234119320357, "vy":1.4427819023201112, "omega":-2.94291150855603, "ax":-5.471318633751794, "ay":-3.182971352607087, "alpha":-2.041727190832712, "fx":[-95.79168145007849,-99.7413041378272,-91.60219578314236,-85.12707654524776], "fy":[-49.26222982193733,-42.38509722050334,-58.318978421117926,-66.59942245711453]}, - {"t":1.3534239749441384, "x":4.446735192482507, "y":3.454153800279176, "heading":1.6223453659410885, "vx":2.200535040583747, "vy":1.4198674888085216, "omega":-2.957610032653734, "ax":-5.489466713121966, "ay":-3.2153984390117536, "alpha":-1.2073366482887784, "fx":[-94.75028313621944,-97.5183072187189,-92.44068635775749,-88.78775575346957], "fy":[-52.12122229400355,-47.53136209075399,-57.049071107159584,-62.07037427492916]}, - {"t":1.3606230386406497, "x":4.4624347344330735, "y":3.4642921953190053, "heading":1.6010533429265732, "vx":2.1610160200561026, "vy":1.3967196306364127, "omega":-2.966301726087897, "ax":-5.498042648692446, "ay":-3.2397317308518536, "alpha":-0.4574875843068545, "fx":[-93.95629356417422,-95.15952335950423,-93.1471149110787,-91.81759747246544], "fy":[-54.23414887106387,-52.33618492859893,-55.9530358880826,-57.904269406448726]}, - {"t":1.367822102337161, "x":4.477849554207197, "y":3.4742633168985204, "heading":1.5796987478573947, "vx":2.1214352608220293, "vy":1.373396595546401, "omega":-2.9695952083476853, "ax":-5.49939054914684, "ay":-3.2579554171540903, "alpha":0.21918755790205507, "fx":[-93.37240121827185,-92.72728992973798,-93.72804396569737,-94.34450379776028], "fy":[-55.792998187783866,-56.76338133894926,-55.035663944795616,-54.07551438166061]}, - {"t":1.3750211660336724, "x":4.492979394645945, "y":3.4840660622277224, "heading":1.5583204427996449, "vx":2.081844797966728, "vy":1.349942366977914, "omega":-2.9680172631568653, "ax":-5.495399737248039, "ay":-3.271394974182141, "alpha":0.8310603706945291, "fx":[-92.9631226526903,-90.27813229100684,-94.19001745539173,-96.4694362382684], "fy":[-56.93253123434732,-60.79905940982464,-54.29912453010271,-50.55125475753023]}, - {"t":1.3822202297301838, "x":4.5078243242357665, "y":3.4936996108087843, "heading":1.5369534974698331, "vx":2.0422830652204875, "vy":1.326391386182329, "omega":-2.962034406612589, "ax":-5.4875714132821765, "ay":-3.2810138956627553, "alpha":1.3846996455397425, "fx":[-92.69767386515132,-87.86031993866914,-94.53907409461358,-98.2710105358027], "fy":[-57.74890058956157,-64.44603292413724,-53.74371786865137,-47.297778958140086]}, - {"t":1.3894192934266951, "x":4.52238464924899, "y":3.5031633651213796, "heading":1.5156296231053707, "vx":2.002777689077114, "vy":1.302771158158314, "omega":-2.9520658656638115, "ax":-5.477074617529491, "ay":-3.2875740011887844, "alpha":1.8856335320244884, "fx":[-92.55031921623105,-85.5127906120843,-94.7805102618626,-99.81026837462876], "fy":[-58.31201692742009,-67.71894417079032,-53.36833132908034,-44.283479985149775]}, - {"t":1.3966183571232065, "x":4.536660844549191, "y":3.51245690591419, "heading":1.49437751290216, "vx":1.9633478800349735, "vy":1.2791037035167607, "omega":-2.9384910697584896, "ax":-5.464797166887336, "ay":-3.2917157796214647, "alpha":2.3387958855606383, "fx":[-92.49986085033554,-83.26515868487175,-94.91878323462636,-101.1347420048231], "fy":[-58.67369021727303,-70.64014545409349,-53.17072013727755,-41.48001846809061]}, - {"t":1.4038174208197178, "x":4.550653500291314, "y":3.5215799558665237, "heading":1.473223128519339, "vx":1.9240064571420368, "vy":1.255406431948454, "omega":-2.9216539292051995, "ax":-5.451392980049379, "ay":-3.2939955745355816, "alpha":2.7487653191712655, "fx":[-92.5289172393377,-81.13845805340787,-94.95749813932744,-102.28166581122107], "fy":[-58.87302695584,-73.23639868687978,-53.14767742024534,-38.86258585050075]}, - {"t":1.4110164845162292, "x":4.564363281970286, "y":3.5305323485744875, "heading":1.4521899557838278, "vx":1.884761531843946, "vy":1.2316927479913455, "omega":-2.901865392585724, "ax":-5.437325447337055, "ay":-3.2949012926924857, "alpha":3.1198788044360044, "fx":[-92.62322733516552,-79.14629396497641,-94.8994478781599,-103.28043123953692], "fy":[-58.94005745323267,-75.53637872527221,-53.295131215176006,-36.40974554658631]}, - {"t":1.4182155482127403, "x":4.577790901467892, "y":3.539314001491056, "heading":1.4312992419839012, "vx":1.8456178796099043, "vy":1.2079725437115345, "omega":-2.8794051863471934, "ax":-5.422906143193511, "ay":-3.294858504620251, "alpha":3.4562752541497423, "fx":[-92.7710530014888,-77.29614871672635,-94.74668887793165,-104.15443687065884], "fy":[-58.898213589048765,-77.56889627492238,-53.60819213984425,-34.103099684987896]}, - {"t":1.425414611909252, "x":4.590937096970913, "y":3.5479248922548985, "heading":1.4105702206393227, "vx":1.8065780328648515, "vy":1.1842526474657808, "omega":-2.854523240639893, "ax":-5.408328429879654, "ay":-3.2942329763832743, "alpha":3.7619037282337153, "fx":[-92.962689904998,-75.59066759717605,-94.50064268329405,-104.92247635165548], "fy":[-58.7660504030404,-79.36171694998747,-54.08116831566931,-31.92690579477933]}, - {"t":1.4326136756057632, "x":4.603802619886578, "y":3.556365038184268, "heading":1.390020326006784, "vx":1.7676431320064945, "vy":1.160537254437649, "omega":-2.8274410560801946, "ax":-5.393695913616671, "ay":-3.2933324191190456, "alpha":4.040516292656152, "fx":[-93.19007555694417,-74.02882342679449,-94.1622174147224,-105.59978047904414], "fy":[-58.55846261537807,-80.94084329913858,-54.70756005456745,-29.8677026088221]}, - {"t":1.4398127393022746, "x":4.616388227147129, "y":3.5646344788250146, "heading":1.3696653977459312, "vx":1.7288135715647548, "vy":1.1368283445786247, "omega":-2.798353121922571, "ax":-5.379046062157409, "ay":-3.292408559231935, "alpha":4.295656094022328, "fx":[-93.44647780372507,-72.60691226965051,-93.73194503485048,-106.19880264274585], "fy":[-58.28755835207196,-82.33013670516388,-55.48004472582229,-27.913970425486667]}, - {"t":1.447011802998786, "x":4.62869467755415, "y":3.5727332614537315, "heading":1.3495198753758793, "vx":1.6900894763368146, "vy":1.1131260856457743, "omega":-2.7674284200833967, "ax":-5.36436951242226, "ay":-3.2916598295961057, "alpha":4.530645531650607, "fx":[-93.72624766028152,-71.31936906417761,-93.21013118883674,-106.72981419100776], "fy":[-57.96329643807335,-83.55117588416171,-56.3904608483178,-26.055834330491184]}, - {"t":1.4542108666952973, "x":4.6407227310502694, "y":3.580661429412664, "heading":1.329596981904163, "vx":1.651471038525263, "vy":1.089429216865264, "omega":-2.734812014314729, "ax":-5.349625684948163, "ay":-3.29123465839399, "alpha":4.74857628166974, "fx":[-94.02462278249558,-70.15941315905766,-92.59701429554404,-107.20135872017924], "fy":[-57.593958083779256,-84.62327118961564,-57.4297997049541,-24.28481036071158]}, - {"t":1.4614099303918089, "x":4.6524731500133445, "y":3.58841901312141, "heading":1.3099088960151268, "vx":1.612958742466828, "vy":1.0657354089193196, "omega":-2.7006267111952456, "ax":-5.3347553203255496, "ay":-3.291235230661899, "alpha":4.952301409706681, "fx":[-94.33757058168403,-69.11954441632683,-91.89293014316266,-107.62060122631274], "fy":[-57.18650079084022,-85.5635747361386,-58.58821180226143,-22.59359094627384]}, - {"t":1.46860899408832, "x":4.6639467018436065, "y":3.5960060235827105, "heading":1.2904669123007322, "vx":1.574553499110501, "vy":1.042041596853582, "omega":-2.664974777902444, "ax":-5.319690485760131, "ay":-3.2917216052828246, "alpha":5.144429240368224, "fx":[-94.66166233568885,-68.19191416873296,-91.09847731924528,-107.99359844169128], "fy":[-56.74682731957743,-86.3872438834965,-59.85503472479445,-20.975864720197773]}, - {"t":1.4758080577848314, "x":4.6751441622596275, "y":3.603422448178185, "heading":1.2712815891250162, "vx":1.5362567084577885, "vy":1.018344283345968, "omega":-2.6279397041188375, "ax":-5.30436250895946, "ay":-3.292716100507572, "alpha":5.327318597190021, "fx":[-94.99397161913949,-67.36859502899645,-90.2146779066912,-108.32550971188296], "fy":[-56.27999253794002,-87.10762921092824,-61.21884763555087,-19.426165580539003]}, - {"t":1.4830071214813427, "x":4.6860663188382015, "y":3.6106682485336696, "heading":1.2523628838044736, "vx":1.498070264886402, "vy":0.994639810403885, "omega":-2.589587998206057, "ax":-5.288708197042799, "ay":-3.294207891980189, "alpha":5.503075218503037, "fx":[-95.33199190077627,-66.64177113970193,-89.2431270567059,-108.62076270307638], "fy":[-55.79036424580784,-87.73646824227623,-62.66755602045666,-17.93974624085896]}, - {"t":1.490206185177854, "x":4.696713974431401, "y":3.6177433602213735, "heading":1.2337202748576668, "vx":1.459996517703629, "vy":0.970924597959969, "omega":-2.5499710091813603, "ax":-5.272674604402347, "ay":-3.2961577870630214, "alpha":5.673549421970314, "fx":[-95.67356933216752,-66.00386724243693,-88.18612439535902,-108.88318453748744], "fy":[-55.281749471164176,-88.28407339370207,-64.1885083980758,-16.512472116238623]}, - {"t":1.4974052488743654, "x":4.707087950176106, "y":3.6246476940559806, "heading":1.2153628711383129, "vx":1.4220381973755585, "vy":0.94719534809715, "omega":-2.509126765507291, "ax":-5.256222537956312, "ay":-3.298503138126768, "alpha":5.840335342371784, "fx":[-96.01684765433016,-65.44763163744905,-87.04678017114641,-109.11610627292708], "fy":[-54.75749454497955,-88.75950757461638,-65.76864427396767,-15.14073198774011]}, - {"t":1.5046043125708768, "x":4.717189087881618, "y":3.631381138733665, "heading":1.1972995077308042, "vx":1.3841983165217722, "vy":0.9234492139026323, "omega":-2.4670818193685697, "ax":-5.23932893119536, "ay":-3.301162845253255, "alpha":6.004772245577317, "fx":[-96.36022283486318,-64.96618505346966,-85.82908950304022,-109.32244667519748], "fy":[-54.220565025764984,-89.17074417693397,-67.39467014233266,-13.82136246512342]}, - {"t":1.511803376267388, "x":4.7270182516430035, "y":3.6379435645570575, "heading":1.1795388285686648, "vx":1.346480053819122, "vy":0.8996839323070973, "omega":-2.4238530814896153, "ax":-5.221988182791999, "ay":-3.304042378181218, "alpha":6.167948496199854, "fx":[-96.70230556829486,-64.55304482537659,-84.53796921830343,-109.50477977971748], "fy":[-53.673609962151914,-89.52481029945467,-69.05325786491613,-12.551583808425471]}, - {"t":1.5190024399638995, "x":4.736576328583976, "y":3.644334827986395, "heading":1.1620893558440355, "vx":1.3088866282687726, "vy":0.8758979207705975, "omega":-2.379449627388671, "ax":-5.204212541044007, "ay":-3.307038722664898, "alpha":6.33070872017582, "fx":[-97.04189016658866,-64.20213159749669,-83.17925364000052,-109.66538966729514], "fy":[-53.11901385201733,-89.82791335454095,-70.73125745992341,-11.32894511781983]}, - {"t":1.5262015036604109, "x":4.745864228684651, "y":3.6505547767585456, "heading":1.1449595464138245, "vx":1.2714211706956131, "vy":0.8520903383593029, "omega":-2.333874452068065, "ax":-5.186031617113765, "ay":-3.3100451360950003, "alpha":6.493664565644788, "fx":[-97.37792867334974,-63.907764018001046,-81.75964804015382,-109.80631508373015], "fy":[-52.55893883071733,-90.08555195068983,-72.41591467445375,-10.151277257384669]}, - {"t":1.5334005673569222, "x":4.75488288369683, "y":3.6566032553224863, "heading":1.1281578355737258, "vx":1.2340865987518892, "vy":0.828261112586227, "omega":-2.2871261472362097, "ax":-5.16749112348176, "ay":-3.3129555866944775, "alpha":6.657209241805962, "fx":[-97.70950926282974,-63.664645505242554,-80.28664114874148,-109.92938593129432], "fy":[-51.99535903269786,-90.30261233162172,-74.09508284216523,-9.01665223453846]}, - {"t":1.5405996310534336, "x":4.763633245192118, "y":3.662480110352989, "heading":1.1116926687578157, "vx":1.1968855010027868, "vy":0.8044109342939, "omega":-2.239200473863444, "ax":-5.148650955685614, "ay":-3.315668749010805, "alpha":6.821535741922679, "fx":[-98.0358381667628,-63.467846088671195,-78.76838092298674,-110.03625321540969], "fy":[-51.43008862821672,-90.4834518117422,-75.75741847118009,-7.923347988672954]}, - {"t":1.547798694749945, "x":4.772116281825278, "y":3.668185196123914, "heading":1.0955725219172143, "vx":1.1598200348217018, "vy":0.7805412237732386, "omega":-2.1900918035493135, "ax":-5.129582757594115, "ay":-3.318091440861578, "alpha":6.9866583661579424, "fx":[-98.35622451217448,-63.31278150693263,-77.21352031031662,-110.1284136898137], "fy":[-50.86480471175644,-90.63197066120358,-77.39255094713354,-6.86981776047405]}, - {"t":1.5549977584464565, "x":4.780332975925617, "y":3.6737183795485406, "heading":1.0798059115222554, "vx":1.1228918418132552, "vy":0.7566540721396267, "omega":-2.1397944049455786, "ax":-5.110367126695695, "ay":-3.320141412893147, "alpha":7.152436897829737, "fx":[-98.67006755943667,-63.19519110103462,-75.63104163954426,-110.207230185183], "fy":[-50.301065983859274,-90.75167384252246,-78.99121860914664,-5.854663395874291]}, - {"t":1.5621968221429676, "x":4.788284319551912, "y":3.679079544725573, "heading":1.0644013953036136, "vx":1.086101983355615, "vy":0.7327521626267833, "omega":-2.0883035561328245, "ax":-5.091090626839936, "ay":-3.3217494333114006, "alpha":7.318602575400193, "fx":[-98.97684592179128,-63.11111557890278,-74.03006958525357,-110.27394841101918], "fy":[-49.74032797748963,-90.84572388707711,-80.54536578280145,-4.876612052913024]}, - {"t":1.569395885839479, "x":4.795971310160719, "y":3.684268596864504, "heading":1.049367564985362, "vx":1.049450897648282, "vy":0.7088386768725238, "omega":-2.0356164700230663, "ax":-5.071842772635068, "ay":-3.3228606510311303, "alpha":7.484784878652017, "fx":[-99.27610841377717,-63.05687535775988,-72.41968288704419,-110.32971086745556], "fy":[-49.183955449465905,-90.91698608779676,-82.04819823961631,-3.934495905264008]}, - {"t":1.5765949495359903, "x":4.803394946043606, "y":3.6892854655010128, "heading":1.0347130323559983, "vx":1.0129383784693913, "vy":0.6849171913911191, "omega":-1.981733026926965, "ax":-5.052713137588863, "ay":-3.3234352587259326, "alpha":7.650538111878994, "fx":[-99.56746623736376,-63.02904993671184,-70.80873449037031,-110.37556838691432], "fy":[-48.633232443309254,-90.96806705205267,-83.49419733492095,-3.0272345017759297]}, - {"t":1.5837940132325017, "x":4.810556221686392, "y":3.694130106949867, "heading":1.020446410065671, "vx":0.9765635747516892, "vy":0.6609915692723191, "omega":-1.9266563157469605, "ax":-5.033788715007733, "ay":-3.323448513348431, "alpha":7.815366821607069, "fx":[-99.85058626033374,-63.0244585529708,-69.20568845246574,-110.41248973217164], "fy":[-48.089370449780034,-91.00134754189804,-84.8790956491193,-2.1538195206063575]}, - {"t":1.590993076929013, "x":4.817456123193731, "y":3.698802505977639, "heading":1.0065762885273226, "vx":0.940325009157568, "vy":0.6370658517326474, "omega":-1.8703929921866091, "ax":-5.01515163157555, "ay":-3.322890198948539, "alpha":7.978749216084037, "fx":[-100.12518518414568,-63.0401422419504,-67.61848014045653,-110.4413696048055], "fy":[-47.55351501903882,-91.0190104125598,-86.19981898206488,-1.3133016749623996]}, - {"t":1.5981921406255244, "x":4.824095623906659, "y":3.7033026767086046, "heading":0.9931112102390628, "vx":0.9042206131141928, "vy":0.6131441535339032, "omega":-1.81295346836153, "ax":-4.996877278911222, "ay":-3.321763636977482, "alpha":8.14015793962221, "fx":[-100.39102443300447,-63.07334732503975,-66.05440401555153,-110.46303535747803], "fy":[-47.02675112225832,-91.02306435742386,-87.45440103275311,-0.5047795890300081]}, - {"t":1.6053912043220355, "x":4.830475680320781, "y":3.7076306628034, "heading":0.9800596427415168, "vx":0.8682477752996602, "vy":0.5892305655265467, "omega":-1.7543519528545268, "ax":-4.979032895573029, "ay":-3.3200843574798657, "alpha":8.299077764832571, "fx":[-100.6479056236127,-63.12151028035339,-64.52003115203253,-110.47825265564131], "fy":[-46.51010751742612,-91.0153640782844,-88.64187777404285,0.27260952337045574]}, - {"t":1.612590268018547, "x":4.83659722839026, "y":3.711786536970523, "heading":0.967429951286818, "vx":0.8324034003374045, "vy":0.5653290667592579, "omega":-1.6946063634031958, "ax":-4.961676601249364, "ay":-3.317878544048004, "alpha":8.45501998253299, "fx":[-100.89566650590932,-63.18224392362094,-63.02115658676582,-110.48773029384051], "fy":[-46.004560322473274,-90.99762740887611,-89.76216863867079,1.0196975449877068]}, - {"t":1.6197893317150585, "x":4.842461180279376, "y":3.715770399885394, "heading":0.9552303721361648, "vx":0.79668397444352, "vy":0.5414434477833676, "omega":-1.6337381359936645, "ax":-4.944856858606951, "ay":-3.3151813585871377, "alpha":8.607533467128414, "fx":[-101.1341772809314,-63.25332478985774,-61.562774862565746,-110.49212433758736], "fy":[-45.51103597657353,-90.97144985645797,-90.8159512073062,1.7372916271337584]}, - {"t":1.6269883954115698, "x":4.848068421600626, "y":3.719582378600691, "heading":0.9434689872317268, "vx":0.7610856349482774, "vy":0.5175772460174114, "omega":-1.5717719542939534, "ax":-4.928612319871694, "ay":-3.312035237251569, "alpha":8.756212561247633, "fx":[-101.36333722617418,-63.332681608872576,-60.14908087899172,-110.49204173323145], "fy":[-45.03041372809139,-90.9383169535209,-91.80453523033329,2.4261789936778326]}, - {"t":1.6341874591080812, "x":4.853419809157301, "y":3.72322262453554, "heading":0.9321537008163744, "vx":0.7256042409221103, "vy":0.4937336933793468, "omega":-1.5087354223253386, "ax":-4.9129720020872645, "ay":-3.308488233053566, "alpha":8.90070205029903, "fx":[-101.58307157110468,-63.41838476086194,-58.783492290134504,-110.48804350345996], "fy":[-44.563527768293866,-90.89961475931294,-92.72974077633516,3.087130491760077]}, - {"t":1.6413865228045923, "x":4.858516169189947, "y":3.7266913111306343, "heading":0.9212922184098714, "vx":0.6902354425399069, "vy":0.46991567585043553, "omega":-1.4446587013215666, "ax":-4.897955729022005, "ay":-3.3045924623086536, "alpha":9.04069958206489, "fx":[-101.7933285803842,-63.508636604899465,-57.468689282122625,-110.48064762491909], "fy":[-44.111169102887594,-90.85663880042084,-93.59378413551684,3.720903009417599]}, - {"t":1.648585586501104, "x":4.863358296110744, "y":3.72998863125253, "heading":0.910892028399338, "vx":0.6549747472639845, "vy":0.44612570422326386, "omega":-1.3795741291692576, "ax":-4.883574777225598, "ay":-3.300402694264424, "alpha":9.175955930379736, "fx":[-101.9940768129652,-63.60176258373637,-56.20666752869386,-110.4703316657017], "fy":[-43.67408723352758,-90.81060169515642,-94.39917396114568,4.328240882961854]}, - {"t":1.6557846501976152, "x":4.867946951697595, "y":3.733114794423888, "heading":0.9009603863693894, "vx":0.619817581376061, "vy":0.4223658950031162, "omega":-1.3135158379500729, "ax":-4.869832665824046, "ay":-3.2959751087022116, "alpha":9.30627351624272, "fx":[-102.18530253238924,-63.69620301415586,-54.998800267098346,-110.45753524553862], "fy":[-43.25299170770935,-90.76263967234486,-95.14861915598937,4.9098763834570045]}, - {"t":1.6629837138941266, "x":4.872282864710724, "y":3.736070023948425, "heading":0.8915043021856102, "vx":0.5847593458234418, "vy":0.39863796025345266, "omega":-1.2465193821294849, "ax":-4.856726037015436, "ay":-3.291366231464043, "alpha":9.431503582947052, "fx":[-102.36700725268518,-63.79050548934554,-53.845905926803745,-110.44266236674642], "fy":[-42.84855357576249,-90.71381815852634,-95.8449490937658,5.466529407893034]}, - {"t":1.670182777590638, "x":4.876366730888488, "y":3.7388545539903415, "heading":0.882530529754724, "vx":0.5497954657264623, "vy":0.37494320510459617, "omega":-1.1786213870819735, "ax":-4.844245581415953, "ay":-3.2866320481190554, "alpha":9.55154238909372, "fx":[-102.53920540766116,-63.88331782618202,-52.748318233582005,-110.42608365304837], "fy":[-42.46140678576299,-90.66513658289476,-96.49104610205272,5.998906467069002]}, - {"t":1.6773818412871493, "x":4.880199213275638, "y":3.741468626658788, "heading":0.8740455593150503, "vx":0.514921433224305, "vy":0.3512825316431912, "omega":-1.1098592250229595, "ax":-4.832376970376227, "ay":-3.2818272882154065, "alpha":9.666326741659606, "fx":[-102.70192213547504,-63.97338150139151,-51.70595624867028,-110.40813852341904], "fy":[-42.09214953697789,-90.61753252488016,-97.08978964207776,6.507699065090886]}, - {"t":1.6845809049836606, "x":4.883780942835842, "y":3.743912489138872, "heading":0.8660556120599493, "vx":0.4801328436090117, "vy":0.327656447954379, "omega":-1.040270723098461, "ax":-4.821101766763607, "ay":-3.277004866348491, "alpha":9.77582913839366, "fx":[-102.85519117410978,-64.0595255315965,-50.718392434862,-110.38913731905548], "fy":[-41.74134560228457,-90.5718853076356,-97.644011236829,6.993581572467355]}, - {"t":1.691779968680172, "x":4.887112519300766, "y":3.746186390902248, "heading":0.8585666368627476, "vx":0.44542542490271686, "vy":0.30406508118775816, "omega":-0.9698939064449532, "ax":-4.810398292353877, "ay":-3.272215463941383, "alpha":9.880052741825263, "fx":[-102.99905286351414,-64.14066075702087,-49.784917294789366,-110.36936339558778], "fy":[-41.409525628138624,-90.52901912574193,-98.15645903270844,7.457208670615711]}, - {"t":1.6989790323766834, "x":4.890194512209587, "y":3.7482905810226086, "heading":0.8515843088513921, "vx":0.41079506119047166, "vy":0.2805081936341343, "omega":-0.8987667774316612, "ax":-4.800242437698571, "ay":-3.2675072329975814, "alpha":9.97902635799393, "fx":[-103.1335522536314,-64.21577449934992,-48.90459964545479,-110.34907518510464], "fy":[-41.097188410552235,-90.4897057781198,-98.62977075346116,7.899212471005956]}, - {"t":1.7061780960731947, "x":4.893027462095503, "y":3.7502253056145873, "heading":0.8451140295723533, "vx":0.37623781012278246, "vy":0.25698520093497274, "omega":-0.8269271310512971, "ax":-4.790608405535087, "ay":-3.2629256035873455, "alpha":10.072799550363412, "fx":[-103.25873731596496,-64.28392556602705,-48.0763419498391,-110.32850822945602], "fy":[-40.80480214762162,-90.45466706763987,-99.06645381292098,8.320199378452806]}, - {"t":1.713377159769706, "x":4.8956118817789775, "y":3.7519908054087416, "heading":0.8391609284835416, "vx":0.34174991506629243, "vy":0.23349519167776933, "omega":-0.7544124054860398, "ax":-4.781469383645389, "ay":-3.258513176902442, "alpha":10.161437983223813, "fx":[-103.37465725849658,-64.34423958104063,-47.29893045094378,-110.30787718104168], "fy":[-40.53280566221563,-90.4245769164559,-99.46887139685764,8.72074678741801]}, - {"t":1.7205762234662174, "x":4.897948257731024, "y":3.7535873134703777, "heading":0.8337298655230092, "vx":0.3073278124105102, "vy":0.21003694776132673, "omega":-0.6812595661966616, "ax":-4.772798146698098, "ay":-3.254309687132175, "alpha":10.245019056963134, "fx":[-103.48136094264149,-64.39590462405543,-46.57108010222376,-110.287377764939], "fy":[-40.281609589784566,-90.40006323892024,-99.83923341033224,9.101399684000784]}, - {"t":1.7277752871627288, "x":4.900037051473493, "y":3.7550150530659705, "heading":0.8288254345121017, "vx":0.27296813454183905, "vy":0.18660896503548788, "omega":-0.6075050214336113, "ax":-4.764567589150531, "ay":-3.250352016843741, "alpha":10.323627873980785, "fx":[-103.57889540166492,-64.43816716349836,-45.891474459954445,-110.26718869307862], "fy":[-40.05159752323341,-90.38170960628632,-100.17959130154256,9.462667232897436]}, - {"t":1.73497435085924, "x":4.9018787009869635, "y":3.756274235677774, "heading":0.8244519671668505, "vx":0.2386677089812106, "vy":0.16320947383014528, "omega":-0.5331845667897434, "ax":-4.756751193428647, "ay":-3.2466742522681113, "alpha":10.397353552896805, "fx":[-103.66730445944238,-64.4703282719968,-45.25880084778718,-110.24747351991786], "fy":[-39.84312710723951,-90.37005673120406,-100.4918358838828,9.805019416685791]}, - {"t":1.7421734145557517, "x":4.903473622100292, "y":3.757365059164791, "heading":0.8206135375085343, "vx":0.20442355415126115, "vy":0.13983645908624345, "omega":-0.45833335628729055, "ax":-4.749323438508741, "ay":-3.2433077667224897, "alpha":10.466285895691396, "fx":[-103.74662744816942,-64.49174011297796,-44.67178116682629,-110.2283824274857], "fy":[-39.65653107454586,-90.36560379618436,-100.7776974063122,10.128883794983242]}, - {"t":1.7493724782522628, "x":4.9048222098391046, "y":3.758287706066499, "heading":0.8173139664823863, "vx":0.17023287220210218, "vy":0.11648767988621787, "omega":-0.3829858974583097, "ax":-4.742260155045512, "ay":-3.240281321771696, "alpha":10.530512401040486, "fx":[-103.81689802439824,-64.5018026910255,-44.129198790989385,-110.21005392613604], "fy":[-39.492118214468235,-90.36880964456294,-101.03874722604728,10.43464245370502]}, - {"t":1.7565715419487742, "x":4.905924839713433, "y":3.759042342044562, "heading":0.8145568266117182, "vx":0.1360930392805015, "vy":0.09316068825616708, "omega":-0.30717606792631635, "ax":-4.735538833150297, "ay":-3.237621177719852, "alpha":10.59011561026412, "fx":[-103.87814308087206,-64.4999608564247,-43.629921987838614,-110.19261645821624], "fy":[-39.35017426882592,-90.38009385156032,-101.27640054911618,10.722629194881591]}, - {"t":1.7637706056452855, "x":4.906781868927325, "y":3.7596291144570344, "heading":0.8123454465326728, "vx":0.10200159358334925, "vy":0.06985284717258741, "omega":-0.23093715109460564, "ax":-4.7291388893271735, "ay":-3.2353512058490668, "alpha":10.64517076753052, "fx":[-103.9303817536076,-64.48570155850344,-43.17292432824135,-110.1761898900097], "fy":[-39.23096274569833,-90.39983768805612,-101.49191978449444,10.993127031733453]}, - {"t":1.770969669341797, "x":4.90739363749554, "y":3.760048151059288, "heading":0.8106829152720518, "vx":0.06795622148943373, "vy":0.046561347761094815, "omega":-0.15430188867891265, "ax":-4.723041898457442, "ay":-3.233492996443741, "alpha":10.695743774469564, "fx":[-103.97362452224074,-64.45855134043549,-42.75730250837849,-110.1608868785369], "fy":[-39.134725645741824,-90.42838498943452,-101.68641814629927,11.246366036177967]}, - {"t":1.7781687330383082, "x":4.907760469254383, "y":3.760299558825952, "heading":0.8095720861469603, "vx":0.03395474202114647, "vy":0.02328322571747261, "omega":-0.07730254796494138, "ax":-4.716549742100524, "ay":-3.234201932226026, "alpha":10.737861369721225, "fx":[-103.99592009199864,-64.40402725871128,-42.352572278584155,-110.1561267320457], "fy":[-39.09361063753002,-90.47614494156878,-101.87409460501176,11.392452259563196]}, - {"t":1.7853677967348196, "x":4.9078826904296875, "y":3.760383367538452, "heading":0.8090155801802581, "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":[] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/example/part2.traj b/src/main/deploy/choreo/example/part2.traj deleted file mode 100644 index 75ffe7d..0000000 --- a/src/main/deploy/choreo/example/part2.traj +++ /dev/null @@ -1,119 +0,0 @@ -{ - "name":"part2", - "version":"v2025.0.0", - "snapshot":{ - "waypoints":[ - {"x":4.9078826904296875, "y":3.760383367538452, "heading":0.8090155801802582, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.425954580307007, "y":2.7354159355163574, "heading":0.0, "intervals":50, "split":false, "fixTranslation":false, "fixHeading":false, "overrideIntervals":false}, - {"x":0.9423216581344604, "y":2.7604076862335205, "heading":2.287602367280704, "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":16.54, "h":8.21}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":3.4012425877153873, "y":4.089264137670398, "r":0.4039800907934945}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":5.632396150380373, "y":2.8408535793423653, "r":0.3632390667360681}}, "enabled":true}], - "targetDt":0.04 - }, - "params":{ - "waypoints":[ - {"x":{"exp":"4.9078826904296875 m", "val":4.9078826904296875}, "y":{"exp":"3.760383367538452 m", "val":3.760383367538452}, "heading":{"exp":"0.8090155801802582 rad", "val":0.8090155801802582}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.425954580307007 m", "val":3.425954580307007}, "y":{"exp":"2.7354159355163574 m", "val":2.7354159355163574}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":50, "split":false, "fixTranslation":false, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"0.9423216581344604 m", "val":0.9423216581344604}, "y":{"exp":"2.7604076862335205 m", "val":2.7604076862335205}, "heading":{"exp":"2.287602367280704 rad", "val":2.287602367280704}, "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":"16.54 m", "val":16.54}, "h":{"exp":"8.21 m", "val":8.21}}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"3.4012425877153873 m", "val":3.4012425877153873}, "y":{"exp":"4.089264137670398 m", "val":4.089264137670398}, "r":{"exp":"0.40398009079349445 m", "val":0.4039800907934945}}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"5.632396150380373 m", "val":5.632396150380373}, "y":{"exp":"2.8408535793423653 m", "val":2.8408535793423653}, "r":{"exp":"0.3632390667360681 m", "val":0.3632390667360681}}}, "enabled":true}], - "targetDt":{ - "exp":"0.04 s", - "val":0.04 - } - }, - "trajectory":{ - "waypoints":[0.0,0.7994156289209916,1.7068603968853615], - "samples":[ - {"t":0.0, "x":4.9078826904296875, "y":3.760383367538452, "heading":0.8090155801802582, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.053118972765389, "ay":-4.105234880961415, "alpha":10.714379512606314, "fx":[-97.39774431181642,-51.92832383291756,-16.489644289957717,-109.95386367760102], "fy":[-54.03978331799626,-98.538338999643,-109.98312138171272,-16.754239159941413]}, - {"t":0.021605827808675433, "x":4.90693666855757, "y":3.759425181506001, "heading":0.8090155801802582, "vx":-0.08757099061364443, "vy":-0.08869699795222051, "omega":0.23149303882617184, "ax":-4.111442482260953, "ay":-4.092371818142605, "alpha":10.467816112190851, "fx":[-97.55022824689114,-52.90540805465878,-19.40396257306523,-109.87824207249916], "fy":[-53.75063146523571,-98.00911637001214,-109.4924032059897,-17.188143745639433]}, - {"t":0.043211655617350866, "x":4.904084989890133, "y":3.756553625723371, "heading":0.8140171789160434, "vx":-0.17640210893064767, "vy":-0.17711607878408564, "omega":0.45765887127904575, "ax":-4.172750935326155, "ay":-4.0793662677488705, "alpha":10.195560499142305, "fx":[-97.62193675293597,-53.80660773229847,-22.67476764423837,-109.80588579667324], "fy":[-53.605962996920184,-97.50817366399218,-108.84542800989408,-17.595847352133227]}, - {"t":0.0648174834260263, "x":4.899299731621763, "y":3.751774738077452, "heading":0.8239052776840112, "vx":-0.26655784712779396, "vy":-0.2652541639335867, "omega":0.6779423958364473, "ax":-4.2385466086384245, "ay":-4.065257574902856, "alpha":9.891248927094766, "fx":[-97.62105756403297,-54.662152151233286,-26.368004623185975,-109.73464589671262], "fy":[-53.59219577055091,-97.02089019176346,-107.99081092260252,-17.991575824178902]}, - {"t":0.08642331123470173, "x":4.892551226899752, "y":3.745094847192523, "heading":0.8385527843526542, "vx":-0.358135155313081, "vy":-0.35308741909485125, "omega":0.8916510169680024, "ax":-4.310497773902604, "ay":-4.048807531155133, "alpha":9.54766834641478, "fx":[-97.55631553352904,-55.50782204531782,-30.556150997587512,-109.66104659519654], "fy":[-53.69337039357595,-96.52793815904198,-106.85902394857654,-18.395898058381356]}, - {"t":0.10802913904337716, "x":4.883807324799593, "y":3.736521085657933, "heading":0.8578176426906954, "vx":-0.4512670279856994, "vy":-0.44056525744345737, "omega":1.0979362952349812, "ax":-4.390359545054994, "ay":-4.02837525445769, "alpha":9.15769857389941, "fx":[-97.43760146897996,-56.38632709492043,-35.310482236416085,-109.580627878726], "fy":[-53.89047138913231,-96.00419655631676,-105.35832952624116,-18.833044366132267]}, - {"t":0.1296349668520526, "x":4.873032591286585, "y":3.7260620620247904, "heading":0.8815394652304372, "vx":-0.5461243803343323, "vy":-0.5276016395399994, "omega":1.2957959537464043, "ax":-4.479824557221796, "ay":-4.001806128238854, "alpha":8.715664377631168, "fx":[-97.27673435936246,-57.348954995341536,-40.688146471139035,-109.4882998883222], "fy":[-54.16055290954621,-95.41717294657548,-103.37236202156208,-19.32822102057409]}, - {"t":0.151240794660728, "x":4.860187504490874, "y":3.7137287466977353, "heading":0.9095362094822604, "vx":-0.642914698330742, "vy":-0.6140639736704302, "omega":1.4841050975277097, "ax":-4.580282191652788, "ay":-3.966378746499524, "alpha":8.219106192580675, "fx":[-97.08837402982408,-58.4574707914635,-46.71265280158224,-109.37866056421754], "fy":[-54.47563798262863,-94.72470710940917,-100.76237528034451,-19.905150018969948]}, - {"t":0.17284662246940344, "x":4.845227735346645, "y":3.699535610027358, "heading":0.9416015286694216, "vx":-0.7418754866787346, "vy":-0.6997608698912888, "omega":1.6616856906658255, "ax":-4.692492960177855, "ay":-3.91887532465506, "alpha":7.670728395375765, "fx":[-96.89110206449035,-59.78618986566452,-53.34830423396864,-109.24625428818486], "fy":[-54.80132932582532,-93.87161044669256,-97.37881849518894,-20.584033669014385]}, - {"t":0.1944524502780789, "x":4.828103645794404, "y":3.683502008552312, "heading":0.9775036235740874, "vx":-0.843260681569759, "vy":-0.784431415359453, "omega":1.8274181275434316, "ax":-4.816248815229283, "ay":-3.855843416305515, "alpha":7.079816221019131, "fx":[-96.70869414876542,-61.42402449344286,-60.473569728012414,-109.08576882121066], "fy":[-55.095005505838245,-92.7847215591623,-93.087337379691,-21.380108587945752]}, - {"t":0.2160582780867543, "x":4.808760159832562, "y":3.665653741870563, "heading":1.0169865049722429, "vx":-0.94731972415534, "vy":-0.8677401042693649, "omega":1.980383417731838, "ax":-4.95018723343286, "ay":-3.774060003035366, "alpha":6.461813546015241, "fx":[-96.57160090798013,-63.47615739344595,-67.8651939435849,-108.89212162847215], "fy":[-55.30338946947473,-91.36554393157806,-87.81062870807217,-22.303161085727886]}, - {"t":0.23766410589542972, "x":4.787137130097951, "y":3.646024610732248, "heading":1.059774328090913, "vx":-1.0542726171415937, "vy":-0.949281794834556, "omega":2.1199962485388095, "ax":-5.091976209911364, "ay":-3.671053923947955, "alpha":5.834674407691132, "fx":[-96.51875014514532,-66.06425386060033,-75.20876442753249,-108.6604651223188], "fy":[-55.358938061195424,-89.47947389886639,-81.57808082582658,-23.35781467831455]}, - {"t":0.25926993370410517, "x":4.763170200190506, "y":3.624657746094704, "heading":1.1055786019918803, "vx":-1.1642889783388104, "vy":-1.0285979537917378, "omega":2.2460592191110695, "ax":-5.238980846993143, "ay":-3.5453441926941256, "alpha":5.211687952112412, "fx":[-96.59988257125512,-69.32230812927529,-82.14574452876778,-108.38632558653588], "fy":[-55.17400965055708,-86.94104611682934,-74.56261099002674,-24.54349446706659]}, - {"t":0.2808757615127806, "x":4.736791963977621, "y":3.601606531576954, "heading":1.1541065707280822, "vx":-1.2774814964118928, "vy":-1.1051980499415743, "omega":2.3586620515969585, "ax":-5.3892355902041365, "ay":-3.395992731898158, "alpha":4.592443450755721, "fx":[-96.87839121868598,-73.38382293311909,-88.34958388715934,-108.06562353839207], "fy":[-54.63149754410823,-83.49491775652527,-67.07780840734696,-25.855235056688564]}, - {"t":0.302481589321456, "x":4.707933039366802, "y":3.576935168083434, "heading":1.2050674168737432, "vx":-1.3939203925942287, "vy":-1.1785712841464793, "omega":2.4578855940150657, "ax":-5.542120549921713, "ay":-3.2213281485360605, "alpha":3.956213687632661, "fx":[-97.43464023139128,-78.35116832804763,-93.59897548158168,-107.69475521868335], "fy":[-53.56909549920055,-78.79661791976372,-59.52548694029469,-27.284280057068603]}, - {"t":0.32408741713013145, "x":4.676522671763576, "y":3.550719282869784, "heading":1.258172069791457, "vx":-1.5136624948907589, "vy":-1.2481707454389883, "omega":2.543362865724382, "ax":-5.698027664784195, "ay":-3.017261731331246, "alpha":3.2622330765477026, "fx":[-98.3687516762954,-84.2339662485339,-97.81374784754036,-107.27081514688456], "fy":[-51.7526221140912,-72.40705015744476,-52.313239014605855,-28.81812365758469]}, - {"t":0.3456932449388069, "x":4.642488787276588, "y":3.52304727398516, "heading":1.313123529923277, "vx":-1.636773099465155, "vy":-1.3133611828598375, "omega":2.613846111848037, "ax":-5.856649763953627, "ay":-2.7761263371220046, "alpha":2.459314193256597, "fx":[-99.79908573370987,-90.84675666696651,-101.0418575072152,-106.79204709585824], "fy":[-48.83064487689014,-63.834806344560505,-45.779244161743264,-30.43976331799444]}, - {"t":0.3672990727474823, "x":4.605757972932301, "y":3.494023054157989, "heading":1.3695978389342414, "vx":-1.7633108658008565, "vy":-1.3733416904748244, "omega":2.6669816308349708, "ax":-6.014283443611661, "ay":-2.4870831127039574, "alpha":1.50359244383911, "fx":[-101.84472652459078,-97.68718889830768,-103.41424743923928,-106.2587992937984], "fy":[-44.258091671813865,-52.68339330005679,-40.1513992157957,-32.125404334088394]}, - {"t":0.3889049005561578, "x":4.566256412766768, "y":3.4637703702046663, "heading":1.427220184818962, "vx":-1.8932544382760976, "vy":-1.4270771799537705, "omega":2.6994679902709837, "ax":-6.161531893662511, "ay":-2.138437049533417, "alpha":0.3748809146079782, "fx":[-104.55851296141776,-103.896088681451,-105.09430010232974,-105.67467642634652], "fy":[-37.17484961182356,-38.93616196823704,-35.54386717205454,-33.84193065693533]}, - {"t":0.4105107283648331, "x":4.523912945492831, "y":3.4324380625657827, "heading":1.4855444253917882, "vx":-2.0263794354082316, "vy":-1.4732798826256817, "omega":2.7075676027607627, "ax":-6.2828348401672, "ay":-1.7201156277803713, "alpha":-0.9314506686605634, "fx":[-107.7234785768581,-108.46446296541691,-106.24022072322232,-105.04872955500434], "fy":[-26.25301558843847,-23.263567824477978,-31.97686517475807,-35.54125005416595]}, - {"t":0.4321165561735086, "x":4.47866488963071, "y":3.4002051459756246, "heading":1.5440436647973854, "vx":-2.162125283115231, "vy":-1.510444404690516, "omega":2.687442840001407, "ax":-6.355635379653868, "ay":-1.2239363946049615, "alpha":-2.457791424156134, "fx":[-110.33967407203384,-110.7054430872342,-106.98533293276296,-104.3997071149261], "fy":[-9.69705968260821,-7.026890428465009,-29.40275439098317,-37.148526991661576]}, - {"t":0.453722383982184, "x":4.430466940282021, "y":3.367285070280456, "heading":1.6021080920441133, "vx":-2.299444046742758, "vy":-1.5368885636811218, "omega":2.6343402217014504, "ax":-6.343837383170817, "ay":-0.6497418117484947, "alpha":-4.325389125367602, "fx":[-109.78594013753737,-110.63815502887542,-107.4338135724055,-103.76952629023909], "fy":[13.834804870053706,8.205427648518636,-27.723784963746713,-38.52413679668966]}, - {"t":0.47532821179085943, "x":4.3793048590934545, "y":3.333927667041628, "heading":1.6590251932636628, "vx":-2.4365079048897846, "vy":-1.5509267733858565, "omega":2.5408866090532407, "ax":-6.203250906004711, "ay":-0.0387286869415115, "alpha":-6.561695189001022, "fx":[-102.21062794870392,-108.9112579092912,-107.66929832128794,-103.27090784461552], "fy":[42.22229020611519,21.260494352529076,-26.784602644019607,-39.33323744914289]}, - {"t":0.49693403959953486, "x":4.325214213499871, "y":3.3004095707280485, "heading":1.7139231518202362, "vx":-2.5705342758189325, "vy":-1.5517635387271709, "omega":2.399115752666671, "ax":-5.948613822408698, "ay":0.5079679838073601, "alpha":-8.711516544275115, "fx":[-87.2946140305322,-106.4172594555378,-107.77907085465444,-103.24593194744362], "fy":[67.91925492617011,31.550416308149195,-26.310069281544077,-38.59804170387992]}, - {"t":0.5185398674082103, "x":4.268287251011242, "y":3.2670009976337657, "heading":1.7657580336654333, "vx":-2.6990590017662015, "vy":-1.540788469936709, "omega":2.2108962262586354, "ax":-5.846685330662942, "ay":0.8453860856629571, "alpha":-9.05349975403623, "fx":[-79.02525305478443,-105.13425694639108,-108.01108827725068,-105.63118008851951], "fy":[77.37760979232885,35.51697637195508,-25.08314116987203,-30.29234327027934]}, - {"t":0.5401456952168858, "x":4.208607196135751, "y":3.233908305360905, "heading":1.8135262768328275, "vx":-2.8253814782720137, "vy":-1.5225232037380247, "omega":2.015287869507043, "ax":-5.7159028119322945, "ay":0.9084137395434584, "alpha":-9.77281555795635, "fx":[-72.11125258704797,-104.66660818906216,-107.17807004307096,-104.947554653924], "fy":[83.76442786446287,36.70896248625089,-28.082963221711612,-30.582995969990133]}, - {"t":0.5617515230255613, "x":4.146228364995691, "y":3.2012249603105434, "heading":1.8570682395263092, "vx":-2.9488782901977464, "vy":-1.502896172902414, "omega":1.804138099355894, "ax":-5.581582901058681, "ay":0.965686993730958, "alpha":-10.388449231627854, "fx":[-65.27422374904347,-104.19631669883988,-106.21661738158784,-104.0773546369312], "fy":[89.08954695416853,37.81855770414418,-31.116830177731945,-30.087036655890696]}, - {"t":0.5833573508342368, "x":4.081212634061613, "y":3.1689790414241163, "heading":1.8960481366440636, "vx":-3.0694730092578677, "vy":-1.4820317059987855, "omega":1.579687054058176, "ax":-5.427476354520788, "ay":1.0359464872851896, "alpha":-10.967525886440052, "fx":[-58.05663342100072,-103.60140329100108,-105.10403590287588,-102.517206800029], "fy":[93.83017465071,39.16283504122608,-34.199494212284904,-28.30890212552159]}, - {"t":0.6049631786429123, "x":4.013627323769714, "y":3.13720031559709, "heading":1.9301785831256384, "vx":-3.1867381288093015, "vy":-1.4596492245754993, "omega":1.342724578268562, "ax":-5.232354057652073, "ay":1.185367155586674, "alpha":-11.47637717335887, "fx":[-50.644505784790134,-102.81998015682292,-103.88971933858691,-98.64917637322812], "fy":[97.8801583128201,40.84232672620355,-37.111171443966406,-20.960288981649523]}, - {"t":0.6265690064515878, "x":3.943553946191676, "y":3.1059400574748386, "heading":1.959189259158185, "vx":-3.2997874696129563, "vy":-1.4340383859218344, "omega":1.094767949193557, "ax":-4.800001511827557, "ay":1.9767568045446304, "alpha":-11.771932937691869, "fx":[-44.93058361012843,-101.90256503224424,-103.05584570501524,-76.69761491562879], "fy":[100.42376938943096,42.631518953767866,-38.51859923724323,29.959581477098233]}, - {"t":0.6481748342602632, "x":3.871138957656409, "y":3.075417857733979, "heading":1.982842626958918, "vx":-3.403495475758884, "vy":-1.3913289187832156, "omega":0.8404255931665117, "ax":-4.512967057502431, "ay":2.7345246856182746, "alpha":-11.569522334338217, "fx":[-46.06702046269876,-100.98502550713222,-104.34254801324504,-55.662519518592006], "fy":[99.55578432494929,44.07612218939932,-33.41433425397101,75.8363576855871]}, - {"t":0.6697806620689387, "x":3.79655026733244, "y":3.0459952988784083, "heading":2.0010007176108773, "vx":-3.5010018649095063, "vy":-1.3322472492871746, "omega":0.5904564857821756, "ax":-4.472910150884507, "ay":3.0835315744760496, "alpha":-10.827525557884387, "fx":[-47.61234132129311,-98.86210840465031,-106.56756416813002,-51.289673526440765], "fy":[98.2588255674586,47.59594266575244,-21.81830713366966,85.76349812592207]}, - {"t":0.6913864898776142, "x":3.719864220272483, "y":3.0179307086667984, "heading":2.0137580187712025, "vx":-3.597642791433193, "vy":-1.2656249970464313, "omega":0.35651883298449305, "ax":-4.266496408167565, "ay":3.662670021406585, "alpha":-9.53415391155341, "fx":[-46.3249347134888,-93.24772509544512,-107.19015654880702,-43.52471624884098], "fy":[97.90755703741604,56.14366079560266,2.678367686727948,92.47429081091786]}, - {"t":0.7129923176862897, "x":3.6411383441796903, "y":2.991440721694397, "heading":2.0214609032872155, "vx":-3.6898239781743936, "vy":-1.1864899792439232, "omega":0.15052554527006076, "ax":-3.3875243546710627, "ay":4.879164746436683, "alpha":-5.943687713045782, "fx":[-38.00754892655301,-75.00731503643604,-86.85112349885118,-30.61729260835495], "fy":[99.63237552372274,76.13038301525917,57.65343400876373,98.55659259575404]}, - {"t":0.7345981454949652, "x":3.5606259745001223, "y":2.966944449333553, "heading":2.0247131322991274, "vx":-3.763014246079111, "vy":-1.0810715858822526, "omega":0.02210725199345373, "ax":-1.8173246989298304, "ay":5.789594733262031, "alpha":-0.8379837260016897, "fx":[-27.82381242275834,-35.3767485582028,-34.29546762319358,-26.152663982912983], "fy":[99.8553056922092,97.34557954691444,97.05115334568444,99.66536087516822]}, - {"t":0.7562039733036406, "x":3.478898762355071, "y":2.94493832835587, "heading":2.025190777779021, "vx":-3.802279050596642, "vy":-0.955982598993379, "omega":0.004001919902988968, "ax":-1.366329227747129, "ay":5.769290448197697, "alpha":-0.038045712392610107, "fx":[-23.109836552224884,-23.459766769030058,-23.37246607440238,-23.021407496456195], "fy":[98.18796516399644,98.096555898268,98.0799677767434,98.17143030344543]}, - {"t":0.7778098011123161, "x":3.3964284666074596, "y":2.925630119349789, "heading":2.0252772425713492, "vx":-3.8317997246213062, "vy":-0.8313323029913836, "omega":0.0031799107921758455, "ax":-1.1274427587310354, "ay":5.610238842457897, "alpha":0.006857730729068987, "fx":[-19.19913052846813,-19.13804372249485,-19.15584355024081,-19.21689714461849], "fy":[95.41874234985508,95.43328394622564,95.43837259460588,95.42383103179348]}, - {"t":0.7994156289209916, "x":3.3133761097708123, "y":2.9089779595926166, "heading":2.0253459471763717, "vx":-3.8561590587305874, "vy":-0.7101184485956957, "omega":0.0033280777414663723, "ax":-0.9021675407147598, "ay":5.335513564228651, "alpha":0.0030759025231783055, "fx":[-15.354318299591386,-15.32831369892481,-15.336908069014736,-15.36290687195098], "fy":[90.75076872185502,90.7566779087666,90.7603495412293,90.75444024299216]}, - {"t":0.8210214567296671, "x":3.2298500299203448, "y":2.8948806030013716, "heading":2.025417853050988, "vx":-3.8756511352698473, "vy":-0.5948402612561193, "omega":0.003394535161738433, "ax":-0.6828819771492718, "ay":4.88331904145655, "alpha":0.000501390380250436, "fx":[-11.616860549611625,-11.61294382327643,-11.614393597496385,-11.618310196429167], "fy":[83.06294242323963,83.06385683438668,83.06477688726527,83.06386247716908]}, - {"t":0.8426272845383426, "x":3.145953990164342, "y":2.8831683822073826, "heading":2.025491194793182, "vx":-3.890405365681782, "vy":-0.4893321109115832, "omega":0.0034053681159590503, "ax":-0.4782642909416513, "ay":4.189751636887734, "alpha":-0.00016621889868997418, "fx":[-8.134795787144062,-8.135956976816509,-8.135481709956665,-8.13432050827173], "fy":[71.26685197598292,71.26654325645255,71.26610112487374,71.26640984578377]}, - {"t":0.864233112347018, "x":3.0617869320213336, "y":2.873573869619506, "heading":2.0255647705903215, "vx":-3.9007386615989055, "vy":-0.3988090584838708, "omega":0.003401776819055407, "ax":-0.30386118714187443, "ay":3.2601396494913355, "alpha":-0.00020840516068240265, "fx":[-5.1682369441234055,-5.169511398926516,-5.1689467586486035,-5.167672302305923], "fy":[55.45461943813957,55.45420210164987,55.45346582130485,55.453883160467335]}, - {"t":0.8858389401556935, "x":2.9774373211790386, "y":2.8657182055947144, "heading":2.0256382687945176, "vx":-3.907303834086032, "vy":-0.3283710425847255, "omega":0.0033972740530392636, "ax":-0.1759841739202552, "ay":2.2642903070875686, "alpha":-0.00012181839375982424, "fx":[-2.993265242755673,-2.99392728783318,-2.993615605839913,-2.9929536432183452], "fy":[38.515318643033446,38.51505413513292,38.51454186608363,38.51480636973173]}, - {"t":0.907444767964369, "x":2.892975711599513, "y":2.859151976102938, "heading":2.0257116697127264, "vx":-3.911106117844805, "vy":-0.2794491761009386, "omega":0.003394642065799759, "ax":-0.09716022608660292, "ay":1.443426173347196, "alpha":-0.00005932790377659609, "fx":[-1.652585514951798,-1.6528895217603878,-1.6527496033586848,-1.6524459429828355], "fy":[24.552468488168337,24.552336163205744,24.55206393730743,24.552196244606307]}, - {"t":0.9290505957730444, "x":2.808450348506115, "y":2.853451149504505, "heading":2.025785013764672, "vx":-3.913205344959485, "vy":-0.24826275874506384, "omega":0.0033933602373265124, "ax":-0.053938547734035634, "ay":0.8885888358906449, "alpha":-0.00003471806545185606, "fx":[-0.9174347423047802,-0.9176071142135735,-0.9175232505633694,-0.9173519480741796], "fy":[15.114763713555854,15.114682801253,15.114519994795844,15.1146008944721]}, - {"t":0.95065642358172, "x":2.723889718067781, "y":2.8482946289626345, "heading":2.0258583301216526, "vx":-3.914370731934077, "vy":-0.2290640613640992, "omega":0.0033926101247825093, "ax":-0.03102751621340607, "ay":0.5489580029691495, "alpha":-0.000027692506317473945, "fx":[-0.527726703407791,-0.527868472337662,-0.527810539082194,-0.5276709773401956], "fy":[9.33771390708889,9.337658767772377,9.337523167850383,9.337578396874886]}, - {"t":0.9722622513903956, "x":2.639309256049021, "y":2.8434736403311005, "heading":2.0259316302718307, "vx":-3.915041107106714, "vy":-0.21720336927775344, "omega":0.0033920118052594233, "ax":-0.01867024370293245, "ay":0.3472640714370178, "alpha":-0.00002484333482918568, "fx":[-0.3175455971217686,-0.31766622157018576,-0.31760354680469816,-0.31748664795695325], "fy":[5.906951126733096,5.906894495951982,5.906773637658466,5.906830716501386]}, - {"t":0.9938680791990708, "x":2.554717194279996, "y":2.8388618352173363, "heading":2.02600491749482, "vx":-3.9154444931773056, "vy":-0.20970044154614564, "omega":0.0033914750444449106, "ax":-0.011710208798678163, "ay":0.22607837698123565, "alpha":-0.00002174756244372749, "fx":[-0.1991616350163528,-0.19926092566275727,-0.1992123833124708,-0.1991142603365113], "fy":[3.8456093107980975,3.84556183849317,3.8454468286982446,3.845496045111306]}, - {"t":1.0154739070077463, "x":2.470118041534185, "y":2.834383851612406, "heading":2.0260781931206475, "vx":-3.9156975019322138, "vy":-0.20481583106182427, "omega":0.003391005170355493, "ax":-0.007486063155026553, "ay":0.1490759805850723, "alpha":-0.000016881592124170015, "fx":[-0.12733203230805298,-0.12738350247397406,-0.12734536505671437,-0.1272822694299842], "fy":[2.535818220149471,2.5357489780372573,2.5356583220208555,2.5357335813409536]}, - {"t":1.0370797348164218, "x":2.385514408265286, "y":2.829993431247059, "heading":2.0261514585944567, "vx":-3.915859244523706, "vy":-0.20159492109489377, "omega":0.0033906404295829216, "ax":-0.004462326032864441, "ay":0.09334844313945286, "alpha":0.00015314232120934267, "fx":[-0.07611033890434525,-0.0753355956361859,-0.07570676771212068,-0.07645885389130011], "fy":[1.5873066525472637,1.58759674359636,1.5883376264819202,1.5880802112896557]}, - {"t":1.0586855626250973, "x":2.3009079861718837, "y":2.8256595941719445, "heading":2.0262247161877394, "vx":-3.9159556567715983, "vy":-0.19957805070621484, "omega":0.0033939491962051917, "ax":0.010705169521576486, "ay":0.04320488987407694, "alpha":0.030701451693613973, "fx":[0.14430321240253835,0.29233889450515677,0.21988114539190445,0.07184422988194711], "fy":[0.6246536511650879,0.697026731242063,0.8450888569699343,0.7728420196586493]}, - {"t":1.0802913904337728, "x":2.2163030211949697, "y":2.8213576294500986, "heading":2.026298045269664, "vx":-3.9157243627222527, "vy":-0.1986445732951027, "omega":0.004057279474973782, "ax":1.711723299578117, "ay":0.07104250864291851, "alpha":4.804713674844004, "fx":[24.225412010576648,44.79251728686947,34.98513452999799,12.460630408534612], "fy":[-17.672664221788974,-4.70734369701298,19.00634917123178,8.20730972748321]}, - {"t":1.1018972182424482, "x":2.1321000811810222, "y":2.8170823307448583, "heading":2.026385706151372, "vx":-3.87874116385547, "vy":-0.1971096410862675, "omega":0.10786709580364148, "ax":4.799179634378508, "ay":0.6115602981268081, "alpha":12.711830117417596, "fx":[74.84726577973494,105.08941885683892,88.68588357209255,57.9081214533557], "fy":[-69.08414271941983,-12.290606881320588,55.82293960658542,67.16167274794182]}, - {"t":1.1235030460511235, "x":2.0494168243108812, "y":2.812966355560469, "heading":2.0287162640495273, "vx":-3.775050915052186, "vy":-0.1838963745903175, "omega":0.38251670845370034, "ax":5.8535066898040045, "ay":0.404783495156889, "alpha":7.625360798445429, "fx":[95.33505756741124,108.27369652533451,98.41455674808924,96.24258499502302], "fy":[-48.37578348911286,-8.097071102638488,44.744903666176064,39.26895666133981]}, - {"t":1.1451088738597992, "x":1.9692199672546091, "y":2.8090876010114716, "heading":2.0369808541863192, "vx":-3.648581057435351, "vy":-0.17515069209416392, "omega":0.5472689408439361, "ax":6.310537290594701, "ay":0.3240138438641531, "alpha":3.4483205659576917, "fx":[107.03403787776098,109.44784422925464,105.89005394953892,106.98979878557986], "fy":[-19.848117710209248,-2.0596971235442645,26.90426454498287,17.04908139144332]}, - {"t":1.166714701668475, "x":1.8918622698026255, "y":2.8053789520595926, "heading":2.0488050526872295, "vx":-3.512236675354537, "vy":-0.16815010477600795, "omega":0.6217727612211321, "ax":6.423149991350422, "ay":0.320810138385618, "alpha":1.717008450534781, "fx":[109.48707332444084,109.9375888503468,108.54047244272564,109.05863949880434], "fy":[-6.991367561825952,1.4340903838166998,16.917130429445525,10.467701397117796]}, - {"t":1.1883205294771502, "x":1.8174766900610664, "y":2.801820808828116, "heading":2.062238967902298, "vx":-3.3734592026521244, "vy":-0.16121873616677096, "omega":0.6588701501494272, "ax":6.465195878716174, "ay":0.3201287877251893, "alpha":0.7725089878310917, "fx":[110.16036385997651,110.18816572492658,109.67821956878794,109.85777901747426], "fy":[-0.09840404910248046,3.5937428506988143,10.792919006110136,7.492938521717859]}, - {"t":1.2099263572858256, "x":1.746099326256522, "y":2.7984122645220273, "heading":2.076474402914702, "vx":-3.233773293747225, "vy":-0.15430208870258053, "omega":0.6755608463211599, "ax":6.484614761469674, "ay":0.3192461832373861, "alpha":0.17855726459513982, "fx":[110.35667384973485,110.33824705475566,110.24043890824125,110.27040691607031], "fy":[4.156163312507053,5.002857068279526,6.694236438318861,5.867888111109601]}, - {"t":1.2315321850945011, "x":1.6777445246288354, "y":2.7951529541049696, "heading":2.091070454234601, "vx":-3.093667823805316, "vy":-0.14740451063897672, "omega":0.6794187238339906, "ax":6.494808823101159, "ay":0.31825907907015266, "alpha":-0.2290895776490013, "fx":[110.39418232502534,110.43997991886648,110.54566739158938,110.51952937962363], "fy":[7.0441387940996,5.9533212293377336,3.766676153782216,4.889847315197643]}, - {"t":1.2531380129031766, "x":1.612419197013873, "y":2.792042441175917, "heading":2.105749858191748, "vx":-2.9533421027231266, "vy":-0.14052825977803937, "omega":0.6744690538665441, "ax":6.50063228261287, "ay":0.31727128533619603, "alpha":-0.525980277649794, "fx":[110.37228713582708,110.51593517773654,110.72209726547052,110.6852609562981], "fy":[9.13913532857793,6.605183936456464,1.5721574961547755,4.270298376099544]}, - {"t":1.274743840711852, "x":1.5501270819955355, "y":2.789080264782052, "heading":2.120322320431869, "vx":-2.8128905609774764, "vy":-0.13367335101842837, "omega":0.6631048145568834, "ax":6.504160429584389, "ay":0.31632118415156285, "alpha":-0.7517132682865345, "fx":[110.32742761631158,110.57692648522496,110.82829635656132,110.80298115921228], "fy":[10.734351437292066,7.053295418705185,-0.13428525449608927,3.868769738575912]}, - {"t":1.2963496685205276, "x":1.4908703622939283, "y":2.7862659726072714, "heading":2.1346492488742883, "vx":-2.6723627906958756, "vy":-0.1268389699814134, "omega":0.6468634271207879, "ax":6.506389320960457, "ay":0.3154177461614779, "alpha":-0.9290498749377988, "fx":[110.27442694751888,110.62853979887583,110.8937126481156,110.89060344106127], "fy":[11.99430128786287,7.3574893459891,-1.4995819867584212,3.608453806122925]}, - {"t":1.317955496329203, "x":1.4346503816357552, "y":2.7835991320246007, "heading":2.1486252686963896, "vx":-2.531786863370999, "vy":-0.12002410847004802, "omega":0.6267905354972104, "ax":6.507839649076902, "ay":0.3145592349851666, "alpha":-1.0720052978929453, "fx":[110.21971003202324,110.67383152649508,110.93428919969972,110.95813074249595], "fy":[13.018119724526953,7.557467649233274,-2.616989185610591,3.4436521471966564]}, - {"t":1.3395613241378783, "x":1.3814679987725436, "y":2.781079331784712, "heading":2.16216759707845, "vx":-2.3911796005065726, "vy":-0.11322779580332984, "omega":0.6036289736209476, "ax":6.508802063101373, "ay":0.31374068216699785, "alpha":-1.1896821568982634, "fx":[110.1661377995381,110.71455187439008,110.95917536079864,111.01157801472934], "fy":[13.868996779880188,7.680693223572373,-3.5484993331617978,3.345366268141033]}, - {"t":1.361167151946554, "x":1.3313237768525394, "y":2.77870618045098, "heading":2.175209500742832, "vx":-2.250551543890453, "vy":-0.10644916864785332, "omega":0.5779249057919501, "ax":6.50944744602812, "ay":0.312956666596123, "alpha":-1.288249512510038, "fx":[110.11496108321036,110.75174158532938,110.97387197962244,111.05477951698914], "fy":[14.589081678665137,7.74683528565636,-4.336938112343354,3.294234564317146]}, - {"t":1.3827729797552295, "x":1.2842180911450334, "y":2.7764793039744897, "heading":2.1876960467437176, "vx":-2.1099095432419475, "vy":-0.09968748079780045, "omega":0.5500912086500482, "ax":6.509880578693684, "ay":0.31220225944572005, "alpha":-1.3720360435876504, "fx":[110.066672384215,110.78604054206696,110.98181272267205,111.0902983670419], "fy":[15.20756351667696,7.770402664074407,-5.012867033315133,3.276785269764625]}, - {"t":1.404378807563905, "x":1.240151193381925, "y":2.774398343278306, "heading":2.1995812226768767, "vx":-1.9692581844036516, "vy":-0.0929420925387368, "omega":0.5204472341449972, "ax":6.510167977814315, "ay":0.31147326928323804, "alpha":-1.4441679610407472, "fx":[110.02139920706792,110.8178550515903,110.98520670965408,111.11991735492302], "fy":[15.745304344879024,7.762371685348847,-5.598710105877609,3.2833188365245616]}, - {"t":1.4259846353725805, "x":1.199123251739583, "y":2.772462952128747, "heading":2.2108259160013146, "vx":-1.8286006160694428, "vy":-0.08621245471559798, "omega":0.4892447898519449, "ax":6.5103531168529525, "ay":0.31076623163593237, "alpha":-1.5069585731013542, "fx":[109.97909373187063,110.84745201539717,110.98551002289724,111.14491920136768], "fy":[16.217621920358123,7.731229982677661,-6.111320713632473,3.3066475391534156]}, - {"t":1.447590463181256, "x":1.1611343765112, "y":2.7706727953484527, "heading":2.2213964546871474, "vx":-1.6879390476530445, "vy":-0.07949809302612108, "omega":0.45668570240670986, "ax":6.510465122761179, "ay":0.3100783165487498, "alpha":-1.5621554880217237, "fx":[109.9396271578581,110.87501354405124,110.98370077910457,111.16625424432374], "fy":[16.63602790353821,7.683667332380812,-6.5636335688824365,3.341312106307058]}, - {"t":1.4691962909899314, "x":1.1261846370521598, "y":2.769027547347228, "heading":2.2312635273360306, "vx":-1.5472750592562794, "vy":-0.07279859431156484, "omega":0.4229340399221352, "ax":6.510523933951305, "ay":0.3094072206162408, "alpha":-1.611102713265642, "fx":[109.90283755171528,110.90066935096094,110.98044548132832,111.18464478739982], "fy":[17.00935053639513,7.625045751390238,-6.965759720000407,3.3830766063800644]}, - {"t":1.490802118798607, "x":1.0942740732321805, "y":2.767526890923886, "heading":2.240401367377016, "vx":-1.406609800195067, "vy":-0.06611359518016949, "omega":0.3881248321172279, "ax":6.5105434587731565, "ay":0.30875106729702173, "alpha":-1.654850016426411, "fx":[109.86855428836316,110.92451626144852,110.9762028286346,111.20065223949074], "fy":[17.34448100406228,7.559727347709141,-7.325731381064901,3.42859228258631]}, - {"t":1.5124079466072824, "x":1.0654027033353484, "y":2.7661705162906234, "heading":2.248787125668012, "vx":-1.2659441192839165, "vy":-0.059442772784405275, "omega":0.35237042761313514, "ax":6.51053387299733, "ay":0.3081085216562918, "alpha":-1.694216245460327, "fx":[109.8366240173845,110.94663136922826,110.97129712192675,111.21472090418123], "fy":[17.646797261396184,7.491293166820347,-7.649921481223593,3.47518223629812]}, - {"t":1.534013774415958, "x":1.0395705296814963, "y":2.764958120323444, "heading":2.2564003804518906, "vx":-1.1252786454813877, "vy":-0.05278583311911388, "omega":0.31576548314305874, "ax":6.510502620462608, "ay":0.307478709578485, "alpha":-1.7298547611539927, "fx":[109.80690411748172,110.96707737990184,110.96595671421846,111.22720881442464], "fy":[17.920603118178942,7.422734258617191,-7.943504176159449,3.520666289700319]}, - {"t":1.5556196022246334, "x":1.0167775427387216, "y":2.763889406046552, "heading":2.2632227551086026, "vx":-0.9846138469157422, "vy":-0.04614250106512742, "omega":0.27839053903954825, "ax":6.5104551458758335, "ay":0.30686091767200185, "alpha":-1.762296379608271, "fx":[109.77926072300896,110.98590677512358,110.96034045596798,111.23840895537668], "fy":[18.169407913046552,7.356578160582326,-8.210759211457294,3.56323877391115]}, - {"t":1.5772254300333088, "x":0.9970237241316496, "y":2.76296408226182, "heading":2.269237613158656, "vx":-0.843950074077844, "vy":-0.03951251691669402, "omega":0.24031466691387984, "ax":6.510395474327566, "ay":0.30625449930773047, "alpha":-1.7919767485565752, "fx":[109.75357048754184,111.00316463023798,110.95455780459253,111.24856400325484], "fy":[18.396101777904697,7.294979232402416,-8.455261257282448,3.6013858715988545]}, - {"t":1.5988312578419843, "x":0.9803090488517407, "y":2.762181863231351, "heading":2.2744298104718963, "vx":-0.7032875904931427, "vy":-0.032895634939019085, "omega":0.2015975258474164, "ax":6.510326605424438, "ay":0.3056588636870258, "alpha":-1.8192567793208092, "fx":[109.72972117276188,111.01889009738215,110.94868299633758,111.25787689779722], "fy":[18.60308537801472,7.239788909179644,-8.680020097088352,3.6338250685897298]}, - {"t":1.6204370856506598, "x":0.9666334868968952, "y":2.7615424683886545, "heading":2.2787854919020103, "vx":-0.5626265948781038, "vy":-0.026291622162001812, "omega":0.16229097713364554, "ax":6.510250787262297, "ay":0.3050734659484634, "alpha":-1.8444381086986696, "fx":[109.70761175562993,111.03311710718812,110.94276516107529,111.26651855940742], "fy":[18.792366478278534,7.19261012252996,-8.887586539400933,3.659459405144111]}, - {"t":1.6420429134593353, "x":0.9559970044962992, "y":2.76104562207358, "heading":2.2822919228088616, "vx":-0.42196723737722097, "vy":-0.019700257387723505, "omega":0.12244036495334312, "ax":6.51016970924707, "ay":0.3044977983814326, "alpha":-1.8677749536328896, "fx":[109.687152306971,111.0458745724724,110.93683565300675,111.27463359548828], "fy":[18.965632710035276,7.154839971773177,-9.080133394186651,3.6773424165206223]}, - {"t":1.6636487412680108, "x":0.9483995650294588, "y":2.7606910532866378, "heading":2.284937348250875, "vx":-0.2813096316339742, "vy":-0.013121330387773505, "omega":0.08208554091979416, "ax":6.510084639290302, "ay":0.30393138258369595, "alpha":-1.889483308787114, "fx":[109.66826377726544,111.05718627907322,110.93091344085978,111.282344568244], "fy":[19.124307017335273,7.127703536897803,-9.259518052479448,3.6866509197736783]}, - {"t":1.6852545690766862, "x":0.9438411297164584, "y":2.7604784954588344, "heading":2.28671087431357, "vx":-0.14065386389756504, "vy":-0.006554641270017514, "omega":0.041261689902773464, "ax":6.5099965223775405, "ay":0.30337376230433605, "alpha":-1.9097481599943869, "fx":[109.65087776008518,111.0670705861207,110.92500912682024,111.28975521852186], "fy":[19.26959033328103,7.112280909607735,-9.427331314097447,3.686663647124744]}, - {"t":1.7068603968853615, "x":0.9423216581344604, "y":2.7604076862335205, "heading":2.287602367280704, "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":[] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/leftStation_K.traj b/src/main/deploy/choreo/leftStation_K.traj new file mode 100644 index 0000000..a75b91a --- /dev/null +++ b/src/main/deploy/choreo/leftStation_K.traj @@ -0,0 +1,70 @@ +{ + "name":"leftStation_K", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":1.2438292503356934, "y":7.022215366363525, "heading":2.1765898115132356, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.812013864517212, "y":5.024738311767578, "heading":2.0344433342221064, "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}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.2438292503356934 m", "val":1.2438292503356934}, "y":{"exp":"7.022215366363525 m", "val":7.022215366363525}, "heading":{"exp":"2.1765898115132356 rad", "val":2.1765898115132356}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.812013864517212 m", "val":3.812013864517212}, "y":{"exp":"5.024738311767578 m", "val":5.024738311767578}, "heading":{"exp":"2.0344433342221064 rad", "val":2.0344433342221064}, "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}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.05313], + "samples":[ + {"t":0.0, "x":1.24383, "y":7.02222, "heading":2.17659, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.28119, "ay":-7.21871, "alpha":-0.51435, "fx":[121.38776,120.84823,120.68104,121.21904], "fy":[-93.68145,-94.3758,-94.59143,-93.90161]}, + {"t":0.03009, "x":1.24803, "y":7.01895, "heading":2.17659, "vx":0.27927, "vy":-0.21721, "omega":-0.01548, "ax":9.2804, "ay":-7.21809, "alpha":-0.51414, "fx":[121.37724,120.83796,120.67092,121.20866], "fy":[-93.67364,-94.36762,-94.5832,-93.89374]}, + {"t":0.06018, "x":1.26063, "y":7.00914, "heading":2.17612, "vx":0.55851, "vy":-0.43439, "omega":-0.03095, "ax":9.27951, "ay":-7.21739, "alpha":-0.51392, "fx":[121.36543,120.82649,120.65935,121.19675], "fy":[-93.66465,-94.35816,-94.574,-93.88501]}, + {"t":0.09027, "x":1.28164, "y":6.99281, "heading":2.17519, "vx":0.83772, "vy":-0.65156, "omega":-0.04641, "ax":9.27848, "ay":-7.2166, "alpha":-0.5137, "fx":[121.35202,120.81351,120.64603,121.18302], "fy":[-93.65427,-94.34717,-94.5636,-93.8752]}, + {"t":0.12036, "x":1.31105, "y":6.96993, "heading":2.1738, "vx":1.11691, "vy":-0.8687, "omega":-0.06187, "ax":9.2773, "ay":-7.21568, "alpha":-0.51347, "fx":[121.33662,120.79863,120.63058,121.16706], "fy":[-93.64219,-94.33437,-94.5517,-93.86403]}, + {"t":0.15045, "x":1.34885, "y":6.94053, "heading":2.17193, "vx":1.39605, "vy":-1.08582, "omega":-0.07732, "ax":9.27592, "ay":-7.21461, "alpha":-0.51324, "fx":[121.31873,120.78136,120.6125,121.14838], "fy":[-93.62802,-94.31933,-94.53791,-93.85108]}, + {"t":0.18054, "x":1.39506, "y":6.90459, "heading":2.16961, "vx":1.67516, "vy":-1.3029, "omega":-0.09276, "ax":9.27429, "ay":-7.21334, "alpha":-0.51299, "fx":[121.29763,120.76097,120.59109,121.12628], "fy":[-93.61121,-94.30153,-94.52166,-93.83582]}, + {"t":0.21063, "x":1.44966, "y":6.86212, "heading":2.16682, "vx":1.95422, "vy":-1.51995, "omega":-0.1082, "ax":9.27234, "ay":-7.21182, "alpha":-0.51274, "fx":[121.27234,120.7365,120.56538,121.09977], "fy":[-93.59102,-94.28019,-94.50221,-93.81749]}, + {"t":0.24072, "x":1.51266, "y":6.81312, "heading":2.16356, "vx":2.23322, "vy":-1.73695, "omega":-0.12362, "ax":9.26995, "ay":-7.20996, "alpha":-0.51249, "fx":[121.24145,120.70653,120.53396,121.06745], "fy":[-93.56634,-94.25421,-94.47843,-93.79501]}, + {"t":0.2708, "x":1.58405, "y":6.7576, "heading":2.15984, "vx":2.51214, "vy":-1.95389, "omega":-0.13904, "ax":9.26696, "ay":-7.20764, "alpha":-0.51222, "fx":[121.2028,120.66892,120.49473,121.02721], "fy":[-93.53554,-94.22192,-94.44868,-93.76673]}, + {"t":0.30089, "x":1.66384, "y":6.69554, "heading":2.15566, "vx":2.79098, "vy":-2.17076, "omega":-0.15446, "ax":9.26312, "ay":-7.20465, "alpha":-0.51194, "fx":[121.15307,120.62036,120.44437,120.97572], "fy":[-93.49605,-94.18072,-94.41035,-93.73009]}, + {"t":0.33098, "x":1.75201, "y":6.62696, "heading":2.15101, "vx":3.0697, "vy":-2.38755, "omega":-0.16986, "ax":9.25801, "ay":-7.20068, "alpha":-0.51165, "fx":[121.08668,120.55528,120.37738,120.90745], "fy":[-93.44357,-94.12628,-94.35912,-93.68079]}, + {"t":0.36107, "x":1.84857, "y":6.55186, "heading":2.1459, "vx":3.34827, "vy":-2.60421, "omega":-0.18526, "ax":9.25086, "ay":-7.19511, "alpha":-0.51134, "fx":[120.99362,120.4637,120.28384,120.81247], "fy":[-93.37037,-94.0508,-94.2872,-93.61114]}, + {"t":0.39116, "x":1.9535, "y":6.47025, "heading":2.14033, "vx":3.62662, "vy":-2.82071, "omega":-0.20064, "ax":9.24014, "ay":-7.18677, "alpha":-0.51102, "fx":[120.85389,120.32566,120.14395,120.67092], "fy":[-93.26105,-93.93869,-94.17907,-93.50576]}, + {"t":0.42125, "x":2.06681, "y":6.38212, "heading":2.13429, "vx":3.90465, "vy":-3.03695, "omega":-0.21602, "ax":9.2223, "ay":-7.1729, "alpha":-0.51068, "fx":[120.62093,120.09473,119.91159,120.43659], "fy":[-93.07968,-93.75372,-93.99857,-93.32882]}, + {"t":0.45134, "x":2.18847, "y":6.28749, "heading":2.12779, "vx":4.18215, "vy":-3.25278, "omega":-0.23138, "ax":9.18672, "ay":-7.14523, "alpha":-0.51031, "fx":[120.15566,119.63212,119.44898,119.97136], "fy":[-92.71895,-93.38755,-93.63766,-92.97329]}, + {"t":0.48143, "x":2.31847, "y":6.18639, "heading":2.12083, "vx":4.45857, "vy":-3.46778, "omega":-0.24674, "ax":9.08083, "ay":-7.06287, "alpha":-0.50983, "fx":[118.76917,118.25046,118.07359,118.59119], "fy":[-91.64729,-92.30367,-92.56129,-91.90898]}, + {"t":0.51152, "x":2.45673, "y":6.07885, "heading":2.1134, "vx":4.7318, "vy":-3.68029, "omega":-0.26208, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[0.0,0.00002,0.0,-0.00002], "fy":[-0.00002,0.0,0.00002,0.0]}, + {"t":0.54161, "x":2.59911, "y":5.96811, "heading":2.10552, "vx":4.7318, "vy":-3.68029, "omega":-0.26208, "ax":-9.08083, "ay":7.06287, "alpha":0.50983, "fx":[-118.77171,-118.25584,-118.07099,-118.58587], "fy":[91.64434,92.29664,92.56427,91.91598]}, + {"t":0.5717, "x":2.73738, "y":5.86057, "heading":2.09763, "vx":4.45857, "vy":-3.46778, "omega":-0.24674, "ax":-9.18672, "ay":7.14523, "alpha":0.51031, "fx":[-120.16056,-119.64291,-119.44397,-119.96069], "fy":[92.71294,93.37358,93.64373,92.98721]}, + {"t":0.60179, "x":2.86737, "y":5.75946, "heading":2.09021, "vx":4.18215, "vy":-3.25278, "omega":-0.23138, "ax":-9.2223, "ay":7.1729, "alpha":0.51068, "fx":[-120.62804,-120.11061,-119.90432,-120.42087], "fy":[93.0708,93.73323,94.00752,93.34924]}, + {"t":0.63188, "x":2.98904, "y":5.66483, "heading":2.08324, "vx":3.90465, "vy":-3.03695, "omega":-0.21602, "ax":-9.24014, "ay":7.18677, "alpha":0.51102, "fx":[-120.86305,-120.34631,-120.13457,-120.65049], "fy":[93.2495,93.91211,94.19072,93.53225]}, + {"t":0.66197, "x":3.10234, "y":5.57671, "heading":2.07674, "vx":3.62662, "vy":-2.82071, "omega":-0.20064, "ax":-9.25086, "ay":7.19511, "alpha":0.51134, "fx":[-121.00469,-120.48877,-120.27251,-120.78766], "fy":[93.35634,94.01854,94.30135,93.64329]}, + {"t":0.69206, "x":3.20728, "y":5.49509, "heading":2.07071, "vx":3.34827, "vy":-2.60421, "omega":-0.18525, "ax":-9.25801, "ay":7.20068, "alpha":0.51164, "fx":[-121.09951,-120.58445,-120.36425,-120.87859], "fy":[93.42724,94.08878,94.37557,93.71816]}, + {"t":0.72215, "x":3.30383, "y":5.41999, "heading":2.06513, "vx":3.0697, "vy":-2.38755, "omega":-0.16986, "ax":-9.26312, "ay":7.20465, "alpha":0.51194, "fx":[-121.16751,-120.6533,-120.42959,-120.94313], "fy":[93.47763,94.1384,94.42892,93.77226]}, + {"t":0.75223, "x":3.39201, "y":5.35141, "heading":2.06002, "vx":2.79098, "vy":-2.17076, "omega":-0.15446, "ax":-9.26696, "ay":7.20764, "alpha":0.51222, "fx":[-121.2187,-120.70529,-120.47845,-120.99122], "fy":[93.51522,94.17521,94.46917,93.81328]}, + {"t":0.78232, "x":3.47179, "y":5.28936, "heading":2.05537, "vx":2.51214, "vy":-1.95389, "omega":-0.13904, "ax":-9.26995, "ay":7.20996, "alpha":0.51248, "fx":[-121.25865,-120.74598,-120.51634,-121.02841], "fy":[93.54431,94.20353,94.50064,93.8455]}, + {"t":0.81241, "x":3.54318, "y":5.23383, "heading":2.05119, "vx":2.23322, "vy":-1.73695, "omega":-0.12362, "ax":-9.27234, "ay":7.21182, "alpha":0.51274, "fx":[-121.29072,-120.77872,-120.54656,-121.058], "fy":[93.56747,94.22599,94.52595,93.8715]}, + {"t":0.8425, "x":3.60618, "y":5.18483, "heading":2.04747, "vx":1.95422, "vy":-1.51995, "omega":-0.1082, "ax":-9.27429, "ay":7.21334, "alpha":0.51299, "fx":[-121.31702,-120.80561,-120.57124,-121.08211], "fy":[93.58634,94.24423,94.54674,93.89291]}, + {"t":0.87259, "x":3.66078, "y":5.14236, "heading":2.04422, "vx":1.67516, "vy":-1.3029, "omega":-0.09276, "ax":-9.27592, "ay":7.21461, "alpha":0.51323, "fx":[-121.33898,-120.82808,-120.59176,-121.10215], "fy":[93.60201,94.25937,94.56413,93.91082]}, + {"t":0.90268, "x":3.70699, "y":5.10642, "heading":2.04142, "vx":1.39605, "vy":-1.08582, "omega":-0.07732, "ax":-9.2773, "ay":7.21568, "alpha":0.51347, "fx":[-121.35759,-120.8471,-120.6091,-121.11911], "fy":[93.61524,94.27217,94.57887,93.92599]}, + {"t":0.93277, "x":3.7448, "y":5.07702, "heading":2.0391, "vx":1.11691, "vy":-0.8687, "omega":-0.06187, "ax":-9.27848, "ay":7.2166, "alpha":0.5137, "fx":[-121.37356,-120.86337,-120.62396,-121.13368], "fy":[93.62657,94.28319,94.59153,93.93896]}, + {"t":0.96286, "x":3.7742, "y":5.05415, "heading":2.03724, "vx":0.83772, "vy":-0.65156, "omega":-0.04641, "ax":-9.27951, "ay":7.21739, "alpha":0.51392, "fx":[-121.38739,-120.87742,-120.63685,-121.14636], "fy":[93.6364,94.29282,94.60249,93.95011]}, + {"t":0.99295, "x":3.79521, "y":5.03781, "heading":2.03584, "vx":0.55851, "vy":-0.43439, "omega":-0.03095, "ax":-9.2804, "ay":7.21809, "alpha":0.51414, "fx":[-121.39948,-120.88961,-120.64814,-121.15756], "fy":[93.64502,94.30136,94.61206,93.95976]}, + {"t":1.02304, "x":3.80781, "y":5.02801, "heading":2.03491, "vx":0.27927, "vy":-0.21721, "omega":-0.01548, "ax":-9.28119, "ay":7.21871, "alpha":0.51435, "fx":[-121.41012,-120.90026,-120.65813,-121.16755], "fy":[93.65265,94.30905,94.62047,93.96811]}, + {"t":1.05313, "x":3.81201, "y":5.02474, "heading":2.03444, "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/leftStation_L.traj b/src/main/deploy/choreo/leftStation_L.traj new file mode 100644 index 0000000..08884c0 --- /dev/null +++ b/src/main/deploy/choreo/leftStation_L.traj @@ -0,0 +1,69 @@ +{ + "name":"leftStation_L", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":0.8754363656044006, "y":7.439517498016357, "heading":2.2035452332948458, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.3041272163391113, "y":5.3192315101623535, "heading":2.1451009156211556, "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":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.8754363656044006 m", "val":0.8754363656044006}, "y":{"exp":"7.439517498016357 m", "val":7.439517498016357}, "heading":{"exp":"2.2035452332948458 rad", "val":2.2035452332948458}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.3041272163391113 m", "val":3.3041272163391113}, "y":{"exp":"5.3192315101623535 m", "val":5.3192315101623535}, "heading":{"exp":"2.1451009156211556 rad", "val":2.1451009156211556}, "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":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.04791], + "samples":[ + {"t":0.0, "x":0.87544, "y":7.43952, "heading":2.20355, "vx":0.0, "vy":0.0, "omega":0.0, "ax":8.8539, "ay":-7.7375, "alpha":-0.2605, "fx":[115.64466,115.34527,115.27922,115.57801], "fy":[-100.6933,-101.03592,-101.11218,-100.77073]}, + {"t":0.03082, "x":0.87964, "y":7.43584, "heading":2.20355, "vx":0.27289, "vy":-0.23848, "omega":-0.00803, "ax":8.85393, "ay":-7.73593, "alpha":-0.24706, "fx":[115.63553,115.35166,115.28902,115.57234], "fy":[-100.68365,-101.00854,-101.08095,-100.7571]}, + {"t":0.06164, "x":0.89226, "y":7.42482, "heading":2.2033, "vx":0.54577, "vy":-0.47691, "omega":-0.01564, "ax":8.85376, "ay":-7.73437, "alpha":-0.23616, "fx":[115.62565,115.35439,115.29445,115.56521], "fy":[-100.67207,-100.98257,-101.05194,-100.74239]}, + {"t":0.09246, "x":0.91328, "y":7.40645, "heading":2.20282, "vx":0.81865, "vy":-0.71529, "omega":-0.02292, "ax":8.85337, "ay":-7.7328, "alpha":-0.22751, "fx":[115.61457,115.35333,115.29548,115.55625], "fy":[-100.65849,-100.95754,-101.02458,-100.72641]}, + {"t":0.12328, "x":0.94272, "y":7.38073, "heading":2.20211, "vx":1.09152, "vy":-0.95362, "omega":-0.02993, "ax":8.85275, "ay":-7.73118, "alpha":-0.22072, "fx":[115.60173,115.34837,115.29209,115.54501], "fy":[-100.64272,-100.93277,-100.99808,-100.70886]}, + {"t":0.1541, "x":0.98057, "y":7.34766, "heading":2.20119, "vx":1.36437, "vy":-1.1919, "omega":-0.03674, "ax":8.85186, "ay":-7.72946, "alpha":-0.21542, "fx":[115.58646,115.3393,115.28418,115.53092], "fy":[-100.62445,-100.90741,-100.97147,-100.6893]}, + {"t":0.18493, "x":1.02682, "y":7.30726, "heading":2.20005, "vx":1.6372, "vy":-1.43013, "omega":-0.04338, "ax":8.85066, "ay":-7.72757, "alpha":-0.21125, "fx":[115.56792,115.32566,115.27138,115.51324], "fy":[-100.60312,-100.88048,-100.94367,-100.66707]}, + {"t":0.21575, "x":1.08149, "y":7.25951, "heading":2.19872, "vx":1.90998, "vy":-1.6683, "omega":-0.04989, "ax":8.84908, "ay":-7.72544, "alpha":-0.20792, "fx":[115.54498,115.30667,115.25298,115.49091], "fy":[-100.57789,-100.85073,-100.91335,-100.64125]}, + {"t":0.24657, "x":1.14456, "y":7.20442, "heading":2.19718, "vx":2.18272, "vy":-1.90641, "omega":-0.0563, "ax":8.847, "ay":-7.72295, "alpha":-0.20521, "fx":[115.51605,115.28099,115.22772,115.46241], "fy":[-100.54743,-100.81652,-100.87881,-100.61043]}, + {"t":0.27739, "x":1.21603, "y":7.142, "heading":2.19544, "vx":2.45539, "vy":-2.14444, "omega":-0.06262, "ax":8.84426, "ay":-7.71992, "alpha":-0.20292, "fx":[115.47872,115.24645,115.19347,115.42538], "fy":[-100.50967,-100.77554,-100.83766,-100.57249]}, + {"t":0.30821, "x":1.29591, "y":7.07223, "heading":2.19351, "vx":2.72798, "vy":-2.38237, "omega":-0.06887, "ax":8.84056, "ay":-7.71609, "alpha":-0.20087, "fx":[115.42913,115.1994,115.14665,115.37603], "fy":[-100.4612,-100.72414,-100.78623,-100.52399]}, + {"t":0.33903, "x":1.38419, "y":6.99514, "heading":2.19139, "vx":3.00046, "vy":-2.62019, "omega":-0.07507, "ax":8.83541, "ay":-7.711, "alpha":-0.19891, "fx":[115.36069,115.13343,115.0809,115.30781], "fy":[-100.39616,-100.6562,-100.71836,-100.45899]}, + {"t":0.36985, "x":1.48086, "y":6.91072, "heading":2.18908, "vx":3.27277, "vy":-2.85785, "omega":-0.0812, "ax":8.82788, "ay":-7.70377, "alpha":-0.19681, "fx":[115.26093,115.03635,114.98411,115.20836], "fy":[-100.30341,-100.56028,-100.62254,-100.36632]}, + {"t":0.40067, "x":1.58593, "y":6.81898, "heading":2.18658, "vx":3.54486, "vy":-3.09529, "omega":-0.08726, "ax":8.81593, "ay":-7.69255, "alpha":-0.19421, "fx":[115.10319,114.88195,114.83026,115.05119], "fy":[-100.15913,-100.41204,-100.47435,-100.22208]}, + {"t":0.43149, "x":1.69937, "y":6.71993, "heading":2.18389, "vx":3.81657, "vy":-3.33238, "omega":-0.09325, "ax":8.79433, "ay":-7.67257, "alpha":-0.19029, "fx":[114.8184,114.60218,114.55168,114.76761], "fy":[-99.90158,-100.14847,-100.21061,-99.96432]}, + {"t":0.46231, "x":1.82118, "y":6.61358, "heading":2.18101, "vx":4.08762, "vy":-3.56885, "omega":-0.09911, "ax":8.74395, "ay":-7.62634, "alpha":-0.18211, "fx":[114.15454,113.94866,113.90138,114.10699], "fy":[-99.30554,-99.53998,-99.60104,-99.36714]}, + {"t":0.49314, "x":1.95131, "y":6.49996, "heading":2.17796, "vx":4.35712, "vy":-3.80391, "omega":-0.10473, "ax":8.49581, "ay":-7.39982, "alpha":-0.1434, "fx":[110.8871,110.72849,110.697,110.85545], "fy":[-96.38419,-96.56233,-96.61439,-96.43657]}, + {"t":0.52396, "x":2.08964, "y":6.37921, "heading":2.17473, "vx":4.61897, "vy":-4.03198, "omega":-0.10915, "ax":-8.48169, "ay":7.41705, "alpha":0.25715, "fx":[-110.77897,-110.49386,-110.43694,-110.72153], "fy":[96.51769,96.8367,96.93011,96.61212]}, + {"t":0.55478, "x":2.22797, "y":6.25846, "heading":2.17137, "vx":4.35755, "vy":-3.80337, "omega":-0.10122, "ax":-8.73917, "ay":7.63187, "alpha":0.21936, "fx":[-114.11909,-113.8714,-113.81219,-114.05949], "fy":[99.34652,99.62816,99.70414,99.42329]}, + {"t":0.5856, "x":2.35812, "y":6.14486, "heading":2.16825, "vx":4.0882, "vy":-3.56815, "omega":-0.09446, "ax":-8.79169, "ay":7.67556, "alpha":0.21174, "fx":[-114.80043,-114.56062,-114.50075,-114.7402], "fy":[99.92171,100.19529,100.26856,99.99574]}, + {"t":0.61642, "x":2.47995, "y":6.03853, "heading":2.16533, "vx":3.81723, "vy":-3.33158, "omega":-0.08793, "ax":-8.81433, "ay":7.69434, "alpha":0.20845, "fx":[-115.09418,-114.85788,-114.79741,-115.03337], "fy":[100.16876,100.43871,100.51125,100.24202]}, + {"t":0.64724, "x":2.59342, "y":5.9395, "heading":2.16262, "vx":3.54557, "vy":-3.09444, "omega":-0.08151, "ax":-8.82695, "ay":7.70477, "alpha":0.20663, "fx":[-115.258,-115.02372,-114.96268,-115.19662], "fy":[100.30595,100.5738,100.64621,100.37908]}, + {"t":0.67806, "x":2.6985, "y":5.84779, "heading":2.16011, "vx":3.27351, "vy":-2.85697, "omega":-0.07514, "ax":-8.83499, "ay":7.71141, "alpha":0.20546, "fx":[-115.36255,-115.12962,-115.06804,-115.30064], "fy":[100.39314,100.6596,100.73214,100.46639]}, + {"t":0.70888, "x":2.7952, "y":5.7634, "heading":2.1578, "vx":3.00121, "vy":-2.6193, "omega":-0.06881, "ax":-8.84058, "ay":7.716, "alpha":0.20466, "fx":[-115.43511,-115.20314,-115.14106,-115.3727], "fy":[100.45342,100.71887,100.79165,100.5269]}, + {"t":0.7397, "x":2.8835, "y":5.68633, "heading":2.15568, "vx":2.72874, "vy":-2.38148, "omega":-0.0625, "ax":-8.84467, "ay":7.71937, "alpha":0.20408, "fx":[-115.48842,-115.25718,-115.19463,-115.42556], "fy":[100.49756,100.76225,100.83532,100.57133]}, + {"t":0.77052, "x":2.9634, "y":5.6166, "heading":2.15375, "vx":2.45613, "vy":-2.14356, "omega":-0.05621, "ax":-8.84781, "ay":7.72195, "alpha":0.20364, "fx":[-115.52925,-115.29858,-115.23561,-115.46596], "fy":[100.53129,100.79538,100.86875,100.60536]}, + {"t":0.80134, "x":3.0349, "y":5.5542, "heading":2.15202, "vx":2.18344, "vy":-1.90557, "omega":-0.04993, "ax":-8.85029, "ay":7.72398, "alpha":0.20329, "fx":[-115.56151,-115.3313,-115.26794,-115.49784], "fy":[100.55791,100.82151,100.89517,100.63226]}, + {"t":0.83217, "x":3.09799, "y":5.49914, "heading":2.15048, "vx":1.91066, "vy":-1.66751, "omega":-0.04367, "ax":-8.85229, "ay":7.72563, "alpha":0.20302, "fx":[-115.58763,-115.35779,-115.2941,-115.52362], "fy":[100.57946,100.84267,100.9166,100.65409]}, + {"t":0.86299, "x":3.15267, "y":5.45141, "heading":2.14913, "vx":1.63783, "vy":-1.42939, "omega":-0.03741, "ax":-8.85395, "ay":7.72699, "alpha":0.2028, "fx":[-115.6092,-115.37967,-115.31568,-115.54489], "fy":[100.59728,100.86016,100.93434,100.67215]}, + {"t":0.89381, "x":3.19895, "y":5.41103, "heading":2.14798, "vx":1.36494, "vy":-1.19124, "omega":-0.03116, "ax":-8.85533, "ay":7.72814, "alpha":0.20261, "fx":[-115.6273,-115.39803,-115.33378,-115.56273], "fy":[100.61228,100.87489,100.94928,100.68737]}, + {"t":0.92463, "x":3.23681, "y":5.37798, "heading":2.14702, "vx":1.09201, "vy":-0.95305, "omega":-0.02491, "ax":-8.85651, "ay":7.72912, "alpha":0.20246, "fx":[-115.64269,-115.41364,-115.34917,-115.57791], "fy":[100.62509,100.88747,100.96204,100.70035]}, + {"t":0.95545, "x":3.26626, "y":5.35228, "heading":2.14625, "vx":0.81904, "vy":-0.71483, "omega":-0.01867, "ax":-8.85753, "ay":7.72997, "alpha":0.20233, "fx":[-115.65593,-115.42706,-115.36241,-115.59098], "fy":[100.63618,100.89837,100.97308,100.71158]}, + {"t":0.98627, "x":3.2873, "y":5.33392, "heading":2.14568, "vx":0.54605, "vy":-0.47659, "omega":-0.01244, "ax":-8.85841, "ay":7.73071, "alpha":0.20222, "fx":[-115.66742,-115.4387,-115.37392,-115.60234], "fy":[100.64588,100.90792,100.98272,100.72137]}, + {"t":1.01709, "x":3.29992, "y":5.3229, "heading":2.14529, "vx":0.27302, "vy":-0.23832, "omega":-0.00621, "ax":-8.85828, "ay":7.73241, "alpha":0.20135, "fx":[-115.66445,-115.43864,-115.37159,-115.60061], "fy":[100.66945,100.92811,101.00545,100.74346]}, + {"t":1.04791, "x":3.30413, "y":5.31923, "heading":2.1451, "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/leftTaxi.traj b/src/main/deploy/choreo/leftTaxi.traj new file mode 100644 index 0000000..69a9353 --- /dev/null +++ b/src/main/deploy/choreo/leftTaxi.traj @@ -0,0 +1,60 @@ +{ + "name":"leftTaxi", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.703075408935547, "y":6.927060127258301, "heading":3.141592653589793, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.952167510986328, "y":6.94295072555542, "heading":-3.1183409396980335, "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":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"7.703075408935547 m", "val":7.703075408935547}, "y":{"exp":"6.927060127258301 m", "val":6.927060127258301}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.952167510986328 m", "val":5.952167510986328}, "y":{"exp":"6.94295072555542 m", "val":6.94295072555542}, "heading":{"exp":"-3.1183409396980335 rad", "val":-3.1183409396980335}, "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":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.77285], + "samples":[ + {"t":0.0, "x":7.70308, "y":6.92706, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-11.75792, "ay":0.09372, "alpha":0.1555, "fx":[-153.33331,-153.3311,-153.33149,-153.3337], "fy":[1.08424,1.36237,1.35991,1.08228]}, + {"t":0.03091, "x":7.69746, "y":6.9271, "heading":3.14159, "vx":-0.36348, "vy":0.0029, "omega":0.00481, "ax":-11.75647, "ay":0.09485, "alpha":0.15547, "fx":[-153.31448,-153.31224,-153.31267,-153.3149], "fy":[1.09898,1.37703,1.37454,1.09699]}, + {"t":0.06183, "x":7.6806, "y":6.92724, "heading":-3.14144, "vx":-0.72692, "vy":0.00583, "omega":0.00961, "ax":-11.75474, "ay":0.09598, "alpha":0.15544, "fx":[-153.2919,-153.28963,-153.2901,-153.29236], "fy":[1.11381,1.39176,1.38921,1.11175]}, + {"t":0.09274, "x":7.65251, "y":6.92747, "heading":-3.14115, "vx":-1.0903, "vy":0.0088, "omega":0.01442, "ax":-11.75263, "ay":0.09712, "alpha":0.15542, "fx":[-153.26433,-153.26204,-153.26256,-153.26484], "fy":[1.12874,1.4066,1.40394,1.12658]}, + {"t":0.12366, "x":7.61319, "y":6.92778, "heading":-3.1407, "vx":-1.45362, "vy":0.0118, "omega":0.01922, "ax":-11.74999, "ay":0.09826, "alpha":0.1554, "fx":[-153.2299,-153.22758,-153.22816,-153.23047], "fy":[1.14381,1.42157,1.41875,1.1415]}, + {"t":0.15457, "x":7.56264, "y":6.9282, "heading":-3.14011, "vx":-1.81686, "vy":0.01484, "omega":0.02403, "ax":-11.7466, "ay":0.09942, "alpha":0.15538, "fx":[-153.18567,-153.18332,-153.18399,-153.18633], "fy":[1.15906,1.43671,1.4337,1.15656]}, + {"t":0.18548, "x":7.50086, "y":6.9287, "heading":-3.13936, "vx":-2.17999, "vy":0.01791, "omega":0.02883, "ax":-11.74208, "ay":0.1006, "alpha":0.15536, "fx":[-153.12675,-153.12438,-153.12515,-153.12752], "fy":[1.17459,1.45209,1.44886,1.17185]}, + {"t":0.2164, "x":7.42786, "y":6.9293, "heading":-3.13847, "vx":-2.54298, "vy":0.02102, "omega":0.03363, "ax":-11.73577, "ay":0.1018, "alpha":0.15535, "fx":[-153.04435,-153.04195,-153.04287,-153.04526], "fy":[1.19052,1.46786,1.46435,1.18751]}, + {"t":0.24731, "x":7.34364, "y":6.93, "heading":-3.13743, "vx":-2.90578, "vy":0.02417, "omega":0.03844, "ax":-11.72631, "ay":0.10305, "alpha":0.15535, "fx":[-152.92088,-152.91845,-152.9196,-152.92202], "fy":[1.20715,1.48425,1.48043,1.20382]}, + {"t":0.27822, "x":7.24821, "y":6.9308, "heading":-3.13624, "vx":-3.26829, "vy":0.02735, "omega":0.04324, "ax":-11.71057, "ay":0.10441, "alpha":0.15535, "fx":[-152.71539,-152.71294,-152.71446,-152.7169], "fy":[1.22514,1.50188,1.4977,1.22146]}, + {"t":0.30914, "x":7.14158, "y":6.93169, "heading":-3.13491, "vx":-3.6303, "vy":0.03058, "omega":0.04804, "ax":-11.67915, "ay":0.106, "alpha":0.15537, "fx":[-152.30536,-152.30288,-152.30515,-152.30761], "fy":[1.24648,1.52251,1.51793,1.2424]}, + {"t":0.34005, "x":7.02377, "y":6.93269, "heading":-3.13342, "vx":-3.99135, "vy":0.03386, "omega":0.05284, "ax":-11.58544, "ay":0.10854, "alpha":0.15544, "fx":[-151.08215,-151.07965,-151.08411,-151.08659], "fy":[1.28091,1.55486,1.54985,1.2764]}, + {"t":0.37097, "x":6.89485, "y":6.93379, "heading":-3.13179, "vx":-4.3495, "vy":0.03721, "omega":0.05765, "ax":0.00129, "ay":0.14523, "alpha":0.0098, "fx":[0.02122,0.02113,0.01236,0.01245], "fy":[1.88958,1.89835,1.89826,1.88949]}, + {"t":0.40188, "x":6.76039, "y":6.93501, "heading":-3.13001, "vx":-4.34946, "vy":0.0417, "omega":0.05795, "ax":11.5855, "ay":-0.10152, "alpha":-0.15526, "fx":[151.08292,151.08061,151.08507,151.08736], "fy":[-1.18989,-1.46351,-1.45774,-1.18461]}, + {"t":0.43279, "x":6.63146, "y":6.93625, "heading":-3.12822, "vx":-3.99131, "vy":0.03856, "omega":0.05315, "ax":11.67916, "ay":-0.10577, "alpha":-0.15549, "fx":[152.3054,152.30294,152.30523,152.30767], "fy":[-1.24429,-1.5205,-1.51408,-1.23837]}, + {"t":0.46371, "x":6.51366, "y":6.93739, "heading":-3.12657, "vx":-3.63026, "vy":0.03529, "omega":0.04835, "ax":11.71054, "ay":-0.10794, "alpha":-0.15567, "fx":[152.71503,152.7125,152.71406,152.71657], "fy":[-1.27232,-1.54956,-1.54261,-1.26587]}, + {"t":0.49462, "x":6.40703, "y":6.93843, "heading":-3.12508, "vx":-3.26824, "vy":0.03196, "omega":0.04353, "ax":11.72625, "ay":-0.10958, "alpha":-0.15583, "fx":[152.92019,152.91762,152.91881,152.92137], "fy":[-1.29367,-1.57157,-1.56415,-1.28675]}, + {"t":0.52553, "x":6.3116, "y":6.93937, "heading":-3.12373, "vx":-2.90574, "vy":0.02857, "omega":0.03872, "ax":11.73569, "ay":-0.11102, "alpha":-0.156, "fx":[153.04338,153.04076,153.04174,153.04433], "fy":[-1.31233,-1.59074,-1.5829,-1.30499]}, + {"t":0.55645, "x":6.22738, "y":6.9402, "heading":-3.12254, "vx":-2.54295, "vy":0.02514, "omega":0.03389, "ax":11.74198, "ay":-0.11235, "alpha":-0.15616, "fx":[153.12551,153.12286,153.12369,153.12632], "fy":[-1.32964,-1.60848,-1.60026,-1.32193]}, + {"t":0.58736, "x":6.15438, "y":6.94092, "heading":-3.12149, "vx":-2.17996, "vy":0.02166, "omega":0.02907, "ax":11.74647, "ay":-0.11362, "alpha":-0.15633, "fx":[153.18417,153.18148,153.18221,153.18488], "fy":[-1.34617,-1.62541,-1.61685,-1.33812]}, + {"t":0.61828, "x":6.0926, "y":6.94153, "heading":-3.12059, "vx":-1.81683, "vy":0.01815, "omega":0.02423, "ax":11.74984, "ay":-0.11485, "alpha":-0.1565, "fx":[153.22814,153.22542,153.22607,153.22878], "fy":[-1.36221,-1.64181,-1.63297,-1.35387]}, + {"t":0.64919, "x":6.04205, "y":6.94204, "heading":-3.11984, "vx":-1.4536, "vy":0.0146, "omega":0.01939, "ax":11.75246, "ay":-0.11606, "alpha":-0.15667, "fx":[153.26232,153.25956,153.26016,153.2629], "fy":[-1.37791,-1.65787,-1.64878,-1.36932]}, + {"t":0.6801, "x":6.00273, "y":6.94244, "heading":-3.11924, "vx":-1.09028, "vy":0.01101, "omega":0.01455, "ax":11.75455, "ay":-0.11725, "alpha":-0.15684, "fx":[153.28964,153.28685,153.2874,153.29017], "fy":[-1.39336,-1.67369,-1.66438,-1.38457]}, + {"t":0.71102, "x":5.97464, "y":6.94272, "heading":-3.11879, "vx":-0.72691, "vy":0.00739, "omega":0.0097, "ax":11.75626, "ay":-0.11843, "alpha":-0.15702, "fx":[153.31198,153.30915,153.30967,153.31247], "fy":[-1.40864,-1.68931,-1.67984,-1.39968]}, + {"t":0.74193, "x":5.95779, "y":6.94289, "heading":-3.11849, "vx":-0.36347, "vy":0.00373, "omega":0.00485, "ax":11.75767, "ay":-0.12061, "alpha":-0.15685, "fx":[153.33044,153.32757,153.32805,153.3309], "fy":[-1.43731,-1.71731,-1.7085,-1.42819]}, + {"t":0.77285, "x":5.95217, "y":6.94295, "heading":-3.11834, "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/left_F.traj b/src/main/deploy/choreo/left_F.traj new file mode 100644 index 0000000..7b4447d --- /dev/null +++ b/src/main/deploy/choreo/left_F.traj @@ -0,0 +1,73 @@ +{ + "name":"left_F", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.531867027282715, "y":1.572913646697998, "heading":3.141592653589793, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.352854251861572, "y":2.958143472671509, "heading":2.2142976806797927, "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":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"7.531867027282715 m", "val":7.531867027282715}, "y":{"exp":"1.572913646697998 m", "val":1.572913646697998}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.352854251861572 m", "val":5.352854251861572}, "y":{"exp":"2.958143472671509 m", "val":2.958143472671509}, "heading":{"exp":"2.2142976806797927 rad", "val":2.2142976806797927}, "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":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.93824], + "samples":[ + {"t":0.0, "x":7.53187, "y":1.57291, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-9.91231, "ay":6.3113, "alpha":-4.21897, "fx":[-128.7069,-132.00483,-129.9342,-126.41125], "fy":[83.27407,77.92835,81.31324,86.70135]}, + {"t":0.02469, "x":7.52885, "y":1.57484, "heading":3.14159, "vx":-0.24474, "vy":0.15583, "omega":-0.10417, "ax":-9.91175, "ay":6.3101, "alpha":-4.21766, "fx":[-128.70013,-131.99625,-129.92631,-126.40537], "fy":[83.25865,77.9145,81.29748,86.68404]}, + {"t":0.04938, "x":7.51978, "y":1.58061, "heading":3.13902, "vx":-0.48947, "vy":0.31163, "omega":-0.2083, "ax":-9.91108, "ay":6.30884, "alpha":-4.21636, "fx":[-128.68488,-131.98445,-129.9242,-126.39944], "fy":[83.25319,77.90275,81.26906,86.66362]}, + {"t":0.07407, "x":7.50468, "y":1.59023, "heading":3.13388, "vx":-0.73418, "vy":0.4674, "omega":-0.31241, "ax":-9.91027, "ay":6.30749, "alpha":-4.21507, "fx":[-128.66089,-131.96907,-129.92754,-126.39324], "fy":[83.25747,77.893,81.22785,86.6398]}, + {"t":0.09876, "x":7.48353, "y":1.60369, "heading":3.12616, "vx":-0.97887, "vy":0.62313, "omega":-0.41648, "ax":-9.9093, "ay":6.30603, "alpha":-4.21379, "fx":[-128.62786,-131.94967,-129.93589,-126.38653], "fy":[83.2712,77.88519,81.17364,86.61215]}, + {"t":0.12345, "x":7.45634, "y":1.621, "heading":3.11588, "vx":-1.22353, "vy":0.77883, "omega":-0.52052, "ax":-9.90812, "ay":6.30445, "alpha":-4.21252, "fx":[-128.58536,-131.92561,-129.94872,-126.37901], "fy":[83.294,77.8793,81.10622,86.58006]}, + {"t":0.14814, "x":7.42311, "y":1.64215, "heading":3.10303, "vx":-1.46817, "vy":0.93449, "omega":-0.62453, "ax":-9.9067, "ay":6.3027, "alpha":-4.21126, "fx":[-128.53287,-131.89611,-129.96528,-126.37032], "fy":[83.32534,77.87525,81.02528,86.54274]}, + {"t":0.17283, "x":7.38384, "y":1.66714, "heading":3.08761, "vx":-1.71277, "vy":1.09011, "omega":-0.72851, "ax":-9.90497, "ay":6.30075, "alpha":-4.21001, "fx":[-128.46966,-131.86007,-129.9846,-126.35994], "fy":[83.36452,77.87294,80.93039,86.49915]}, + {"t":0.19752, "x":7.33853, "y":1.69598, "heading":3.06962, "vx":-1.95733, "vy":1.24568, "omega":-0.83246, "ax":-9.90284, "ay":6.29854, "alpha":-4.20876, "fx":[-128.39474,-131.81608,-130.00536,-126.34706], "fy":[83.41061,77.87212,80.82096,86.44792]}, + {"t":0.22221, "x":7.28718, "y":1.72865, "heading":3.04907, "vx":-2.20184, "vy":1.40119, "omega":-0.93637, "ax":-9.90019, "ay":6.29598, "alpha":-4.20753, "fx":[-128.30665,-131.76219,-130.0257,-126.33051], "fy":[83.46226,77.87231,80.6961,86.38727]}, + {"t":0.24691, "x":7.2298, "y":1.76517, "heading":3.02595, "vx":-2.44628, "vy":1.55664, "omega":-1.04026, "ax":-9.89684, "ay":6.29294, "alpha":-4.2063, "fx":[-128.20327,-131.69566,-130.04294,-126.30836], "fy":[83.51757,77.87258,80.55444,86.31476]}, + {"t":0.2716, "x":7.16639, "y":1.80552, "heading":3.00026, "vx":-2.69064, "vy":1.71202, "omega":-1.14411, "ax":-9.89251, "ay":6.28923, "alpha":-4.20508, "fx":[-128.08129,-131.61245,-130.05305,-126.27752], "fy":[83.57372,77.87124,80.39381,86.22692]}, + {"t":0.29629, "x":7.09694, "y":1.84971, "heading":2.97201, "vx":-2.93489, "vy":1.8673, "omega":-1.24794, "ax":-9.88675, "ay":6.28453, "alpha":-4.20387, "fx":[-127.93531,-131.50628,-130.04964,-126.2327], "fy":[83.62625,77.86519,80.21055,86.11859]}, + {"t":0.32098, "x":7.02146, "y":1.89773, "heading":2.9412, "vx":-3.179, "vy":2.02247, "omega":-1.35174, "ax":-9.87879, "ay":6.2783, "alpha":-4.20268, "fx":[-127.75591,-131.36662,-130.02184,-126.16442], "fy":[83.66764,77.84858,79.99811,85.98137]}, + {"t":0.34567, "x":6.93996, "y":1.94958, "heading":2.90783, "vx":-3.42291, "vy":2.17749, "omega":-1.4555, "ax":-9.86718, "ay":6.26952, "alpha":-4.20153, "fx":[-127.52515,-131.17399,-129.94943,-126.05431], "fy":[83.68407,77.80965,79.74378,85.80015]}, + {"t":0.37036, "x":6.85244, "y":2.00525, "heading":2.87189, "vx":-3.66653, "vy":2.33228, "omega":-1.55924, "ax":-9.84879, "ay":6.256, "alpha":-4.20048, "fx":[-127.20438,-130.88735,-129.78959,-125.86238], "fy":[83.64643,77.72237,79.4198,85.54372]}, + {"t":0.39505, "x":6.75891, "y":2.06475, "heading":2.83339, "vx":-3.90971, "vy":2.48675, "omega":-1.66295, "ax":-9.81556, "ay":6.23207, "alpha":-4.19968, "fx":[-126.693,-130.40123,-129.43211,-125.48402], "fy":[83.48024,77.51791,78.95323,85.13282]}, + {"t":0.41974, "x":6.65938, "y":2.12804, "heading":2.79233, "vx":-4.15206, "vy":2.64062, "omega":-1.76664, "ax":-9.73812, "ay":6.17713, "alpha":-4.19968, "fx":[-125.62365,-129.33313,-128.47739,-124.53664], "fy":[82.91623,76.94345,78.0767,84.28203]}, + {"t":0.44443, "x":6.5539, "y":2.19513, "heading":2.74871, "vx":-4.3925, "vy":2.79314, "omega":-1.87034, "ax":-9.35928, "ay":5.91121, "alpha":-4.20228, "fx":[-120.77324,-124.33672,-123.43345,-119.66576], "fy":[79.60671,73.78311,74.44976,80.50764]}, + {"t":0.46912, "x":6.44259, "y":2.26589, "heading":2.70253, "vx":-4.62358, "vy":2.93909, "omega":-1.97409, "ax":9.32553, "ay":-5.96464, "alpha":4.16476, "fx":[120.23549,123.84069,123.08729,119.28518], "fy":[-80.4032,-74.63048,-75.04331,-81.05725]}, + {"t":0.49381, "x":6.33127, "y":2.33664, "heading":2.65379, "vx":-4.39333, "vy":2.79182, "omega":-1.87126, "ax":9.72698, "ay":-6.1947, "alpha":4.18831, "fx":[125.15972,128.97672,128.63929,124.61395], "fy":[-83.60021,-77.56047,-77.82845,-84.14596]}, + {"t":0.5185, "x":6.22577, "y":2.40368, "heading":2.60759, "vx":-4.15317, "vy":2.63887, "omega":-1.76785, "ax":9.80955, "ay":-6.24153, "alpha":4.1942, "fx":[126.09276,129.96581,129.85625,125.7822], "fy":[-84.3696,-78.26662,-78.27192,-84.66961]}, + {"t":0.54319, "x":6.12621, "y":2.46694, "heading":2.56394, "vx":-3.91096, "vy":2.48476, "omega":-1.6643, "ax":9.84528, "ay":-6.26152, "alpha":4.19791, "fx":[126.44748,130.34693,130.42777,126.33828], "fy":[-84.77204,-78.64572,-78.38511,-84.81761]}, + {"t":0.56788, "x":6.03265, "y":2.52638, "heading":2.52285, "vx":-3.66788, "vy":2.33016, "omega":-1.56065, "ax":9.86527, "ay":-6.27251, "alpha":4.20082, "fx":[126.61361,130.52527,130.77681,126.68756], "fy":[-85.04302,-78.9128,-78.39684,-84.84118]}, + {"t":0.59257, "x":5.94509, "y":2.582, "heading":2.48432, "vx":-3.4243, "vy":2.17529, "omega":-1.45693, "ax":9.87808, "ay":-6.27941, "alpha":4.2033, "fx":[126.69792,130.61268,131.02009,126.94091], "fy":[-85.24773,-79.12612,-78.36932,-84.81024]}, + {"t":0.61726, "x":5.86356, "y":2.63379, "heading":2.44834, "vx":-3.1804, "vy":2.02025, "omega":-1.35314, "ax":9.88702, "ay":-6.28409, "alpha":4.2055, "fx":[126.74137,130.65269,131.20306,127.14068], "fy":[-85.41176,-79.30757,-78.32598,-84.75245]}, + {"t":0.64195, "x":5.78805, "y":2.68176, "heading":2.41493, "vx":-2.93629, "vy":1.86509, "omega":-1.24931, "ax":9.89363, "ay":-6.28745, "alpha":4.20751, "fx":[126.76286,130.66594,131.34728,127.3064], "fy":[-85.5475,-79.467,-78.2773,-84.68124]}, + {"t":0.66664, "x":5.71856, "y":2.72589, "heading":2.38409, "vx":-2.69201, "vy":1.70985, "omega":-1.14542, "ax":9.89872, "ay":-6.28995, "alpha":4.20938, "fx":[126.77221,130.66356,131.46445,127.44816], "fy":[-85.66183,-79.60931,-78.22842,-84.60412]}, + {"t":0.69133, "x":5.65511, "y":2.76619, "heading":2.35581, "vx":-2.4476, "vy":1.55455, "omega":-1.04149, "ax":9.90279, "ay":-6.29187, "alpha":4.21114, "fx":[126.7749,130.65216,131.56156,127.57166], "fy":[-85.75903,-79.7371,-78.18197,-84.52572]}, + {"t":0.71603, "x":5.5977, "y":2.80266, "heading":2.33009, "vx":-2.2031, "vy":1.3992, "omega":-0.93752, "ax":9.90611, "ay":-6.29338, "alpha":4.21282, "fx":[126.77421,130.63594,131.64313,127.68029], "fy":[-85.84198,-79.85185,-78.13928,-84.4491]}, + {"t":0.74072, "x":5.54632, "y":2.83529, "heading":2.30694, "vx":-1.95851, "vy":1.24381, "omega":-0.8335, "ax":9.90888, "ay":-6.29457, "alpha":4.21444, "fx":[126.77211,130.61772,131.71225,127.77618], "fy":[-85.91277,-79.95439,-78.101,-84.37642]}, + {"t":0.76541, "x":5.50099, "y":2.86408, "heading":2.28636, "vx":-1.71386, "vy":1.08839, "omega":-0.72944, "ax":9.91124, "ay":-6.29554, "alpha":4.21601, "fx":[126.76986,130.59948,131.77115,127.86071], "fy":[-85.97299,-80.04519,-78.06736,-84.30927]}, + {"t":0.7901, "x":5.46169, "y":2.88903, "heading":2.26835, "vx":-1.46914, "vy":0.93295, "omega":-0.62535, "ax":9.91327, "ay":-6.29632, "alpha":4.21756, "fx":[126.76825,130.58266,131.82146,127.93481], "fy":[-86.02385,-80.12453,-78.03838,-84.24884]}, + {"t":0.81479, "x":5.42844, "y":2.91015, "heading":2.25291, "vx":-1.22438, "vy":0.7775, "omega":-0.52121, "ax":9.91505, "ay":-6.29696, "alpha":4.21908, "fx":[126.7678,130.56833,131.86445,127.99913], "fy":[-86.06636,-80.19259,-78.01398,-84.19604]}, + {"t":0.83948, "x":5.40123, "y":2.92743, "heading":2.24004, "vx":-0.97957, "vy":0.62202, "omega":-0.41704, "ax":9.91661, "ay":-6.29748, "alpha":4.2206, "fx":[126.76884,130.55729,131.9011,128.05413], "fy":[-86.10129,-80.24944,-77.99401,-84.15158]}, + {"t":0.86417, "x":5.38007, "y":2.94086, "heading":2.22975, "vx":-0.73473, "vy":0.46653, "omega":-0.31283, "ax":9.918, "ay":-6.29791, "alpha":4.22211, "fx":[126.77161,130.55017,131.93218,128.10015], "fy":[-86.12931,-80.29516,-77.97831,-84.11602]}, + {"t":0.88886, "x":5.36495, "y":2.95046, "heading":2.22202, "vx":-0.48984, "vy":0.31103, "omega":-0.20859, "ax":9.91926, "ay":-6.29827, "alpha":4.22362, "fx":[126.77627,130.54746,131.95829,128.13745], "fy":[-86.15089,-80.32978,-77.96675,-84.08979]}, + {"t":0.91355, "x":5.35588, "y":2.95622, "heading":2.21687, "vx":-0.24493, "vy":0.15553, "omega":-0.1043, "ax":9.92012, "ay":-6.29899, "alpha":4.22443, "fx":[126.7794,130.54693,131.97535,128.1627], "fy":[-86.17172,-80.35757,-77.96696,-84.07864]}, + {"t":0.93824, "x":5.35285, "y":2.95814, "heading":2.2143, "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/left_J.traj b/src/main/deploy/choreo/left_J.traj new file mode 100644 index 0000000..32dcc79 --- /dev/null +++ b/src/main/deploy/choreo/left_J.traj @@ -0,0 +1,75 @@ +{ + "name":"left_J", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.432836532592773, "y":6.58942985534668, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.96180534362793, "y":5.222855091094971, "heading":-2.034444356476718, "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":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"7.432836532592773 m", "val":7.432836532592773}, "y":{"exp":"6.58942985534668 m", "val":6.58942985534668}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.96180534362793 m", "val":4.96180534362793}, "y":{"exp":"5.222855091094971 m", "val":5.222855091094971}, "heading":{"exp":"-2.034444356476718 rad", "val":-2.034444356476718}, "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":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.98121], + "samples":[ + {"t":0.0, "x":7.43284, "y":6.58943, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-10.27737, "ay":-5.69593, "alpha":4.60768, "fx":[-131.24026,-134.86443,-136.67141,-133.32365], "fy":[-79.19778,-72.84017,-69.41798,-75.66132]}, + {"t":0.02453, "x":7.42974, "y":6.58772, "heading":3.14159, "vx":-0.25211, "vy":-0.13972, "omega":0.11303, "ax":-10.27686, "ay":-5.69467, "alpha":4.60614, "fx":[-131.23504,-134.85698,-136.66334,-133.31755], "fy":[-79.1794,-72.82378,-69.40368,-75.64504]}, + {"t":0.04906, "x":7.42047, "y":6.58258, "heading":-3.13882, "vx":-0.5042, "vy":-0.27941, "omega":0.22602, "ax":-10.27624, "ay":-5.69336, "alpha":4.60464, "fx":[-131.23047,-134.8557,-136.65163,-133.30271], "fy":[-79.15682,-72.79253,-69.39316,-75.64106]}, + {"t":0.07359, "x":7.40501, "y":6.57401, "heading":-3.13328, "vx":-0.75628, "vy":-0.41907, "omega":0.33897, "ax":-10.27549, "ay":-5.69199, "alpha":4.60316, "fx":[-131.22634,-134.86028,-136.63598,-133.27892], "fy":[-79.12977,-72.74636,-69.38639,-75.6492]}, + {"t":0.09812, "x":7.38337, "y":6.56202, "heading":-3.12496, "vx":-1.00834, "vy":-0.5587, "omega":0.45189, "ax":-10.27459, "ay":-5.69053, "alpha":4.60171, "fx":[-131.22249,-134.87034,-136.61597,-133.2459], "fy":[-79.09782,-72.68517,-69.38343,-75.66918]}, + {"t":0.12265, "x":7.35554, "y":6.5466, "heading":-3.11388, "vx":-1.26038, "vy":-0.69829, "omega":0.56477, "ax":-10.27351, "ay":-5.68897, "alpha":4.60028, "fx":[-131.21872,-134.88536,-136.59105,-133.20335], "fy":[-79.06038,-72.60887,-69.38438,-75.70067]}, + {"t":0.14718, "x":7.32153, "y":6.52776, "heading":-3.10002, "vx":-1.51239, "vy":-0.83784, "omega":0.67761, "ax":-10.27222, "ay":-5.68729, "alpha":4.59886, "fx":[-131.21479,-134.90469,-136.56049,-133.15085], "fy":[-79.01665,-72.51734,-69.38938,-75.74318]}, + {"t":0.17171, "x":7.28134, "y":6.50549, "heading":-3.0834, "vx":-1.76437, "vy":-0.97735, "omega":0.79042, "ax":-10.27065, "ay":-5.68545, "alpha":4.59746, "fx":[-131.21037,-134.92746,-136.52336,-133.08784], "fy":[-78.96562,-72.41041,-69.39858,-75.7961]}, + {"t":0.19624, "x":7.23497, "y":6.47981, "heading":-3.06401, "vx":-2.01631, "vy":-1.11682, "omega":0.9032, "ax":-10.26874, "ay":-5.68341, "alpha":4.59608, "fx":[-131.20494,-134.95255,-136.47844,-133.01357], "fy":[-78.906,-72.28784,-69.41209,-75.85862]}, + {"t":0.22077, "x":7.18242, "y":6.4507, "heading":-3.04185, "vx":-2.2682, "vy":-1.25623, "omega":1.01594, "ax":-10.2664, "ay":-5.68112, "alpha":4.59469, "fx":[-131.19773,-134.97842,-136.42413,-132.92702], "fy":[-78.83617,-72.14923,-69.4299,-75.92964]}, + {"t":0.2453, "x":7.12369, "y":6.41818, "heading":-3.01693, "vx":-2.52004, "vy":-1.39559, "omega":1.12865, "ax":-10.26348, "ay":-5.67848, "alpha":4.5933, "fx":[-131.18747,-135.00293,-136.35826,-132.82666], "fy":[-78.75407,-71.99396,-69.45179,-76.00767]}, + {"t":0.26983, "x":7.05879, "y":6.38224, "heading":-2.98925, "vx":-2.77181, "vy":-1.53489, "omega":1.24133, "ax":-10.2598, "ay":-5.67538, "alpha":4.5919, "fx":[-131.17213,-135.02304,-136.27782,-132.71024], "fy":[-78.65697,-71.821,-69.47712,-76.09061]}, + {"t":0.29436, "x":6.98771, "y":6.34288, "heading":-2.9588, "vx":-3.02348, "vy":-1.6741, "omega":1.35397, "ax":-10.25505, "ay":-5.67162, "alpha":4.59047, "fx":[-131.14835,-135.03421,-136.17839,-132.57424], "fy":[-78.5411,-71.62856,-69.50451,-76.17537]}, + {"t":0.31889, "x":6.91045, "y":6.3001, "heading":-2.92558, "vx":-3.27504, "vy":-1.81323, "omega":1.46657, "ax":-10.24873, "ay":-5.66689, "alpha":4.58899, "fx":[-131.11034,-135.02929,-136.05311,-132.41285], "fy":[-78.40099,-71.41347,-69.53118,-76.25725]}, + {"t":0.34342, "x":6.82703, "y":6.25392, "heading":-2.88961, "vx":-3.52645, "vy":-1.95224, "omega":1.57914, "ax":-10.24, "ay":-5.66066, "alpha":4.58745, "fx":[-131.04768,-134.99622,-135.89049,-132.21591], "fy":[-78.22795,-71.16984,-69.55168,-76.32843]}, + {"t":0.36795, "x":6.73745, "y":6.20433, "heading":-2.85087, "vx":-3.77764, "vy":-2.0911, "omega":1.69167, "ax":-10.22726, "ay":-5.65193, "alpha":4.58582, "fx":[-130.9401,-134.91265,-135.66938,-131.96394], "fy":[-78.00667,-70.88586,-69.55483,-76.37474]}, + {"t":0.39248, "x":6.6417, "y":6.15133, "heading":-2.80937, "vx":-4.02851, "vy":-2.22974, "omega":1.80417, "ax":-10.20711, "ay":-5.63853, "alpha":4.5841, "fx":[-130.74349,-134.73146,-135.34515,-131.61486], "fy":[-77.70579,-70.53523,-69.51548,-76.36677]}, + {"t":0.41701, "x":6.53981, "y":6.09494, "heading":-2.76512, "vx":-4.2789, "vy":-2.36806, "omega":1.91661, "ax":-10.17071, "ay":-5.61491, "alpha":4.58229, "fx":[-130.34237,-134.33165,-134.80322,-131.05893], "fy":[-77.2463,-70.04804,-69.36684,-76.22982]}, + {"t":0.44154, "x":6.43179, "y":6.03516, "heading":-2.7181, "vx":-4.52839, "vy":-2.50579, "omega":2.02902, "ax":-10.08592, "ay":-5.56082, "alpha":4.58036, "fx":[-129.3147,-133.27508,-133.62816,-129.89507], "fy":[-76.34464,-69.16656,-68.8632,-75.69527]}, + {"t":0.46607, "x":6.31767, "y":5.97202, "heading":-2.66833, "vx":-4.7758, "vy":-2.6422, "omega":2.14138, "ax":-9.67171, "ay":-5.29998, "alpha":4.57459, "fx":[-123.96698,-127.74138,-128.19031,-124.60793], "fy":[-72.54259,-65.63564,-65.85469,-72.43057]}, + {"t":0.4906, "x":6.19761, "y":5.90561, "heading":-2.6158, "vx":-5.01304, "vy":-2.77221, "omega":2.25359, "ax":9.63155, "ay":5.37298, "alpha":-4.55398, "fx":[123.50415,127.32693,127.61173,123.96901], "fy":[73.30986,66.46861,66.99286,73.49992]}, + {"t":0.51514, "x":6.07754, "y":5.83923, "heading":-2.56052, "vx":-4.77678, "vy":-2.64041, "omega":2.14188, "ax":10.07255, "ay":5.58503, "alpha":-4.57389, "fx":[129.43585,133.40545,133.18507,129.38944], "fy":[76.11072,68.94693,69.74316,76.53197]}, + {"t":0.53967, "x":5.96339, "y":5.77614, "heading":-2.50798, "vx":-4.5297, "vy":-2.50341, "omega":2.02968, "ax":10.1634, "ay":5.62813, "alpha":-4.57888, "fx":[130.75389,134.72602,134.24683,130.42818], "fy":[76.51892,69.31825,70.46412,77.27959]}, + {"t":0.5642, "x":5.85534, "y":5.71642, "heading":-2.45819, "vx":-4.28039, "vy":-2.36535, "omega":1.91736, "ax":10.20272, "ay":5.64647, "alpha":-4.58214, "fx":[131.39092,135.33689,134.64665,130.83128], "fy":[76.57766,69.39764,70.88479,77.67749]}, + {"t":0.58873, "x":5.75341, "y":5.6601, "heading":-2.41116, "vx":-4.03011, "vy":-2.22684, "omega":1.80496, "ax":10.22472, "ay":5.65651, "alpha":-4.58477, "fx":[131.79715,135.7031,134.82602,131.02719], "fy":[76.52209,69.39103,71.20086,77.94734]}, + {"t":0.61326, "x":5.65763, "y":5.60717, "heading":-2.36688, "vx":-3.7793, "vy":-2.08808, "omega":1.6925, "ax":10.23882, "ay":5.66277, "alpha":-4.58704, "fx":[132.09573,135.95319,134.90688,131.13325], "fy":[76.41802,69.35388,71.46507,78.15089]}, + {"t":0.63779, "x":5.568, "y":5.55766, "heading":-2.32536, "vx":-3.52814, "vy":-1.94917, "omega":1.57997, "ax":10.24866, "ay":5.667, "alpha":-4.5891, "fx":[132.33379,136.13731,134.93667,131.19435], "fy":[76.29172,69.30668,71.69711,78.3127]}, + {"t":0.66232, "x":5.48454, "y":5.51155, "heading":-2.28661, "vx":-3.27673, "vy":-1.81016, "omega":1.4674, "ax":10.25593, "ay":5.67, "alpha":-4.59103, "fx":[132.53302,136.27934,134.9379,131.23115], "fy":[76.15607,69.25821,71.9057,78.4449]}, + {"t":0.68685, "x":5.40724, "y":5.46885, "heading":-2.25061, "vx":-3.02515, "vy":-1.67107, "omega":1.35478, "ax":10.26154, "ay":5.67221, "alpha":-4.59285, "fx":[132.70472,136.3923,134.92268,131.25426], "fy":[76.01839,69.21246,72.09512,78.55447]}, + {"t":0.71138, "x":5.33612, "y":5.42957, "heading":-2.21738, "vx":-2.77344, "vy":-1.53193, "omega":1.24212, "ax":10.26601, "ay":5.67389, "alpha":-4.59462, "fx":[132.85527,136.48404,134.8982,131.2696], "fy":[75.88322,69.17119,72.26762,78.6459]}, + {"t":0.73591, "x":5.27118, "y":5.39369, "heading":-2.18691, "vx":-2.52161, "vy":-1.39275, "omega":1.12941, "ax":10.26966, "ay":5.67519, "alpha":-4.59635, "fx":[132.98851,136.55964,134.86901,131.28056], "fy":[75.75359,69.13501,72.42442,78.72239]}, + {"t":0.76044, "x":5.21241, "y":5.36124, "heading":-2.1592, "vx":-2.26969, "vy":-1.25354, "omega":1.01666, "ax":10.27271, "ay":5.6762, "alpha":-4.59807, "fx":[133.10688,136.62263,134.83815,131.28919], "fy":[75.63167,69.10395,72.56621,78.78634]}, + {"t":0.78497, "x":5.15983, "y":5.33219, "heading":-2.13427, "vx":-2.0177, "vy":-1.1143, "omega":0.90387, "ax":10.27531, "ay":5.67699, "alpha":-4.59977, "fx":[133.212,136.67556,134.80775,131.29673], "fy":[75.51903,69.07772,72.69338,78.83964]}, + {"t":0.8095, "x":5.11342, "y":5.30657, "heading":-2.11209, "vx":-1.76564, "vy":-0.97504, "omega":0.79104, "ax":10.27754, "ay":5.67763, "alpha":-4.60147, "fx":[133.30497,136.72035,134.77935,131.30389], "fy":[75.41682,69.05589,72.80617,78.88386]}, + {"t":0.83403, "x":5.0732, "y":5.28436, "heading":-2.09269, "vx":-1.51353, "vy":-0.83577, "omega":0.67816, "ax":10.27949, "ay":5.67813, "alpha":-4.60316, "fx":[133.3866,136.75848,134.75407,131.31113], "fy":[75.32596,69.03793,72.90471,78.92028]}, + {"t":0.85856, "x":5.03917, "y":5.26557, "heading":-2.07605, "vx":-1.26137, "vy":-0.69648, "omega":0.56525, "ax":10.28121, "ay":5.67852, "alpha":-4.60486, "fx":[133.45745,136.79112,134.73276,131.31868], "fy":[75.24713,69.02335,72.98908,78.94998]}, + {"t":0.88309, "x":5.01132, "y":5.25019, "heading":-2.06219, "vx":-1.00917, "vy":-0.55719, "omega":0.45229, "ax":10.28274, "ay":5.67883, "alpha":-4.60656, "fx":[133.51797,136.81919,134.7161,131.32669], "fy":[75.18086,69.01167,73.05935,78.97386]}, + {"t":0.90762, "x":4.98966, "y":5.23823, "heading":-2.05109, "vx":-0.75694, "vy":-0.41788, "omega":0.33929, "ax":10.28412, "ay":5.67907, "alpha":-4.60826, "fx":[133.56849,136.84344,134.70457,131.33523], "fy":[75.12757,69.00248,73.11556,78.99264]}, + {"t":0.93215, "x":4.97419, "y":5.22969, "heading":-2.04277, "vx":-0.50466, "vy":-0.27857, "omega":0.22625, "ax":10.28536, "ay":5.67926, "alpha":-4.60997, "fx":[133.6093,136.86446,134.69859,131.34437], "fy":[75.08759,68.99543,73.15773,79.00693]}, + {"t":0.95668, "x":4.9649, "y":5.22456, "heading":-2.03722, "vx":-0.25236, "vy":-0.13926, "omega":0.11316, "ax":10.28776, "ay":5.67709, "alpha":-4.61319, "fx":[133.6582,136.89845,134.71498,131.37006], "fy":[75.02975,68.95894,73.15544,78.99067]}, + {"t":0.98121, "x":4.96181, "y":5.22286, "heading":-2.03444, "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/middle_G.traj b/src/main/deploy/choreo/middle_G.traj new file mode 100644 index 0000000..2045522 --- /dev/null +++ b/src/main/deploy/choreo/middle_G.traj @@ -0,0 +1,59 @@ +{ + "name":"middle_G", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.497458457946777, "y":3.870664834976196, "heading":3.141592653589793, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.848470211029053, "y":3.854306697845459, "heading":-3.123539493628586, "intervals":22, "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":"7.497458457946777 m", "val":7.497458457946777}, "y":{"exp":"3.8706648349761963 m", "val":3.870664834976196}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.848470211029053 m", "val":5.848470211029053}, "y":{"exp":"3.854306697845459 m", "val":3.854306697845459}, "heading":{"exp":"-3.123539493628586 rad", "val":-3.123539493628586}, "intervals":22, "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.74945], + "samples":[ + {"t":0.0, "x":7.49746, "y":3.87066, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-11.75782, "ay":-0.11664, "alpha":0.1287, "fx":[-153.32986,-153.33214,-153.33247,-153.33019], "fy":[-1.63728,-1.40712,-1.40502,-1.63483]}, + {"t":0.03123, "x":7.49173, "y":3.87061, "heading":3.14159, "vx":-0.36716, "vy":-0.00364, "omega":0.00402, "ax":-11.75633, "ay":-0.11662, "alpha":0.12866, "fx":[-153.31038,-153.31266,-153.31301,-153.31074], "fy":[-1.63704,-1.40697,-1.40487,-1.63459]}, + {"t":0.06245, "x":7.47453, "y":3.87044, "heading":-3.14147, "vx":-0.73427, "vy":-0.00728, "omega":0.00804, "ax":-11.75452, "ay":-0.11661, "alpha":0.12863, "fx":[-153.2868,-153.28908,-153.28947,-153.2872], "fy":[-1.63674,-1.40678,-1.4047,-1.63432]}, + {"t":0.09368, "x":7.44587, "y":3.87015, "heading":-3.14122, "vx":-1.10133, "vy":-0.01093, "omega":0.01205, "ax":-11.75229, "ay":-0.11658, "alpha":0.12859, "fx":[-153.25768,-153.25995,-153.26039,-153.25811], "fy":[-1.63637,-1.40651,-1.40449,-1.63401]}, + {"t":0.12491, "x":7.40575, "y":3.86976, "heading":-3.14084, "vx":-1.46832, "vy":-0.01457, "omega":0.01607, "ax":-11.74946, "ay":-0.11656, "alpha":0.12855, "fx":[-153.22079,-153.22307,-153.22356,-153.22129], "fy":[-1.6359,-1.40616,-1.40423,-1.63363]}, + {"t":0.15613, "x":7.35417, "y":3.86924, "heading":-3.14034, "vx":-1.83522, "vy":-0.01821, "omega":0.02008, "ax":-11.74577, "ay":-0.11652, "alpha":0.12852, "fx":[-153.17258,-153.17485,-153.17541,-153.17314], "fy":[-1.63529,-1.4057,-1.40388,-1.63314]}, + {"t":0.18736, "x":7.29113, "y":3.86862, "heading":-3.13971, "vx":-2.202, "vy":-0.02184, "omega":0.0241, "ax":-11.74073, "ay":-0.11647, "alpha":0.12848, "fx":[-153.10685,-153.10912,-153.10978,-153.10751], "fy":[-1.63448,-1.40505,-1.40339,-1.63248]}, + {"t":0.21859, "x":7.21665, "y":3.86788, "heading":-3.13896, "vx":-2.56863, "vy":-0.02548, "omega":0.02811, "ax":-11.73346, "ay":-0.1164, "alpha":0.12844, "fx":[-153.01197,-153.01424,-153.01503,-153.01277], "fy":[-1.63335,-1.40413,-1.40264,-1.63152]}, + {"t":0.24982, "x":7.13072, "y":3.86703, "heading":-3.13808, "vx":-2.93503, "vy":-0.02912, "omega":0.03212, "ax":-11.72204, "ay":-0.11628, "alpha":0.12839, "fx":[-152.86298,-152.86524,-152.86626,-152.864], "fy":[-1.63162,-1.4027,-1.40142,-1.62999]}, + {"t":0.28104, "x":7.03335, "y":3.86606, "heading":-3.13708, "vx":-3.30107, "vy":-0.03275, "omega":0.03613, "ax":-11.70152, "ay":-0.11608, "alpha":0.12835, "fx":[-152.59516,-152.59742,-152.59883,-152.59658], "fy":[-1.62861,-1.40017,-1.39912,-1.62722]}, + {"t":0.31227, "x":6.92456, "y":3.86498, "heading":-3.13595, "vx":-3.66647, "vy":-0.03637, "omega":0.04014, "ax":-11.65378, "ay":-0.11561, "alpha":0.1283, "fx":[-151.97209,-151.97434,-151.97667,-151.97443], "fy":[-1.62179,-1.39436,-1.39358,-1.62067]}, + {"t":0.3435, "x":6.80439, "y":3.86379, "heading":-3.1347, "vx":-4.03038, "vy":-0.03998, "omega":0.04414, "ax":-11.41801, "ay":-0.11327, "alpha":0.12818, "fx":[-148.89526,-148.89745,-148.90428,-148.90209], "fy":[-1.58876,-1.36605,-1.3656,-1.58799]}, + {"t":0.37472, "x":6.67296, "y":3.86249, "heading":-3.13332, "vx":-4.38693, "vy":-0.04352, "omega":0.04815, "ax":11.41801, "ay":0.11327, "alpha":-0.12818, "fx":[148.89526,148.89746,148.90428,148.90209], "fy":[1.58861,1.3659,1.36576,1.58815]}, + {"t":0.40595, "x":6.54154, "y":3.86118, "heading":-3.13181, "vx":-4.03038, "vy":-0.03998, "omega":0.04414, "ax":11.65378, "ay":0.11561, "alpha":-0.1283, "fx":[151.97209,151.97435,151.97667,151.97442], "fy":[1.62132,1.39389,1.39405,1.62114]}, + {"t":0.43718, "x":6.42137, "y":3.85999, "heading":-3.13044, "vx":-3.66647, "vy":-0.03637, "omega":0.04014, "ax":11.70152, "ay":0.11608, "alpha":-0.12835, "fx":[152.59516,152.59743,152.59883,152.59656], "fy":[1.62785,1.39941,1.39987,1.62798]}, + {"t":0.4684, "x":6.31258, "y":3.85891, "heading":-3.12918, "vx":-3.30107, "vy":-0.03275, "omega":0.03613, "ax":11.72204, "ay":0.11628, "alpha":-0.12839, "fx":[152.86298,152.86526,152.86625,152.86398], "fy":[1.6306,1.40168,1.40243,1.63101]}, + {"t":0.49963, "x":6.21521, "y":3.85794, "heading":-3.12805, "vx":-2.93503, "vy":-0.02912, "omega":0.03212, "ax":11.73346, "ay":0.1164, "alpha":-0.12844, "fx":[153.01197,153.01425,153.01502,153.01275], "fy":[1.6321,1.40288,1.40389,1.63277]}, + {"t":0.53086, "x":6.12928, "y":3.85709, "heading":-3.12705, "vx":-2.56863, "vy":-0.02548, "omega":0.02811, "ax":11.74073, "ay":0.11647, "alpha":-0.12848, "fx":[153.10686,153.10914,153.10977,153.10749], "fy":[1.63303,1.4036,1.40484,1.63393]}, + {"t":0.56208, "x":6.0548, "y":3.85635, "heading":-3.12617, "vx":-2.202, "vy":-0.02184, "omega":0.0241, "ax":11.74577, "ay":0.11652, "alpha":-0.12852, "fx":[153.17259,153.17487,153.1754,153.17312], "fy":[1.63367,1.40407,1.40551,1.63477]}, + {"t":0.59331, "x":5.99176, "y":3.85573, "heading":-3.12542, "vx":-1.83522, "vy":-0.01821, "omega":0.02008, "ax":11.74946, "ay":0.11656, "alpha":-0.12855, "fx":[153.22081,153.22309,153.22354,153.22127], "fy":[1.63413,1.40439,1.406,1.6354]}, + {"t":0.62454, "x":5.94018, "y":3.85522, "heading":-3.12479, "vx":-1.46832, "vy":-0.01457, "omega":0.01607, "ax":11.75229, "ay":0.11658, "alpha":-0.12859, "fx":[153.25769,153.25997,153.26037,153.25809], "fy":[1.63449,1.40462,1.40638,1.6359]}, + {"t":0.65576, "x":5.90006, "y":3.85482, "heading":-3.12429, "vx":-1.10133, "vy":-0.01093, "omega":0.01205, "ax":11.75452, "ay":0.11661, "alpha":-0.12863, "fx":[153.28682,153.2891,153.28945,153.28717], "fy":[1.63477,1.4048,1.40667,1.6363]}, + {"t":0.68699, "x":5.8714, "y":3.85453, "heading":-3.12392, "vx":-0.73427, "vy":-0.00728, "omega":0.00804, "ax":11.75633, "ay":0.11662, "alpha":-0.12866, "fx":[153.3104,153.31268,153.313,153.31072], "fy":[1.63501,1.40494,1.4069,1.63662]}, + {"t":0.71822, "x":5.8542, "y":3.85436, "heading":-3.12366, "vx":-0.36716, "vy":-0.00364, "omega":0.00402, "ax":11.75782, "ay":0.11664, "alpha":-0.1287, "fx":[153.32988,153.33216,153.33245,153.33017], "fy":[1.63522,1.40506,1.40708,1.63689]}, + {"t":0.74945, "x":5.84847, "y":3.85431, "heading":-3.12354, "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/rightStation_D.traj b/src/main/deploy/choreo/rightStation_D.traj new file mode 100644 index 0000000..6a02424 --- /dev/null +++ b/src/main/deploy/choreo/rightStation_D.traj @@ -0,0 +1,70 @@ +{ + "name":"rightStation_D", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":1.184930443763733, "y":0.974328339099884, "heading":0.927294690583144, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.882387161254883, "y":2.832714557647705, "heading":1.0370881119316695, "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":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.184930443763733 m", "val":1.184930443763733}, "y":{"exp":"0.974328339099884 m", "val":0.974328339099884}, "heading":{"exp":"0.9272946905831441 rad", "val":0.927294690583144}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.882387161254883 m", "val":3.882387161254883}, "y":{"exp":"2.832714557647705 m", "val":2.832714557647705}, "heading":{"exp":"1.0370881119316697 rad", "val":1.0370881119316695}, "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":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.0567], + "samples":[ + {"t":0.0, "x":1.18493, "y":0.97433, "heading":0.92729, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.67933, "ay":6.67539, "alpha":0.99211, "fx":[125.90842,126.86132,126.5457,125.58887], "fy":[87.51153,86.12764,86.59411,87.97596]}, + {"t":0.03019, "x":1.18934, "y":0.97737, "heading":0.92729, "vx":0.29223, "vy":0.20154, "omega":0.02995, "ax":9.67882, "ay":6.67443, "alpha":0.90408, "fx":[125.92984,126.7981,126.51073,125.63903], "fy":[87.45775,86.19742,86.62222,87.88159]}, + {"t":0.06038, "x":1.20258, "y":0.9865, "heading":0.9282, "vx":0.58445, "vy":0.40305, "omega":0.05725, "ax":9.67819, "ay":6.67341, "alpha":0.80687, "fx":[125.95256,126.72738,126.47126,125.69349], "fy":[87.39905,86.27501,86.65376,87.77784]}, + {"t":0.09057, "x":1.22463, "y":1.00171, "heading":0.92993, "vx":0.87665, "vy":0.60453, "omega":0.08161, "ax":9.67741, "ay":6.67231, "alpha":0.69887, "fx":[125.97672,126.64778,126.42649,125.75296], "fy":[87.33456,86.36173,86.68923,87.66299]}, + {"t":0.12077, "x":1.25551, "y":1.023, "heading":0.93239, "vx":1.16883, "vy":0.80598, "omega":0.10271, "ax":9.67645, "ay":6.67112, "alpha":0.57768, "fx":[126.00262,126.55731,126.37525,125.81856], "fy":[87.263,86.45957,86.72943,87.5345]}, + {"t":0.15096, "x":1.29521, "y":1.05037, "heading":0.93549, "vx":1.46097, "vy":1.00739, "omega":0.12015, "ax":9.67526, "ay":6.66981, "alpha":0.44215, "fx":[126.03004,126.45466,126.31662,125.89047], "fy":[87.18357,86.56925,86.77451,87.39092]}, + {"t":0.18115, "x":1.34373, "y":1.08383, "heading":0.93912, "vx":1.75309, "vy":1.20876, "omega":0.1335, "ax":9.67379, "ay":6.66835, "alpha":0.28311, "fx":[126.06114,126.33323,126.2469,125.97376], "fy":[87.09166,86.69881,86.82819,87.22321]}, + {"t":0.21134, "x":1.40107, "y":1.12336, "heading":0.94315, "vx":2.04515, "vy":1.41009, "omega":0.14205, "ax":9.67195, "ay":6.66668, "alpha":0.11322, "fx":[126.09076,126.2,126.16909,126.05926], "fy":[86.99248,86.83575,86.88396,87.04243]}, + {"t":0.24153, "x":1.46722, "y":1.16897, "heading":0.94744, "vx":2.33716, "vy":1.61137, "omega":0.14547, "ax":9.66962, "ay":6.66472, "alpha":-0.12056, "fx":[126.13448,126.01968,126.06437,126.17919], "fy":[86.86082,87.02821,86.96535,86.79814]}, + {"t":0.27172, "x":1.54219, "y":1.22066, "heading":0.95183, "vx":2.6291, "vy":1.81259, "omega":0.14183, "ax":9.66664, "ay":6.66237, "alpha":-0.31392, "fx":[126.15652,125.8566,125.96422,126.26471], "fy":[86.74308,87.17792,87.02288,86.58618]}, + {"t":0.30192, "x":1.62597, "y":1.27842, "heading":0.95611, "vx":2.92095, "vy":2.01373, "omega":0.13235, "ax":9.66268, "ay":6.65943, "alpha":-0.73711, "fx":[126.24043,125.5349,125.77631,126.48379], "fy":[86.51314,87.53206,87.17954,86.15188]}, + {"t":0.33211, "x":1.71856, "y":1.34226, "heading":0.96011, "vx":3.21269, "vy":2.21479, "omega":0.11009, "ax":9.65738, "ay":6.65568, "alpha":-0.8621, "fx":[126.21056,125.38599,125.66752,126.49497], "fy":[86.40945,87.59913,87.18713,85.9851]}, + {"t":0.3623, "x":1.81996, "y":1.41216, "heading":0.96343, "vx":3.50426, "vy":2.41574, "omega":0.08407, "ax":9.64956, "ay":6.65036, "alpha":-1.85412, "fx":[126.43278,124.65733,125.23864,127.02256], "fy":[85.89112,88.44123,87.58222,84.98891]}, + {"t":0.39249, "x":1.93016, "y":1.48812, "heading":0.96597, "vx":3.79559, "vy":2.61652, "omega":0.02809, "ax":9.63836, "ay":6.643, "alpha":-1.50981, "fx":[126.17452,124.7308,125.20438,126.65739], "fy":[85.95375,88.02687,87.32501,85.21421]}, + {"t":0.42268, "x":2.04915, "y":1.57015, "heading":0.96682, "vx":4.08659, "vy":2.81708, "omega":-0.0175, "ax":9.6171, "ay":6.62952, "alpha":-4.47799, "fx":[126.90302,122.62314,123.89956,128.23211], "fy":[84.44319,90.5204,88.58112,82.27168]}, + {"t":0.45287, "x":2.17691, "y":1.65822, "heading":0.96629, "vx":4.37694, "vy":3.01724, "omega":-0.15269, "ax":9.58136, "ay":6.60705, "alpha":-1.61122, "fx":[125.48181,123.93111,124.40427,125.9766], "fy":[85.40825,87.63394,86.94278,84.65961]}, + {"t":0.48306, "x":2.31342, "y":1.75233, "heading":0.96168, "vx":4.66622, "vy":3.21672, "omega":-0.20134, "ax":9.46228, "ay":6.52812, "alpha":-8.92657, "fx":[126.97965,118.76558,119.48495,128.35171], "fy":[81.2202,92.4835,89.5725,77.25097]}, + {"t":0.51326, "x":2.45862, "y":1.85242, "heading":0.9556, "vx":4.9519, "vy":3.41381, "omega":-0.47085, "ax":0.07443, "ay":-0.10487, "alpha":33.83583, "fx":[-19.12244,10.62801,21.05944,-8.68234], "fy":[-11.07279,-21.33876,8.20696,18.73439]}, + {"t":0.54345, "x":2.60816, "y":1.95544, "heading":0.94139, "vx":4.95415, "vy":3.41064, "omega":0.55071, "ax":-9.47597, "ay":-6.52538, "alpha":1.75703, "fx":[-123.94041,-122.66262,-123.04192,-124.65144], "fy":[-84.97688,-86.65018,-85.47564,-83.28165]}, + {"t":0.57364, "x":2.75341, "y":2.05544, "heading":0.95801, "vx":4.66805, "vy":3.21363, "omega":0.60376, "ax":-9.58117, "ay":-6.59389, "alpha":-6.43804, "fx":[-122.57374,-128.91429,-127.21508,-121.08086], "fy":[-89.29157,-80.01729,-83.00323,-91.64578]}, + {"t":0.60383, "x":2.88998, "y":2.14946, "heading":0.97624, "vx":4.37878, "vy":3.01455, "omega":0.40938, "ax":-9.62382, "ay":-6.6236, "alpha":-1.94172, "fx":[-124.88031,-126.75316,-126.11125,-124.26385], "fy":[-87.27224,-84.54181,-85.52037,-88.17338]}, + {"t":0.63402, "x":3.01779, "y":2.23745, "heading":0.9886, "vx":4.08823, "vy":2.81458, "omega":0.35076, "ax":-9.64134, "ay":-6.63674, "alpha":-2.6671, "fx":[-124.90579,-127.45416,-126.54303,-124.01959], "fy":[-87.73096,-84.00826,-85.42313,-89.03057]}, + {"t":0.66421, "x":3.13683, "y":2.3194, "heading":0.99919, "vx":3.79714, "vy":2.6142, "omega":0.27023, "ax":-9.65307, "ay":-6.64526, "alpha":-1.50875, "fx":[-125.44563,-126.87293,-126.31695,-124.89862], "fy":[-87.29128,-85.21052,-86.04804,-88.08778]}, + {"t":0.69441, "x":3.24707, "y":2.3953, "heading":1.00735, "vx":3.5057, "vy":2.41357, "omega":0.22468, "ax":-9.6604, "ay":-6.65055, "alpha":-1.50376, "fx":[-125.55466,-126.9699,-126.40039,-124.99177], "fy":[-87.34198,-85.27787,-86.13397,-88.15978]}, + {"t":0.7246, "x":3.34851, "y":2.46514, "heading":1.01413, "vx":3.21403, "vy":2.21278, "omega":0.17928, "ax":-9.66585, "ay":-6.65442, "alpha":-1.10272, "fx":[-125.75111,-126.7823,-126.34758,-125.32003], "fy":[-87.2123,-85.70967,-86.35582,-87.83757]}, + {"t":0.75479, "x":3.44114, "y":2.52892, "heading":1.01955, "vx":2.92221, "vy":2.01187, "omega":0.14599, "ax":-9.66987, "ay":-6.65723, "alpha":-0.98288, "fx":[-125.84232,-126.75766,-126.3616,-125.44881], "fy":[-87.19306,-85.85949,-86.44606,-87.76344]}, + {"t":0.78498, "x":3.52496, "y":2.58662, "heading":1.02395, "vx":2.63026, "vy":1.81088, "omega":0.11632, "ax":-9.67303, "ay":-6.65942, "alpha":-0.79638, "fx":[-125.93843,-126.67694,-126.34854,-125.61157], "fy":[-87.14241,-86.06706,-86.55065,-87.61599]}, + {"t":0.81517, "x":3.59996, "y":2.63826, "heading":1.02747, "vx":2.33822, "vy":1.60982, "omega":0.09227, "ax":-9.67556, "ay":-6.66114, "alpha":-0.68185, "fx":[-126.00417,-126.6345,-126.34898,-125.71958], "fy":[-87.11737,-86.1999,-86.61893,-87.52971]}, + {"t":0.84536, "x":3.66615, "y":2.68383, "heading":1.03025, "vx":2.0461, "vy":1.40871, "omega":0.07169, "ax":-9.67763, "ay":-6.66254, "alpha":-0.56894, "fx":[-126.0622,-126.58672,-126.34539,-125.82127], "fy":[-87.09076,-86.32765,-86.6805,-87.43986]}, + {"t":0.87555, "x":3.72351, "y":2.72332, "heading":1.03242, "vx":1.75391, "vy":1.20756, "omega":0.05451, "ax":-9.67937, "ay":-6.66369, "alpha":-0.47835, "fx":[-126.1088,-126.54889,-126.34424,-125.90413], "fy":[-87.07095,-86.43096,-86.72927,-87.36759]}, + {"t":0.90575, "x":3.77206, "y":2.75674, "heading":1.03406, "vx":1.46168, "vy":1.00637, "omega":0.04007, "ax":-9.68084, "ay":-6.66465, "alpha":-0.39772, "fx":[-126.1487,-126.51403,-126.34294,-125.9772], "fy":[-87.05345,-86.52243,-86.77104,-87.30208]}, + {"t":0.93594, "x":3.81177, "y":2.78409, "heading":1.03527, "vx":1.1694, "vy":0.80516, "omega":0.02806, "ax":-9.68211, "ay":-6.66547, "alpha":-0.32758, "fx":[-126.18276,-126.48331,-126.34208,-126.04075], "fy":[-87.0386,-86.60194,-86.80652,-87.24452]}, + {"t":0.96613, "x":3.84267, "y":2.80536, "heading":1.03612, "vx":0.87708, "vy":0.60392, "omega":0.01817, "ax":-9.68321, "ay":-6.66617, "alpha":-0.2654, "fx":[-126.21238,-126.45565,-126.34133,-126.09693], "fy":[-87.02552,-86.67225,-86.8373,-87.19298]}, + {"t":0.99632, "x":3.86473, "y":2.82056, "heading":1.03667, "vx":0.58473, "vy":0.40266, "omega":0.01016, "ax":-9.68417, "ay":-6.66677, "alpha":-0.20995, "fx":[-126.23845,-126.43072,-126.34063,-126.14688], "fy":[-87.01383,-86.73477,-86.86433,-87.14667]}, + {"t":1.02651, "x":3.87797, "y":2.82967, "heading":1.03697, "vx":0.29235, "vy":0.20138, "omega":0.00382, "ax":-9.68321, "ay":-6.66995, "alpha":-0.12641, "fx":[-126.26221,-126.29682,-126.39582,-126.15138], "fy":[-87.00243,-86.95333,-86.80682,-87.16293]}, + {"t":1.0567, "x":3.88239, "y":2.83271, "heading":1.03709, "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/rightTaxi.traj b/src/main/deploy/choreo/rightTaxi.traj new file mode 100644 index 0000000..7f7792a --- /dev/null +++ b/src/main/deploy/choreo/rightTaxi.traj @@ -0,0 +1,58 @@ +{ + "name":"rightTaxi", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.623931407928467, "y":0.7727105021476746, "heading":3.141592653589793, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.9004435539245605, "y":0.7169238328933716, "heading":3.141592497257328, "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}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"7.623931407928467 m", "val":7.623931407928467}, "y":{"exp":"0.7727105021476746 m", "val":0.7727105021476746}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.9004435539245605 m", "val":5.9004435539245605}, "y":{"exp":"0.7169238328933716 m", "val":0.7169238328933716}, "heading":{"exp":"3.141592497257328 rad", "val":3.141592497257328}, "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}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.76699], + "samples":[ + {"t":0.0, "x":7.62393, "y":0.77271, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-11.75325, "ay":-0.38043, "alpha":0.0, "fx":[-153.27151,-153.27151,-153.27151,-153.27151], "fy":[-4.96116,-4.96117,-4.96117,-4.96116]}, + {"t":0.03335, "x":7.6174, "y":0.7725, "heading":3.14159, "vx":-0.39194, "vy":-0.01269, "omega":0.0, "ax":-11.75178, "ay":-0.38039, "alpha":0.0, "fx":[-153.25238,-153.25238,-153.25238,-153.25238], "fy":[-4.96054,-4.96055,-4.96055,-4.96054]}, + {"t":0.0667, "x":7.59779, "y":0.77186, "heading":3.14159, "vx":-0.78383, "vy":-0.02537, "omega":0.0, "ax":-11.74999, "ay":-0.38033, "alpha":0.0, "fx":[-153.229,-153.229,-153.229,-153.229], "fy":[-4.95979,-4.95979,-4.95979,-4.95979]}, + {"t":0.10004, "x":7.56512, "y":0.77081, "heading":3.14159, "vx":-1.17567, "vy":-0.03805, "omega":0.0, "ax":-11.74775, "ay":-0.38026, "alpha":0.0, "fx":[-153.19978,-153.19978,-153.19978,-153.19978], "fy":[-4.95884,-4.95884,-4.95884,-4.95884]}, + {"t":0.13339, "x":7.51938, "y":0.76933, "heading":3.14159, "vx":-1.56743, "vy":-0.05074, "omega":0.0, "ax":-11.74487, "ay":-0.38016, "alpha":0.0, "fx":[-153.16222,-153.16222,-153.16222,-153.16222], "fy":[-4.95763,-4.95763,-4.95763,-4.95763]}, + {"t":0.16674, "x":7.46058, "y":0.76742, "heading":3.14159, "vx":-1.95909, "vy":-0.06341, "omega":0.0, "ax":-11.74103, "ay":-0.38004, "alpha":0.0, "fx":[-153.11216,-153.11216,-153.11216,-153.11216], "fy":[-4.95601,-4.95601,-4.95601,-4.95601]}, + {"t":0.20009, "x":7.38872, "y":0.7651, "heading":3.14159, "vx":-2.35062, "vy":-0.07609, "omega":0.0, "ax":-11.73565, "ay":-0.37987, "alpha":0.0, "fx":[-153.04209,-153.04209,-153.04209,-153.04209], "fy":[-4.95374,-4.95374,-4.95374,-4.95374]}, + {"t":0.23343, "x":7.30381, "y":0.76235, "heading":3.14159, "vx":-2.74198, "vy":-0.08875, "omega":0.0, "ax":-11.7276, "ay":-0.3796, "alpha":0.0, "fx":[-152.93706,-152.93706,-152.93706,-152.93706], "fy":[-4.95034,-4.95034,-4.95034,-4.95034]}, + {"t":0.26678, "x":7.20585, "y":0.75918, "heading":3.14159, "vx":-3.13306, "vy":-0.10141, "omega":0.0, "ax":-11.71419, "ay":-0.37917, "alpha":0.0, "fx":[-152.76216,-152.76216,-152.76216,-152.76216], "fy":[-4.94468,-4.94468,-4.94468,-4.94468]}, + {"t":0.30013, "x":7.09486, "y":0.75559, "heading":3.14159, "vx":-3.5237, "vy":-0.11406, "omega":0.0, "ax":-11.68741, "ay":-0.3783, "alpha":0.0, "fx":[-152.41297,-152.41297,-152.41297,-152.41297], "fy":[-4.93337,-4.93338,-4.93338,-4.93337]}, + {"t":0.33348, "x":6.97085, "y":0.75157, "heading":3.14159, "vx":-3.91345, "vy":-0.12667, "omega":0.0, "ax":-11.60745, "ay":-0.37572, "alpha":0.0, "fx":[-151.37021,-151.37021,-151.37021,-151.37021], "fy":[-4.89962,-4.89962,-4.89962,-4.89962]}, + {"t":0.36682, "x":6.83389, "y":0.74714, "heading":3.14159, "vx":-4.30053, "vy":-0.1392, "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.40017, "x":6.69048, "y":0.7425, "heading":3.14159, "vx":-4.30053, "vy":-0.1392, "omega":0.0, "ax":11.60745, "ay":0.37572, "alpha":0.0, "fx":[151.37021,151.37021,151.37021,151.37021], "fy":[4.89962,4.89962,4.89962,4.89962]}, + {"t":0.43352, "x":6.55352, "y":0.73806, "heading":3.14159, "vx":-3.91345, "vy":-0.12667, "omega":0.0, "ax":11.68741, "ay":0.3783, "alpha":0.0, "fx":[152.41297,152.41297,152.41297,152.41297], "fy":[4.93337,4.93338,4.93338,4.93337]}, + {"t":0.46687, "x":6.42952, "y":0.73405, "heading":3.14159, "vx":-3.5237, "vy":-0.11406, "omega":0.0, "ax":11.71419, "ay":0.37917, "alpha":0.0, "fx":[152.76216,152.76216,152.76216,152.76216], "fy":[4.94468,4.94468,4.94468,4.94468]}, + {"t":0.50021, "x":6.31852, "y":0.73046, "heading":3.14159, "vx":-3.13306, "vy":-0.10141, "omega":0.0, "ax":11.7276, "ay":0.3796, "alpha":0.0, "fx":[152.93706,152.93706,152.93706,152.93706], "fy":[4.95034,4.95034,4.95034,4.95034]}, + {"t":0.53356, "x":6.22057, "y":0.72729, "heading":3.14159, "vx":-2.74198, "vy":-0.08875, "omega":0.0, "ax":11.73565, "ay":0.37987, "alpha":0.0, "fx":[153.04209,153.04209,153.04209,153.04209], "fy":[4.95374,4.95374,4.95374,4.95374]}, + {"t":0.56691, "x":6.13565, "y":0.72454, "heading":3.14159, "vx":-2.35062, "vy":-0.07609, "omega":0.0, "ax":11.74103, "ay":0.38004, "alpha":0.0, "fx":[153.11216,153.11216,153.11216,153.11216], "fy":[4.95601,4.95601,4.95601,4.95601]}, + {"t":0.60026, "x":6.06379, "y":0.72221, "heading":3.14159, "vx":-1.95909, "vy":-0.06341, "omega":0.0, "ax":11.74487, "ay":0.38016, "alpha":0.0, "fx":[153.16222,153.16222,153.16222,153.16222], "fy":[4.95763,4.95763,4.95763,4.95763]}, + {"t":0.6336, "x":6.00499, "y":0.72031, "heading":3.14159, "vx":-1.56743, "vy":-0.05074, "omega":0.0, "ax":11.74775, "ay":0.38026, "alpha":0.0, "fx":[153.19978,153.19978,153.19978,153.19978], "fy":[4.95884,4.95884,4.95884,4.95884]}, + {"t":0.66695, "x":5.95926, "y":0.71883, "heading":3.14159, "vx":-1.17567, "vy":-0.03805, "omega":0.0, "ax":11.74999, "ay":0.38033, "alpha":0.0, "fx":[153.229,153.229,153.229,153.229], "fy":[4.95979,4.95979,4.95979,4.95979]}, + {"t":0.7003, "x":5.92658, "y":0.71777, "heading":3.14159, "vx":-0.78383, "vy":-0.02537, "omega":0.0, "ax":11.75178, "ay":0.38039, "alpha":0.0, "fx":[153.25238,153.25238,153.25238,153.25238], "fy":[4.96054,4.96055,4.96055,4.96054]}, + {"t":0.73365, "x":5.90698, "y":0.71714, "heading":3.14159, "vx":-0.39194, "vy":-0.01269, "omega":0.0, "ax":11.75325, "ay":0.38043, "alpha":0.0, "fx":[153.27151,153.27151,153.27151,153.27151], "fy":[4.96116,4.96117,4.96117,4.96116]}, + {"t":0.76699, "x":5.90044, "y":0.71692, "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/right_E.traj b/src/main/deploy/choreo/right_E.traj new file mode 100644 index 0000000..a1f4965 --- /dev/null +++ b/src/main/deploy/choreo/right_E.traj @@ -0,0 +1,74 @@ +{ + "name":"right_E", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.41381311416626, "y":1.7384134531021118, "heading":3.141592653589793, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.023182392120361, "y":2.810075521469116, "heading":2.150412055946228, "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":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"7.41381311416626 m", "val":7.41381311416626}, "y":{"exp":"1.7384134531021118 m", "val":1.7384134531021118}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.023182392120361 m", "val":5.023182392120361}, "y":{"exp":"2.810075521469116 m", "val":2.810075521469116}, "heading":{"exp":"2.150412055946228 rad", "val":2.150412055946228}, "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":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.9454], + "samples":[ + {"t":0.0, "x":7.41381, "y":1.73841, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-10.71844, "ay":4.815, "alpha":-4.4471, "fx":[-139.02991,-141.87005,-140.64942,-137.55791], "fy":[64.57119,58.05233,60.91843,67.62355]}, + {"t":0.02424, "x":7.41066, "y":1.73983, "heading":3.14159, "vx":-0.25983, "vy":0.11672, "omega":-0.1078, "ax":-10.7177, "ay":4.81396, "alpha":-4.44559, "fx":[-139.02101,-141.85943,-140.63896,-137.54943], "fy":[64.55733,58.04084,60.90514,67.60779]}, + {"t":0.04848, "x":7.40122, "y":1.74407, "heading":3.13898, "vx":-0.51963, "vy":0.23342, "omega":-0.21557, "ax":-10.71685, "ay":4.81286, "alpha":-4.4441, "fx":[-139.00501,-141.84514,-140.6325,-137.54151], "fy":[64.55488,58.03396,60.87825,67.58697]}, + {"t":0.07272, "x":7.38547, "y":1.75114, "heading":3.13375, "vx":-0.77942, "vy":0.35008, "omega":-0.3233, "ax":-10.71585, "ay":4.81171, "alpha":-4.44263, "fx":[-138.98164,-141.82686,-140.62969,-137.5339], "fy":[64.56365,58.03169,60.8377,67.56084]}, + {"t":0.09696, "x":7.36343, "y":1.76104, "heading":3.12592, "vx":-1.03918, "vy":0.46672, "omega":-0.43099, "ax":-10.71468, "ay":4.81048, "alpha":-4.44118, "fx":[-138.95059,-141.80416,-140.63011,-137.52632], "fy":[64.5834,58.03404,60.78343,67.52901]}, + {"t":0.1212, "x":7.33509, "y":1.77377, "heading":3.11547, "vx":-1.29892, "vy":0.58334, "omega":-0.53865, "ax":-10.71331, "ay":4.80917, "alpha":-4.43975, "fx":[-138.91146,-141.77647,-140.63318,-137.51845], "fy":[64.61376,58.0411,60.7154,67.49097]}, + {"t":0.14545, "x":7.30046, "y":1.78933, "heading":3.10241, "vx":-1.55862, "vy":0.69991, "omega":-0.64627, "ax":-10.71168, "ay":4.80774, "alpha":-4.43833, "fx":[-138.86372,-141.74302,-140.63816,-137.50985], "fy":[64.6543,58.05298,60.63352,67.44598]}, + {"t":0.16969, "x":7.25953, "y":1.8077, "heading":3.08675, "vx":-1.81828, "vy":0.81646, "omega":-0.75386, "ax":-10.70974, "ay":4.80617, "alpha":-4.43691, "fx":[-138.80668,-141.70282,-140.64408,-137.49994], "fy":[64.7044,58.06978,60.53771,67.39315]}, + {"t":0.19393, "x":7.2123, "y":1.82891, "heading":3.06847, "vx":-2.0779, "vy":0.93297, "omega":-0.86142, "ax":-10.7074, "ay":4.80443, "alpha":-4.43549, "fx":[-138.73936,-141.65457,-140.64962,-137.48789], "fy":[64.76327,58.09155,60.42778,67.33133]}, + {"t":0.21817, "x":7.15879, "y":1.85294, "heading":3.04759, "vx":-2.33745, "vy":1.04943, "omega":-0.96894, "ax":-10.70454, "ay":4.80245, "alpha":-4.43406, "fx":[-138.6604,-141.59648,-140.65297,-137.47244], "fy":[64.82984,58.11823,60.30343,67.25903]}, + {"t":0.24241, "x":7.09898, "y":1.87979, "heading":3.0241, "vx":-2.59694, "vy":1.16585, "omega":-1.07642, "ax":-10.70099, "ay":4.80015, "alpha":-4.43261, "fx":[-138.56781,-141.52606,-140.65157,-137.4517], "fy":[64.90265,58.14953,60.16411,67.17434]}, + {"t":0.26665, "x":7.03288, "y":1.90946, "heading":2.99801, "vx":-2.85635, "vy":1.28221, "omega":-1.18388, "ax":-10.69649, "ay":4.79741, "alpha":-4.43113, "fx":[-138.45854,-141.43969,-140.64168,-137.42267], "fy":[64.9796,58.18474,60.00888,67.07471]}, + {"t":0.29089, "x":6.9605, "y":1.94195, "heading":2.96931, "vx":-3.11564, "vy":1.3985, "omega":-1.29129, "ax":-10.69065, "ay":4.79405, "alpha":-4.4296, "fx":[-138.32775,-141.33184,-140.6175,-137.38046], "fy":[65.05761,58.22236,59.83599,66.9565]}, + {"t":0.31513, "x":6.88183, "y":1.97726, "heading":2.93801, "vx":-3.37479, "vy":1.51471, "omega":-1.39867, "ax":-10.68278, "ay":4.78974, "alpha":-4.428, "fx":[-138.16728,-141.19352,-140.56959,-137.31665], "fy":[65.13179,58.25941,59.64223,66.81423]}, + {"t":0.33937, "x":6.79688, "y":2.01538, "heading":2.9041, "vx":-3.63375, "vy":1.63082, "omega":-1.50601, "ax":-10.67168, "ay":4.78391, "alpha":-4.4263, "fx":[-137.96228,-141.00883,-140.4812,-137.21577], "fy":[65.19373,58.2899,59.42132,66.6388]}, + {"t":0.36361, "x":6.70566, "y":2.05632, "heading":2.8676, "vx":-3.89244, "vy":1.74679, "omega":-1.61331, "ax":-10.65494, "ay":4.77543, "alpha":-4.42446, "fx":[-137.68285,-140.74642,-140.31924,-137.04648], "fy":[65.22732,58.30099,59.15996,66.41302]}, + {"t":0.38786, "x":6.60818, "y":2.10007, "heading":2.82849, "vx":-4.15073, "vy":1.86255, "omega":-1.72056, "ax":-10.62696, "ay":4.76164, "alpha":-4.42243, "fx":[-137.25901,-140.33387,-140.00736,-136.73521], "fy":[65.19603,58.26165,58.82593,66.09837]}, + {"t":0.4121, "x":6.50444, "y":2.14662, "heading":2.78678, "vx":-4.40834, "vy":1.97798, "omega":-1.82776, "ax":-10.57102, "ay":4.73463, "alpha":-4.42002, "fx":[-136.48112,-139.55587,-139.31874,-136.06147], "fy":[64.9927,58.07747,58.32097,65.58216]}, + {"t":0.43634, "x":6.39447, "y":2.19596, "heading":2.74247, "vx":-4.66459, "vy":2.09275, "omega":-1.93491, "ax":-10.40458, "ay":4.65541, "alpha":-4.41607, "fx":[-134.31231,-137.35111,-137.13666,-133.93544], "fy":[64.09281,57.27912,57.15548,64.31335]}, + {"t":0.46058, "x":6.27834, "y":2.24806, "heading":2.69557, "vx":-4.91681, "vy":2.2056, "omega":-2.04196, "ax":-0.0501, "ay":-0.11207, "alpha":-0.01427, "fx":[-0.65635,-0.66186,-0.65034,-0.64483], "fy":[-1.45298,-1.4645,-1.47001,-1.45849]}, + {"t":0.48482, "x":6.15913, "y":2.30149, "heading":2.64607, "vx":-4.91802, "vy":2.20288, "omega":-2.0423, "ax":10.3987, "ay":-4.66852, "alpha":4.41524, "fx":[134.06797,137.14417,137.21173,134.00505], "fy":[-64.57661,-57.7997,-57.00851,-64.13968]}, + {"t":0.50906, "x":6.04297, "y":2.35352, "heading":2.59656, "vx":-4.66595, "vy":2.08971, "omega":-1.93527, "ax":10.56858, "ay":-4.74006, "alpha":4.41972, "fx":[136.12751,139.2437,139.57444,136.34444], "fy":[-65.70403,-58.84907,-57.74041,-64.96284]}, + {"t":0.5333, "x":5.93297, "y":2.40278, "heading":2.54965, "vx":-4.40975, "vy":1.97481, "omega":-1.82814, "ax":10.62594, "ay":-4.7639, "alpha":4.42234, "fx":[136.77911,139.89948,140.40919,137.19462], "fy":[-66.17126,-59.32363,-57.89391,-65.11092]}, + {"t":0.55754, "x":5.82919, "y":2.44925, "heading":2.50533, "vx":-4.15217, "vy":1.85933, "omega":-1.72093, "ax":10.65483, "ay":-4.77567, "alpha":4.42451, "fx":[137.0798,140.19148,140.8518,137.66595], "fy":[-66.46024,-59.64878,-57.91353,-65.09136]}, + {"t":0.58178, "x":5.73167, "y":2.49292, "heading":2.46362, "vx":-3.89389, "vy":1.74356, "omega":-1.61368, "ax":10.67226, "ay":-4.7826, "alpha":4.42645, "fx":[137.24337,140.33893,141.13304,137.98302], "fy":[-66.66942,-59.90966,-57.88825,-65.00815]}, + {"t":0.60602, "x":5.64041, "y":2.53378, "heading":2.4245, "vx":-3.63518, "vy":1.62763, "omega":-1.50638, "ax":10.68395, "ay":-4.78711, "alpha":4.42825, "fx":[137.34116,140.4156,141.33066,138.22074], "fy":[-66.83246,-60.13414,-57.84699,-64.89698]}, + {"t":0.63027, "x":5.55543, "y":2.57183, "heading":2.38798, "vx":-3.37619, "vy":1.51158, "omega":-1.39903, "ax":10.69235, "ay":-4.79023, "alpha":4.42996, "fx":[137.40354,140.45344,141.4784,138.41106], "fy":[-66.96446,-60.33369,-57.80144,-64.77379]}, + {"t":0.65451, "x":5.47673, "y":2.60707, "heading":2.35407, "vx":-3.117, "vy":1.39546, "omega":-1.29165, "ax":10.6987, "ay":-4.79249, "alpha":4.4316, "fx":[137.44549,140.46861,141.59345,138.5698], "fy":[-67.0735,-60.51375,-57.75682,-64.64702]}, + {"t":0.67875, "x":5.40432, "y":2.63949, "heading":2.32276, "vx":-2.85765, "vy":1.27929, "omega":-1.18422, "ax":10.70366, "ay":-4.79417, "alpha":4.43319, "fx":[137.47514,140.47023,141.68557,138.70563], "fy":[-67.16449,-60.67708,-57.71551,-64.52167]}, + {"t":0.70299, "x":5.33819, "y":2.66909, "heading":2.29405, "vx":-2.59818, "vy":1.16307, "omega":-1.07676, "ax":10.70767, "ay":-4.79545, "alpha":4.43475, "fx":[137.49719,140.46384,141.76084,138.82369], "fy":[-67.24078,-60.82517,-57.67853,-64.40097]}, + {"t":0.72723, "x":5.27835, "y":2.69587, "heading":2.26795, "vx":-2.33862, "vy":1.04683, "omega":-0.96925, "ax":10.71098, "ay":-4.79643, "alpha":4.4363, "fx":[137.51449,140.45307,141.82323,138.92717], "fy":[-67.30478,-60.95888,-57.64615,-64.28713]}, + {"t":0.75147, "x":5.22481, "y":2.71984, "heading":2.24445, "vx":-2.07897, "vy":0.93055, "omega":-0.86171, "ax":10.71375, "ay":-4.7972, "alpha":4.43784, "fx":[137.52881,140.44035,141.87555,139.01818], "fy":[-67.35835,-61.07867,-57.61828,-64.18175]}, + {"t":0.77571, "x":5.17756, "y":2.74099, "heading":2.22356, "vx":-1.81926, "vy":0.81427, "omega":-0.75413, "ax":10.71613, "ay":-4.7978, "alpha":4.43938, "fx":[137.5413,140.42742,141.9198,139.09815], "fy":[-67.403,-61.18482,-57.59462,-64.08601]}, + {"t":0.79995, "x":5.13661, "y":2.75932, "heading":2.20528, "vx":-1.55949, "vy":0.69796, "omega":-0.64652, "ax":10.71818, "ay":-4.79828, "alpha":4.44091, "fx":[137.55269,140.41553,141.95751,139.16809], "fy":[-67.43994,-61.2775,-57.5748,-64.00079]}, + {"t":0.82419, "x":5.10195, "y":2.77483, "heading":2.18961, "vx":-1.29967, "vy":0.58165, "omega":-0.53887, "ax":10.71998, "ay":-4.79864, "alpha":4.44245, "fx":[137.56347,140.40562,141.98984,139.22872], "fy":[-67.47017,-61.3568,-57.55841,-63.92678]}, + {"t":0.84843, "x":5.0736, "y":2.78752, "heading":2.17655, "vx":-1.03981, "vy":0.46532, "omega":-0.43118, "ax":10.72157, "ay":-4.79892, "alpha":4.44399, "fx":[137.57395,140.39839,142.01769,139.28061], "fy":[-67.49454,-61.42279,-57.54507,-63.86451]}, + {"t":0.87268, "x":5.05154, "y":2.79739, "heading":2.1661, "vx":-0.77991, "vy":0.34899, "omega":-0.32345, "ax":10.72299, "ay":-4.79914, "alpha":4.44553, "fx":[137.58437,140.3944,142.04178,139.32415], "fy":[-67.51371,-61.47551,-57.53443,-63.81437]}, + {"t":0.89692, "x":5.03579, "y":2.80444, "heading":2.15826, "vx":-0.51997, "vy":0.23266, "omega":-0.21569, "ax":10.72427, "ay":-4.79929, "alpha":4.44709, "fx":[137.5949,140.39406,142.06266,139.3597], "fy":[-67.52823,-61.51499,-57.52621,-63.7767]}, + {"t":0.92116, "x":5.02633, "y":2.80867, "heading":2.15303, "vx":-0.26001, "vy":0.11632, "omega":-0.10789, "ax":10.72588, "ay":-4.79837, "alpha":4.45052, "fx":[137.6117,140.40269,142.08784,139.3933], "fy":[-67.52622,-61.52988,-57.50271,-63.73902]}, + {"t":0.9454, "x":5.02318, "y":2.81008, "heading":2.15041, "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/example/test.chor b/src/main/deploy/choreo/test.chor similarity index 66% rename from src/main/deploy/choreo/example/test.chor rename to src/main/deploy/choreo/test.chor index 7babf58..a7f20fb 100644 --- a/src/main/deploy/choreo/example/test.chor +++ b/src/main/deploy/choreo/test.chor @@ -1,6 +1,6 @@ { "name":"test", - "version":"v2025.0.0", + "version":1, "type":"Swerve", "variables":{ "expressions":{}, @@ -28,12 +28,12 @@ } }, "mass":{ - "exp":"150 lbs", - "val":68.0388555 + "exp":"115 lbs", + "val":52.16312255 }, "inertia":{ - "exp":"6 kg m ^ 2", - "val":6.0 + "exp":"1 kg m ^ 2", + "val":1.0 }, "gearing":{ "exp":"6.5", @@ -44,25 +44,29 @@ "val":0.0508 }, "vmax":{ - "exp":"502.65482457436696 rad / s", - "val":502.65482457436696 + "exp":"20000 RPM", + "val":2094.395102393195 }, "tmax":{ - "exp":"0.8717213114754098 N m", - "val":0.8717213114754098 + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"1.5", + "val":1.5 }, "bumper":{ "front":{ - "exp":"16 in", - "val":0.4064 + "exp":"14.25 in", + "val":0.36195 }, "side":{ - "exp":"16 in", - "val":0.4064 + "exp":"14.25 in", + "val":0.36195 }, "back":{ - "exp":"16 in", - "val":0.4064 + "exp":"14.25 in", + "val":0.36195 } }, "differentialTrackWidth":{ @@ -71,4 +75,4 @@ } }, "generationFeatures":[] -} \ No newline at end of file +} diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json deleted file mode 100644 index fd334f8..0000000 --- a/src/main/deploy/pathplanner/navgrid.json +++ /dev/null @@ -1,3579 +0,0 @@ -{ - "field_size": { - "x": 16.46, - "y": 8.23 - }, - "nodeSizeMeters": 0.2, - "grid": [ - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - false, - false, - false, - false - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - false, - false, - true, - false - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - false, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true - ], - [ - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true - ], - [ - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ] - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path deleted file mode 100644 index 89c0963..0000000 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ /dev/null @@ -1,180 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.75, - "y": 4.66 - }, - "prevControl": null, - "nextControl": { - "x": 1.750000000000003, - "y": 4.66 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.16, - "y": 5.16 - }, - "prevControl": { - "x": 3.232033278975385, - "y": 5.151257501778375 - }, - "nextControl": { - "x": 3.0879667210246153, - "y": 5.168742498221626 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.84, - "y": 5.16 - }, - "prevControl": { - "x": 3.8117780004859023, - "y": 5.791220816297228 - }, - "nextControl": { - "x": 3.8682219995140974, - "y": 4.528779183702772 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.1605082259803137, - "y": 2.671897365212321 - }, - "prevControl": { - "x": 3.191015148395487, - "y": 2.6439249115674097 - }, - "nextControl": { - "x": 3.13000130356514, - "y": 2.699869818857231 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.1605082259803137, - "y": 1.7892169856332496 - }, - "prevControl": { - "x": 3.0581303777088693, - "y": 1.6997051339695315 - }, - "nextControl": { - "x": 3.2628860742517576, - "y": 1.878728837296966 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.1605082259803137, - "y": 0.9781052854795105 - }, - "prevControl": { - "x": 3.3800013035651397, - "y": 1.2327118906379662 - }, - "nextControl": { - "x": 2.9410151483954876, - "y": 0.7234986803210544 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.335506900634398, - "y": 0.9781052854795105 - }, - "prevControl": { - "x": 1.8534334104500785, - "y": 0.9781052854795105 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 4.0, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 3.0, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 0, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 3.0, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 1, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 5.0, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 2, - "rotationDegrees": 0, - "rotateFast": false - } - ], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0.8, - "maxWaypointRelativePos": 0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 179.2178340432465, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/kotlin/frc/team449/Robot.kt b/src/main/kotlin/frc/team449/Robot.kt index b8e6a68..501bfaf 100644 --- a/src/main/kotlin/frc/team449/Robot.kt +++ b/src/main/kotlin/frc/team449/Robot.kt @@ -1,5 +1,6 @@ package frc.team449 +import choreo.auto.AutoChooser import edu.wpi.first.wpilibj.PowerDistribution import edu.wpi.first.wpilibj2.command.button.CommandXboxController import frc.team449.subsystems.RobotConstants @@ -39,6 +40,8 @@ class Robot : RobotBase(), Logged { @Log.NT override val drive: SwerveDrive = SwerveDrive.createSwerveKraken(field) + val autoChooser = AutoChooser() + @Log.NT override val poseSubsystem: PoseSubsystem = createPoseSubsystem(ahrs, drive, field) diff --git a/src/main/kotlin/frc/team449/RobotLoop.kt b/src/main/kotlin/frc/team449/RobotLoop.kt index 17d4f37..f2d6df3 100644 --- a/src/main/kotlin/frc/team449/RobotLoop.kt +++ b/src/main/kotlin/frc/team449/RobotLoop.kt @@ -7,12 +7,11 @@ import edu.wpi.first.math.util.Units import edu.wpi.first.wpilibj.* import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard -import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.CommandScheduler import edu.wpi.first.wpilibj2.command.InstantCommand -import frc.team449.auto.RoutineChooser +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers +import frc.team449.auto.Routines import frc.team449.commands.light.BlairChasing -import frc.team449.commands.light.BreatheHue import frc.team449.commands.light.Rainbow import frc.team449.subsystems.drive.swerve.SwerveSim import frc.team449.subsystems.elevator.ElevatorConstants @@ -24,6 +23,7 @@ import monologue.Logged import monologue.Monologue import org.littletonrobotics.urcl.URCL import kotlin.jvm.optionals.getOrNull +import kotlin.math.PI /** The main class of the robot, constructs all the subsystems * and initializes default commands . */ @@ -32,12 +32,9 @@ class RobotLoop : TimedRobot(), Logged { @Log.NT private val robot = Robot() - private val routineChooser: RoutineChooser = RoutineChooser(robot) - @Log.NT private val field = robot.field - private var autoCommand: Command? = null - private var routineMap = hashMapOf() + private val controllerBinder = ControllerBindings(robot.driveController, robot.mechController, robot) override fun robotInit() { @@ -63,12 +60,16 @@ class RobotLoop : TimedRobot(), Logged { // Generate Auto Routines println("Generating Auto Routines : ${Timer.getFPGATimestamp()}") - routineMap = routineChooser.routineMap() - println("DONE Generating Auto Routines : ${Timer.getFPGATimestamp()}") + val routines = Routines(robot) + + routines.addOptions(robot.autoChooser) - routineChooser.createOptions() + println("Putting the thing on the other thing") + SmartDashboard.putData("Auto Chooser", robot.autoChooser) + + RobotModeTriggers.autonomous().whileTrue(robot.autoChooser.selectedCommandScheduler()) + println("DONE Generating Auto Routines : ${Timer.getFPGATimestamp()}") - SmartDashboard.putData("Routine Chooser", routineChooser) SmartDashboard.putData("Command Scheduler", CommandScheduler.getInstance()) robot.light.defaultCommand = BlairChasing(robot.light) @@ -107,25 +108,11 @@ class RobotLoop : TimedRobot(), Logged { Monologue.updateAll() } - override fun autonomousInit() { - /** Every time auto starts, we update the chosen auto command. */ - this.autoCommand = routineMap[if (DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Red) "Red" + routineChooser.selected else "Blue" + routineChooser.selected] - CommandScheduler.getInstance().schedule(this.autoCommand) - - if (DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Red) { - BreatheHue(robot.light, 0).schedule() - } else { - BreatheHue(robot.light, 95).schedule() - } - } + override fun autonomousInit() {} override fun autonomousPeriodic() {} override fun teleopInit() { - if (autoCommand != null) { - CommandScheduler.getInstance().cancel(autoCommand) - } - (robot.light.currentCommand ?: InstantCommand()).cancel() robot.drive.defaultCommand = robot.driveCommand @@ -143,11 +130,7 @@ class RobotLoop : TimedRobot(), Logged { override fun disabledPeriodic() {} - override fun testInit() { - if (autoCommand != null) { - CommandScheduler.getInstance().cancel(autoCommand) - } - } + override fun testInit() {} override fun testPeriodic() {} diff --git a/src/main/kotlin/frc/team449/auto/AutoUtil.kt b/src/main/kotlin/frc/team449/auto/AutoUtil.kt deleted file mode 100644 index 2784090..0000000 --- a/src/main/kotlin/frc/team449/auto/AutoUtil.kt +++ /dev/null @@ -1,51 +0,0 @@ -package frc.team449.auto - -import edu.wpi.first.math.MatBuilder -import edu.wpi.first.math.MathUtil -import edu.wpi.first.math.Nat -import edu.wpi.first.math.controller.PIDController -import frc.team449.auto.choreo.ChoreoTrajectory -import frc.team449.subsystems.FieldConstants -import frc.team449.subsystems.RobotConstants -import kotlin.math.PI - -object AutoUtil { - - /** If you need snap to angle during auto (for vision assisted pointing), use this controller since - * the orthogonal command is not set up yet during auto - */ - val turnController = PIDController( - RobotConstants.SNAP_KP, - RobotConstants.SNAP_KI, - RobotConstants.SNAP_KD - ) - - /** You will need to change this based on how the field is set up each year */ - fun transformForRed(pathGroup: MutableList): MutableList { - for (index in 0 until pathGroup.size) { - for (time in pathGroup[index].objectiveTimestamps) { - val currentMatrix = pathGroup[index].stateMap.get(time) - - val newMatrix = MatBuilder.fill( - Nat.N2(), - Nat.N3(), - FieldConstants.fieldLength - currentMatrix[0, 0], - currentMatrix[0, 1], - MathUtil.angleModulus(PI - currentMatrix[0, 2]), - -currentMatrix[1, 0], - currentMatrix[1, 1], - -currentMatrix[1, 2] - ) - - pathGroup[index].stateMap.put(time, newMatrix) - } - } - - return pathGroup - } - - /** Add other methods that return commands that do groups of actions that are done - * across different auto routines. For Charged UP, these methods were things such as - * dropping a cone/cube, or getting in ground intake position, etc. - */ -} diff --git a/src/main/kotlin/frc/team449/auto/RoutineChooser.kt b/src/main/kotlin/frc/team449/auto/RoutineChooser.kt deleted file mode 100644 index 7451f0d..0000000 --- a/src/main/kotlin/frc/team449/auto/RoutineChooser.kt +++ /dev/null @@ -1,21 +0,0 @@ -package frc.team449.auto - -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser -import edu.wpi.first.wpilibj2.command.Command -import frc.team449.Robot -import frc.team449.auto.routines.DoNothing - -class RoutineChooser(private val robot: Robot) : SendableChooser() { - - fun routineMap(): HashMap { - return hashMapOf( - "RedDoNothing" to DoNothing(robot).createCommand(), - "BlueDoNothing" to DoNothing(robot).createCommand() - ) - } - - fun createOptions() { - /** Add auto options here */ - this.setDefaultOption("Do Nothing", "DoNothing") - } -} diff --git a/src/main/kotlin/frc/team449/auto/Routines.kt b/src/main/kotlin/frc/team449/auto/Routines.kt new file mode 100644 index 0000000..50fb56a --- /dev/null +++ b/src/main/kotlin/frc/team449/auto/Routines.kt @@ -0,0 +1,192 @@ +package frc.team449.auto + +import choreo.auto.AutoChooser +import choreo.auto.AutoFactory +import choreo.auto.AutoRoutine +import choreo.auto.AutoTrajectory +import edu.wpi.first.wpilibj2.command.Commands +import frc.team449.Robot + +class Routines( + robot: Robot +) { + val autoFactory = AutoFactory( + robot.poseSubsystem::pose, + robot.poseSubsystem::resetOdometry, + robot.drive::followTrajectory, + true, + robot.drive + ) + + // do nothing + fun doNothing(): AutoRoutine { + val nothing: AutoRoutine = autoFactory.newRoutine("Nothing") + return nothing + } + + // right taxi + fun rightTaxi(): AutoRoutine { + val rTaxi: AutoRoutine = autoFactory.newRoutine("Right Taxi") + val rTaxiTrajectory: AutoTrajectory = rTaxi.trajectory("rightTaxi") + rTaxi.active().onTrue( + Commands.sequence( + rTaxiTrajectory.resetOdometry(), + rTaxiTrajectory.cmd() + ) + ) + return rTaxi + } + + // left taxi + fun leftTaxi(): AutoRoutine { + val lTaxi: AutoRoutine = autoFactory.newRoutine("Left Taxi") + val lTaxiTrajectory: AutoTrajectory = lTaxi.trajectory("leftTaxi") + lTaxi.active().onTrue( + Commands.sequence( + lTaxiTrajectory.resetOdometry(), + lTaxiTrajectory.cmd() + ) + ) + return lTaxi + } + + // coral at l1 one on reef E + fun reefE(): AutoRoutine { + val E: AutoRoutine = autoFactory.newRoutine("L1 reef E") + val reefETrajectory: AutoTrajectory = E.trajectory("right_E") + E.active().onTrue( + Commands.sequence( + reefETrajectory.resetOdometry(), + reefETrajectory.cmd() + ) + ) + return E + } + + // coral at l1 one on reef F + fun reefF(): AutoRoutine { + val F: AutoRoutine = autoFactory.newRoutine("L1 reef F") + val reefFTrajectory: AutoTrajectory = F.trajectory("left_F") + F.active().onTrue( + Commands.sequence( + reefFTrajectory.resetOdometry(), + reefFTrajectory.cmd() + ) + ) + return F + } + + // coral at l1 one on reef G + fun reefG(): AutoRoutine { + val g: AutoRoutine = autoFactory.newRoutine("L1 reef G") + val reefGTrajectory: AutoTrajectory = g.trajectory("middle_G") + g.active().onTrue( + Commands.sequence( + reefGTrajectory.resetOdometry(), + reefGTrajectory.cmd() + ) + ) + return g + } + + // coral at l1 one on reef J + fun reefJ(): AutoRoutine { + val J: AutoRoutine = autoFactory.newRoutine("L1 reef J") + val reefJTrajectory: AutoTrajectory = J.trajectory("left_J") + J.active().onTrue( + Commands.sequence( + reefJTrajectory.resetOdometry(), + reefJTrajectory.cmd() + ) + ) + return J + } + + // placing L4 at reef E then l1 at reef D + fun reefED(): AutoRoutine { + val E_D: AutoRoutine = autoFactory.newRoutine("L4 reef E and L1 reef D") + val reefETrajectory: AutoTrajectory = E_D.trajectory("right_E") + val reefEtoStationTrajectory: AutoTrajectory = E_D.trajectory("E_rightStation") + val stationToDTrajectory: AutoTrajectory = E_D.trajectory("rightStation_D") + E_D.active().onTrue( + Commands.sequence( + Commands.sequence( + reefETrajectory.resetOdometry(), + reefETrajectory.cmd(), + Commands.waitSeconds(0.7) + ), + Commands.sequence( + reefEtoStationTrajectory.cmd(), + Commands.waitSeconds(0.9) + ), + Commands.sequence( + stationToDTrajectory.cmd() + ) + ) + ) + return E_D + } + + // placing L4 at reef E then l1 at reef L + fun reefJL(): AutoRoutine { + val J_L: AutoRoutine = autoFactory.newRoutine("L4 reef J and L1 reef L") + val reefJTrajectory: AutoTrajectory = J_L.trajectory("left_J") + val reefJtoStationTrajectory: AutoTrajectory = J_L.trajectory("J_leftStation") + val stationToLTrajectory: AutoTrajectory = J_L.trajectory("leftStation_L") + J_L.active().onTrue( + Commands.sequence( + Commands.sequence( + reefJTrajectory.resetOdometry(), + reefJTrajectory.cmd(), + Commands.waitSeconds(0.7) + ), + Commands.sequence( + reefJtoStationTrajectory.cmd(), + Commands.waitSeconds(0.9) + ), + Commands.sequence( + stationToLTrajectory.cmd() + ) + ) + ) + return J_L + } + + fun reefJK(): AutoRoutine { + val J_K: AutoRoutine = autoFactory.newRoutine("L4 reef J and L1 reef K") + val reefJTrajectory: AutoTrajectory = J_K.trajectory("left_J") + val reefJtoStationTrajectory: AutoTrajectory = J_K.trajectory("J_leftStation") + val stationToKTrajectory: AutoTrajectory = J_K.trajectory("leftStation_K") + J_K.active().onTrue( + Commands.sequence( + Commands.sequence( + reefJTrajectory.resetOdometry(), + reefJTrajectory.cmd(), + Commands.waitSeconds(0.7) + ), + Commands.sequence( + reefJtoStationTrajectory.cmd(), + Commands.waitSeconds(0.9) + ), + Commands.sequence( + stationToKTrajectory.cmd() + ) + ) + ) + return J_K + } + +// autoChooser that will be displayed on dashboard + fun addOptions(autoChooser: AutoChooser) { + autoChooser.addRoutine("Nothing", this::doNothing) + autoChooser.addRoutine("RightTaxi", this::rightTaxi) + autoChooser.addRoutine("LeftTaxi", this::leftTaxi) + autoChooser.addRoutine("E", this::reefE) + autoChooser.addRoutine("F", this::reefF) + autoChooser.addRoutine("G", this::reefG) + autoChooser.addRoutine("J", this::reefJ) + autoChooser.addRoutine("E & D ", this::reefED) + autoChooser.addRoutine("J & L", this::reefJL) + autoChooser.addRoutine("J & K", this::reefJK) + } +} diff --git a/src/main/kotlin/frc/team449/auto/choreo/ChoreoRoutine.kt b/src/main/kotlin/frc/team449/auto/choreo/ChoreoRoutine.kt deleted file mode 100644 index c9b0140..0000000 --- a/src/main/kotlin/frc/team449/auto/choreo/ChoreoRoutine.kt +++ /dev/null @@ -1,87 +0,0 @@ -package frc.team449.auto.choreo - -import edu.wpi.first.math.controller.PIDController -import edu.wpi.first.math.geometry.Pose2d -import edu.wpi.first.math.geometry.Rotation2d -import edu.wpi.first.wpilibj2.command.* -import frc.team449.auto.AutoConstants -import frc.team449.subsystems.drive.swerve.SwerveDrive -import frc.team449.subsystems.vision.PoseSubsystem -import kotlin.math.abs - -/** - * Class for easily creating an autonomous routine command given Choreo Trajectories and parallel/stop events - * @param xController PID Controller to use for x-position error (output is next desired x velocity, not volts) - * @param yController PID Controller to use for y-position error (output is next desired y velocity, not volts) - * @param thetaController PID Controller to use for rotation error (output is next desired rotation velocity, not volts) - * @param drive Swerve Drivetrain to use - * @param stopEventMap Map stop event commands in between trajectories. Index 0 is before the first trajectory, index n is after the nth trajectory. - * @param parallelEventMap Map parallel commands to be run during trajectory following. This cannot require the same drive subsystem. Index 0 refers to the command during the first trajectory. - * @param poseTol Tolerance within final pose to say it is "good enough" - * @param resetPosition Whether to reset the pose to the first pose in every trajectory. By default you reset to the first pose in the first trajectory, unless you are in resetPositionTolerance. - * @param resetPositionTolerance If within this tolerance from initial pose to the first pose in the first trajectory, do not reset pose. - * @param timeout Maximum time to wait after trajectory has finished to get in tolerance. If it gets in tolerance before the timeout, the command will still end, so you will not see this in simulation - * @param debug Whether to run on trajectory expected velocities only (no feedback control) - */ -class ChoreoRoutine( - private val xController: PIDController = PIDController(AutoConstants.DEFAULT_X_KP, 0.0, AutoConstants.DEFAULT_X_KD), - private val yController: PIDController = PIDController(AutoConstants.DEFAULT_Y_KP, 0.0, AutoConstants.DEFAULT_Y_KD), - private val thetaController: PIDController = PIDController(AutoConstants.DEFAULT_ROTATION_KP, 0.0, AutoConstants.DEFAULT_ROTATION_KD), - private val drive: SwerveDrive, - private val poseSubsystem: PoseSubsystem, - private val stopEventMap: HashMap = HashMap(), - private val parallelEventMap: HashMap = HashMap(), - private val poseTol: Pose2d = Pose2d(0.05, 0.05, Rotation2d.fromDegrees(1.5)), - private val resetPosition: Boolean = false, - private val resetPositionTolerance: Pose2d = Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0)), - private val timeout: Double = 0.65, - private val debug: Boolean = false -) { - - private fun resetPose(trajectory: ChoreoTrajectory): Command { - val poseError = poseSubsystem.pose.relativeTo(trajectory.initialPose()) - - if (abs(poseError.x) < resetPositionTolerance.x && - abs(poseError.y) < resetPositionTolerance.y && - abs(poseError.rotation.radians) < resetPositionTolerance.rotation.radians - ) { - return PrintCommand("Pose not reset.") - } - - return InstantCommand({ poseSubsystem.pose = trajectory.initialPose() }) - } - - /** - * Create a command that follows all specified actions for an auto routine. - * @param trajectories The list of parsed Choreo trajectories you want to run - */ - fun createRoutine(trajectories: MutableList): Command { - val ezraGallun = SequentialCommandGroup( - resetPose(trajectories[0]), - stopEventMap.getOrDefault(0, InstantCommand()) - ) - - for (i in 0 until trajectories.size) { - ezraGallun.addCommands( - ParallelDeadlineGroup( - ChoreoFollower( - drive, - poseSubsystem, - trajectories[i], - xController, - yController, - thetaController, - poseTol, - timeout, - resetPosition, - debug - ).andThen(drive::stop, drive), - parallelEventMap.getOrDefault(i, InstantCommand()) - ) - ) - ezraGallun.addCommands(stopEventMap.getOrDefault(i + 1, InstantCommand())) - } - - return ezraGallun - } -} diff --git a/src/main/kotlin/frc/team449/auto/choreo/ChoreoRoutineStructure.kt b/src/main/kotlin/frc/team449/auto/choreo/ChoreoRoutineStructure.kt deleted file mode 100644 index cf5d35c..0000000 --- a/src/main/kotlin/frc/team449/auto/choreo/ChoreoRoutineStructure.kt +++ /dev/null @@ -1,21 +0,0 @@ -package frc.team449.auto.choreo - -import edu.wpi.first.wpilibj2.command.Command - -/** - * Interface to guide you as to how to create a Choreo routine class. - */ -interface ChoreoRoutineStructure { - - val routine: ChoreoRoutine - - val trajectory: MutableList - - /** - * Method for returning the command of the entire auto routine - * @see ChoreoRoutine.createRoutine - */ - fun createCommand(): Command { - return routine.createRoutine(trajectory) - } -} diff --git a/src/main/kotlin/frc/team449/auto/choreo/ChoreoTrajectory.kt b/src/main/kotlin/frc/team449/auto/choreo/ChoreoTrajectory.kt deleted file mode 100644 index 6b1bb67..0000000 --- a/src/main/kotlin/frc/team449/auto/choreo/ChoreoTrajectory.kt +++ /dev/null @@ -1,130 +0,0 @@ -package frc.team449.auto.choreo - -import edu.wpi.first.math.InterpolatingMatrixTreeMap -import edu.wpi.first.math.MatBuilder -import edu.wpi.first.math.MathUtil -import edu.wpi.first.math.Nat -import edu.wpi.first.math.geometry.Pose2d -import edu.wpi.first.math.geometry.Rotation2d -import edu.wpi.first.math.numbers.N2 -import edu.wpi.first.math.numbers.N3 -import edu.wpi.first.wpilibj.Filesystem -import org.json.simple.JSONArray -import org.json.simple.JSONObject -import org.json.simple.parser.JSONParser -import java.io.File -import java.io.FileReader -import kotlin.math.abs - -/** - * Data structure for parsed Choreo Trajectory, and interpolates between points given in the Choreo file. Use the parser function below to create a ChoreoTrajectory. - * @see InterpolatingMatrixTreeMap - */ -class ChoreoTrajectory( - val name: String, - val stateMap: InterpolatingMatrixTreeMap, - val totalTime: Double, - val objectiveTimestamps: ArrayList -) { - - fun initialPose(): Pose2d { - val initialState = stateMap.get(0.0) - - return Pose2d( - initialState[0, 0], - initialState[0, 1], - Rotation2d(initialState[0, 2]) - ) - } - - fun sample(t: Double): ChoreoState { - val timeSeconds = MathUtil.clamp(t, 0.0, totalTime) - val interpolatedMat = stateMap.get(timeSeconds) - - return ChoreoState( - interpolatedMat[0, 0], - interpolatedMat[0, 1], - interpolatedMat[0, 2], - interpolatedMat[1, 0], - interpolatedMat[1, 1], - interpolatedMat[1, 2] - ) - } - - class ChoreoState( - val xPos: Double, - val yPos: Double, - val theta: Double, - val xVel: Double, - val yVel: Double, - val thetaVel: Double - ) - - companion object { - - /** - * @param filenames Filenames of .traj document file. By default, it will check in the deploy/choreo folder - */ - fun createTrajectory( - filenames: List, - subFolder: String = "" - ): MutableList { - val trajList = mutableListOf() - - for (filename in filenames) { - val path = Filesystem.getDeployDirectory().absolutePath.plus("/choreo/$subFolder/$filename.traj") - val document = (JSONParser().parse(FileReader(File(path).absolutePath)) as JSONObject)["trajectory"] as JSONObject - - val trajectory = document["samples"] as JSONArray - - val info = parse(trajectory) - - val last = trajectory.last() as JSONObject - val totalTime = last["t"] as Double - - trajList.add( - ChoreoTrajectory( - filename, - info.first, - totalTime, - info.second - ) - ) - } - - return trajList - } - - private fun deadband(value: Double, deadband: Double = 1e-6): Double { - return if (abs(value) > deadband) value else 0.0 - } - - private fun parse(trajectory: JSONArray): Pair, ArrayList> { - val stateMap = InterpolatingMatrixTreeMap() - - val timestamps = arrayListOf() - - trajectory.forEach { state -> - state as JSONObject - val stateTime = state["t"].toString().toDouble() - - timestamps.add(stateTime) - - val matrix = MatBuilder.fill( - Nat.N2(), - Nat.N3(), - deadband(state["x"].toString().toDouble()), - deadband(state["y"].toString().toDouble()), - deadband(state["heading"].toString().toDouble()), - deadband(state["vx"].toString().toDouble()), - deadband(state["vy"].toString().toDouble()), - deadband(state["omega"].toString().toDouble()) - ) - - stateMap.put(stateTime, matrix) - } - - return stateMap to timestamps - } - } -} diff --git a/src/main/kotlin/frc/team449/auto/routines/DoNothing.kt b/src/main/kotlin/frc/team449/auto/routines/DoNothing.kt deleted file mode 100644 index b14ac6a..0000000 --- a/src/main/kotlin/frc/team449/auto/routines/DoNothing.kt +++ /dev/null @@ -1,25 +0,0 @@ -package frc.team449.auto.routines - -import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.InstantCommand -import frc.team449.Robot -import frc.team449.auto.choreo.ChoreoRoutine -import frc.team449.auto.choreo.ChoreoRoutineStructure -import frc.team449.auto.choreo.ChoreoTrajectory - -class DoNothing( - private val robot: Robot -) : ChoreoRoutineStructure { - - override val routine = - ChoreoRoutine( - drive = robot.drive, - poseSubsystem = robot.poseSubsystem - ) - - override val trajectory: MutableList = mutableListOf() - - override fun createCommand(): Command { - return InstantCommand() - } -} diff --git a/src/main/kotlin/frc/team449/auto/routines/ExampleRoutine.kt b/src/main/kotlin/frc/team449/auto/routines/ExampleRoutine.kt deleted file mode 100644 index a1d0f93..0000000 --- a/src/main/kotlin/frc/team449/auto/routines/ExampleRoutine.kt +++ /dev/null @@ -1,41 +0,0 @@ -package frc.team449.auto.routines - -import edu.wpi.first.wpilibj2.command.InstantCommand -import edu.wpi.first.wpilibj2.command.PrintCommand -import frc.team449.Robot -import frc.team449.auto.AutoUtil -import frc.team449.auto.choreo.ChoreoRoutine -import frc.team449.auto.choreo.ChoreoRoutineStructure -import frc.team449.auto.choreo.ChoreoTrajectory - -class ExampleRoutine( - robot: Robot, - isRed: Boolean -) : ChoreoRoutineStructure { - - override val routine = - ChoreoRoutine( - drive = robot.drive, - poseSubsystem = robot.poseSubsystem, - parallelEventMap = hashMapOf( - 0 to InstantCommand(), - 1 to InstantCommand() - ), - stopEventMap = hashMapOf( - 0 to PrintCommand("Going to start moving!"), - 1 to PrintCommand("Finished the 1st trajectory!"), - 2 to PrintCommand("Finished the routine!") - ), - debug = false, - timeout = 1.5 - ) - - override val trajectory: MutableList = - if (isRed) { - AutoUtil.transformForRed( - ChoreoTrajectory.createTrajectory(arrayListOf("part1", "part2"), "example") - ) - } else { - ChoreoTrajectory.createTrajectory(arrayListOf("part1", "part2"), "example") - } -} diff --git a/src/main/kotlin/frc/team449/auto/choreo/ChoreoFollower.kt b/src/main/kotlin/frc/team449/commands/PIDPoseAlign.kt similarity index 62% rename from src/main/kotlin/frc/team449/auto/choreo/ChoreoFollower.kt rename to src/main/kotlin/frc/team449/commands/PIDPoseAlign.kt index 727deac..7d808c1 100644 --- a/src/main/kotlin/frc/team449/auto/choreo/ChoreoFollower.kt +++ b/src/main/kotlin/frc/team449/commands/PIDPoseAlign.kt @@ -1,4 +1,5 @@ -package frc.team449.auto.choreo + +package frc.team449.commands import edu.wpi.first.math.controller.PIDController import edu.wpi.first.math.geometry.Pose2d @@ -12,29 +13,23 @@ import frc.team449.subsystems.vision.PoseSubsystem import kotlin.math.PI /** - * Follower command so a robot can follow a parsed Choreo Trajectory with feedback - * @see ChoreoTrajectory + * Follower command so a robot go to a predefined pose * @param drivetrain Swerve Drivetrain to use - * @param trajectory Parsed Choreo Trajectory * @param xController PID Controller to use for x-position error (output is next desired x velocity, not volts) * @param yController PID Controller to use for y-position error (output is next desired y velocity, not volts) * @param thetaController PID Controller to use for rotation error (output is next desired rotation velocity, not volts) * @param poseTol Tolerance within final pose to say it is "good enough" * @param timeout Maximum time to wait after trajectory has finished to get in tolerance. A very low timeout may end this command before you get in tolerance. - * @param resetPose Whether to reset the pose to the first pose in the trajectory - * @param debug Whether to run on trajectory expected velocities only (no feedback control) */ -class ChoreoFollower( +class PIDPoseAlign( private val drivetrain: SwerveDrive, private val poseSubsystem: PoseSubsystem, - private val trajectory: ChoreoTrajectory, + private val pose: Pose2d, private val xController: PIDController = PIDController(AutoConstants.DEFAULT_X_KP, 0.0, 0.0), private val yController: PIDController = PIDController(AutoConstants.DEFAULT_Y_KP, 0.0, 0.0), private val thetaController: PIDController = PIDController(AutoConstants.DEFAULT_ROTATION_KP, 0.0, 0.0), poseTol: Pose2d = Pose2d(0.035, 0.035, Rotation2d(0.035)), - private val timeout: Double = 0.65, - private val resetPose: Boolean = false, - private val debug: Boolean = false + private val timeout: Double = 0.65 ) : Command() { private val timer = Timer() @@ -53,20 +48,12 @@ class ChoreoFollower( thetaController.setTolerance(poseTol.rotation.radians) } - private fun calculate(currPose: Pose2d, desState: ChoreoTrajectory.ChoreoState): ChassisSpeeds { - val xFF = desState.xVel - val yFF = desState.yVel - val angFF = desState.thetaVel - - val xPID = xController.calculate(currPose.x, desState.xPos) - val yPID = yController.calculate(currPose.y, desState.yPos) - val angPID = thetaController.calculate(currPose.rotation.radians, desState.theta) + private fun calculate(currPose: Pose2d, desState: Pose2d): ChassisSpeeds { + val xPID = xController.calculate(currPose.x, desState.x) + val yPID = yController.calculate(currPose.y, desState.y) + val angPID = thetaController.calculate(currPose.rotation.radians, desState.rotation.radians) - return if (debug) { - ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, angFF, currPose.rotation) - } else { - ChassisSpeeds.fromFieldRelativeSpeeds(xFF + xPID, yFF + yPID, angFF + angPID, currPose.rotation) - } + return ChassisSpeeds.fromFieldRelativeSpeeds(xPID, yPID, angPID, currPose.rotation) } private fun allControllersAtReference(): Boolean { @@ -78,24 +65,16 @@ class ChoreoFollower( yController.reset() thetaController.reset() - if (resetPose) { - poseSubsystem.pose = trajectory.initialPose() - } - timer.restart() } override fun execute() { - val currTime = timer.get() - - val desiredMatrix = trajectory.sample(currTime) - - drivetrain.set(calculate(poseSubsystem.pose, desiredMatrix)) + drivetrain.driveFieldRelative(calculate(poseSubsystem.pose, pose)) } override fun isFinished(): Boolean { - return (timer.hasElapsed(trajectory.totalTime) && allControllersAtReference()) || - timer.hasElapsed(trajectory.totalTime + timeout) + return allControllersAtReference() || + timer.hasElapsed(timeout) } override fun end(interrupted: Boolean) { diff --git a/src/main/kotlin/frc/team449/commands/driveAlign/OrbitAlign.kt b/src/main/kotlin/frc/team449/commands/driveAlign/OrbitAlign.kt index 8c21595..9ccc0f5 100644 --- a/src/main/kotlin/frc/team449/commands/driveAlign/OrbitAlign.kt +++ b/src/main/kotlin/frc/team449/commands/driveAlign/OrbitAlign.kt @@ -138,7 +138,7 @@ class OrbitAlign( poseSubsystem.heading ) - drive.set( + drive.driveFieldRelative( desVel ) } diff --git a/src/main/kotlin/frc/team449/commands/driveAlign/ProfiledPoseAlign.kt b/src/main/kotlin/frc/team449/commands/driveAlign/ProfiledPoseAlign.kt index 3331be3..acabd55 100644 --- a/src/main/kotlin/frc/team449/commands/driveAlign/ProfiledPoseAlign.kt +++ b/src/main/kotlin/frc/team449/commands/driveAlign/ProfiledPoseAlign.kt @@ -93,7 +93,7 @@ class ProfiledPoseAlign( val yFeedback = yPID.calculate(poseSubsystem.pose.y, yProfCalc.position) val headingFeedback = headingPID.calculate(poseSubsystem.heading.radians) - drive.set( + drive.driveFieldRelative( ChassisSpeeds.fromFieldRelativeSpeeds( xFeedback + xProfCalc.velocity, yFeedback + yProfCalc.velocity, diff --git a/src/main/kotlin/frc/team449/subsystems/RobotConstants.kt b/src/main/kotlin/frc/team449/subsystems/RobotConstants.kt index 1954f16..a3b2f58 100644 --- a/src/main/kotlin/frc/team449/subsystems/RobotConstants.kt +++ b/src/main/kotlin/frc/team449/subsystems/RobotConstants.kt @@ -46,7 +46,7 @@ object RobotConstants { const val ALIGN_ROT_SPEED = 7 * PI / 2 // Robot Dimensions (INCLUDING BUMPERS) - val ROBOT_WIDTH = Units.inchesToMeters(27.25 + 3.25 * 2) - val ROBOT_LENGTH = Units.inchesToMeters(27.5 + 3.25 * 2) + val ROBOT_WIDTH = Units.inchesToMeters(27 + 3.25 * 2) + val ROBOT_LENGTH = Units.inchesToMeters(27 + 3.25 * 2) val ROBOT_WEIGHT = 55.0 // TODO: find (incl. bumpers + battery) (kg) } diff --git a/src/main/kotlin/frc/team449/subsystems/drive/DriveSubsystem.kt b/src/main/kotlin/frc/team449/subsystems/drive/DriveSubsystem.kt deleted file mode 100644 index 04ddc6a..0000000 --- a/src/main/kotlin/frc/team449/subsystems/drive/DriveSubsystem.kt +++ /dev/null @@ -1,37 +0,0 @@ -package frc.team449.subsystems.drive - -import edu.wpi.first.math.MathUtil -import edu.wpi.first.math.geometry.Pose2d -import edu.wpi.first.math.geometry.Rotation2d -import edu.wpi.first.math.geometry.Translation2d -import edu.wpi.first.math.kinematics.ChassisSpeeds -import edu.wpi.first.wpilibj2.command.Subsystem - -/** A drivetrain that uses closed-loop velocity control. */ -interface DriveSubsystem : Subsystem { - var heading: Rotation2d - get() = Rotation2d(MathUtil.angleModulus(this.pose.rotation.radians)) - set(value) { - this.pose = Pose2d(Translation2d(this.pose.x, this.pose.y), value) - } - - var pose: Pose2d - - /** Sets the drivetrain's desired speeds. */ - fun set(desiredSpeeds: ChassisSpeeds) - - /** Sets all the robot's drive motors to 0. */ - fun stop() - - /** - * Used to simulate a drivetrain. Only one instance of this class should be made per drivetrain. - */ - interface SimController { - fun update() - - /** - * Simulate the current drawn by the drivetrain - */ - fun getCurrentDraw(): Double - } -} diff --git a/src/main/kotlin/frc/team449/subsystems/drive/differential/DifferentialDrive.kt b/src/main/kotlin/frc/team449/subsystems/drive/differential/DifferentialDrive.kt index 5dbec4d..8ff23cf 100644 --- a/src/main/kotlin/frc/team449/subsystems/drive/differential/DifferentialDrive.kt +++ b/src/main/kotlin/frc/team449/subsystems/drive/differential/DifferentialDrive.kt @@ -10,7 +10,6 @@ import edu.wpi.first.math.kinematics.DifferentialDriveKinematics import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds import edu.wpi.first.wpilibj2.command.SubsystemBase import frc.team449.subsystems.RobotConstants -import frc.team449.subsystems.drive.DriveSubsystem import frc.team449.system.AHRS import frc.team449.system.encoder.Encoder import frc.team449.system.encoder.QuadEncoder @@ -34,7 +33,7 @@ open class DifferentialDrive( private val feedForward: DifferentialDriveFeedforward, private val makeSidePID: () -> PIDController, private val trackwidth: Double -) : DriveSubsystem, SubsystemBase() { +) : SubsystemBase() { init { leftEncoder.resetPosition(0.0) rightEncoder.resetPosition(0.0) @@ -69,7 +68,7 @@ open class DifferentialDrive( private var rightVel = 0.0 /** Calculate left and right side speeds from given [ChassisSpeeds]. */ - override fun set(desiredSpeeds: ChassisSpeeds) { + fun set(desiredSpeeds: ChassisSpeeds) { prevWheelSpeeds = desiredWheelSpeeds prevLeftVel = desiredWheelSpeeds.leftMetersPerSecond @@ -99,13 +98,13 @@ open class DifferentialDrive( } /** The (x, y, theta) position of the robot on the field. */ - override var pose: Pose2d + open var pose: Pose2d get() = this.poseEstimator.estimatedPosition set(pose) { this.poseEstimator.resetPosition(ahrs.heading, leftEncoder.position, rightEncoder.position, pose) } - override fun stop() { + fun stop() { this.set(ChassisSpeeds(0.0, 0.0, 0.0)) } diff --git a/src/main/kotlin/frc/team449/subsystems/drive/differential/DifferentialSim.kt b/src/main/kotlin/frc/team449/subsystems/drive/differential/DifferentialSim.kt index 94b87e5..5741698 100644 --- a/src/main/kotlin/frc/team449/subsystems/drive/differential/DifferentialSim.kt +++ b/src/main/kotlin/frc/team449/subsystems/drive/differential/DifferentialSim.kt @@ -70,7 +70,7 @@ class DifferentialSim( driveSim.pose = value } - override var heading: Rotation2d + var heading: Rotation2d get() = driveSim.heading.rotateBy(offset) set(value) { offset = value - driveSim.heading diff --git a/src/main/kotlin/frc/team449/subsystems/drive/holonomic/HolonomicDrive.kt b/src/main/kotlin/frc/team449/subsystems/drive/holonomic/HolonomicDrive.kt deleted file mode 100644 index e399a6b..0000000 --- a/src/main/kotlin/frc/team449/subsystems/drive/holonomic/HolonomicDrive.kt +++ /dev/null @@ -1,11 +0,0 @@ -package frc.team449.subsystems.drive.holonomic - -import frc.team449.subsystems.drive.DriveSubsystem - -interface HolonomicDrive : DriveSubsystem { - /** The max speed that the drivetrain can translate at, in meters per second. */ - var maxLinearSpeed: Double - - /** The max speed that the drivetrain can turn in place at, in radians per second. */ - var maxRotSpeed: Double -} diff --git a/src/main/kotlin/frc/team449/subsystems/drive/holonomic/HolonomicOI.kt b/src/main/kotlin/frc/team449/subsystems/drive/holonomic/HolonomicOI.kt deleted file mode 100644 index 743a5a3..0000000 --- a/src/main/kotlin/frc/team449/subsystems/drive/holonomic/HolonomicOI.kt +++ /dev/null @@ -1,139 +0,0 @@ -package frc.team449.subsystems.drive.holonomic - -import edu.wpi.first.math.MathUtil -import edu.wpi.first.math.filter.SlewRateLimiter -import edu.wpi.first.math.geometry.Translation2d -import edu.wpi.first.math.kinematics.ChassisSpeeds -import edu.wpi.first.util.sendable.Sendable -import edu.wpi.first.util.sendable.SendableBuilder -import edu.wpi.first.wpilibj.Timer -import edu.wpi.first.wpilibj.XboxController -import edu.wpi.first.wpilibj2.command.Command -import frc.team449.subsystems.RobotConstants -import java.util.function.DoubleSupplier -import kotlin.math.abs -import kotlin.math.hypot - -/** - * Create an OI for controlling a holonomic drivetrain (probably swerve). - * The x and y axes on one joystick are used to control x and y velocity (m/s), - * while the x axis on another joystick is used to control rotational velocity (m/s). - *

The magnitude of the acceleration is clamped - *

Note that the joystick's X - * axis corresponds to the robot's/field's Y and vice versa - * - * @param drive The drivetrain this OI is controlling - * @param xThrottle The Y axis of the strafing joystick - * @param yThrottle The X axis of the strafing joystick - * @param rotThrottle The X axis of the rotating joystick - * @param rotRamp Used to ramp angular velocity - * @param maxAccel Max accel, used for ramping - * @param fieldOriented Whether the OI x and y translation should - * be relative to the field rather than relative to the robot. This better be true. - */ -class HolonomicOI( - private val drive: HolonomicDrive, - private val xThrottle: DoubleSupplier, - private val yThrottle: DoubleSupplier, - private val rotThrottle: DoubleSupplier, - private val rotRamp: SlewRateLimiter, - private val maxAccel: Double, - private val fieldOriented: () -> Boolean -) : Command(), Sendable { - - /** Previous X velocity (scaled and clamped). */ - private var prevX = 0.0 - - /** Previous Y velocity (scaled and clamped) */ - private var prevY = 0.0 - - private var prevTime = Double.NaN - - private var dx = 0.0 - private var dy = 0.0 - private var magAcc = 0.0 - private var dt = 0.0 - private var magAccClamped = 0.0 - - /** - * @return The [ChassisSpeeds] for the given x, y and - * rotation input from the joystick */ - override fun execute() { - val currTime = Timer.getFPGATimestamp() - if (this.prevTime.isNaN()) { - this.prevTime = currTime - RobotConstants.LOOP_TIME - } - this.dt = currTime - prevTime - this.prevTime = currTime - - val xScaled = xThrottle.asDouble * drive.maxLinearSpeed - val yScaled = yThrottle.asDouble * drive.maxLinearSpeed - - // Clamp the acceleration - this.dx = xScaled - this.prevX - this.dy = yScaled - this.prevY - this.magAcc = hypot(dx / dt, dy / dt) - this.magAccClamped = MathUtil.clamp(magAcc, -this.maxAccel, this.maxAccel) - - // Scale the change in x and y the same as the acceleration - val factor = if (magAcc == 0.0) 0.0 else magAccClamped / magAcc - val dxClamped = dx * factor - val dyClamped = dy * factor - val xClamped = prevX + dxClamped - val yClamped = prevY + dyClamped - - this.prevX = xClamped - this.prevY = yClamped - - val rotRaw = rotThrottle.asDouble - val rotScaled = rotRamp.calculate(rotRaw * drive.maxRotSpeed) - - // translation velocity vector - val vel = Translation2d(xClamped, yClamped) - - if (this.fieldOriented()) { - drive.set( - ChassisSpeeds.fromFieldRelativeSpeeds( - vel.x, - vel.y, - rotScaled, - drive.heading - ) - ) - } else { - drive.set( - ChassisSpeeds( - vel.x, - vel.y, - rotScaled - ) - ) - } - } - - override fun initSendable(builder: SendableBuilder) { - builder.addDoubleProperty("currX", this.xThrottle::getAsDouble, null) - builder.addDoubleProperty("currY", this.yThrottle::getAsDouble, null) - builder.addDoubleProperty("prevX", { this.prevX }, null) - builder.addDoubleProperty("prevY", { this.prevY }, null) - builder.addDoubleProperty("dx", { this.dx }, null) - builder.addDoubleProperty("dy", { this.dy }, null) - builder.addDoubleProperty("dt", { this.dt }, null) - builder.addDoubleProperty("magAcc", { this.magAcc }, null) - builder.addDoubleProperty("magAccClamped", { this.magAccClamped }, null) - } - - companion object { - fun createHolonomicOI(drive: HolonomicDrive, driveController: XboxController): HolonomicOI { - return HolonomicOI( - drive, - { if (abs(driveController.leftY) < RobotConstants.DRIVE_RADIUS_DEADBAND) .0 else -driveController.leftY }, - { if (abs(driveController.leftX) < RobotConstants.DRIVE_RADIUS_DEADBAND) .0 else -driveController.leftX }, - { if (abs(driveController.getRawAxis(4)) < RobotConstants.ROTATION_DEADBAND) .0 else -driveController.getRawAxis(4) }, - SlewRateLimiter(RobotConstants.ROT_RATE_LIMIT, RobotConstants.NEG_ROT_RATE_LIM, 0.0), - RobotConstants.MAX_ACCEL, - { true } - ) - } - } -} diff --git a/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveDrive.kt b/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveDrive.kt index 6b37249..64378f6 100644 --- a/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveDrive.kt +++ b/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveDrive.kt @@ -1,5 +1,10 @@ package frc.team449.subsystems.drive.swerve +import choreo.trajectory.SwerveSample +import edu.wpi.first.math.MathUtil +import edu.wpi.first.math.controller.PIDController +import edu.wpi.first.math.geometry.Pose2d +import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.math.geometry.Translation2d import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.math.kinematics.SwerveDriveKinematics @@ -12,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase import frc.team449.subsystems.RobotConstants import frc.team449.subsystems.drive.swerve.SwerveModuleKraken.Companion.createKrakenModule import frc.team449.subsystems.drive.swerve.SwerveModuleNEO.Companion.createNEOModule +import frc.team449.subsystems.vision.PoseSubsystem.* import kotlin.math.hypot /** @@ -30,6 +36,22 @@ open class SwerveDrive( protected val field: Field2d, val maxModuleSpeed: Double ) : SubsystemBase() { + private val xController: PIDController + get() = PIDController(10.0, 0.0, 0.0) + private val yController: PIDController + get() = PIDController(10.0, 0.0, 0.0) + private val headingController: PIDController + get() = PIDController(7.5, 0.0, 0.0) + + var heading: Rotation2d + get() = Rotation2d(MathUtil.angleModulus(this.pose.rotation.radians)) + set(value) { + this.pose = Pose2d(Translation2d(this.pose.x, this.pose.y), value) + } + + var pose: Pose2d = Pose2d() + + var desiredSpeeds: ChassisSpeeds = ChassisSpeeds() /** The kinematics that convert [ChassisSpeeds] into multiple [SwerveModuleState] objects. */ val kinematics = SwerveDriveKinematics( @@ -39,11 +61,9 @@ open class SwerveDrive( /** The current speed of the robot's drive. */ var currentSpeeds = ChassisSpeeds() - var desiredSpeeds: ChassisSpeeds = ChassisSpeeds() - protected var speedMagnitude: Double = 0.0 - fun set(desiredSpeeds: ChassisSpeeds) { + fun driveFieldRelative(desiredSpeeds: ChassisSpeeds) { this.desiredSpeeds = desiredSpeeds // Converts the desired [ChassisSpeeds] into an array of [SwerveModuleState]. @@ -65,16 +85,31 @@ open class SwerveDrive( module.update() } + fun followTrajectory(sample: SwerveSample) { + // Get the current pose of the robot + + // Generate the next speeds for the robot + val speeds = ChassisSpeeds( + sample.vx, + sample.vy, + sample.omega + ) + + val newspeeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, heading) + + // Apply the generated speeds + driveFieldRelative(newspeeds) + } + fun setVoltage(volts: Double) { modules.forEach { it.setVoltage(volts) } } - fun getModuleVel(): Double { - var totalVel = 0.0 - modules.forEach { totalVel += it.state.speedMetersPerSecond } - return totalVel / modules.size + /** Stops the robot's drive. */ + fun stop() { + this.driveFieldRelative(ChassisSpeeds(0.0, 0.0, 0.0)) } override fun periodic() { @@ -91,11 +126,6 @@ open class SwerveDrive( speedMagnitude = hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond) } - /** Stops the robot's drive. */ - fun stop() { - this.set(ChassisSpeeds(0.0, 0.0, 0.0)) - } - /** @return An array of [SwerveModulePosition] for each module, containing distance and angle. */ fun getPositions(): Array { return Array(modules.size) { i -> modules[i].position } @@ -155,6 +185,10 @@ open class SwerveDrive( ) } + fun set(fromFieldRelativeSpeeds: ChassisSpeeds?) { + TODO("Not yet implemented") + } + companion object { /** Create a [SwerveDrive] using [SwerveConstants]. */ fun createSwerveKraken(field: Field2d): SwerveDrive { diff --git a/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveOrthogonalCommand.kt b/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveOrthogonalCommand.kt index 1496cae..3f4a992 100644 --- a/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveOrthogonalCommand.kt +++ b/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveOrthogonalCommand.kt @@ -186,7 +186,7 @@ class SwerveOrthogonalCommand( rotScaled, poseEstimator.heading ) - drive.set( + drive.driveFieldRelative( desVel ) @@ -194,7 +194,7 @@ class SwerveOrthogonalCommand( desiredVel[1] = desVel.vyMetersPerSecond desiredVel[2] = desVel.omegaRadiansPerSecond } else { - drive.set( + drive.driveFieldRelative( ChassisSpeeds( vel.x, vel.y, diff --git a/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveSim.kt b/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveSim.kt index cd8f789..a5740c8 100644 --- a/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveSim.kt +++ b/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveSim.kt @@ -34,7 +34,7 @@ class SwerveSim( currHeading = currHeading.plus(Rotation2d(super.desiredSpeeds.omegaRadiansPerSecond * (currTime - lastTime))) this.lastTime = currTime - set(super.desiredSpeeds) + driveFieldRelative(super.desiredSpeeds) // Updates the robot's currentSpeeds. currentSpeeds = kinematics.toChassisSpeeds( diff --git a/src/main/kotlin/frc/team449/subsystems/superstructure/SuperstructureGoal.kt b/src/main/kotlin/frc/team449/subsystems/superstructure/SuperstructureGoal.kt index 365fdb1..75a9c9a 100644 --- a/src/main/kotlin/frc/team449/subsystems/superstructure/SuperstructureGoal.kt +++ b/src/main/kotlin/frc/team449/subsystems/superstructure/SuperstructureGoal.kt @@ -50,6 +50,33 @@ object SuperstructureGoal { Degrees.of(180.0), DriveDynamics(RobotConstants.MAX_LINEAR_SPEED, RobotConstants.MAX_ACCEL, RobotConstants.MAX_ROT_SPEED) ) + val L1_PREMOVE = SuperstructureState( + L1.pivot, + STOW.elevator, + STOW.wrist, + DriveDynamics(RobotConstants.MAX_LINEAR_SPEED, RobotConstants.MAX_ACCEL, RobotConstants.MAX_ROT_SPEED) + ) + + val L2_PREMOVE = SuperstructureState( + L2.pivot, + STOW.elevator, + STOW.wrist, + DriveDynamics(RobotConstants.MAX_LINEAR_SPEED, RobotConstants.MAX_ACCEL, RobotConstants.MAX_ROT_SPEED) + ) + + val L3_PREMOVE = SuperstructureState( + L3.pivot, + STOW.elevator, + STOW.wrist, + DriveDynamics(RobotConstants.MAX_LINEAR_SPEED, RobotConstants.MAX_ACCEL, RobotConstants.MAX_ROT_SPEED) + ) + + val L4_PREMOVE = SuperstructureState( + L4.pivot, + STOW.elevator, + STOW.wrist, + DriveDynamics(RobotConstants.MAX_LINEAR_SPEED, RobotConstants.MAX_ACCEL, RobotConstants.MAX_ROT_SPEED) + ) val L1_PREMOVE = SuperstructureState( L1.pivot, diff --git a/src/main/kotlin/frc/team449/subsystems/vision/PoseSubsystem.kt b/src/main/kotlin/frc/team449/subsystems/vision/PoseSubsystem.kt index 8fb58df..ead0844 100644 --- a/src/main/kotlin/frc/team449/subsystems/vision/PoseSubsystem.kt +++ b/src/main/kotlin/frc/team449/subsystems/vision/PoseSubsystem.kt @@ -101,6 +101,14 @@ class PoseSubsystem( } } + fun resetOdometry(newPose: Pose2d) { + this.poseEstimator.resetPose(newPose) + } + + fun getPose() { + getPose() + } + override fun periodic() { if (isReal) { this.poseEstimator.update( @@ -214,6 +222,8 @@ class PoseSubsystem( private fun setRobotPose() { this.field.robotPose = this.pose + drive.pose = this.pose + this.field.getObject("FL").pose = this.pose.plus( Transform2d( drive.modules[0].location, diff --git a/vendordeps/ChoreoLib2025.json b/vendordeps/ChoreoLib2025.json new file mode 100644 index 0000000..16de3a6 --- /dev/null +++ b/vendordeps/ChoreoLib2025.json @@ -0,0 +1,44 @@ +{ + "fileName": "ChoreoLib2025.json", + "name": "ChoreoLib", + "version": "2025.0.0", + "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", + "frcYear": "2025", + "mavenUrls": [ + "https://lib.choreo.autos/dep", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://lib.choreo.autos/dep/ChoreoLib2025.json", + "javaDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-java", + "version": "2025.0.0" + }, + { + "groupId": "com.google.code.gson", + "artifactId": "gson", + "version": "2.11.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-cpp", + "version": "2025.0.0", + "libName": "ChoreoLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +}