Test discretization in sim

It helps a tiny bit, but position control on paths fixes drift/skew anyway
(had to only do velocity feed forward to get drift/skew to show up)
Also learned how to use the auto node :)

Author: BenBean18

zebROS_ws/src/auto_node/paths/2025_4_Note.traj

@@ -1,21 +1,23 @@
-0.0,1.3475940227508545,5.588765621185303,3.1415926535897927,0.0,0.0,0.0,1
-0.04024229871318875,1.3621203259005277,5.572484480288233,3.137516245083241,-0.10129661170008848,0.24064733898034257,-0.2697185369239833,1
-0.0804845974263775,1.3863285970291863,5.545350632383985,3.1293639837951717,-0.2025794139247941,0.4812576668079537,-0.5394140976223522,1
-0.12072689613956623,1.420216958721915,5.507365497590794,3.117136616727843,-0.30384365353544857,0.721824555734495,-0.8090784734503345,1
-0.160969194852755,1.463783093898426,5.458531031544731,3.1008350261246895,-0.4050859698597918,0.9623385795414152,-1.0787000566349692,1
-0.20121149356594373,1.5170240028539397,5.398849996691044,3.080460155769061,-0.506304833663133,1.2027852875822915,-1.3482616120622144,1
-0.24145379227913247,1.5799355575159946,5.328326457294214,3.0560129309522104,-0.6075007044643156,1.4431411700785943,-1.6177358283442955,1
-0.28169609099232124,1.6525115907115813,5.24696678901991,3.0274941935109396,-0.7086756560623695,1.6833646100519275,-1.8870753102811255,1
-0.32193838970551,1.7347417145480797,5.154782098226876,2.9949046897173175,-0.8098320631906188,1.9233718133982327,-2.156185889312798,1
-0.3621806884186987,1.8266045487563909,5.0517957397774165,2.9582451586569336,-0.91097010442882,2.162952745559081,-2.424833263329828,1
-0.40242298713188746,1.9280323189046995,4.938082656168727,2.917516299014205,-1.0120907837246809,2.4012688146207157,-2.6920847145297584,1
-0.4426652858450762,2.024623521765578,4.829722428231165,2.8764958194475576,-1.0193373857599208,2.400583398709476,-2.6924914291065063,1
-0.48290758455826494,2.106842124621578,4.737501841160799,2.8395472668181436,-0.9181521387017788,2.162253882738708,-2.4252525980211503,1
-0.5231498832714537,2.1793950754621716,4.656120739336731,2.806682815679028,-0.8166643603910583,1.9226864580014735,-2.1566026418142292,1
-0.5633921819846425,2.2422826445081223,4.585579310371776,2.7779111762571125,-0.7149601375151765,1.682710873571503,-1.887479202513453,1
-0.6036344806978312,2.295500958248499,4.525882441857388,2.75323912225895,-0.6130875915878863,1.4425350661016465,-1.618116966126617,1
-0.64387677941102,2.339046975530622,4.477034089619105,2.732671997989585,-0.5110822424870104,1.2022421369199714,-1.348609597252295,1
-0.6841190781242087,2.3729184830864294,4.439037277982849,2.716213958210034,-0.408973649756458,0.9618735014679554,-1.079003748941,1
-0.7243613768373974,2.397113854378065,4.411894368690093,2.703868105555916,-0.3067879581608221,0.7214526848511779,-0.8093259487934356,1
-0.7646036755505862,2.41163186911327,4.395607262398508,2.6956365803095776,-0.20454908167257763,0.4809942587647568,-0.5395926778034363,1
-0.8048459742637749,2.4164715932383283,4.3901775330574635,2.6915206236546805,-0.10227936242294676,0.24050782235873255,-0.2698148419888686,1
+0.0,1.34759,5.58877,3.14159,0.0,0.0,0.0,1
+0.03674,1.35163,5.58424,3.14159,-0.0928,0.21968,-0.24621,1
+0.07348,1.36374,5.57067,3.13818,-0.1856,0.43933,-0.49239,1
+0.11022,1.38391,5.54806,3.13136,-0.27838,0.65895,-0.73856,1
+0.14696,1.41215,5.51641,3.12114,-0.37114,0.87851,-0.98468,1
+0.1837,1.44846,5.47571,3.1075,-0.46388,1.09802,-1.23076,1
+0.22044,1.49284,5.42597,3.09046,-0.55659,1.31746,-1.47677,1
+0.25718,1.54527,5.3672,3.07001,-0.64927,1.53679,-1.72268,1
+0.29392,1.60576,5.29939,3.04615,-0.74191,1.75599,-1.96845,1
+0.33066,1.67429,5.22256,3.0189,-0.8345,1.97494,-2.21397,1
+0.3674,1.75087,5.13672,2.98824,-0.92703,2.19343,-2.45898,1
+0.40414,1.83544,5.0419,2.95418,-1.0195,2.41054,-2.70246,1
+0.44088,1.92399,4.94261,2.91672,-1.02569,2.40983,-2.70292,1
+0.47762,2.00854,4.84777,2.87904,-0.93324,2.1927,-2.45945,1
+0.51436,2.08509,4.76191,2.84475,-0.84053,1.97422,-2.21444,1
+0.5511,2.1536,4.68507,2.81387,-0.74761,1.75528,-1.96892,1
+0.58784,2.21406,4.61724,2.7864,-0.65451,1.53613,-1.72313,1
+0.62458,2.26647,4.55845,2.76236,-0.56128,1.31684,-1.47718,1
+0.66132,2.31082,4.5087,2.74173,-0.46793,1.09747,-1.23113,1
+0.69806,2.34711,4.46799,2.72454,-0.37447,0.87804,-0.985,1
+0.7348,2.37534,4.43633,2.71078,-0.28093,0.65857,-0.73882,1
+0.77154,2.3955,4.4137,2.70046,-0.18733,0.43907,-0.49258,1
+0.80828,2.4076,4.40013,2.69358,-0.09368,0.21954,-0.24631,1

