uploading recordings

2025-04-30 15:17:43 +02:00
parent 4ce1397d38
commit 3ed8fd5c5c
52 changed files with 118471 additions and 0 deletions
--- a/test/joint_trajectories_cart/trajectory_0001_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0001_cart.csv
@@ -0,0 +1,49 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018827.987727,0.98945062782806,0.3385907998268771,0.3547658488225996,0.3548421467685384,0.5400812646549652,0.9297790405611733
+1746018828.0438428,0.9790511074898933,0.3385982738312275,0.35476833660709517,0.35484387763999325,0.5400804356434588,0.9297799370759682
+1746018828.0980728,0.9686448732330093,0.3386023290932313,0.35477102085890877,0.35484392178328955,0.5400804470429994,0.9297799854891357
+1746018828.150571,0.9582374791097322,0.3386058647672078,0.35477337912832924,0.3548439836894107,0.5400804795215272,0.9297800451300114
+1746018828.2014976,0.9478290263272051,0.3386088843312398,0.3547753962783834,0.3548440618459587,0.5400805303671868,0.9297801153340102
+1746018828.250987,0.9374197007077508,0.33861147542544184,0.3547771309807368,0.3548441551233814,0.5400805979258549,0.9297801955713083
+1746018828.299157,0.9270096449290561,0.3386137045013258,0.35477862780822406,0.3548442627545597,0.5400806810211118,0.9297802855268571
+1746018828.3461087,0.9165989728101251,0.3386156245084926,0.3547799222645473,0.354844384233516,0.5400807788019361,0.9297803850388385
+1746018828.391931,0.9061877761961924,0.33861727808352854,0.35478104293707247,0.3548445192630661,0.5400808906752442,0.9297804940706165
+1746018828.4367027,0.8957761300305052,0.3386186999260437,0.35478201309934854,0.35484466771664,0.5400810162556035,0.9297806126903317
+1746018828.480493,0.885364096084589,0.3386199185463974,0.35478285188878494,0.35484482961065383,0.5400811553285416,0.9297807410563901
+1746018828.523364,0.874951725744389,0.33862095757169225,0.354783575185974,0.354845005084438,0.5400813078235711,0.9297808794072243
+1746018828.565371,0.8645390621193361,0.3386218367348407,0.35478419628019464,0.3548451943857001,0.5400814737943136,0.9297810280541893
+1746018828.6065636,0.8541261416585598,0.3386225726328759,0.3547847263794221,0.354845397860099,0.540081653403877,0.9297811873768238
+1746018828.6469865,0.8437129954033898,0.3386231793149332,0.3547851750056843,0.3548456159439455,0.5400818469142277,0.9297813578199472
+1746018828.6866806,0.8332996499680384,0.33862366874292565,0.35478555030483927,0.3548458491593261,0.5400820546786156,0.9297815398921883
+1746018828.7256827,0.8228861283147662,0.3386240511559741,0.35478585929172735,0.35484609811114,0.540082277136425,0.9297817341657004
+1746018828.7640262,0.8124724503719865,0.3386243353612869,0.35478610804601607,0.35484636348567705,0.5400825148099425,0.9297819412768413
+1746018828.801742,0.8020586335311176,0.33862452896828976,0.35478630187005794,0.354846646050476,0.5400827683027233,0.9297821619276997
+1746018828.838858,0.7916446930489613,0.3386246385785615,0.35478644541722654,0.35484694665526634,0.5400830382992801,0.9297823968883666
+1746018828.8753996,0.7812306423758255,0.3386246699410691,0.3547865427971164,0.35484726623384416,0.540083325565927,0.9297826469998716
+1746018828.9113908,0.7708164934247872,0.33862462807993604,0.3547865976624941,0.35484760580679175,0.5400836309526341,0.9297829131777642
+1746018828.9468534,0.7604022567939512,0.3386245174003163,0.3547866132817376,0.3548479664849486,0.5400839553958138,0.9297831964162833
+1746018828.981932,0.7499879419508725,0.33862434177669787,0.3547865925996949,0.3548483494736006,0.5400842999219613,0.9297834977931297
+1746018829.0168324,0.7395735573863236,0.3386241046270163,0.35478653828923035,0.3548487560773453,0.5400846656521302,0.92978381847482
+1746018829.0515716,0.7291591107430333,0.338623808975238,0.3547864527952661,0.35484918770559803,0.5400850538072088,0.9297841597226251
+1746018829.0861652,0.7187446089238667,0.3386234575045385,0.3547863383727542,0.35484964587875323,0.5400854657140032,0.9297845228991151
+1746018829.120628,0.7083300581829869,0.33862305260275155,0.3547861971197193,0.35485013223496914,0.5400859028121348,0.9297849094753228
+1746018829.1549747,0.6979154642028539,0.3386225964014569,0.3547860310063113,0.3548506485375911,0.5400863666617777,0.9297853210385344
+1746018829.1892185,0.6875008321593309,0.33862209080979605,0.3547858419006139,0.3548511966832113,0.5400868589522595,0.9297857593007371
+1746018829.2233727,0.6770861667767518,0.33862153754391094,0.35478563159183485,0.3548517787103644,0.5400873815115691,0.9297862261077503
+1746018829.2574494,0.6666714723744348,0.3386209381527294,0.3547854018113845,0.35485239680887254,0.5400879363168252,0.9297867234490398
+1746018829.2914605,0.6562567529058541,0.3386202940407016,0.35478515425227664,0.35485305332983186,0.5400885255057382,0.9297872534682682
+1746018829.3254178,0.6458420119914607,0.3386196064879625,0.3547848905872013,0.3548537507962404,0.5400891513891459,0.9297878184745573
+1746018829.359332,0.6354272529459345,0.3386188766683402,0.3547846124855788,0.35485449191425694,0.5400898164646668,0.92978842095451
+1746018829.393214,0.6250124788005298,0.3386181056655299,0.3547843216298531,0.3548552795850811,0.5400905234315458,0.9297890635849653
+1746018829.4270732,0.6145976923210283,0.3386172944877095,0.3547840197312423,0.3548561169174115,0.5400912752067503,0.9297897492464945
+1746018829.46092,0.6041828960217211,0.3386164440808249,0.35478370854514174,0.3548570072404519,0.5400920749423886,0.9297904810376029
+1746018829.4947634,0.5937680921757482,0.3386155553407147,0.35478338988635155,0.3548579541173864,0.5400929260445052,0.9297912622896114
+1746018829.5286126,0.5833532828220642,0.3386146291242265,0.35478306564426487,0.3548589613592577,0.5400938321933121,0.9297920965821272
+1746018829.562476,0.5729384697692176,0.3386136662594256,0.3547827377981535,0.3548600330391174,0.5400947973649016,0.9297929877590374
+1746018829.5963614,0.5625236545961011,0.33861266755498065,0.3547824084326578,0.35486117350630475,0.5400958258544716,0.9297939399448671
+1746018829.6302772,0.5521088386497741,0.3386116338087682,0.35478207975358095,0.35486238740066606,0.5400969223010706,0.9297949575613353
+1746018829.6642306,0.5416940230404215,0.33861056581572724,0.35478175410406476,0.3548636796664716,0.5400980917138504,0.9297960453438767
+1746018829.6982281,0.5312792086334933,0.33860946437494666,0.3547814339812177,0.3548650555657311,0.5400993394997665,0.9297972083578123
+1746018829.732277,0.5208643960390388,0.33860833029595816,0.3547811220532441,0.3548665206905392,0.5401006714926271,0.9297984520137942
+1746018829.7663827,0.510449585598229,0.338607164404173,0.35478082117710863,0.3548680809740007,0.5401020939833232,0.9297997820820324
+1746018829.8005512,0.5000347773670611,0.3386059675453585,0.3547805344167477,0.3548697426991863,0.5401036137509987,0.9298012047046857
--- a/test/joint_trajectories_cart/trajectory_0002_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0002_cart.csv
@@ -0,0 +1,50 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018829.9629352,0.5102308375666442,0.33860537194136614,0.35477988772721825,0.35486990276464075,0.5401023480303815,0.9297999290938809
+1746018829.9963524,0.5204328851034223,0.3386115837486437,0.3547815871128587,0.3548656967057072,0.5400998130540181,0.9297969484277177
+1746018830.029709,0.5306352568720152,0.3386125547994577,0.35478185566022347,0.35486429081603743,0.5400984591601475,0.929795755583826
+1746018830.0630164,0.5408376413778965,0.33861353594040705,0.35478211203886995,0.3548629219579096,0.5400971778041765,0.9297946120687574
+1746018830.0962808,0.5510400180632159,0.33861448801991634,0.3547823756375527,0.35486163330249554,0.5400959737673785,0.9297935412447313
+1746018830.1295083,0.561242386946426,0.338615410187246,0.3547826434393109,0.3548604196295559,0.5400948419363,0.9297925381920231
+1746018830.162706,0.5714447475169135,0.3386163016911055,0.35478291322351,0.35485927629083747,0.5400937773615129,0.9297915983966191
+1746018830.1958811,0.5816470990133458,0.3386171617678129,0.35478318288543653,0.35485819891060466,0.5400927754491894,0.9297907176327769
+1746018830.2290413,0.5918494404299254,0.3386179896408494,0.35478345043028775,0.3548571833706688,0.5400918319332397,0.9297898919463334
+1746018830.2621944,0.6020517705213024,0.3386187845147894,0.35478371395961616,0.35485622579919324,0.5400909428521983,0.9297891176408328
+1746018830.2953491,0.6122540878048776,0.33861954556848267,0.35478397165804665,0.3548553225590352,0.5400901045273436,0.9297883912635053
+1746018830.3285139,0.6224563905604233,0.3386202719474199,0.35478422178013236,0.3548544702359173,0.5400893135420816,0.9297877095913418
+1746018830.3616977,0.6326586768269248,0.33862096275526304,0.35478446263728514,0.3548536656266058,0.5400885667225659,0.9297870696174111
+1746018830.39491,0.6428609443964908,0.33862161704447313,0.35478469258467193,0.3548529057272062,0.5400878611195548,0.9297864685375535
+1746018830.4281607,0.6530631908051621,0.33862223380595297,0.3547849100079784,0.35485218772169463,0.5400871939914349,0.9297859037375167
+1746018830.4614599,0.6632654133203727,0.3386228119575897,0.3547851133099109,0.35485150897074424,0.5400865627883917,0.9297853727806085
+1746018830.4948182,0.6734676089247714,0.3386233503315536,0.3547853008962955,0.35485086700091045,0.5400859651376393,0.9297848733958944
+1746018830.5282469,0.6836697742960235,0.338623847660165,0.3547854711616042,0.3548502594942173,0.5400853988296749,0.9297844034669644
+1746018830.561757,0.6938719057821414,0.33862430256010556,0.3547856224737223,0.35484968427814606,0.5400848618054711,0.9297839610212794
+1746018830.595361,0.7040739993717672,0.33862471351469964,0.35478575315772803,0.35484913931606493,0.540084352144552,0.9297835442200874
+1746018830.6290712,0.7142760506587136,0.3386250788539257,0.3547858614784251,0.3548486226980787,0.540083868053884,0.929783151348902
+1746018830.662901,0.7244780547999153,0.3386253967317582,0.3547859456213176,0.35484813263229864,0.5400834078575071,0.9297827808085288
+1746018830.6968637,0.7346800064657361,0.33862566510034064,0.35478600367166335,0.35484766743652335,0.5400829699868459,0.9297824311066042
+1746018830.730974,0.744881899781361,0.338625881680388,0.35478603359115696,0.35484722553030124,0.5400825529716318,0.9297821008496331
+1746018830.765247,0.7550837282576983,0.33862604392709517,0.3547860331917262,0.35484680542734776,0.5400821554313623,0.9297817887354863
+1746018830.7997239,0.7652854847098542,0.33862614899064114,0.3547860001057848,0.3548464057282994,0.5400817760672394,0.9297814935463206
+1746018830.8346977,0.7754871611607881,0.33862619367019925,0.3547859317521677,0.3548460251137502,0.5400814136544997,0.9297812141418956
+1746018830.8701878,0.7856887487271866,0.33862617436007514,0.3547858252967799,0.35484566233753684,0.5400810670350813,0.9297809494532385
+1746018830.906216,0.7958902374838415,0.3386260869862885,0.3547856776067546,0.354845316220228,0.5400807351105352,0.9297806984766164
+1746018830.9428046,0.8060916163019007,0.33862592693146554,0.3547854851966493,0.35484498564274897,0.5400804168350929,0.9297804602677694
+1746018830.979979,0.8162928726551361,0.3386256889453817,0.3547852441647786,0.3548446695400894,0.540080111208812,0.9297802339363596
+1746018831.0177665,0.8264939923867874,0.3386253670377588,0.3547849501173374,0.35484436689499904,0.5400798172706679,0.9297800186405696
+1746018831.0561965,0.8366949594275045,0.3386249543490016,0.354784598077269,0.35484407673159696,0.540079534091475,0.9297798135817845
+1746018831.0953019,0.8468957554521337,0.3386244429933126,0.35478418237399745,0.35484379810875677,0.5400792607664834,0.9297796179992969
+1746018831.1351182,0.8570963594594918,0.3386238238669661,0.35478369650897196,0.3548435301131466,0.540078996407449,0.9297794311649141
+1746018831.175685,0.8672967472543252,0.3386230864123113,0.354783132990414,0.35484327185171877,0.5400787401339501,0.9297792523773852
+1746018831.2170453,0.8774968908039783,0.3386222183250202,0.35478248312854505,0.3548430224434288,0.5400784910636309,0.9297790809564763
+1746018831.2592473,0.8876967574331005,0.3386212051879557,0.3547817367796329,0.35484278100987465,0.5400782483009794,0.9297789162365343
+1746018831.3023446,0.8978963088068814,0.3386200300091943,0.3547808820231383,0.354842546664443,0.5400780109240952,0.9297787575592836
+1746018831.3463962,0.9080954996352522,0.3386186726335796,0.35477990475047566,0.3548423184994226,0.5400777779687443,0.9297786042655434
+1746018831.3914697,0.9182942760046386,0.3386171089854539,0.3547787881357081,0.3548420955703074,0.5400775484086952,0.9297784556854235
+1746018831.4376402,0.9284925732063453,0.3386153100832404,0.3547775119465214,0.35484187687627416,0.5400773211309818,0.9297783111263974
+1746018831.484993,0.9386903128753308,0.33861324074150284,0.3547760516362452,0.35484166133530687,0.5400770949041368,0.9297781698584056
+1746018831.5336254,0.9488873991701418,0.33861085783851486,0.3547743771312058,0.3548414477518555,0.5400768683365978,0.9297780310947452
+1746018831.5836492,0.9590837135976907,0.3386081079698914,0.3547724511872236,0.35484123477385576,0.5400766398211737,0.9297778939669725
+1746018831.635194,0.9692791078880415,0.33860492421897315,0.3547702271257263,0.35484102083439917,0.5400764074594073,0.929777757491109
+1746018831.6884105,0.9794733940066245,0.3386012216309549,0.354767645658593,0.3548408040708161,0.5400761689563759,0.9297776205210437
+1746018831.7434766,0.9896663298695357,0.33859689074179633,0.35476463034426053,0.35484058220979026,0.5400759214711092,0.9297774816826611
+1746018831.800604,0.9998575984452447,0.33859178811412316,0.35476108093583825,0.35484035240015555,0.5400756613986569,0.9297773392782596
--- a/test/joint_trajectories_cart/trajectory_0003_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0003_cart.csv
@@ -0,0 +1,49 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018831.9868114,0.9894510705865257,0.33859111094126054,0.35476610526485686,0.35484239062022366,0.5400812230399126,0.9297791634638071
+1746018832.0429308,0.9790510923743092,0.3385982632609757,0.3547683300166405,0.3548438777872087,0.5400804352570674,0.929779937132873
+1746018832.0971608,0.9686448736754288,0.3386023294126957,0.35477102107492137,0.3548439217773581,0.5400804470354574,0.9297799854850345
+1746018832.1496592,0.9582374790977437,0.33860586475829935,0.3547733791224178,0.3548439836894648,0.540080479521734,0.9297800451300654
+1746018832.2005854,0.9478290263275049,0.3386088843314691,0.3547753962785372,0.354844061845958,0.5400805303671822,0.9297801153340085
+1746018832.250075,0.9374197007077442,0.33861147542543657,0.3547771309807332,0.354844155123382,0.5400805979258545,0.9297801955713082
+1746018832.298245,0.9270096449290561,0.33861370450132566,0.3547786278082245,0.3548442627545595,0.5400806810211117,0.9297802855268567
+1746018832.3451965,0.9165989728101249,0.3386156245084924,0.3547799222645471,0.35484438423351605,0.5400807788019362,0.9297803850388383
+1746018832.391019,0.9061877761961924,0.3386172780835289,0.35478104293707247,0.3548445192630661,0.5400808906752443,0.9297804940706169
+1746018832.4357908,0.8957761300305053,0.3386186999260437,0.35478201309934815,0.35484466771664025,0.540081016255604,0.9297806126903315
+1746018832.479581,0.885364096084589,0.3386199185463975,0.354782851888785,0.3548448296106538,0.5400811553285415,0.9297807410563901
+1746018832.522452,0.874951725744389,0.33862095757169225,0.35478357518597403,0.3548450050844381,0.5400813078235712,0.9297808794072245
+1746018832.5644588,0.8645390621193363,0.3386218367348407,0.35478419628019475,0.35484519438570006,0.5400814737943135,0.9297810280541892
+1746018832.6056514,0.8541261416585597,0.33862257263287604,0.35478472637942204,0.3548453978600991,0.5400816534038773,0.9297811873768241
+1746018832.6460745,0.8437129954033898,0.33862317931493324,0.35478517500568424,0.35484561594394526,0.5400818469142276,0.929781357819947
+1746018832.6857686,0.8332996499680385,0.3386236687429251,0.35478555030483927,0.3548458491593254,0.5400820546786163,0.929781539892188
+1746018832.7247705,0.822886128314766,0.3386240511559741,0.3547858592917275,0.3548460981111399,0.5400822771364249,0.9297817341657006
+1746018832.7631142,0.8124724503719867,0.338624335361287,0.35478610804601624,0.3548463634856769,0.5400825148099422,0.9297819412768412
+1746018832.80083,0.8020586335311176,0.33862452896828965,0.3547863018700582,0.3548466460504759,0.5400827683027231,0.9297821619276995
+1746018832.8379457,0.7916446930489611,0.3386246385785615,0.35478644541722626,0.3548469466552664,0.5400830382992803,0.9297823968883668
+1746018832.8744876,0.7812306423758256,0.33862466994106916,0.3547865427971168,0.3548472662338443,0.540083325565927,0.929782646999872
+1746018832.9104788,0.7708164934247871,0.33862462807993604,0.354786597662494,0.35484760580679164,0.5400836309526343,0.9297829131777638
+1746018832.9459414,0.7604022567939512,0.3386245174003163,0.3547866132817376,0.3548479664849486,0.5400839553958138,0.9297831964162833
+1746018832.9810197,0.7499879419508725,0.33862434177669787,0.3547865925996949,0.3548483494736006,0.5400842999219613,0.9297834977931297
+1746018833.0159202,0.7395735573863236,0.3386241046270163,0.35478653828923035,0.3548487560773453,0.5400846656521302,0.92978381847482
+1746018833.0506594,0.7291591107430333,0.338623808975238,0.3547864527952661,0.35484918770559803,0.5400850538072088,0.9297841597226251
+1746018833.085253,0.7187446089238667,0.3386234575045385,0.3547863383727542,0.35484964587875323,0.5400854657140032,0.9297845228991151
+1746018833.1197162,0.7083300581829869,0.33862305260275155,0.3547861971197193,0.35485013223496914,0.5400859028121348,0.9297849094753228
+1746018833.1540627,0.6979154642028539,0.3386225964014569,0.3547860310063113,0.3548506485375911,0.5400863666617777,0.9297853210385344
+1746018833.1883066,0.6875008321593309,0.33862209080979605,0.3547858419006139,0.3548511966832113,0.5400868589522595,0.9297857593007371
+1746018833.2224605,0.6770861667767518,0.33862153754391094,0.35478563159183485,0.3548517787103644,0.5400873815115691,0.9297862261077503
+1746018833.2565372,0.6666714723744348,0.3386209381527294,0.3547854018113845,0.35485239680887254,0.5400879363168252,0.9297867234490398
+1746018833.2905486,0.6562567529058541,0.3386202940407016,0.35478515425227664,0.35485305332983186,0.5400885255057382,0.9297872534682682
+1746018833.3245058,0.6458420119914607,0.3386196064879625,0.3547848905872013,0.3548537507962404,0.5400891513891459,0.9297878184745573
+1746018833.3584201,0.6354272529459345,0.3386188766683402,0.3547846124855788,0.35485449191425694,0.5400898164646668,0.92978842095451
+1746018833.3923018,0.6250124788005298,0.3386181056655299,0.3547843216298531,0.3548552795850811,0.5400905234315458,0.9297890635849653
+1746018833.4261613,0.6145976923210283,0.3386172944877095,0.3547840197312423,0.3548561169174115,0.5400912752067503,0.9297897492464945
+1746018833.460008,0.6041828960217211,0.3386164440808249,0.35478370854514174,0.3548570072404519,0.5400920749423886,0.9297904810376029
+1746018833.4938514,0.5937680921757482,0.3386155553407147,0.35478338988635155,0.3548579541173864,0.5400929260445052,0.9297912622896114
+1746018833.5277004,0.5833532828220642,0.3386146291242265,0.35478306564426487,0.3548589613592577,0.5400938321933121,0.9297920965821272
+1746018833.5615637,0.5729384697692176,0.3386136662594256,0.3547827377981535,0.3548600330391174,0.5400947973649016,0.9297929877590374
+1746018833.5954494,0.5625236545961011,0.33861266755498065,0.3547824084326578,0.35486117350630475,0.5400958258544716,0.9297939399448671
+1746018833.6293652,0.5521088386497741,0.3386116338087682,0.35478207975358095,0.35486238740066606,0.5400969223010706,0.9297949575613353
+1746018833.6633184,0.5416940230404215,0.33861056581572724,0.35478175410406476,0.3548636796664716,0.5400980917138504,0.9297960453438767
+1746018833.6973162,0.5312792086334933,0.33860946437494666,0.3547814339812177,0.3548650555657311,0.5400993394997665,0.9297972083578123
+1746018833.731365,0.5208643960390388,0.33860833029595816,0.3547811220532441,0.3548665206905392,0.5401006714926271,0.9297984520137942
+1746018833.7654707,0.510449585598229,0.338607164404173,0.35478082117710863,0.3548680809740007,0.5401020939833232,0.9297997820820324
+1746018833.7996392,0.5000347773670611,0.3386059675453585,0.3547805344167477,0.3548697426991863,0.5401036137509987,0.9298012047046857
--- a/test/joint_trajectories_cart/trajectory_0004_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0004_cart.csv
@@ -0,0 +1,50 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018833.9629507,0.5102307535480057,0.33860553079152406,0.35477975608674506,0.3548703179616748,0.5401022341663749,0.9298001210477349
+1746018833.9963675,0.5204328843983944,0.3386115813910952,0.354781588792492,0.35486569898265385,0.5400998168763496,0.9297969506009277
+1746018834.0297241,0.5306352568887265,0.3386125548044641,0.3547818556267326,0.3548642907525997,0.5400984591649294,0.9297957555507093
+1746018834.0630317,0.5408376413777933,0.33861353594059973,0.3547821120393196,0.35486292195787833,0.5400971778037434,0.9297946120686386
+1746018834.0962958,0.551040018063216,0.3386144880199135,0.3547823756375488,0.35486163330250264,0.5400959737673802,0.9297935412447351
+1746018834.1295235,0.561242386946426,0.33861541018724595,0.354782643439311,0.3548604196295558,0.5400948419363,0.9297925381920227
+1746018834.1627212,0.5714447475169135,0.33861630169110535,0.35478291322350985,0.3548592762908375,0.5400937773615128,0.929791598396619
+1746018834.1958961,0.5816470990133457,0.33861716176781287,0.3547831828854367,0.3548581989106046,0.5400927754491893,0.9297907176327768
+1746018834.2290564,0.5918494404299255,0.3386179896408494,0.35478345043028764,0.35485718337066896,0.54009183193324,0.9297898919463335
+1746018834.2622097,0.6020517705213024,0.3386187845147895,0.3547837139596164,0.3548562257991931,0.540090942852198,0.9297891176408328
+1746018834.2953644,0.6122540878048774,0.3386195455684826,0.3547839716580464,0.3548553225590352,0.5400901045273439,0.9297883912635054
+1746018834.328529,0.6224563905604233,0.3386202719474199,0.3547842217801323,0.3548544702359173,0.5400893135420816,0.9297877095913417
+1746018834.3617127,0.6326586768269249,0.3386209627552631,0.354784462637285,0.3548536656266058,0.5400885667225659,0.9297870696174111
+1746018834.394925,0.6428609443964907,0.338621617044473,0.35478469258467216,0.35485290572720624,0.5400878611195548,0.9297864685375534
+1746018834.428176,0.6530631908051621,0.338622233805953,0.3547849100079783,0.3548521877216947,0.540087193991435,0.9297859037375168
+1746018834.4614751,0.6632654133203728,0.3386228119575897,0.354785113309911,0.35485150897074424,0.5400865627883916,0.9297853727806089
+1746018834.4948335,0.6734676089247712,0.33862335033155355,0.3547853008962955,0.3548508670009105,0.5400859651376394,0.9297848733958944
+1746018834.528262,0.6836697742960236,0.3386238476601651,0.3547854711616042,0.3548502594942174,0.540085398829675,0.9297844034669644
+1746018834.5617723,0.6938719057821414,0.33862430256010545,0.3547856224737222,0.35484968427814606,0.5400848618054711,0.9297839610212792
+1746018834.5953763,0.7040739993717672,0.33862471351469964,0.35478575315772803,0.3548491393160649,0.540084352144552,0.9297835442200871
+1746018834.6290865,0.7142760506587137,0.33862507885392557,0.35478586147842484,0.35484862269807815,0.5400838680538848,0.9297831513489022
+1746018834.662916,0.7244780547999153,0.3386253967317582,0.3547859456213176,0.3548481326322985,0.5400834078575071,0.9297827808085285
+1746018834.696879,0.7346800064657361,0.33862566510034064,0.3547860036716633,0.3548476674365234,0.540082969986846,0.9297824311066042
+1746018834.7309892,0.7448818997813609,0.33862588168038793,0.354786033591157,0.35484722553030146,0.5400825529716319,0.9297821008496333
+1746018834.7652624,0.7550837282576984,0.3386260439270948,0.3547860331917262,0.35484680542734776,0.5400821554313623,0.929781788735486
+1746018834.7997391,0.7652854847098542,0.33862614899064114,0.3547860001057848,0.3548464057282994,0.5400817760672394,0.9297814935463206
+1746018834.8347127,0.7754871611607881,0.33862619367019925,0.3547859317521677,0.3548460251137502,0.5400814136544997,0.9297812141418956
+1746018834.870203,0.7856887487271866,0.33862617436007514,0.3547858252967799,0.35484566233753684,0.5400810670350813,0.9297809494532385
+1746018834.906231,0.7958902374838415,0.3386260869862885,0.3547856776067546,0.354845316220228,0.5400807351105352,0.9297806984766164
+1746018834.9428196,0.8060916163019007,0.33862592693146554,0.3547854851966493,0.35484498564274897,0.5400804168350929,0.9297804602677694
+1746018834.979994,0.8162928726551361,0.3386256889453817,0.3547852441647786,0.3548446695400894,0.540080111208812,0.9297802339363596
+1746018835.0177815,0.8264939923867874,0.3386253670377588,0.3547849501173374,0.35484436689499904,0.5400798172706679,0.9297800186405696
+1746018835.0562117,0.8366949594275045,0.3386249543490016,0.354784598077269,0.35484407673159696,0.540079534091475,0.9297798135817845
+1746018835.0953171,0.8468957554521337,0.3386244429933126,0.35478418237399745,0.35484379810875677,0.5400792607664834,0.9297796179992969
+1746018835.1351335,0.8570963594594918,0.3386238238669661,0.35478369650897196,0.3548435301131466,0.540078996407449,0.9297794311649141
+1746018835.1757002,0.8672967472543252,0.3386230864123113,0.354783132990414,0.35484327185171877,0.5400787401339501,0.9297792523773852
+1746018835.2170606,0.8774968908039783,0.3386222183250202,0.35478248312854505,0.3548430224434288,0.5400784910636309,0.9297790809564763
+1746018835.2592626,0.8876967574331005,0.3386212051879557,0.3547817367796329,0.35484278100987465,0.5400782483009794,0.9297789162365343
+1746018835.3023596,0.8978963088068814,0.3386200300091943,0.3547808820231383,0.354842546664443,0.5400780109240952,0.9297787575592836
+1746018835.3464115,0.9080954996352522,0.3386186726335796,0.35477990475047566,0.3548423184994226,0.5400777779687443,0.9297786042655434
+1746018835.391485,0.9182942760046386,0.3386171089854539,0.3547787881357081,0.3548420955703074,0.5400775484086952,0.9297784556854235
+1746018835.4376554,0.9284925732063453,0.3386153100832404,0.3547775119465214,0.35484187687627416,0.5400773211309818,0.9297783111263974
+1746018835.485008,0.9386903128753308,0.33861324074150284,0.3547760516362452,0.35484166133530687,0.5400770949041368,0.9297781698584056
+1746018835.5336404,0.9488873991701418,0.33861085783851486,0.3547743771312058,0.3548414477518555,0.5400768683365978,0.9297780310947452
+1746018835.5836644,0.9590837135976907,0.3386081079698914,0.3547724511872236,0.35484123477385576,0.5400766398211737,0.9297778939669725
+1746018835.635209,0.9692791078880415,0.33860492421897315,0.3547702271257263,0.35484102083439917,0.5400764074594073,0.929777757491109
+1746018835.6884258,0.9794733940066245,0.3386012216309549,0.354767645658593,0.3548408040708161,0.5400761689563759,0.9297776205210437
+1746018835.743492,0.9896663298695357,0.33859689074179633,0.35476463034426053,0.35484058220979026,0.5400759214711092,0.9297774816826611
+1746018835.8006194,0.9998575984452447,0.33859178811412316,0.35476108093583825,0.35484035240015555,0.5400756613986569,0.9297773392782596
--- a/test/joint_trajectories_cart/trajectory_0005_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0005_cart.csv
@@ -0,0 +1,49 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018835.987803,0.9894514649621959,0.33859106643705594,0.35476579441382267,0.35484249959314046,0.5400810193952152,0.9297792152802237
+1746018836.043924,0.9790510872924697,0.33859825627096457,0.3547683235695871,0.35484387737989737,0.5400804356282864,0.9297799369587324
+1746018836.0981538,0.9686448738836344,0.3386023295304469,0.354771021142283,0.35484392178153074,0.5400804470379574,0.9297799854875691
+1746018836.1506522,0.9582374790925539,0.33860586475413884,0.3547733791195893,0.35484398368956804,0.5400804795218502,0.9297800451301365
+1746018836.2015784,0.947829026327638,0.33860888433156894,0.35477539627860416,0.3548440618459579,0.5400805303671801,0.9297801153340087
+1746018836.251068,0.937419700707741,0.33861147542543424,0.3547771309807317,0.3548441551233816,0.5400805979258546,0.9297801955713083
+1746018836.299238,0.9270096449290562,0.3386137045013258,0.3547786278082243,0.3548442627545595,0.5400806810211118,0.9297802855268568
+1746018836.3461897,0.9165989728101251,0.3386156245084926,0.3547799222645473,0.35484438423351583,0.5400807788019361,0.9297803850388383
+1746018836.3920121,0.9061877761961923,0.33861727808352854,0.35478104293707247,0.35484451926306626,0.5400808906752445,0.9297804940706167
+1746018836.4367838,0.8957761300305054,0.3386186999260437,0.35478201309934804,0.3548446677166402,0.5400810162556038,0.9297806126903316
+1746018836.4805741,0.8853640960845888,0.3386199185463974,0.35478285188878533,0.3548448296106537,0.5400811553285414,0.92978074105639
+1746018836.5234451,0.8749517257443892,0.33862095757169225,0.354783575185974,0.35484500508443817,0.5400813078235712,0.9297808794072246
+1746018836.565452,0.8645390621193361,0.33862183673484075,0.35478419628019453,0.3548451943857008,0.5400814737943135,0.929781028054189
+1746018836.6066446,0.8541261416585597,0.33862257263287604,0.35478472637942204,0.35484539786009894,0.5400816534038773,0.9297811873768239
+1746018836.6470675,0.8437129954033898,0.33862317931493324,0.35478517500568424,0.35484561594394537,0.5400818469142276,0.9297813578199473
+1746018836.6867616,0.8332996499680385,0.3386236687429251,0.35478555030483927,0.3548458491593254,0.5400820546786163,0.929781539892188
+1746018836.7257638,0.822886128314766,0.3386240511559741,0.3547858592917275,0.3548460981111399,0.5400822771364249,0.9297817341657006
+1746018836.7641072,0.8124724503719867,0.338624335361287,0.35478610804601624,0.3548463634856769,0.5400825148099422,0.9297819412768412
+1746018836.801823,0.8020586335311176,0.33862452896828965,0.3547863018700582,0.3548466460504759,0.5400827683027231,0.9297821619276995
+1746018836.838939,0.7916446930489611,0.3386246385785615,0.35478644541722626,0.3548469466552664,0.5400830382992803,0.9297823968883668
+1746018836.8754807,0.7812306423758256,0.33862466994106916,0.3547865427971168,0.3548472662338443,0.540083325565927,0.929782646999872
+1746018836.9114718,0.7708164934247871,0.33862462807993604,0.354786597662494,0.35484760580679164,0.5400836309526343,0.9297829131777638
+1746018836.9469345,0.7604022567939512,0.3386245174003163,0.3547866132817376,0.3548479664849486,0.5400839553958138,0.9297831964162833
+1746018836.982013,0.7499879419508725,0.33862434177669787,0.3547865925996949,0.3548483494736006,0.5400842999219613,0.9297834977931297
+1746018837.0169134,0.7395735573863236,0.3386241046270163,0.35478653828923035,0.3548487560773453,0.5400846656521302,0.92978381847482
+1746018837.0516527,0.7291591107430333,0.338623808975238,0.3547864527952661,0.35484918770559803,0.5400850538072088,0.9297841597226251
+1746018837.0862463,0.7187446089238667,0.3386234575045385,0.3547863383727542,0.35484964587875323,0.5400854657140032,0.9297845228991151
+1746018837.1207092,0.7083300581829869,0.33862305260275155,0.3547861971197193,0.35485013223496914,0.5400859028121348,0.9297849094753228
+1746018837.1550558,0.6979154642028539,0.3386225964014569,0.3547860310063113,0.3548506485375911,0.5400863666617777,0.9297853210385344
+1746018837.1892996,0.6875008321593309,0.33862209080979605,0.3547858419006139,0.3548511966832113,0.5400868589522595,0.9297857593007371
+1746018837.2234538,0.6770861667767518,0.33862153754391094,0.35478563159183485,0.3548517787103644,0.5400873815115691,0.9297862261077503
+1746018837.2575305,0.6666714723744348,0.3386209381527294,0.3547854018113845,0.35485239680887254,0.5400879363168252,0.9297867234490398
+1746018837.2915416,0.6562567529058541,0.3386202940407016,0.35478515425227664,0.35485305332983186,0.5400885255057382,0.9297872534682682
+1746018837.3254988,0.6458420119914607,0.3386196064879625,0.3547848905872013,0.3548537507962404,0.5400891513891459,0.9297878184745573
+1746018837.3594131,0.6354272529459345,0.3386188766683402,0.3547846124855788,0.35485449191425694,0.5400898164646668,0.92978842095451
+1746018837.3932948,0.6250124788005298,0.3386181056655299,0.3547843216298531,0.3548552795850811,0.5400905234315458,0.9297890635849653
+1746018837.4271543,0.6145976923210283,0.3386172944877095,0.3547840197312423,0.3548561169174115,0.5400912752067503,0.9297897492464945
+1746018837.4610012,0.6041828960217211,0.3386164440808249,0.35478370854514174,0.3548570072404519,0.5400920749423886,0.9297904810376029
+1746018837.4948444,0.5937680921757482,0.3386155553407147,0.35478338988635155,0.3548579541173864,0.5400929260445052,0.9297912622896114
+1746018837.5286937,0.5833532828220642,0.3386146291242265,0.35478306564426487,0.3548589613592577,0.5400938321933121,0.9297920965821272
+1746018837.562557,0.5729384697692176,0.3386136662594256,0.3547827377981535,0.3548600330391174,0.5400947973649016,0.9297929877590374
+1746018837.5964425,0.5625236545961011,0.33861266755498065,0.3547824084326578,0.35486117350630475,0.5400958258544716,0.9297939399448671
+1746018837.6303582,0.5521088386497741,0.3386116338087682,0.35478207975358095,0.35486238740066606,0.5400969223010706,0.9297949575613353
+1746018837.6643114,0.5416940230404215,0.33861056581572724,0.35478175410406476,0.3548636796664716,0.5400980917138504,0.9297960453438767
+1746018837.6983092,0.5312792086334933,0.33860946437494666,0.3547814339812177,0.3548650555657311,0.5400993394997665,0.9297972083578123
+1746018837.732358,0.5208643960390388,0.33860833029595816,0.3547811220532441,0.3548665206905392,0.5401006714926271,0.9297984520137942
+1746018837.7664638,0.510449585598229,0.338607164404173,0.35478082117710863,0.3548680809740007,0.5401020939833232,0.9297997820820324
+1746018837.8006322,0.5000347773670611,0.3386059675453585,0.3547805344167477,0.3548697426991863,0.5401036137509987,0.9298012047046857
--- a/test/joint_trajectories_cart/trajectory_0006_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0006_cart.csv
@@ -0,0 +1,50 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018837.9624937,0.5102306875593026,0.33860556456342195,0.35478002757694416,0.35487016941200744,0.5401025405160459,0.9298001220997502
+1746018837.9959111,0.5204328850940596,0.3386115801077767,0.35478158624335365,0.3548656954519625,0.5400998163251943,0.9297969485490394
+1746018838.0292678,0.530635256889031,0.33861255481936614,0.35478185564203296,0.35486429075511383,0.5400984591341419,0.92979575554463
+1746018838.062575,0.5408376413777307,0.3386135359405312,0.35478211203927335,0.3548629219583532,0.5400971778038364,0.9297946120689181
+1746018838.0958395,0.5510400180632162,0.33861448801991345,0.3547823756375483,0.3548616333025001,0.5400959737673825,0.9297935412447351
+1746018838.129067,0.561242386946426,0.338615410187246,0.35478264343931115,0.35486041962955583,0.5400948419362998,0.929792538192023
+1746018838.1622646,0.5714447475169135,0.3386163016911057,0.3547829132235098,0.3548592762908375,0.5400937773615131,0.9297915983966191
+1746018838.1954398,0.5816470990133457,0.33861716176781287,0.3547831828854366,0.35485819891060466,0.5400927754491891,0.9297907176327771
+1746018838.2286,0.5918494404299253,0.3386179896408493,0.35478345043028775,0.35485718337066874,0.5400918319332398,0.9297898919463334
+1746018838.261753,0.6020517705213025,0.33861878451478944,0.35478371395961616,0.3548562257991932,0.5400909428521982,0.9297891176408328
+1746018838.2949078,0.6122540878048776,0.33861954556848267,0.35478397165804665,0.3548553225590352,0.5400901045273436,0.9297883912635053
+1746018838.3280725,0.6224563905604233,0.3386202719474199,0.35478422178013236,0.3548544702359173,0.5400893135420816,0.9297877095913418
+1746018838.3612564,0.6326586768269248,0.33862096275526304,0.35478446263728514,0.3548536656266058,0.5400885667225659,0.9297870696174111
+1746018838.3944685,0.6428609443964908,0.33862161704447313,0.35478469258467193,0.3548529057272062,0.5400878611195548,0.9297864685375535
+1746018838.4277194,0.6530631908051621,0.33862223380595297,0.3547849100079784,0.35485218772169463,0.5400871939914349,0.9297859037375167
+1746018838.4610186,0.6632654133203727,0.3386228119575897,0.3547851133099109,0.35485150897074424,0.5400865627883917,0.9297853727806085
+1746018838.494377,0.6734676089247714,0.3386233503315536,0.3547853008962955,0.35485086700091045,0.5400859651376393,0.9297848733958944
+1746018838.5278056,0.6836697742960235,0.338623847660165,0.3547854711616042,0.3548502594942173,0.5400853988296749,0.9297844034669644
+1746018838.5613158,0.6938719057821414,0.33862430256010556,0.3547856224737223,0.35484968427814606,0.5400848618054711,0.9297839610212794
+1746018838.5949197,0.7040739993717672,0.33862471351469964,0.35478575315772803,0.35484913931606493,0.540084352144552,0.9297835442200874
+1746018838.62863,0.7142760506587136,0.3386250788539257,0.3547858614784251,0.3548486226980787,0.540083868053884,0.929783151348902
+1746018838.6624596,0.7244780547999153,0.3386253967317582,0.3547859456213176,0.35484813263229864,0.5400834078575071,0.9297827808085288
+1746018838.6964223,0.7346800064657361,0.33862566510034064,0.35478600367166335,0.35484766743652335,0.5400829699868459,0.9297824311066042
+1746018838.7305326,0.744881899781361,0.338625881680388,0.35478603359115696,0.35484722553030124,0.5400825529716318,0.9297821008496331
+1746018838.7648058,0.7550837282576983,0.33862604392709517,0.3547860331917262,0.35484680542734776,0.5400821554313623,0.9297817887354863
+1746018838.7992826,0.7652854847098542,0.33862614899064114,0.3547860001057848,0.3548464057282994,0.5400817760672394,0.9297814935463206
+1746018838.8342564,0.7754871611607881,0.33862619367019925,0.3547859317521677,0.3548460251137502,0.5400814136544997,0.9297812141418956
+1746018838.8697464,0.7856887487271866,0.33862617436007514,0.3547858252967799,0.35484566233753684,0.5400810670350813,0.9297809494532385
+1746018838.9057746,0.7958902374838415,0.3386260869862885,0.3547856776067546,0.354845316220228,0.5400807351105352,0.9297806984766164
+1746018838.9423633,0.8060916163019007,0.33862592693146554,0.3547854851966493,0.35484498564274897,0.5400804168350929,0.9297804602677694
+1746018838.9795377,0.8162928726551361,0.3386256889453817,0.3547852441647786,0.3548446695400894,0.540080111208812,0.9297802339363596
+1746018839.017325,0.8264939923867874,0.3386253670377588,0.3547849501173374,0.35484436689499904,0.5400798172706679,0.9297800186405696
+1746018839.0557551,0.8366949594275045,0.3386249543490016,0.354784598077269,0.35484407673159696,0.540079534091475,0.9297798135817845
+1746018839.0948606,0.8468957554521337,0.3386244429933126,0.35478418237399745,0.35484379810875677,0.5400792607664834,0.9297796179992969
+1746018839.134677,0.8570963594594918,0.3386238238669661,0.35478369650897196,0.3548435301131466,0.540078996407449,0.9297794311649141
+1746018839.1752436,0.8672967472543252,0.3386230864123113,0.354783132990414,0.35484327185171877,0.5400787401339501,0.9297792523773852
+1746018839.216604,0.8774968908039783,0.3386222183250202,0.35478248312854505,0.3548430224434288,0.5400784910636309,0.9297790809564763
+1746018839.258806,0.8876967574331005,0.3386212051879557,0.3547817367796329,0.35484278100987465,0.5400782483009794,0.9297789162365343
+1746018839.3019032,0.8978963088068814,0.3386200300091943,0.3547808820231383,0.354842546664443,0.5400780109240952,0.9297787575592836
+1746018839.345955,0.9080954996352522,0.3386186726335796,0.35477990475047566,0.3548423184994226,0.5400777779687443,0.9297786042655434
+1746018839.3910284,0.9182942760046386,0.3386171089854539,0.3547787881357081,0.3548420955703074,0.5400775484086952,0.9297784556854235
+1746018839.4371989,0.9284925732063453,0.3386153100832404,0.3547775119465214,0.35484187687627416,0.5400773211309818,0.9297783111263974
+1746018839.4845517,0.9386903128753308,0.33861324074150284,0.3547760516362452,0.35484166133530687,0.5400770949041368,0.9297781698584056
+1746018839.533184,0.9488873991701418,0.33861085783851486,0.3547743771312058,0.3548414477518555,0.5400768683365978,0.9297780310947452
+1746018839.5832078,0.9590837135976907,0.3386081079698914,0.3547724511872236,0.35484123477385576,0.5400766398211737,0.9297778939669725
+1746018839.6347528,0.9692791078880415,0.33860492421897315,0.3547702271257263,0.35484102083439917,0.5400764074594073,0.929777757491109
+1746018839.6879692,0.9794733940066245,0.3386012216309549,0.354767645658593,0.3548408040708161,0.5400761689563759,0.9297776205210437
+1746018839.7430353,0.9896663298695357,0.33859689074179633,0.35476463034426053,0.35484058220979026,0.5400759214711092,0.9297774816826611
+1746018839.8001628,0.9998575984452447,0.33859178811412316,0.35476108093583825,0.35484035240015555,0.5400756613986569,0.9297773392782596
--- a/test/joint_trajectories_cart/trajectory_0007_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0007_cart.csv
@@ -0,0 +1,49 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018839.9870331,0.9894510355610513,0.3385912571647137,0.3547664248803014,0.35484193565140265,0.5400812026739317,0.9297789204019978
+1746018840.043153,0.9790510900580351,0.3385982636222143,0.3547683318795274,0.35484387708550785,0.5400804362558663,0.9297799368189359
+1746018840.0973828,0.9686448737018083,0.338602329453063,0.3547710211121765,0.3548439217787354,0.5400804470334919,0.9297799854855084
+1746018840.1498814,0.9582374790966991,0.3386058647577248,0.3547733791220962,0.3548439836894355,0.5400804795217156,0.9297800451300474
+1746018840.2008076,0.9478290263275285,0.3386088843314891,0.35477539627855115,0.35484406184595746,0.5400805303671812,0.929780115334008
+1746018840.250297,0.9374197007077437,0.33861147542543635,0.35477713098073305,0.3548441551233821,0.5400805979258542,0.9297801955713083
+1746018840.298467,0.9270096449290561,0.33861370450132566,0.3547786278082242,0.3548442627545596,0.5400806810211121,0.9297802855268568
+1746018840.3454187,0.9165989728101249,0.33861562450849236,0.35477992226454746,0.3548443842335158,0.5400807788019358,0.9297803850388381
+1746018840.391241,0.9061877761961923,0.3386172780835289,0.3547810429370727,0.3548445192630661,0.5400808906752443,0.929780494070617
+1746018840.436013,0.8957761300305052,0.33861869992604365,0.3547820130993479,0.35484466771664025,0.5400810162556039,0.9297806126903315
+1746018840.4798033,0.885364096084589,0.3386199185463975,0.3547828518887851,0.3548448296106538,0.5400811553285415,0.9297807410563901
+1746018840.5226743,0.874951725744389,0.33862095757169225,0.35478357518597403,0.3548450050844381,0.5400813078235712,0.9297808794072245
+1746018840.564681,0.8645390621193363,0.3386218367348407,0.35478419628019475,0.35484519438570006,0.5400814737943135,0.9297810280541892
+1746018840.6058736,0.8541261416585597,0.33862257263287604,0.35478472637942204,0.3548453978600991,0.5400816534038773,0.9297811873768241
+1746018840.6462967,0.8437129954033898,0.33862317931493324,0.35478517500568424,0.35484561594394526,0.5400818469142276,0.929781357819947
+1746018840.6859908,0.8332996499680385,0.3386236687429251,0.35478555030483927,0.3548458491593254,0.5400820546786163,0.929781539892188
+1746018840.7249928,0.822886128314766,0.3386240511559741,0.3547858592917275,0.3548460981111399,0.5400822771364249,0.9297817341657006
+1746018840.7633364,0.8124724503719867,0.338624335361287,0.35478610804601624,0.3548463634856769,0.5400825148099422,0.9297819412768412
+1746018840.801052,0.8020586335311176,0.33862452896828965,0.3547863018700582,0.3548466460504759,0.5400827683027231,0.9297821619276995
+1746018840.838168,0.7916446930489611,0.3386246385785615,0.35478644541722626,0.3548469466552664,0.5400830382992803,0.9297823968883668
+1746018840.8747098,0.7812306423758256,0.33862466994106916,0.3547865427971168,0.3548472662338443,0.540083325565927,0.929782646999872
+1746018840.910701,0.7708164934247871,0.33862462807993604,0.354786597662494,0.35484760580679164,0.5400836309526343,0.9297829131777638
+1746018840.9461634,0.7604022567939512,0.3386245174003163,0.3547866132817376,0.3548479664849486,0.5400839553958138,0.9297831964162833
+1746018840.981242,0.7499879419508725,0.33862434177669787,0.3547865925996949,0.3548483494736006,0.5400842999219613,0.9297834977931297
+1746018841.0161424,0.7395735573863236,0.3386241046270163,0.35478653828923035,0.3548487560773453,0.5400846656521302,0.92978381847482
+1746018841.0508816,0.7291591107430333,0.338623808975238,0.3547864527952661,0.35484918770559803,0.5400850538072088,0.9297841597226251
+1746018841.0854752,0.7187446089238667,0.3386234575045385,0.3547863383727542,0.35484964587875323,0.5400854657140032,0.9297845228991151
+1746018841.1199381,0.7083300581829869,0.33862305260275155,0.3547861971197193,0.35485013223496914,0.5400859028121348,0.9297849094753228
+1746018841.154285,0.6979154642028539,0.3386225964014569,0.3547860310063113,0.3548506485375911,0.5400863666617777,0.9297853210385344
+1746018841.1885288,0.6875008321593309,0.33862209080979605,0.3547858419006139,0.3548511966832113,0.5400868589522595,0.9297857593007371
+1746018841.2226827,0.6770861667767518,0.33862153754391094,0.35478563159183485,0.3548517787103644,0.5400873815115691,0.9297862261077503
+1746018841.2567594,0.6666714723744348,0.3386209381527294,0.3547854018113845,0.35485239680887254,0.5400879363168252,0.9297867234490398
+1746018841.2907708,0.6562567529058541,0.3386202940407016,0.35478515425227664,0.35485305332983186,0.5400885255057382,0.9297872534682682
+1746018841.324728,0.6458420119914607,0.3386196064879625,0.3547848905872013,0.3548537507962404,0.5400891513891459,0.9297878184745573
+1746018841.358642,0.6354272529459345,0.3386188766683402,0.3547846124855788,0.35485449191425694,0.5400898164646668,0.92978842095451
+1746018841.392524,0.6250124788005298,0.3386181056655299,0.3547843216298531,0.3548552795850811,0.5400905234315458,0.9297890635849653
+1746018841.4263835,0.6145976923210283,0.3386172944877095,0.3547840197312423,0.3548561169174115,0.5400912752067503,0.9297897492464945
+1746018841.46023,0.6041828960217211,0.3386164440808249,0.35478370854514174,0.3548570072404519,0.5400920749423886,0.9297904810376029
+1746018841.4940736,0.5937680921757482,0.3386155553407147,0.35478338988635155,0.3548579541173864,0.5400929260445052,0.9297912622896114
+1746018841.5279226,0.5833532828220642,0.3386146291242265,0.35478306564426487,0.3548589613592577,0.5400938321933121,0.9297920965821272
+1746018841.561786,0.5729384697692176,0.3386136662594256,0.3547827377981535,0.3548600330391174,0.5400947973649016,0.9297929877590374
+1746018841.5956717,0.5625236545961011,0.33861266755498065,0.3547824084326578,0.35486117350630475,0.5400958258544716,0.9297939399448671
+1746018841.6295874,0.5521088386497741,0.3386116338087682,0.35478207975358095,0.35486238740066606,0.5400969223010706,0.9297949575613353
+1746018841.6635406,0.5416940230404215,0.33861056581572724,0.35478175410406476,0.3548636796664716,0.5400980917138504,0.9297960453438767
+1746018841.6975384,0.5312792086334933,0.33860946437494666,0.3547814339812177,0.3548650555657311,0.5400993394997665,0.9297972083578123
+1746018841.7315872,0.5208643960390388,0.33860833029595816,0.3547811220532441,0.3548665206905392,0.5401006714926271,0.9297984520137942
+1746018841.765693,0.510449585598229,0.338607164404173,0.35478082117710863,0.3548680809740007,0.5401020939833232,0.9297997820820324
+1746018841.7998614,0.5000347773670611,0.3386059675453585,0.3547805344167477,0.3548697426991863,0.5401036137509987,0.9298012047046857
--- a/test/joint_trajectories_cart/trajectory_0008_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0008_cart.csv
@@ -0,0 +1,49 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018842.5402875,0.5104440846719139,0.3386068104107253,0.3547788766321834,0.3548683946803408,0.5401020146176606,0.9297990355283123
+1746018842.5743926,0.5208589434736831,0.3386074486389229,0.3547791992112156,0.35486682726211377,0.5401008346652705,0.9297977812677931
+1746018842.6084414,0.5312738152904978,0.3386085231377703,0.35477945901221725,0.3548652773570319,0.5400993844242197,0.9297964816228174
+1746018842.6424391,0.5416886762189116,0.33860956414884674,0.3547797326537425,0.3548638272386115,0.5400980261039631,0.9297952706303569
+1746018842.6763926,0.5521035288560308,0.3386105734235112,0.35478001315408325,0.3548624634164672,0.5400967515820373,0.9297941380289428
+1746018842.7103083,0.5625183728801105,0.33861155015027633,0.3547802979805631,0.35486118049810284,0.5400955549446282,0.9297930785245245
+1746018842.7441938,0.5729332076996463,0.3386124934899272,0.354780584677762,0.3548599733693764,0.5400944307505756,0.9297920871515345
+1746018842.778057,0.5833480324386696,0.33861340259428574,0.3547808709289912,0.35485883721738815,0.5400933739534488,0.9297911592647436
+1746018842.811906,0.5937628459450863,0.33861427659956217,0.35478115453963016,0.35485776751892834,0.540092379874089,0.929790290524119
+1746018842.8457494,0.6041776467959188,0.3386151146192766,0.3547814334215864,0.354856760027366,0.5400914441741087,0.9297894768787137
+1746018842.879596,0.6145924332994557,0.3386159157362372,0.354781705577983,0.3548558107591475,0.54009056283085,0.9297887145504744
+1746018842.9134552,0.6250072034942205,0.3386166789935513,0.354781969088006,0.35485491598012414,0.5400897321138279,0.9297880000181953
+1746018842.947337,0.6354219551446234,0.3386174033846353,0.354782222091806,0.3548540721919195,0.5400889485626406,0.9297873300017958
+1746018842.981251,0.645836685733121,0.3386180878421396,0.35478246277534065,0.3548532761184714,0.5400882089663114,0.9297867014470579
+1746018843.015208,0.6562513924486427,0.33861873122568253,0.35478268935502055,0.35485252469285716,0.5400875103440117,0.9297861115109097
+1746018843.0492191,0.6666660721709877,0.33861933230824387,0.35478290006200464,0.3548518150445045,0.5400868499270995,0.9297855575473329
+1746018843.0832956,0.6770807214508109,0.33861988976102886,0.3547830931259661,0.35485114448682914,0.5400862251424045,0.9297850370939107
+1746018843.1174493,0.6874953364847085,0.33862040213657035,0.3547832667581168,0.3548505105053418,0.5400856335966832,0.9297845478590666
+1746018843.1516929,0.6979099130848254,0.3386208678497805,0.3547834191332516,0.3548499107462614,0.5400850730621646,0.929784087709971
+1746018843.186039,0.7083244466422343,0.3386212851565882,0.3547835483705282,0.35484934300562093,0.5400845414631047,0.9297836546611234
+1746018843.2205017,0.7187389320832016,0.3386216521297463,0.3547836525126443,0.3548488052188824,0.5400840368632686,0.9297832468635927
+1746018843.255095,0.7291533638172167,0.33862196663125893,0.35478372950302045,0.3548482954510314,0.5400835574542612,0.9297828625948783
+1746018843.2898338,0.7395677356754277,0.3386222262808148,0.35478377716050835,0.35484781188714903,0.5400831015446186,0.9297825002493824
+1746018843.3247337,0.7499820408378017,0.3386224284194115,0.35478379315105446,0.3548473528234164,0.5400826675495859,0.9297821583294369
+1746018843.3598118,0.7603962717469366,0.33862257006723123,0.3547837749556275,0.35484691665852297,0.5400822539814921,0.9297818354368601
+1746018843.3952734,0.7708104200059512,0.3386226478745653,0.3547837198335545,0.3548465018854457,0.5400818594406371,0.9297815302649937
+1746018843.431264,0.7812244762572634,0.338622658064322,0.3547836247802293,0.35484610708353137,0.5400814826066125,0.9297812415911764
+1746018843.4678047,0.7916384300382565,0.33862259636428776,0.3547834864778963,0.354845730910852,0.5400811222299495,0.9297809682696033
+1746018843.5049198,0.8020522696088227,0.33862245792684165,0.3547833012378911,0.3548453720967464,0.5400807771240027,0.929780709224518
+1746018843.5426345,0.8124659817444418,0.33862223723323553,0.354783064932311,0.3548450294344816,0.54008044615696,0.9297804634436779
+1746018843.580977,0.8228795514867315,0.3386219279787632,0.3547827729125347,0.3548447017739518,0.5400801282438424,0.9297802299720244
+1746018843.619978,0.8332929618411472,0.338621522934118,0.3547824199113008,0.35484438801429646,0.5400798223383568,0.9297800079054849
+1746018843.6596708,0.8437061934085042,0.33862101377688675,0.35478199992410064,0.35484408709630927,0.5400795274244228,0.9297797963848167
+1746018843.7000928,0.8541192239329839,0.33862039088530266,0.3547815060643793,0.35484379799448323,0.5400792425071519,0.9297795945893922
+1746018843.741284,0.8645320277438772,0.3386196430839282,0.354780930385305,0.354843519708477,0.5400789666030109,0.9297794017307852
+1746018843.7832894,0.8749445750609143,0.3386187573275949,0.3547802636585345,0.35484325125374316,0.54007869872883,0.929779217046009
+1746018843.826159,0.8853568311228323,0.33861771830529663,0.3547794950971589,0.35484299165096983,0.540078437889182,0.9297790397901903
+1746018843.869948,0.8957687550845774,0.338616507939286,0.3547786120054782,0.3548427399138723,0.5400781830615539,0.9297788692284126
+1746018843.9147182,0.9061802986083771,0.3386151047454902,0.35477759933184766,0.3548424950347194,0.5400779331784775,0.9297787046263575
+1746018843.960539,0.9165914040449656,0.3386134830082501,0.3547764390916355,0.35484225596673186,0.54007768710551,0.9297785452392581
+1746018844.0074894,0.9270020020590983,0.3386116117033171,0.35477510961390757,0.35484202160215017,0.5400774436135003,0.9297783902984681
+1746018844.0556576,0.9374120084910398,0.3386094530747695,0.3547735845455799,0.35484179074428723,0.5400772013429141,0.92977823899467
+1746018844.105146,0.9478213201515535,0.33860696072891644,0.3547718315167805,0.35484156207109024,0.540076958757013,0.9297780904563273
+1746018844.156071,0.9582298091031093,0.3386040770427362,0.3547698103250159,0.35484133408658447,0.5400767140791349,0.9297779437212912
+1746018844.2085686,0.9686373147523818,0.33860072958147797,0.3547674704231579,0.35484110505473054,0.5400764652069426,0.9297777976984619
+1746018844.262797,0.9790436327126595,0.33859682605435887,0.3547647473793561,0.3548408729072569,0.5400762095926251,0.9297776511146897
+1746018844.3189442,0.9894484987886571,0.33859224706338553,0.35476155778354296,0.3548406351121366,0.5400759440716495,0.9297775024393419
+1746018844.3772337,0.999851565403366,0.3385868354337149,0.3547577917453807,0.3548403884810008,0.5400756646117768,0.9297773497741979
--- a/test/joint_trajectories_cart/trajectory_0009_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0009_cart.csv
@@ -0,0 +1,42 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018844.4323194,0.9997484988833173,0.3386066113332168,0.3547773286570418,0.3548351047365334,0.5400714505874364,0.9297745256683936
+1746018844.5018663,0.98732122402018,0.3385451664409863,0.3547349018942071,0.354846349655948,0.5400835848308985,0.9297816002052235
+1746018844.568479,0.9748411095658571,0.3385520261751348,0.3547395708086607,0.3548466148387508,0.5400836743785448,0.9297818039232204
+1746018844.6324482,0.9623581784281144,0.33855865398030294,0.35474397095840726,0.35484667711819945,0.5400836727185443,0.9297818825554784
+1746018844.6940737,0.9498728524019968,0.3385641235034418,0.3547476048144682,0.35484677577752893,0.5400837154536844,0.9297819843882299
+1746018844.753599,0.9373856454778458,0.33856866978576977,0.35475063203313484,0.35484690948381575,0.5400837976802587,0.9297821090157239
+1746018844.8112278,0.9248969226767055,0.3385724628759203,0.35475316630776565,0.35484707565976936,0.5400839156004087,0.929782255290771
+1746018844.8671327,0.9124069624096844,0.33857563247347744,0.35475529427392144,0.35484727262014437,0.5400840665487614,0.929782422581251
+1746018844.921461,0.8999159803571215,0.3385782788009319,0.3547570828719523,0.35484749938081245,0.5400842487284764,0.9297826106707163
+1746018844.9743397,0.8874241462345426,0.3385804804648471,0.354758584631521,0.35484775550792697,0.5400844610130635,0.929782819677958
+1746018845.0258791,0.87493159545736,0.3385822999255165,0.35475984134377203,0.354848041014445,0.540084702809481,0.9297830500029584
+1746018845.076176,0.8624384374344458,0.3385837873890275,0.354760886671796,0.3548483562888922,0.540084973962506,0.9297833022909344
+1746018845.1253145,0.8499447615724599,0.3385849836271595,0.3547617480406406,0.35484870204656777,0.5400852746877851,0.9297835774091013
+1746018845.1733701,0.8374506416943303,0.3385859220542095,0.35476244802875007,0.3548490792968025,0.5400856055253075,0.9297838764327032
+1746018845.2204094,0.8249561393390087,0.33858663027944447,0.35476300540813765,0.35484948932197924,0.5400859673077817,0.9297842006379763
+1746018845.2664921,0.8124613062588918,0.33858713128344486,0.35476343593303095,0.354849933665421,0.5400863611401829,0.9297845515005048
+1746018845.3116713,0.7999661863330724,0.33858744432064586,0.3547637529457608,0.3548504141261897,0.5400867883879061,0.9297849306979307
+1746018845.3559947,0.7874708170493286,0.33858758561985086,0.35476396784810316,0.3548509327593993,0.5400872506717774,0.929785340116324
+1746018845.399506,0.7749752306636584,0.3385875689338237,0.35476409047238505,0.3548514918811314,0.5400877498686912,0.9297857818597388
+1746018845.4422443,0.7624794551157862,0.3385874059748644,0.35476412937712676,0.3548520940772762,0.5400882881170608,0.9297862582626878
+1746018845.4843569,0.7499835147579705,0.33858710676334514,0.35476409208533555,0.3548527422158773,0.5400888678265254,0.9297867719053385
+1746018845.5262134,0.7374874309394219,0.33858667990919283,0.3547639852788763,0.3548534394626725,0.5400894916915682,0.9297873256313656
+1746018845.5678425,0.7249912224779258,0.33858613284124717,0.354763814958972,0.3548541892996497,0.5400901627088501,0.929787922568432
+1746018845.6092708,0.7124949060424977,0.3385854719957992,0.35476358658046525,0.3548549955465056,0.5400908841981867,0.9297885661513248
+1746018845.650524,0.6999984964651418,0.3385847029729266,0.3547633051656786,0.3548558623849477,0.5400916598271854,0.9297892601478194
+1746018845.691625,0.6875020069955975,0.3385838306672683,0.3547629754024121,0.35485679438582624,0.540092493639634,0.9297900086873429
+1746018845.732597,0.6750054495097293,0.3385828593783838,0.35476260172961815,0.35485779653908217,0.5400933900877963,0.9297908162925537
+1746018845.7734613,0.6625088346798526,0.3385817929047351,0.3547621884135743,0.35485887428653395,0.5400943540688262,0.9297916879139299
+1746018845.814238,0.6500121721134119,0.3385806346244482,0.35476173961680124,0.35486003355749,0.5400953909655453,0.9297926289674641
+1746018845.8549469,0.6375154704650281,0.3385793875653645,0.3547612594615541,0.35486128080716817,0.5400965066918748,0.929793645375546
+1746018845.895606,0.6250187375258156,0.338578054466366,0.35476075208938473,0.35486262305786354,0.540097707743227,0.9297947436110687
+1746018845.936233,0.6125219802929873,0.33857663783153635,0.3547602217180199,0.354864067942739,0.5400990012521911,0.9297959307447449
+1746018845.9768445,0.6000252050220882,0.338575139978408,0.3547596726965879,0.3548656237520472,0.5401003950498381,0.9297972144955476
+1746018846.0174568,0.5875284172636248,0.33857356308124165,0.35475910956008433,0.3548672994814761,0.5401018977329638,0.9297986032840703
+1746018846.0580845,0.5750316218854113,0.3385719092100821,0.3547585370838215,0.3548691048821673,0.5401035187375515,0.9298001062884721
+1746018846.0987422,0.5625348230815901,0.338570180366104,0.3547579603384975,0.3548710505117865,0.5401052684186746,0.9298017335024719
+1746018846.1394434,0.550038024368978,0.3385683785135949,0.3547573847464348,0.3548731477858011,0.5401071581369579,0.9298034957946205
+1746018846.1802008,0.5375412285711494,0.3385665056087457,0.35475681613943055,0.35487540902782044,0.5401092003515608,0.9298054049677559
+1746018846.2210262,0.5250444377904748,0.33856456362523696,0.3547562608185833,0.35487784751753065,0.5401114087194537,0.929807473817149
+1746018846.2619305,0.5125476533681845,0.33856255457646495,0.3547557256163532,0.3548804775343148,0.5401137982004399,0.929809716185346
+1746018846.3029237,0.5000508758324201,0.33856048053403154,0.3547552179610149,0.3548833143941388,0.5401163851670047,0.9298121470110581
--- a/test/joint_trajectories_cart/trajectory_0010_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0010_cart.csv
@@ -0,0 +1,24 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018846.3560357,0.49998878872379254,0.3387094903963945,0.354839871013792,0.3548519649120779,0.5400691520460852,0.9297797663797738
+1746018846.4305744,0.5228282430344469,0.33821063928618317,0.3545502223887582,0.35498434878619634,0.540204679145653,0.9298825123247647
+1746018846.5047834,0.5455411342747011,0.33823302523162807,0.3545529006646513,0.35495550847175616,0.5401843539298066,0.929861224081184
+1746018846.5787566,0.5682496190243458,0.33824282490516866,0.3545558734349246,0.35494208952974576,0.5401711614048076,0.9298500914260566
+1746018846.6526036,0.590957900378033,0.3382518826610232,0.3545586043747151,0.3549300690121092,0.5401597067041203,0.92984034853555
+1746018846.7264087,0.6136658024570547,0.33826015393575803,0.3545612588810303,0.3549194956400368,0.5401496477078788,0.9298319011336778
+1746018846.8002665,0.6363732501675361,0.33826758872709356,0.3545637294689188,0.3549101766089429,0.5401407839937716,0.9298245639746534
+1746018846.8742797,0.6590801355391167,0.3382741357964561,0.3545659236467247,0.35490194097059574,0.5401329411008654,0.9298181755715189
+1746018846.9485602,0.6817863171052154,0.3382797358380553,0.35456775201231544,0.35489464026070044,0.5401259702581296,0.9298125976309133
+1746018847.023231,0.7044916159191791,0.3382843183697779,0.3545691246730506,0.3548881453692795,0.5401197440558254,0.9298077116525593
+1746018847.0984266,0.7271958090033783,0.33828779760680266,0.35456994712644235,0.354882343795292,0.540114152717769,0.9298034159586004
+1746018847.1742954,0.7498986194699777,0.33829006706207454,0.3545701154482128,0.3548771371104796,0.5401091008674621,0.9297996230458393
+1746018847.2511272,0.7725997021243378,0.33829099232782356,0.3545695103603376,0.35487243861640466,0.5401045046779525,0.9297962572318157
+1746018847.33048,0.7952986226851875,0.33829040119474424,0.3545679895472882,0.35486817114988534,0.5401002892876888,0.929793252546758
+1746018847.4125922,0.8179948276628553,0.33828806978551607,0.3545653772515819,0.35486426496444184,0.5400963863480642,0.9297905508076562
+1746018847.4977489,0.8406876001143351,0.33828370258031926,0.35456144961280417,0.35486065557954405,0.5400927315353962,0.9297880997916463
+1746018847.5862975,0.8633759933492307,0.33827690283391776,0.3545559132308124,0.3548572814331301,0.5400892617963933,0.9297858513957861
+1746018847.6786654,0.8860587290286966,0.3382671274169874,0.35454837266725203,0.35485408107440236,0.5400859119746221,0.9297837596162838
+1746018847.7753854,0.9087340355841982,0.3382536155115722,0.35453827929523496,0.3548509894502283,0.5400826102309365,0.9297817780774897
+1746018847.8771353,0.9313993822929791,0.3382352715868446,0.35452484743031265,0.3548479324772159,0.54007927120415,0.9297798566366299
+1746018847.984796,0.954051021738706,0.3382104644725058,0.354506910263934,0.3548448183405421,0.5400757848847308,0.9297779361634769
+1746018848.0995455,0.9766831592636704,0.3381766633023656,0.35448265847872484,0.35484152229485677,0.5400719970145644,0.929775939646141
+1746018848.2230148,0.9992863431188839,0.3381297332004651,0.35444913356607816,0.3548378577455304,0.5400676716481043,0.9297737555000983
--- a/test/joint_trajectories_cart/trajectory_0011_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0011_cart.csv
@@ -0,0 +1,18 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018848.2643304,0.9992804959341806,0.3382914629802017,0.35455573842335586,0.3548330928132742,0.5400514429451964,0.929771140807757
+1746018848.4278781,0.9687473750343337,0.33870880840272666,0.35483815456343815,0.35483876954238325,0.5400744701956002,0.9297765280060807
+1746018848.5865493,0.9368038487630073,0.3378119723609305,0.3542951388982721,0.3549107453076916,0.5401656469803148,0.9298279928926948
+1746018848.726085,0.9057733986376945,0.33791008389766325,0.3543592330505417,0.3549104353340919,0.5401597288618876,0.9298285688258593
+1746018848.8568728,0.8746392130039544,0.33793815235574254,0.3543772863297318,0.35491228502665234,0.5401605421626795,0.9298309453883272
+1746018848.9800177,0.843485265823824,0.33795666455609796,0.3543895648695141,0.35491559847201287,0.540162694158454,0.9298342993505829
+1746018849.0967052,0.8123157653640375,0.33796565332673506,0.3543961314896692,0.35492042325308404,0.54016637776686,0.9298387504361137
+1746018849.2078116,0.7811356279812804,0.3379673839653477,0.35439848388738193,0.35492682871816644,0.5401715862204244,0.9298443979473233
+1746018849.3140135,0.7499480704734274,0.3379632350676856,0.3543975618405587,0.3549349671190365,0.5401784141005402,0.9298514030021741
+1746018849.4180765,0.7187553581549995,0.33795413112190864,0.35439403531443364,0.3549450715790903,0.5401870376842642,0.9298599905605887
+1746018849.5209816,0.6875591477915445,0.33794070447293506,0.3543884125624668,0.3549574595973635,0.5401977162780988,0.9298704570970021
+1746018849.623092,0.6563606929302407,0.3379233974367479,0.3543811112756897,0.3549725431800478,0.5402108012761616,0.9298831833208678
+1746018849.7247272,0.6251609682670684,0.33790252797637044,0.35437250813514276,0.3549908444429329,0.5402267523337033,0.9298986517742928
+1746018849.8261704,0.5939607449667617,0.33787833450458526,0.3543629773062246,0.3550130162632814,0.540246160823194,0.9299174694889244
+1746018849.9276726,0.5627606339207122,0.3378510083771943,0.35435292402149887,0.35503986762696044,0.5402697813532412,0.9299403958656436
+1746018850.029455,0.5315611059304126,0.3378207188944283,0.35434281716627125,0.35507239290919174,0.5402985724188303,0.9299683755098243
+1746018850.1317062,0.5003624932217149,0.3377876333507215,0.35433322352514074,0.35511180327864755,0.5403337471166165,0.9300025746597204
--- a/test/joint_trajectories_cart/trajectory_0012_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0012_cart.csv
@@ -0,0 +1,4 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018850.109841,0.499992043122568,0.338709140222359,0.3548392169436988,0.3548463175336527,0.540070041861325,0.9297780547560188
+1746018850.9285998,0.7499973046315764,0.33870293312379074,0.35483361032338595,0.3548395049421963,0.5400746772839561,0.9297769699209532
+1746018851.9807262,0.9998356122475539,0.3386132202252099,0.3547730142926479,0.35483843990819075,0.5400718696528575,0.9297760873954152
--- a/test/joint_trajectories_cart/trajectory_0013_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0013_cart.csv
@@ -0,0 +1,6 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018851.5973852,0.9999433781397413,0.33868086429244976,0.35481929187095496,0.35483836835081034,0.5400740168144992,0.9297762924632146
+1746018852.1919017,0.8749498931177248,0.3386927310763343,0.35484343399624013,0.3549024998178859,0.5400723101628678,0.9298106454335342
+1746018852.650539,0.7499420133523669,0.3386750043994965,0.35484102897222214,0.3549068055176385,0.5400842016916279,0.9298151092698327
+1746018853.0620122,0.6249191509860896,0.3386403636395043,0.3548390731756355,0.354945265870671,0.5401027962998316,0.9298401102697195
+1746018853.4696677,0.499875584105039,0.3385745548597752,0.35484071434453474,0.35504725952431515,0.5401451711343492,0.9299056005373023
--- a/test/joint_trajectories_cart/trajectory_0014_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0014_cart.csv
@@ -0,0 +1,5 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018853.1196609,0.49999084438380526,0.3387084934158545,0.3548393873007208,0.3548478037444571,0.5400698452652806,0.9297787023959861
+1746018853.6607533,0.666064626102785,0.33866578932364383,0.35488467204663354,0.3549347970851296,0.5400421724810315,0.9298238923251426
+1746018854.222654,0.8325228461228629,0.33852860158610715,0.35479637037843237,0.354831378690314,0.5400500613890574,0.9297743564996565
+1746018854.9840958,0.9999875006688111,0.33870240888305936,0.3548337336022367,0.3548387138094628,0.5400744126782482,0.9297764919382692
--- a/test/joint_trajectories_cart/trajectory_0015_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0015_cart.csv
@@ -0,0 +1,6 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018854.5857203,0.9999474315208599,0.33868295563742656,0.3548207000685034,0.3548384000591708,0.5400740653141097,0.9297763119430412
+1746018855.1802669,0.8749498925957069,0.3386927251913073,0.35484343255512185,0.3549025050221844,0.5400723111120405,0.9298106484680194
+1746018855.638904,0.7499420133477953,0.3386750043989203,0.35484102897289266,0.35490680551982146,0.5400842016915113,0.9298151092706827
+1746018856.0503771,0.6249191509860967,0.3386403636395169,0.35483907317563734,0.3549452658706545,0.5401027962998273,0.9298401102697097
+1746018856.4580326,0.4998755841050389,0.338574554859775,0.35484071434453496,0.35504725952431504,0.5401451711343493,0.929905600537302
--- a/test/joint_trajectories_cart/trajectory_0016_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0016_cart.csv
@@ -0,0 +1,4 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018856.150761,0.49998853114052927,0.338707677573972,0.35483961884533083,0.3548504943665232,0.54006909797661,0.9297796613536519
+1746018856.9695346,0.7499973042956015,0.3387029324915161,0.3548336097637454,0.3548395050147193,0.5400746772867365,0.9297769699623466
+1746018858.021661,0.9998356122458891,0.33861322022428136,0.35477301429200314,0.3548384399081783,0.5400718696528185,0.9297760873954044
--- a/test/joint_trajectories_cart/trajectory_0017_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0017_cart.csv
@@ -0,0 +1,7 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018857.5495617,0.9999592984167665,0.3386890622517516,0.35482485824181675,0.354838487803229,0.5400741960315382,0.9297763654757398
+1746018858.03965,0.8999562727380719,0.3387090881666049,0.35484194256190416,0.3548600656380242,0.5400735032343046,0.9297878984267088
+1746018858.4300761,0.799968719413789,0.33870486490712504,0.3548432532415722,0.3548607494088734,0.5400769645450677,0.9297887937355175
+1746018858.7666776,0.6999654810738004,0.3386959861882976,0.35484275087611616,0.35486885616066466,0.5400810797017587,0.9297939844530492
+1746018859.0931258,0.5999551160118943,0.33868197589617544,0.3548426315096846,0.35488723825658325,0.5400882992429289,0.9298055232783259
+1746018859.419361,0.4999369673011772,0.33865879620851624,0.35484395770809113,0.3549268632794481,0.5401026815139718,0.9298300626069816
--- a/test/joint_trajectories_cart/trajectory_0018_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0018_cart.csv
@@ -0,0 +1,4 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018859.1601691,0.49998533177233717,0.3387063621105274,0.35483993505553313,0.35485437982058404,0.5400681850395332,0.9297811219484226
+1746018859.9789567,0.7499973039890306,0.33870293191704115,0.3548336092550731,0.3548395050807673,0.540074677289148,0.9297769700000567
+1746018861.0310829,0.9998356122443709,0.3386132202234346,0.35477301429141483,0.3548384399081676,0.5400718696527826,0.9297760873953944
--- a/test/joint_trajectories_cart/trajectory_0019_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0019_cart.csv
@@ -0,0 +1,7 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018860.5458684,0.9999610750659504,0.3386899739017742,0.3548254772061733,0.35483850024602265,0.5400742140443552,0.929776373006245
+1746018861.03597,0.8999562713841975,0.3387090882560545,0.3548419428312903,0.3548600675532033,0.5400735029793525,0.9297878994221204
+1746018861.4263961,0.7999687194147795,0.33870486490766455,0.3548432532414361,0.3548607494072374,0.5400769645449378,0.9297887937345831
+1746018861.7629974,0.6999654810737996,0.3386959861882967,0.3548427508761163,0.3548688561606662,0.5400810797017589,0.9297939844530497
+1746018862.0894456,0.5999551160118943,0.33868197589617544,0.3548426315096846,0.35488723825658325,0.5400882992429289,0.9298055232783259
+1746018862.4156811,0.4999369673011772,0.33865879620851624,0.35484395770809113,0.3549268632794481,0.5401026815139718,0.9298300626069816
--- a/test/joint_trajectories_cart/trajectory_0020_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0020_cart.csv
@@ -0,0 +1,4 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018862.1515627,0.49998591070260895,0.33870661712031336,0.35483987803105016,0.35485366085642783,0.5400683402487926,0.9297808455433704
+1746018862.970348,0.7499973040441732,0.33870293202059265,0.3548336093474972,0.354839505068949,0.540074677288707,0.9297769699933072
+1746018864.022474,0.9998356122446445,0.33861322022358703,0.3547730142915205,0.3548384399081691,0.5400718696527892,0.9297760873953962
--- a/test/joint_trajectories_cart/trajectory_0021_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0021_cart.csv
@@ -0,0 +1,7 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018863.5637312,0.9999573415902931,0.33868805653797474,0.35482417337976296,0.3548384739247426,0.5400741756848338,0.9297763570464891
+1746018864.053805,0.8999562742297146,0.33870908806717387,0.35484194226548893,0.35486006352541904,0.5400735035161047,0.9297878973286712
+1746018864.4442313,0.7999687194126974,0.3387048649065311,0.35484325324172145,0.35486074941067625,0.5400769645452117,0.9297887937365478
+1746018864.7808328,0.699965481073801,0.3386959861882985,0.35484275087611616,0.3548688561606629,0.5400810797017583,0.9297939844530482
+1746018865.107281,0.5999551160118943,0.33868197589617577,0.3548426315096847,0.3548872382565834,0.5400882992429288,0.9298055232783266
+1746018865.4335163,0.49993696730117715,0.3386587962085161,0.35484395770809085,0.35492686327944817,0.5401026815139722,0.9298300626069815
--- a/test/joint_trajectories_cart/trajectory_0022_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0022_cart.csv
@@ -0,0 +1,4 @@
+timestamp,x,y,z,roll,pitch,yaw
+1746018865.1454973,0.499987575675419,0.33870730662912885,0.35483971389291424,0.35485163273874254,0.5400688123076307,0.9297800812744245
+1746018865.9642751,0.7499973042036086,0.33870293231940085,0.35483360961229005,0.35483950503462186,0.5400746772874496,0.9297769699737076
+1746018867.0164015,0.999835612245434,0.33861322022402784,0.3547730142918266,0.35483843990817504,0.5400718696528076,0.9297760873954016