zebROS_ws/src/auto_node/paths/2025_4_Note0_blue.csv

@@ -1,21 +1,23 @@
-0.0,15.192405977249145,5.588765621185303,4.440892098500626e-16,-0.0,-0.0,0.0,1
-0.04024229871318875,15.177879674099472,5.572484480288233,0.004076408506552109,0.10129661170008848,-0.24064733898034257,-0.2697185369239833,1
-0.0804845974263775,15.153671402970813,5.545350632383985,0.01222866979462145,0.2025794139247941,-0.4812576668079537,-0.5394140976223522,1
-0.12072689613956623,15.119783041278085,5.507365497590794,0.024456036861950192,0.30384365353544857,-0.721824555734495,-0.8090784734503345,1
-0.160969194852755,15.076216906101573,5.458531031544731,0.040757627465103585,0.4050859698597918,-0.9623385795414152,-1.0787000566349692,1
-0.20121149356594373,15.022975997146059,5.398849996691044,0.061132497820731935,0.506304833663133,-1.2027852875822915,-1.3482616120622144,1
-0.24145379227913247,14.960064442484004,5.328326457294214,0.08557972263758273,0.6075007044643156,-1.4431411700785943,-1.6177358283442955,1
-0.28169609099232124,14.887488409288418,5.24696678901991,0.11409846007885349,0.7086756560623695,-1.6833646100519275,-1.8870753102811255,1
-0.32193838970551,14.80525828545192,5.154782098226876,0.14668796387247562,0.8098320631906188,-1.9233718133982327,-2.156185889312798,1
-0.3621806884186987,14.713395451243608,5.0517957397774165,0.1833474949328595,0.91097010442882,-2.162952745559081,-2.424833263329828,1
-0.40242298713188746,14.6119676810953,4.938082656168727,0.22407635457558817,1.0120907837246809,-2.4012688146207157,-2.6920847145297584,1
-0.4426652858450762,14.51537647823442,4.829722428231165,0.2650968341422355,1.0193373857599208,-2.400583398709476,-2.6924914291065063,1
-0.48290758455826494,14.433157875378422,4.737501841160799,0.3020453867716495,0.9181521387017788,-2.162253882738708,-2.4252525980211503,1
-0.5231498832714537,14.360604924537828,4.656120739336731,0.3349098379107649,0.8166643603910583,-1.9226864580014735,-2.1566026418142292,1
-0.5633921819846425,14.297717355491876,4.585579310371776,0.36368147733268064,0.7149601375151765,-1.682710873571503,-1.887479202513453,1
-0.6036344806978312,14.2444990417515,4.525882441857388,0.38835353133084327,0.6130875915878863,-1.4425350661016465,-1.618116966126617,1
-0.64387677941102,14.200953024469378,4.477034089619105,0.40892065560020807,0.5110822424870104,-1.2022421369199714,-1.348609597252295,1
-0.6841190781242087,14.16708151691357,4.439037277982849,0.4253786953797589,0.408973649756458,-0.9618735014679554,-1.079003748941,1
-0.7243613768373974,14.142886145621935,4.411894368690093,0.43772454803387717,0.3067879581608221,-0.7214526848511779,-0.8093259487934356,1
-0.7646036755505862,14.128368130886729,4.395607262398508,0.44595607328021547,0.20454908167257763,-0.4809942587647568,-0.5395926778034363,1
-0.8048459742637749,14.123528406761672,4.3901775330574635,0.45007202993511264,0.10227936242294676,-0.24050782235873255,-0.2698148419888686,1
+0.0,15.192409999999999,5.58877,2.6535897932333796e-06,-0.0,-0.0,0.0,1
+0.03674,15.188369999999999,5.58424,2.6535897932333796e-06,0.0928,-0.21968,-0.24621,1
+0.07348,15.17626,5.57067,0.0034126535897929244,0.1856,-0.43933,-0.49239,1
+0.11022,15.156089999999999,5.54806,0.010232653589793195,0.27838,-0.65895,-0.73856,1
+0.14696,15.127849999999999,5.51641,0.02045265358979309,0.37114,-0.87851,-0.98468,1
+0.1837,15.091539999999998,5.47571,0.03409265358979319,0.46388,-1.09802,-1.23076,1
+0.22044,15.04716,5.42597,0.05113265358979291,0.55659,-1.31746,-1.47677,1
+0.25718,14.994729999999999,5.3672,0.07158265358979321,0.64927,-1.53679,-1.72268,1
+0.29392,14.934239999999999,5.29939,0.0954426535897932,0.74191,-1.75599,-1.96845,1
+0.33066,14.86571,5.22256,0.1226926535897932,0.8345,-1.97494,-2.21397,1
+0.3674,14.78913,5.13672,0.15335265358979333,0.92703,-2.19343,-2.45898,1
+0.40414,14.704559999999999,5.0419,0.1874126535897931,1.0195,-2.41054,-2.70246,1
+0.44088,14.61601,4.94261,0.22487265358979291,1.02569,-2.40983,-2.70292,1
+0.47762,14.53146,4.84777,0.2625526535897933,0.93324,-2.1927,-2.45945,1
+0.51436,14.454909999999998,4.76191,0.2968426535897932,0.84053,-1.97422,-2.21444,1
+0.5511,14.386399999999998,4.68507,0.327722653589793,0.74761,-1.75528,-1.96892,1
+0.58784,14.32594,4.61724,0.35519265358979313,0.65451,-1.53613,-1.72313,1
+0.62458,14.27353,4.55845,0.37923265358979297,0.56128,-1.31684,-1.47718,1
+0.66132,14.22918,4.5087,0.3998626535897931,0.46793,-1.09747,-1.23113,1
+0.69806,14.192889999999998,4.46799,0.41705265358979293,0.37447,-0.87804,-0.985,1
+0.7348,14.16466,4.43633,0.4308126535897929,0.28093,-0.65857,-0.73882,1
+0.77154,14.144499999999999,4.4137,0.44113265358979303,0.18733,-0.43907,-0.49258,1
+0.80828,14.132399999999999,4.40013,0.44801265358979325,0.09368,-0.21954,-0.24631,1

zebROS_ws/src/auto_node/paths/2025_4_Note0_red.csv

@@ -1,37 +1,35 @@
-0.0,2.411632299423218,4.395606517791748,2.6915206236546743,0.0,0.0,0.0,2
-0.03324619866041034,2.4023270954041607,4.4072848073075415,2.692935154067154,0.04254713229535838,-0.18659183894712625,0.23417794086399177,2
-0.0664923973208208,2.386916231898057,4.42681441474642,2.696063143886176,0.09408563822513046,-0.37122235373420703,0.46967516196397,2
-0.09973859598123114,2.3654925423305664,4.454257636732856,2.701186532446313,0.1541044921624097,-0.5533375591616682,0.7068613410857268,2
-0.13298479464164137,2.338177706320519,4.489694925198005,2.7085866763097264,0.22258616511520804,-0.7321743575782187,0.946223761953224,2
-0.1662309933020517,2.30513648036578,4.533232414814746,2.7185592366056275,0.299960918814996,-0.9066145875229974,1.1884398535424257,2
-0.19947719196246216,2.266602365705525,4.5850144637090375,2.7314310168434233,0.387165473292175,-1.0749070302329613,1.4345018981638546,2
-0.23272339062287284,2.2229283233980173,4.645245557290312,2.7475836389801103,0.485848692168354,-1.2340724879146827,1.6859463539987638,2
-0.26596958928328307,2.1747007016309823,4.7142302166131875,2.7674946738208397,0.5988965850332568,-1.3784379184017863,1.945291330328932,2
-0.2992157879436935,2.1230467456029376,4.792441614448432,2.791829542737564,0.7319594389660657,-1.495266014777542,2.2167580715943354,2
-0.332461986604104,2.0707012673220633,4.880507688493236,2.821742138560954,0.8997298046433111,-1.5480754067189986,2.5048570702445687,2
-0.36570818526451443,2.0256435112956157,4.976674548138118,2.8586378353246973,1.109771891292374,-1.4195420857189591,2.763330407367688,2
-0.39895438392492466,1.993127368688955,5.069815387246817,2.8982536890686372,1.1915904777565152,-1.1252079728789806,2.7888091582667833,2
-0.43220058258533534,1.9697538206742655,5.157644561524588,2.9376563658352537,1.1851784070182405,-0.8437654751599463,2.690790352691309,2
-0.4654467812457458,1.9547954992803749,5.240032200341575,2.9756191635443097,1.141868822291494,-0.5812056689524476,2.549001474824365,2
-0.49869297990615624,1.94796306012166,5.316686870326293,3.0114935358661863,1.0790518545945855,-0.3307421477649113,2.38677862912681,2
-0.5319391785665667,1.9490613225282845,5.387372052704344,3.0448845179003707,1.004354885080604,-0.08822456592717412,2.213001687531107,2
-0.5651853772269771,1.9579463384403,5.4519120081273185,3.0755356275501846,0.9219432863156622,0.14875786281729125,2.031849381793251,2
-0.5984315758873876,1.9745080190905413,5.5101742720356945,3.1032802433880757,0.8345199438243036,0.3816876945735098,1.8455823866467025,2
-0.631677774547798,1.998659954246574,5.562055706520936,3.128021475263331,0.7441822786544319,0.6115339265817427,1.655542330594926,2
-0.6649239732082085,2.030333042144775,5.607472896575928,-3.1334549346578986,0.6529738175594688,0.8389666345706398,1.462571914999187,2
-0.6995988410741018,2.071691855146622,5.647602215013633,-3.113708736623575,0.5694671457927261,1.0748292166894824,1.2590591095056272,2
-0.734273708939995,2.121064147232268,5.680510638528707,-3.0971757612678075,0.47679995262581065,1.3075189958372384,1.05239098733796,2
-0.7689485768058882,2.1782806679099305,5.706024939544298,-3.084067592608194,0.3780308178910271,1.5358971266648729,0.8413405813000495,2
-0.8036234446717815,2.2430992280055717,5.7239051687225695,-3.074538929388032,0.2748002748559455,1.7581816588443486,0.6242160950160296,2
-0.8382983125376747,2.315148208038151,5.733806574377176,-3.068726994874275,0.167612304571673,1.9712901532690643,0.39843864326514267,2
-0.872973180403568,2.393795736130065,5.7352039106709345,-3.0667839138546857,0.056037157143396785,2.1691914601903024,0.15967837723894868,2
-0.9076480482694612,2.4777843260111427,5.7272400130777,-3.068919344741569,-0.06158439868391186,2.3378469958957053,-0.0998895280087525,2
-0.9423229161353545,2.56391911369009,5.708550677042603,-3.075426806854445,-0.18767085539145575,2.4353287805934154,-0.39262181101926935,2
-0.9769977840012475,2.6426806945105965,5.679950427116911,-3.0855506550819887,-0.2919650124350504,2.326063805577348,-0.6807486577273891,2
-1.011672651867141,2.707817424740947,5.651874379601165,-3.0960513564456393,-0.302833204854365,2.027687761084963,-0.766712508672808,2
-1.0463475197330339,2.762036909698944,5.627110530064436,-3.105411722143775,-0.2699466868734228,1.7183317111807628,-0.7316860758939688,2
-1.0810223875989275,2.8060098664589557,5.606105146327541,-3.1131722456716058,-0.2238083085949471,1.4182110140555222,-0.6477495048702906,2
-1.1156972554648208,2.839979546320837,5.589286379486456,-3.119215300450425,-0.1742776584514888,1.1258455710635784,-0.5392778269139594,2
-1.150372123330714,2.864103910202422,5.576983742607744,-3.123564583477376,-0.12543041385844142,0.839102204270713,-0.41629248899762944,2
-1.1850469911966068,2.87850040434493,5.569443019968318,-3.126316419122145,-0.07936109966980372,0.5564909102462919,-0.2837437616447357,2
-1.2197218590625,2.883260537001941,5.5668513121346335,-3.127609942808588,-0.03730435805642609,0.27701633170200374,-0.14441000100583004,2
+0.0,2.41163,4.39561,2.69014,0.0,0.0,0.0,2
+0.033340000000000036,2.40851,4.39951,2.69014,0.0481,-0.18745,0.23444,2
+0.06667000000000001,2.39917,4.41126,2.69174,0.10316,-0.3729,0.47026,2
+0.10001000000000004,2.38369,4.4309,2.69518,0.16526,-0.55582,0.70781,2
+0.13334999999999997,2.36216,4.45849,2.70069,0.23473,-0.73546,0.94757,2
+0.16668000000000005,2.33472,4.49412,2.70852,0.31223,-0.91071,1.19022,2
+0.20001999999999998,2.30155,4.53791,2.71892,0.39885,-1.07982,1.43675,2
+0.2333599999999999,2.26288,4.59001,2.73222,0.4963,-1.23982,1.68869,2
+0.26669,2.21913,4.65063,2.74877,0.6074,-1.38505,1.94859,2
+0.3000299999999999,2.17099,4.72013,2.76901,0.73744,-1.50273,2.22074,2
+0.33337000000000006,2.12001,4.79898,2.7936,0.89957,-1.55606,2.5099,2
+0.3666999999999999,2.07032,4.88697,2.82359,1.10345,-1.4252,2.7686,2
+0.40004000000000006,2.02773,4.97964,2.86037,1.18119,-1.12967,2.7911,2
+0.43338,1.99476,5.07102,2.89975,1.17433,-0.84801,2.69117,2
+0.4667199999999999,1.97087,5.15835,2.9389,1.13159,-0.58524,2.54799,2
+0.50005,1.95554,5.24057,2.97662,1.06955,-0.33452,2.38462,2
+0.5333899999999999,1.94844,5.31715,3.01228,0.99561,-0.09168,2.20982,2
+0.5667300000000001,1.94934,5.38778,3.04547,0.9139,0.14565,2.02774,2
+0.6000599999999999,1.95808,5.45226,3.07594,0.82711,0.37897,1.8406,2
+0.6334000000000001,1.97456,5.51044,3.10351,0.73735,0.60922,1.64972,2
+0.66674,1.99866,5.56221,3.12809,0.64665,0.83706,1.45593,2
+0.7000700000000001,2.03033,5.60747,-3.13354,0.55765,1.06297,1.25974,2
+0.73992,2.078,5.65292,-3.11132,0.45197,1.32989,1.02163,2
+0.7797599999999999,2.13619,5.68877,-3.09331,0.3395,1.59104,0.7776,2
+0.8196100000000001,2.20462,5.71472,-3.07978,0.22216,1.84345,0.52494,2
+0.8594499999999999,2.28281,5.73034,-3.07093,0.10021,2.08142,0.25905,2
+0.89929,2.36993,5.73492,-3.06694,-0.0277,2.29157,-0.02868,2
+0.93914,2.46404,5.72728,-3.06804,-0.16487,2.43242,-0.35491,2
+0.9789800000000001,2.55908,5.70633,-3.07461,-0.28601,2.33798,-0.69668,2
+1.01883,2.64532,5.67689,-3.08601,-0.29236,1.99092,-0.78141,2
+1.0586700000000002,2.7176,5.64691,-3.09766,-0.25101,1.6376,-0.72342,2
+1.09851,2.77606,5.62028,-3.10766,-0.19766,1.29664,-0.61314,2
+1.13836,2.82111,5.59855,-3.11553,-0.1426,0.96473,-0.4775,2
+1.1782,2.85306,5.58252,-3.12122,-0.09002,0.63905,-0.32707,2
+1.2180500000000003,2.87213,5.57268,-3.1248,-0.04209,0.31785,-0.16696,2