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/trajectory_0001.csv
+++ b/test/joint_trajectories/trajectory_0001.csv
@@ -0,0 +1,49 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018827.987727,-2.656567797027455,-2.3712849421871565,-0.925558443966878,-2.044218054604814,1.6771913924565691,-1.9509870519726187
+1746018828.0438428,-2.652120856595637,-2.351424109627855,-0.9608171192921588,-2.028431268407176,1.679807183505904,-1.9473698628003278
+1746018828.0980728,-2.647594970314622,-2.332089936716241,-0.9948907900466022,-2.0132860687076333,1.6824665734419977,-1.9436853435421488
+1746018828.150571,-2.6429882791563926,-2.313236694824977,-1.0278764921238777,-1.9987304370812549,1.6851719017578564,-1.9399327881343014
+1746018828.2014976,-2.6382987353157055,-2.294814885433054,-1.0598744170818852,-1.9847129281701954,1.6879242480214058,-1.9361103501228445
+1746018828.250987,-2.6335241948588997,-2.2767822452980093,-1.0909696618664206,-1.9711899177215997,1.6907247392681217,-1.9322160918875375
+1746018828.299157,-2.6286624444814133,-2.25910212117193,-1.1212356609614638,-1.9581237957700224,1.6935745337534254,-1.9282480057032547
+1746018828.3461087,-2.623711198269035,-2.241742463858658,-1.1507362663155503,-1.9454818902536597,1.6964748221383763,-1.9242040100853421
+1746018828.391931,-2.6186680949383185,-2.2246750354050175,-1.1795273865911184,-1.9332356181869745,1.6994268283317515,-1.9200819464537484
+1746018828.4367027,-2.6135306949506067,-2.207874782468742,-1.2076582824800979,-1.921359814465153,1.7024318103456428,-1.9158795756110172
+1746018828.480493,-2.6082964775157835,-2.1913193351437776,-1.235172602413144,-1.909832194679763,1.7054910611489644,-1.9115945740398341
+1746018828.523364,-2.602962837480355,-2.1749886019263873,-1.2621092193549084,-1.898632920554509,1.7086059095158914,-1.9072245300077928
+1746018828.565371,-2.597527082095311,-2.158864439255521,-1.2885029133352541,-1.887744244903236,1.711777720865319,-1.9027669394671458
+1746018828.6065636,-2.5919864276594065,-2.142930379547983,-1.3143849330085462,-1.8771502188832048,1.7150078980867605,-1.8982192017368305
+1746018828.6469865,-2.586337996033707,-2.1271714055894106,-1.3397834613776034,-1.8668364485315454,1.7182978823473665,-1.8935786149534906
+1746018828.6866806,-2.5805788110234262,-2.1115737620113757,-1.3647240048810532,-1.8567898906408584,1.7216491538739787,-1.888842371277539
+1746018828.7256827,-2.5747057946232905,-2.0961247967011114,-1.3892297206643047,-1.8469986802916203,1.7250632327031976,-1.8840075518395907
+1746018828.7640262,-2.568715763122914,-2.080812826569625,-1.4133216935874773,-1.8374519840458203,1.728541679391494,-1.8790711214117943
+1746018828.801742,-2.5626054230689332,-2.065627023295545,-1.4370191720594812,-1.8281398740778985,1.7320860956762694,-1.874029922787756
+1746018828.838858,-2.5563713670810064,-2.0505573155703636,-1.4603397699098795,-1.8190532194872113,1.7356981250775654,-1.8688806708538452
+1746018828.8753996,-2.5500100695191747,-2.035594305069767,-1.48329964006603,-1.8101835917802045,1.7393794534287235,-1.8636199463337473
+1746018828.9113908,-2.5435178820005877,-2.020729193918573,-1.505913624682455,-1.8015231820869368,1.7431318093227937,-1.858244189187151
+1746018828.9468534,-2.536891028764185,-2.005953721841944,-1.5281953854927215,-1.7930647281267673,1.7469569644597502,-1.8527496916424473
+1746018828.981932,-2.5301256018826224,-1.9912601115311623,-1.5501575174630116,-1.7848014492920683,1.75085673387769,-1.8471325908422993
+1746018829.0168324,-2.523217556321584,-1.9766410210193348,-1.5718116482777529,-1.7767269884990602,1.7548329760490322,-1.8413888610798816
+1746018829.0515716,-2.5161627048476034,-1.9620895020763978,-1.5931685257489863,-1.7688353596779463,1.7588875928203578,-1.835514305602535
+1746018829.0861652,-2.5089567127866803,-1.9475989638056517,-1.6142380948881407,-1.7611208999530463,1.7630225291718924,-1.829504547958538
+1746018829.120628,-2.5015950926373596,-1.9331631407645378,-1.6350295660932637,-1.7535782257070403,1.7672397727696323,-1.8233550228616648
+1746018829.1549747,-2.494073198543518,-1.918776065047465,-1.655551475672208,-1.7462021918391264,1.7715413532798507,-1.8170609665472082
+1746018829.1892185,-2.4863862206339724,-1.904432041863381,-1.6758117397321064,-1.7389878536203982,1.7759293414120219,-1.8106174065922302
+1746018829.2233727,-2.478529179238171,-1.8901256282196597,-1.6958177023090106,-1.7319304306255023,1.780405847652105,-1.8040191511719863
+1746018829.2574494,-2.470496918989714,-1.8758516143898851,-1.7155761784824244,-1.7250252722810013,1.784973020643612,-1.7972607777237521
+1746018829.2914605,-2.462284102832323,-1.86160500789886,-1.7350934931122435,-1.7182678246205565,1.7896330451688005,-1.790336620988805
+1746018829.3254178,-2.4538852059461935,-1.8473810198056249,-1.754375515746359,-1.7116535978772094,1.7943881396767472,-1.7832407604030007
+1746018829.359332,-2.445294509616421,-1.8331750531060622,-1.7734276921725305,-1.7051781345753345,1.7992405532988585,-1.7759670068064304
+1746018829.393214,-2.436506095069578,-1.8189826931121282,-1.7922550730254687,-1.69883697781073,1.8041925622855022,-1.7685088884430336
+1746018829.4270732,-2.427513837309443,-1.8047996996958646,-1.8108623398073533,-1.6926256394278254,1.8092464657898573,-1.7608596362219362
+1746018829.46092,-2.418311398988563,-1.790622001314058,-1.8292538286354278,-1.6865395678191908,1.8144045809167348,-1.7530121682137474
+1746018829.4947634,-2.40889222435877,-1.7764456907543673,-1.8474335519925673,-1.6805741150850988,1.8196692369448968,-1.744959073357284
+1746018829.5286126,-2.39924953335109,-1.7622670225665948,-1.8654052187245902,-1.6747245033006095,1.8250427686213335,-1.7366925943552969
+1746018829.562476,-2.3893763158437915,-1.748082412163935,-1.8831722525006964,-1.6689857896451026,1.8305275084148604,-1.7282046097420172
+1746018829.5963614,-2.379265326186706,-1.7338884365990461,-1.9007378089298923,-1.6633528301549456,1.836125777604341,-1.7194866151109052
+1746018829.6302772,-2.3689090780605793,-1.7196818370388856,-1.9181047915060365,-1.657820241864649,1.8418398760636912,-1.710529703498208
+1746018829.6642306,-2.3582998397621493,-1.7054595229808347,-1.9352758665365535,-1.6523823631059535,1.847672070591579,-1.7013245449271
+1746018829.6982281,-2.3474296300191164,-1.6912185782709448,-1.9522534771944489,-1.6470332117384563,1.8536245816183596,-1.6918613651287444
+1746018829.732277,-2.336290214454239,-1.6769562690034054,-1.9690398568195848,-1.6417664410902078,1.859699568106305,-1.6821299234710119
+1746018829.7663827,-2.3248731028346676,-1.6626700533987864,-1.9856370415828184,-1.6365752933929596,1.865899110441612,-1.6721194901433825
+1746018829.8005512,-2.313169547261485,-1.6483575937774173,-2.0020468826151743,-1.6314525505051996,1.8722251910980745,-1.6618188226684314
--- a/test/joint_trajectories/trajectory_0002.csv
+++ b/test/joint_trajectories/trajectory_0002.csv
@@ -0,0 +1,50 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018829.9629352,-2.3246323931825676,-1.662369183730692,-1.9859860284213637,-1.6364661343797922,1.8660312125625174,-1.6719081800763138
+1746018829.9963524,-2.3358193243840324,-1.6763668998952643,-1.9697282755917134,-1.6415486040325569,1.859955867123393,-1.681716456488925
+1746018830.029709,-2.3467455045973478,-1.6903392961166013,-1.9532941100206553,-1.6467039222294941,1.8539985060460893,-1.6912634178945085
+1746018830.0630164,-2.357412817347776,-1.7042910986683502,-1.9366765335125398,-1.651937838254169,1.8481587881336843,-1.7005522701579625
+1746018830.0962808,-2.367829044642684,-1.7182247925608807,-1.9198734973483391,-1.65725640881592,1.842434707104923,-1.7095927192408422
+1746018830.1295083,-2.378001712454484,-1.7321431048843852,-1.902882723676061,-1.662665286327453,1.8368241794365847,-1.718393994115416
+1746018830.162706,-2.387938116128487,-1.746048923035249,-1.8857017302114583,-1.6681698249102748,1.8313250780470778,-1.7269649122867352
+1746018830.1958811,-2.3976453244595373,-1.7599452899679868,-1.8683278179372114,-1.673775117375881,1.8259352440123369,-1.7353138992711519
+1746018830.2290413,-2.4071301838887775,-1.7738354001770886,-1.8507580577374494,-1.6794860312874649,1.8206524972781475,-1.7434490071506985
+1746018830.2621944,-2.4163993230949226,-1.7877225972294317,-1.832989276161644,-1.685307243356319,1.8154746460647377,-1.7513779323942549
+1746018830.2953491,-2.425459157893222,-1.8016103728025703,-1.8150180401770228,-1.6912432723799113,1.8103994950986984,-1.7591080329316544
+1746018830.3285139,-2.4343158963723455,-1.8155023672053947,-1.7968406407629245,-1.697298510924176,1.8054248527936752,-1.7666463444835099
+1746018830.3616977,-2.442975544208546,-1.8294023713737457,-1.7784530751836811,-1.7034772559564346,1.8005485374899468,-1.7739995961558246
+1746018830.39491,-2.4514439101046372,-1.8433143303503392,-1.759851027757332,-1.7097837386400039,1.7957683828525326,-1.7811742253134366
+1746018830.4281607,-2.4597266113085543,-1.8572423482760716,-1.7410298489149851,-1.716222153507498,1.7910822425179438,-1.7881763917501565
+1746018830.4614599,-2.4678290791726623,-1.8711906949387604,-1.7219845323194827,-1.722796687237546,1.7864879940708889,-1.7950119911763036
+1746018830.4948182,-2.4757565647206334,-1.8851638139460283,-1.7027096897813365,-1.7295115472695992,1.781983542424273,-1.801686668046411
+1746018830.5282469,-2.48351414419369,-1.8991663326118113,-1.68319952367387,-1.736370990504302,1.777566822668529,-1.8082058277512736
+1746018830.561757,-2.4911067245523895,-1.9132030736714287,-1.6634477965070025,-1.7433793523530943,1.7732358024496762,-1.8145746481994034
+1746018830.595361,-2.4985390489139676,-1.9272790689689243,-1.6434477972689074,-1.7505410764210922,1.7689884839295225,-1.82079809081341
+1746018830.6290712,-2.5058157019086287,-1.9413995752933326,-1.6231923040851692,-1.7578607451326924,1.76482290537594,-1.82688091096694
+1746018830.662901,-2.5129411149411043,-1.9555700925785702,-1.6026735426741605,-1.765343111640808,1.7607371424262315,-1.8328276678876576
+1746018830.6968637,-2.519919571346382,-1.9697963847260402,-1.5818831399926652,-1.7729931333994968,1.7567293090621519,-1.8386427340513691
+1746018830.730974,-2.52675521143073,-1.9840845033613033,-1.5608120723641956,-1.7808160078275237,1.7527975583311033,-1.844330304091874
+1746018830.765247,-2.5334520373910783,-1.9984408148981199,-1.5394506072601954,-1.7888172105491613,1.7489400828444133,-1.849894403250444
+1746018830.7997239,-2.5400139181075128,-2.01287203135727,-1.5177882377564893,-1.797002536770738,1.745155115080336,-1.8553388953880987
+1746018830.8346977,-2.5464445938050573,-2.027385245476731,-1.4958136085077476,-1.8053781464403147,1.7414409275164555,-1.8606674905830092
+1746018830.8701878,-2.5527476805821965,-2.041987970757935,-1.4735144318635378,-1.8139506139474644,1.7377958326135357,-1.8658837523345089
+1746018830.906216,-2.5589266748046233,-2.056688187224806,-1.4508773924805463,-1.8227269832556683,1.7342181826704879,-1.8709911043942884
+1746018830.9428046,-2.564984957363633,-2.0714943938345094,-1.4278880384536017,-1.8317148295281616,1.7307063695679723,-1.8759928372444565
+1746018830.979979,-2.570925797799328,-2.086415668679616,-1.4045306565761477,-1.8409223285179457,1.7272588244162428,-1.8808921142412387
+1746018831.0177665,-2.576752358289459,-2.1014617383715612,-1.3807881288261408,-1.8503583352558473,1.72387401712112,-1.885691977442181
+1746018831.0561965,-2.5824676975052707,-2.11664305830945,-1.3566417665261248,-1.860032473902345,1.7205504558804474,-1.8903953531338502
+1746018831.0953019,-2.588074774336134,-2.1319709059355443,-1.3320711178065574,-1.8699552410500637,1.7172866866219794,-1.8950050570761443
+1746018831.1351182,-2.5935764514851467,-2.147457489585476,-1.3070537429552438,-1.880138125302221,1.714081292392457,-1.8995237994784817
+1746018831.175685,-2.598975498938122,-2.163116076192637,-1.2815649508896658,-1.8905937466458593,1.7109328927064817,-1.9039541897223107
+1746018831.2170453,-2.6042745973086343,-2.1789611419513553,-1.255577488241776,-1.9013360200397096,1.7078401428628558,-1.9082987408435736
+1746018831.2592473,-2.6094763410618986,-2.1950085511503414,-1.2290611702556746,-1.9123803488177538,1.7048017332351737,-1.9125598737879748
+1746018831.3023446,-2.614583241620377,-2.2112757698523144,-1.2019824396691283,-1.9237438550735724,1.7018163885426851,-1.9167399214511214
+1746018831.3463962,-2.619597730353988,-2.2277821230545776,-1.1743038356968118,-1.9354456562837374,1.698882867106792,-1.9208411325148411
+1746018831.3914697,-2.624522161457751,-2.244549106616396,-1.1459833497468783,-1.9475072002625895,1.6959999600979527,-1.9248656750901965
+1746018831.4376402,-2.629358814719536,-2.2616007688731723,-1.1169736369810401,-1.9599526744268623,1.6931664907773376,-1.9288156401769119
+1746018831.484993,-2.6341098981803106,-2.2789641819084694,-1.0872210423732849,-1.9728095107512091,1.690381313737162,-1.9326930449480468
+1746018831.5336254,-2.6387775506888325,-2.29667002958243,-1.0566643851683533,-1.9861090154205274,1.6876433141434468,-1.9364998358677594
+1746018831.5836492,-2.6433638443520104,-2.3147533496405033,-1.025233424473071,-1.9998871631254325,1.6849514069848492,-1.9402378916487981
+1746018831.635194,-2.647870786881036,-2.333254482166248,-0.9928468977842484,-2.0141856119336783,1.6823045363314142,-1.9439090260547813
+1746018831.6884105,-2.6523003238315535,-2.352220298914748,-0.9594099781441661,-2.0290530185056923,1.6797016746076414,-1.9475149905501257
+1746018831.7434766,-2.6566543407331253,-2.371705822022165,-0.9248109253040582,-2.044546769766889,1.6771418218854173,-1.9510574767971396
+1746018831.800604,-2.660934665098117,-2.3917763936815626,-0.8889165963410672,-2.060735303978048,1.6746240052046204,-1.9545381189944557
--- a/test/joint_trajectories/trajectory_0003.csv
+++ b/test/joint_trajectories/trajectory_0003.csv
@@ -0,0 +1,49 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018831.9868114,-2.6565677523006266,-2.3712859473778694,-0.9255562021889361,-2.0442193525101158,1.6771915874039305,-1.9509871358148483
+1746018832.0429308,-2.6521208586570473,-2.3514240765597316,-0.9608171893623072,-2.028431231308233,1.679807182530988,-1.9473698646276565
+1746018832.0971608,-2.6475949702484503,-2.332089937648089,-0.9948907880305828,-2.0132860697766257,1.6824665734803306,-1.9436853434865617
+1746018832.1496592,-2.6429882791583386,-2.313236694800291,-1.0278764921773265,-1.9987304370528798,1.6851719017566626,-1.9399327881358823
+1746018832.2005854,-2.6382987353156526,-2.294814885433657,-1.0598744170805698,-1.9847129281708982,1.6879242480214387,-1.9361103501228012
+1746018832.250075,-2.633524194858901,-2.2767822452979956,-1.0909696618664504,-1.9711899177215835,1.6907247392681217,-1.9322160918875386
+1746018832.298245,-2.6286624444814133,-2.2591021211719298,-1.1212356609614638,-1.9581237957700226,1.6935745337534254,-1.9282480057032545
+1746018832.3451965,-2.623711198269035,-2.241742463858658,-1.1507362663155505,-1.9454818902536597,1.6964748221383763,-1.924204010085342
+1746018832.391019,-2.618668094938318,-2.224675035405018,-1.1795273865911178,-1.933235618186975,1.6994268283317515,-1.9200819464537484
+1746018832.4357908,-2.6135306949506067,-2.2078747824687426,-1.2076582824800974,-1.9213598144651534,1.7024318103456428,-1.9158795756110172
+1746018832.479581,-2.6082964775157835,-2.1913193351437776,-1.2351726024131437,-1.9098321946797632,1.7054910611489644,-1.9115945740398341
+1746018832.522452,-2.602962837480355,-2.1749886019263873,-1.2621092193549082,-1.8986329205545092,1.7086059095158914,-1.907224530007793
+1746018832.5644588,-2.597527082095311,-2.158864439255521,-1.288502913335254,-1.8877442449032362,1.711777720865319,-1.9027669394671458
+1746018832.6056514,-2.591986427659406,-2.142930379547983,-1.3143849330085464,-1.8771502188832048,1.7150078980867605,-1.8982192017368305
+1746018832.6460745,-2.586337996033707,-2.1271714055894106,-1.3397834613776034,-1.8668364485315452,1.7182978823473665,-1.8935786149534903
+1746018832.6857686,-2.5805788110234267,-2.1115737620113757,-1.3647240048810532,-1.8567898906408586,1.7216491538739778,-1.888842371277539
+1746018832.7247705,-2.5747057946232905,-2.0961247967011114,-1.3892297206643045,-1.8469986802916203,1.7250632327031976,-1.8840075518395907
+1746018832.7631142,-2.568715763122914,-2.080812826569625,-1.4133216935874768,-1.8374519840458203,1.728541679391494,-1.8790711214117943
+1746018832.80083,-2.5626054230689332,-2.0656270232955447,-1.4370191720594814,-1.8281398740778985,1.7320860956762694,-1.874029922787756
+1746018832.8379457,-2.5563713670810064,-2.0505573155703636,-1.4603397699098797,-1.8190532194872113,1.7356981250775654,-1.8688806708538452
+1746018832.8744876,-2.5500100695191747,-2.035594305069767,-1.4832996400660297,-1.810183591780205,1.7393794534287235,-1.8636199463337475
+1746018832.9104788,-2.5435178820005877,-2.020729193918573,-1.5059136246824552,-1.8015231820869366,1.7431318093227937,-1.8582441891871508
+1746018832.9459414,-2.536891028764185,-2.005953721841944,-1.5281953854927215,-1.7930647281267673,1.7469569644597502,-1.8527496916424473
+1746018832.9810197,-2.5301256018826224,-1.9912601115311623,-1.5501575174630116,-1.7848014492920683,1.75085673387769,-1.8471325908422993
+1746018833.0159202,-2.523217556321584,-1.9766410210193348,-1.5718116482777529,-1.7767269884990602,1.7548329760490322,-1.8413888610798816
+1746018833.0506594,-2.5161627048476034,-1.9620895020763978,-1.5931685257489863,-1.7688353596779463,1.7588875928203578,-1.835514305602535
+1746018833.085253,-2.5089567127866803,-1.9475989638056517,-1.6142380948881407,-1.7611208999530463,1.7630225291718924,-1.829504547958538
+1746018833.1197162,-2.5015950926373596,-1.9331631407645378,-1.6350295660932637,-1.7535782257070403,1.7672397727696323,-1.8233550228616648
+1746018833.1540627,-2.494073198543518,-1.918776065047465,-1.655551475672208,-1.7462021918391264,1.7715413532798507,-1.8170609665472082
+1746018833.1883066,-2.4863862206339724,-1.904432041863381,-1.6758117397321064,-1.7389878536203982,1.7759293414120219,-1.8106174065922302
+1746018833.2224605,-2.478529179238171,-1.8901256282196597,-1.6958177023090106,-1.7319304306255023,1.780405847652105,-1.8040191511719863
+1746018833.2565372,-2.470496918989714,-1.8758516143898851,-1.7155761784824244,-1.7250252722810013,1.784973020643612,-1.7972607777237521
+1746018833.2905486,-2.462284102832323,-1.86160500789886,-1.7350934931122435,-1.7182678246205565,1.7896330451688005,-1.790336620988805
+1746018833.3245058,-2.4538852059461935,-1.8473810198056249,-1.754375515746359,-1.7116535978772094,1.7943881396767472,-1.7832407604030007
+1746018833.3584201,-2.445294509616421,-1.8331750531060622,-1.7734276921725305,-1.7051781345753345,1.7992405532988585,-1.7759670068064304
+1746018833.3923018,-2.436506095069578,-1.8189826931121282,-1.7922550730254687,-1.69883697781073,1.8041925622855022,-1.7685088884430336
+1746018833.4261613,-2.427513837309443,-1.8047996996958646,-1.8108623398073533,-1.6926256394278254,1.8092464657898573,-1.7608596362219362
+1746018833.460008,-2.418311398988563,-1.790622001314058,-1.8292538286354278,-1.6865395678191908,1.8144045809167348,-1.7530121682137474
+1746018833.4938514,-2.40889222435877,-1.7764456907543673,-1.8474335519925673,-1.6805741150850988,1.8196692369448968,-1.744959073357284
+1746018833.5277004,-2.39924953335109,-1.7622670225665948,-1.8654052187245902,-1.6747245033006095,1.8250427686213335,-1.7366925943552969
+1746018833.5615637,-2.3893763158437915,-1.748082412163935,-1.8831722525006964,-1.6689857896451026,1.8305275084148604,-1.7282046097420172
+1746018833.5954494,-2.379265326186706,-1.7338884365990461,-1.9007378089298923,-1.6633528301549456,1.836125777604341,-1.7194866151109052
+1746018833.6293652,-2.3689090780605793,-1.7196818370388856,-1.9181047915060365,-1.657820241864649,1.8418398760636912,-1.710529703498208
+1746018833.6633184,-2.3582998397621493,-1.7054595229808347,-1.9352758665365535,-1.6523823631059535,1.847672070591579,-1.7013245449271
+1746018833.6973162,-2.3474296300191164,-1.6912185782709448,-1.9522534771944489,-1.6470332117384563,1.8536245816183596,-1.6918613651287444
+1746018833.731365,-2.336290214454239,-1.6769562690034054,-1.9690398568195848,-1.6417664410902078,1.859699568106305,-1.6821299234710119
+1746018833.7654707,-2.3248731028346676,-1.6626700533987864,-1.9856370415828184,-1.6365752933929596,1.865899110441612,-1.6721194901433825
+1746018833.7996392,-2.313169547261485,-1.6483575937774173,-2.0020468826151743,-1.6314525505051996,1.8722251910980745,-1.6618188226684314
--- a/test/joint_trajectories/trajectory_0004.csv
+++ b/test/joint_trajectories/trajectory_0004.csv
@@ -0,0 +1,50 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018833.9629507,-2.324632188594224,-1.6623693271971933,-1.9859861625555153,-1.6364657863266083,1.8660316432600403,-1.6719081889804013
+1746018833.9963675,-2.335819326188092,-1.6763668965146246,-1.969728275639677,-1.6415486123374243,1.8599558668257767,-1.6817164597772025
+1746018834.0297241,-2.3467455046022967,-1.6903392961676456,-1.953294110027188,-1.6467039221617474,1.8539985059979607,-1.691263417867352
+1746018834.0630317,-2.357412817347484,-1.704291098667895,-1.936676533512226,-1.6519378382544172,1.8481587881339312,-1.7005522701576399
+1746018834.0962958,-2.3678290446426877,-1.7182247925608825,-1.919873497348345,-1.6572564088159167,1.8424347071049256,-1.7095927192408489
+1746018834.1295235,-2.378001712454484,-1.7321431048843847,-1.9028827236760615,-1.6626652863274527,1.8368241794365847,-1.7183939941154158
+1746018834.1627212,-2.387938116128487,-1.7460489230352487,-1.8857017302114587,-1.6681698249102745,1.8313250780470778,-1.7269649122867352
+1746018834.1958961,-2.3976453244595373,-1.7599452899679866,-1.8683278179372114,-1.673775117375881,1.8259352440123369,-1.7353138992711519
+1746018834.2290564,-2.4071301838887775,-1.7738354001770889,-1.8507580577374492,-1.679486031287465,1.8206524972781475,-1.7434490071506987
+1746018834.2622097,-2.4163993230949226,-1.7877225972294317,-1.8329892761616438,-1.685307243356319,1.8154746460647377,-1.7513779323942549
+1746018834.2953644,-2.425459157893222,-1.8016103728025705,-1.815018040177023,-1.6912432723799111,1.8103994950986984,-1.7591080329316544
+1746018834.328529,-2.4343158963723455,-1.815502367205395,-1.7968406407629243,-1.697298510924176,1.8054248527936752,-1.7666463444835099
+1746018834.3617127,-2.442975544208546,-1.8294023713737457,-1.778453075183681,-1.7034772559564348,1.8005485374899468,-1.7739995961558246
+1746018834.394925,-2.4514439101046372,-1.843314330350339,-1.7598510277573318,-1.7097837386400043,1.7957683828525326,-1.7811742253134366
+1746018834.428176,-2.4597266113085543,-1.8572423482760718,-1.741029848914985,-1.716222153507498,1.7910822425179438,-1.7881763917501565
+1746018834.4614751,-2.4678290791726623,-1.8711906949387604,-1.7219845323194825,-1.7227966872375462,1.7864879940708889,-1.7950119911763038
+1746018834.4948335,-2.4757565647206334,-1.885163813946028,-1.7027096897813367,-1.7295115472695992,1.781983542424273,-1.801686668046411
+1746018834.528262,-2.48351414419369,-1.8991663326118116,-1.6831995236738697,-1.736370990504302,1.777566822668529,-1.8082058277512736
+1746018834.5617723,-2.4911067245523895,-1.9132030736714287,-1.6634477965070027,-1.743379352353094,1.7732358024496762,-1.8145746481994034
+1746018834.5953763,-2.4985390489139676,-1.9272790689689243,-1.6434477972689074,-1.7505410764210922,1.7689884839295225,-1.8207980908134098
+1746018834.6290865,-2.5058157019086287,-1.9413995752933328,-1.6231923040851692,-1.7578607451326926,1.7648229053759392,-1.82688091096694
+1746018834.662916,-2.5129411149411043,-1.9555700925785702,-1.6026735426741605,-1.765343111640808,1.7607371424262315,-1.8328276678876574
+1746018834.696879,-2.519919571346382,-1.9697963847260402,-1.5818831399926652,-1.772993133399497,1.7567293090621519,-1.8386427340513691
+1746018834.7309892,-2.52675521143073,-1.9840845033613033,-1.5608120723641956,-1.780816007827524,1.7527975583311033,-1.8443303040918742
+1746018834.7652624,-2.5334520373910787,-1.9984408148981199,-1.5394506072601954,-1.7888172105491613,1.7489400828444133,-1.849894403250444
+1746018834.7997391,-2.5400139181075128,-2.01287203135727,-1.5177882377564893,-1.797002536770738,1.745155115080336,-1.8553388953880987
+1746018834.8347127,-2.5464445938050573,-2.027385245476731,-1.4958136085077476,-1.8053781464403147,1.7414409275164555,-1.8606674905830092
+1746018834.870203,-2.5527476805821965,-2.041987970757935,-1.4735144318635378,-1.8139506139474644,1.7377958326135357,-1.8658837523345089
+1746018834.906231,-2.5589266748046233,-2.056688187224806,-1.4508773924805463,-1.8227269832556683,1.7342181826704879,-1.8709911043942884
+1746018834.9428196,-2.564984957363633,-2.0714943938345094,-1.4278880384536017,-1.8317148295281616,1.7307063695679723,-1.8759928372444565
+1746018834.979994,-2.570925797799328,-2.086415668679616,-1.4045306565761477,-1.8409223285179457,1.7272588244162428,-1.8808921142412387
+1746018835.0177815,-2.576752358289459,-2.1014617383715612,-1.3807881288261408,-1.8503583352558473,1.72387401712112,-1.885691977442181
+1746018835.0562117,-2.5824676975052707,-2.11664305830945,-1.3566417665261248,-1.860032473902345,1.7205504558804474,-1.8903953531338502
+1746018835.0953171,-2.588074774336134,-2.1319709059355443,-1.3320711178065574,-1.8699552410500637,1.7172866866219794,-1.8950050570761443
+1746018835.1351335,-2.5935764514851467,-2.147457489585476,-1.3070537429552438,-1.880138125302221,1.714081292392457,-1.8995237994784817
+1746018835.1757002,-2.598975498938122,-2.163116076192637,-1.2815649508896658,-1.8905937466458593,1.7109328927064817,-1.9039541897223107
+1746018835.2170606,-2.6042745973086343,-2.1789611419513553,-1.255577488241776,-1.9013360200397096,1.7078401428628558,-1.9082987408435736
+1746018835.2592626,-2.6094763410618986,-2.1950085511503414,-1.2290611702556746,-1.9123803488177538,1.7048017332351737,-1.9125598737879748
+1746018835.3023596,-2.614583241620377,-2.2112757698523144,-1.2019824396691283,-1.9237438550735724,1.7018163885426851,-1.9167399214511214
+1746018835.3464115,-2.619597730353988,-2.2277821230545776,-1.1743038356968118,-1.9354456562837374,1.698882867106792,-1.9208411325148411
+1746018835.391485,-2.624522161457751,-2.244549106616396,-1.1459833497468783,-1.9475072002625895,1.6959999600979527,-1.9248656750901965
+1746018835.4376554,-2.629358814719536,-2.2616007688731723,-1.1169736369810401,-1.9599526744268623,1.6931664907773376,-1.9288156401769119
+1746018835.485008,-2.6341098981803106,-2.2789641819084694,-1.0872210423732849,-1.9728095107512091,1.690381313737162,-1.9326930449480468
+1746018835.5336404,-2.6387775506888325,-2.29667002958243,-1.0566643851683533,-1.9861090154205274,1.6876433141434468,-1.9364998358677594
+1746018835.5836644,-2.6433638443520104,-2.3147533496405033,-1.025233424473071,-1.9998871631254325,1.6849514069848492,-1.9402378916487981
+1746018835.635209,-2.647870786881036,-2.333254482166248,-0.9928468977842484,-2.0141856119336783,1.6823045363314142,-1.9439090260547813
+1746018835.6884258,-2.6523003238315535,-2.352220298914748,-0.9594099781441661,-2.0290530185056923,1.6797016746076414,-1.9475149905501257
+1746018835.743492,-2.6566543407331253,-2.371705822022165,-0.9248109253040582,-2.044546769766889,1.6771418218854173,-1.9510574767971396
+1746018835.8006194,-2.660934665098117,-2.3917763936815626,-0.8889165963410672,-2.060735303978048,1.6746240052046204,-1.9545381189944557
--- a/test/joint_trajectories/trajectory_0005.csv
+++ b/test/joint_trajectories/trajectory_0005.csv
@@ -0,0 +1,49 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018835.987803,-2.656567969897686,-2.3712867418535835,-0.9255553455936596,-2.0442193002403903,1.677191601025152,-1.9509874110940175
+1746018836.043924,-2.652120862077586,-2.351424064744933,-0.9608172215999868,-2.0284312113477605,1.6798071801161178,-1.9473698671598543
+1746018836.0981538,-2.6475949702440356,-2.3320899380810047,-0.9948907871519141,-2.0132860702259343,1.6824665734844668,-1.9436853434845773
+1746018836.1506522,-2.642988279159434,-2.3132366947895355,-1.0278764922010586,-1.9987304370401513,1.6851719017560347,-1.9399327881368085
+1746018836.2015784,-2.638298735315631,-2.2948148854339245,-1.059874417079989,-1.9847129281712077,1.687924248021452,-1.936110350122784
+1746018836.251068,-2.6335241948589014,-2.27678224529799,-1.0909696618664633,-1.9711899177215764,1.6907247392681208,-1.932216091887539
+1746018836.299238,-2.6286624444814133,-2.25910212117193,-1.1212356609614633,-1.9581237957700226,1.6935745337534254,-1.9282480057032545
+1746018836.3461897,-2.623711198269035,-2.241742463858658,-1.1507362663155503,-1.9454818902536597,1.6964748221383763,-1.924204010085342
+1746018836.3920121,-2.6186680949383185,-2.2246750354050175,-1.1795273865911184,-1.9332356181869748,1.6994268283317515,-1.9200819464537484
+1746018836.4367838,-2.6135306949506067,-2.2078747824687426,-1.2076582824800977,-1.921359814465153,1.7024318103456428,-1.9158795756110172
+1746018836.4805741,-2.6082964775157835,-2.191319335143777,-1.235172602413144,-1.9098321946797632,1.7054910611489644,-1.9115945740398341
+1746018836.5234451,-2.602962837480355,-2.1749886019263878,-1.262109219354908,-1.8986329205545092,1.7086059095158914,-1.907224530007793
+1746018836.565452,-2.597527082095311,-2.158864439255521,-1.2885029133352544,-1.887744244903236,1.71177772086532,-1.9027669394671456
+1746018836.6066446,-2.591986427659406,-2.142930379547983,-1.3143849330085464,-1.8771502188832048,1.7150078980867605,-1.8982192017368302
+1746018836.6470675,-2.586337996033707,-2.1271714055894106,-1.3397834613776034,-1.8668364485315452,1.7182978823473665,-1.8935786149534906
+1746018836.6867616,-2.5805788110234267,-2.1115737620113757,-1.3647240048810532,-1.8567898906408586,1.7216491538739778,-1.888842371277539
+1746018836.7257638,-2.5747057946232905,-2.0961247967011114,-1.3892297206643045,-1.8469986802916203,1.7250632327031976,-1.8840075518395907
+1746018836.7641072,-2.568715763122914,-2.080812826569625,-1.4133216935874768,-1.8374519840458203,1.728541679391494,-1.8790711214117943
+1746018836.801823,-2.5626054230689332,-2.0656270232955447,-1.4370191720594814,-1.8281398740778985,1.7320860956762694,-1.874029922787756
+1746018836.838939,-2.5563713670810064,-2.0505573155703636,-1.4603397699098797,-1.8190532194872113,1.7356981250775654,-1.8688806708538452
+1746018836.8754807,-2.5500100695191747,-2.035594305069767,-1.4832996400660297,-1.810183591780205,1.7393794534287235,-1.8636199463337475
+1746018836.9114718,-2.5435178820005877,-2.020729193918573,-1.5059136246824552,-1.8015231820869366,1.7431318093227937,-1.8582441891871508
+1746018836.9469345,-2.536891028764185,-2.005953721841944,-1.5281953854927215,-1.7930647281267673,1.7469569644597502,-1.8527496916424473
+1746018836.982013,-2.5301256018826224,-1.9912601115311623,-1.5501575174630116,-1.7848014492920683,1.75085673387769,-1.8471325908422993
+1746018837.0169134,-2.523217556321584,-1.9766410210193348,-1.5718116482777529,-1.7767269884990602,1.7548329760490322,-1.8413888610798816
+1746018837.0516527,-2.5161627048476034,-1.9620895020763978,-1.5931685257489863,-1.7688353596779463,1.7588875928203578,-1.835514305602535
+1746018837.0862463,-2.5089567127866803,-1.9475989638056517,-1.6142380948881407,-1.7611208999530463,1.7630225291718924,-1.829504547958538
+1746018837.1207092,-2.5015950926373596,-1.9331631407645378,-1.6350295660932637,-1.7535782257070403,1.7672397727696323,-1.8233550228616648
+1746018837.1550558,-2.494073198543518,-1.918776065047465,-1.655551475672208,-1.7462021918391264,1.7715413532798507,-1.8170609665472082
+1746018837.1892996,-2.4863862206339724,-1.904432041863381,-1.6758117397321064,-1.7389878536203982,1.7759293414120219,-1.8106174065922302
+1746018837.2234538,-2.478529179238171,-1.8901256282196597,-1.6958177023090106,-1.7319304306255023,1.780405847652105,-1.8040191511719863
+1746018837.2575305,-2.470496918989714,-1.8758516143898851,-1.7155761784824244,-1.7250252722810013,1.784973020643612,-1.7972607777237521
+1746018837.2915416,-2.462284102832323,-1.86160500789886,-1.7350934931122435,-1.7182678246205565,1.7896330451688005,-1.790336620988805
+1746018837.3254988,-2.4538852059461935,-1.8473810198056249,-1.754375515746359,-1.7116535978772094,1.7943881396767472,-1.7832407604030007
+1746018837.3594131,-2.445294509616421,-1.8331750531060622,-1.7734276921725305,-1.7051781345753345,1.7992405532988585,-1.7759670068064304
+1746018837.3932948,-2.436506095069578,-1.8189826931121282,-1.7922550730254687,-1.69883697781073,1.8041925622855022,-1.7685088884430336
+1746018837.4271543,-2.427513837309443,-1.8047996996958646,-1.8108623398073533,-1.6926256394278254,1.8092464657898573,-1.7608596362219362
+1746018837.4610012,-2.418311398988563,-1.790622001314058,-1.8292538286354278,-1.6865395678191908,1.8144045809167348,-1.7530121682137474
+1746018837.4948444,-2.40889222435877,-1.7764456907543673,-1.8474335519925673,-1.6805741150850988,1.8196692369448968,-1.744959073357284
+1746018837.5286937,-2.39924953335109,-1.7622670225665948,-1.8654052187245902,-1.6747245033006095,1.8250427686213335,-1.7366925943552969
+1746018837.562557,-2.3893763158437915,-1.748082412163935,-1.8831722525006964,-1.6689857896451026,1.8305275084148604,-1.7282046097420172
+1746018837.5964425,-2.379265326186706,-1.7338884365990461,-1.9007378089298923,-1.6633528301549456,1.836125777604341,-1.7194866151109052
+1746018837.6303582,-2.3689090780605793,-1.7196818370388856,-1.9181047915060365,-1.657820241864649,1.8418398760636912,-1.710529703498208
+1746018837.6643114,-2.3582998397621493,-1.7054595229808347,-1.9352758665365535,-1.6523823631059535,1.847672070591579,-1.7013245449271
+1746018837.6983092,-2.3474296300191164,-1.6912185782709448,-1.9522534771944489,-1.6470332117384563,1.8536245816183596,-1.6918613651287444
+1746018837.732358,-2.336290214454239,-1.6769562690034054,-1.9690398568195848,-1.6417664410902078,1.859699568106305,-1.6821299234710119
+1746018837.7664638,-2.3248731028346676,-1.6626700533987864,-1.9856370415828184,-1.6365752933929596,1.865899110441612,-1.6721194901433825
+1746018837.8006322,-2.313169547261485,-1.6483575937774173,-2.0020468826151743,-1.6314525505051996,1.8722251910980745,-1.6618188226684314
--- a/test/joint_trajectories/trajectory_0006.csv
+++ b/test/joint_trajectories/trajectory_0006.csv
@@ -0,0 +1,50 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018837.9624937,-2.3246320499756634,-1.6623690828935132,-1.985985865117908,-1.6364665759463006,1.8660315409898365,-1.6719080440240734
+1746018837.9959111,-2.3358193278856954,-1.6763668981739894,-1.969728278761401,-1.6415486065242635,1.8599558635667384,-1.6817164593894947
+1746018838.0292678,-2.346745504588685,-1.6903392961610908,-1.9532941100077603,-1.6467039221533954,1.8539985060146833,-1.6912634178526196
+1746018838.062575,-2.3574128173475404,-1.7042910986678523,-1.9366765335123646,-1.6519378382545549,1.8481587881342065,-1.7005522701579419
+1746018838.0958395,-2.3678290446426877,-1.7182247925608836,-1.919873497348344,-1.6572564088159185,1.842434707104923,-1.7095927192408484
+1746018838.129067,-2.378001712454484,-1.732143104884385,-1.902882723676061,-1.662665286327453,1.8368241794365847,-1.718393994115416
+1746018838.1622646,-2.387938116128487,-1.7460489230352492,-1.8857017302114585,-1.6681698249102745,1.8313250780470778,-1.7269649122867352
+1746018838.1954398,-2.3976453244595373,-1.7599452899679866,-1.8683278179372116,-1.6737751173758808,1.8259352440123369,-1.735313899271152
+1746018838.2286,-2.4071301838887775,-1.7738354001770886,-1.8507580577374494,-1.679486031287465,1.8206524972781475,-1.7434490071506985
+1746018838.261753,-2.4163993230949226,-1.787722597229432,-1.8329892761616438,-1.685307243356319,1.8154746460647377,-1.7513779323942549
+1746018838.2949078,-2.425459157893222,-1.8016103728025703,-1.8150180401770228,-1.6912432723799113,1.8103994950986984,-1.7591080329316544
+1746018838.3280725,-2.4343158963723455,-1.8155023672053947,-1.7968406407629245,-1.697298510924176,1.8054248527936752,-1.7666463444835099
+1746018838.3612564,-2.442975544208546,-1.8294023713737457,-1.7784530751836811,-1.7034772559564346,1.8005485374899468,-1.7739995961558246
+1746018838.3944685,-2.4514439101046372,-1.8433143303503392,-1.759851027757332,-1.7097837386400039,1.7957683828525326,-1.7811742253134366
+1746018838.4277194,-2.4597266113085543,-1.8572423482760716,-1.7410298489149851,-1.716222153507498,1.7910822425179438,-1.7881763917501565
+1746018838.4610186,-2.4678290791726623,-1.8711906949387604,-1.7219845323194827,-1.722796687237546,1.7864879940708889,-1.7950119911763036
+1746018838.494377,-2.4757565647206334,-1.8851638139460283,-1.7027096897813365,-1.7295115472695992,1.781983542424273,-1.801686668046411
+1746018838.5278056,-2.48351414419369,-1.8991663326118113,-1.68319952367387,-1.736370990504302,1.777566822668529,-1.8082058277512736
+1746018838.5613158,-2.4911067245523895,-1.9132030736714287,-1.6634477965070025,-1.7433793523530943,1.7732358024496762,-1.8145746481994034
+1746018838.5949197,-2.4985390489139676,-1.9272790689689243,-1.6434477972689074,-1.7505410764210922,1.7689884839295225,-1.82079809081341
+1746018838.62863,-2.5058157019086287,-1.9413995752933326,-1.6231923040851692,-1.7578607451326924,1.76482290537594,-1.82688091096694
+1746018838.6624596,-2.5129411149411043,-1.9555700925785702,-1.6026735426741605,-1.765343111640808,1.7607371424262315,-1.8328276678876576
+1746018838.6964223,-2.519919571346382,-1.9697963847260402,-1.5818831399926652,-1.7729931333994968,1.7567293090621519,-1.8386427340513691
+1746018838.7305326,-2.52675521143073,-1.9840845033613033,-1.5608120723641956,-1.7808160078275237,1.7527975583311033,-1.844330304091874
+1746018838.7648058,-2.5334520373910783,-1.9984408148981199,-1.5394506072601954,-1.7888172105491613,1.7489400828444133,-1.849894403250444
+1746018838.7992826,-2.5400139181075128,-2.01287203135727,-1.5177882377564893,-1.797002536770738,1.745155115080336,-1.8553388953880987
+1746018838.8342564,-2.5464445938050573,-2.027385245476731,-1.4958136085077476,-1.8053781464403147,1.7414409275164555,-1.8606674905830092
+1746018838.8697464,-2.5527476805821965,-2.041987970757935,-1.4735144318635378,-1.8139506139474644,1.7377958326135357,-1.8658837523345089
+1746018838.9057746,-2.5589266748046233,-2.056688187224806,-1.4508773924805463,-1.8227269832556683,1.7342181826704879,-1.8709911043942884
+1746018838.9423633,-2.564984957363633,-2.0714943938345094,-1.4278880384536017,-1.8317148295281616,1.7307063695679723,-1.8759928372444565
+1746018838.9795377,-2.570925797799328,-2.086415668679616,-1.4045306565761477,-1.8409223285179457,1.7272588244162428,-1.8808921142412387
+1746018839.017325,-2.576752358289459,-2.1014617383715612,-1.3807881288261408,-1.8503583352558473,1.72387401712112,-1.885691977442181
+1746018839.0557551,-2.5824676975052707,-2.11664305830945,-1.3566417665261248,-1.860032473902345,1.7205504558804474,-1.8903953531338502
+1746018839.0948606,-2.588074774336134,-2.1319709059355443,-1.3320711178065574,-1.8699552410500637,1.7172866866219794,-1.8950050570761443
+1746018839.134677,-2.5935764514851467,-2.147457489585476,-1.3070537429552438,-1.880138125302221,1.714081292392457,-1.8995237994784817
+1746018839.1752436,-2.598975498938122,-2.163116076192637,-1.2815649508896658,-1.8905937466458593,1.7109328927064817,-1.9039541897223107
+1746018839.216604,-2.6042745973086343,-2.1789611419513553,-1.255577488241776,-1.9013360200397096,1.7078401428628558,-1.9082987408435736
+1746018839.258806,-2.6094763410618986,-2.1950085511503414,-1.2290611702556746,-1.9123803488177538,1.7048017332351737,-1.9125598737879748
+1746018839.3019032,-2.614583241620377,-2.2112757698523144,-1.2019824396691283,-1.9237438550735724,1.7018163885426851,-1.9167399214511214
+1746018839.345955,-2.619597730353988,-2.2277821230545776,-1.1743038356968118,-1.9354456562837374,1.698882867106792,-1.9208411325148411
+1746018839.3910284,-2.624522161457751,-2.244549106616396,-1.1459833497468783,-1.9475072002625895,1.6959999600979527,-1.9248656750901965
+1746018839.4371989,-2.629358814719536,-2.2616007688731723,-1.1169736369810401,-1.9599526744268623,1.6931664907773376,-1.9288156401769119
+1746018839.4845517,-2.6341098981803106,-2.2789641819084694,-1.0872210423732849,-1.9728095107512091,1.690381313737162,-1.9326930449480468
+1746018839.533184,-2.6387775506888325,-2.29667002958243,-1.0566643851683533,-1.9861090154205274,1.6876433141434468,-1.9364998358677594
+1746018839.5832078,-2.6433638443520104,-2.3147533496405033,-1.025233424473071,-1.9998871631254325,1.6849514069848492,-1.9402378916487981
+1746018839.6347528,-2.647870786881036,-2.333254482166248,-0.9928468977842484,-2.0141856119336783,1.6823045363314142,-1.9439090260547813
+1746018839.6879692,-2.6523003238315535,-2.352220298914748,-0.9594099781441661,-2.0290530185056923,1.6797016746076414,-1.9475149905501257
+1746018839.7430353,-2.6566543407331253,-2.371705822022165,-0.9248109253040582,-2.044546769766889,1.6771418218854173,-1.9510574767971396
+1746018839.8001628,-2.660934665098117,-2.3917763936815626,-0.8889165963410672,-2.060735303978048,1.6746240052046204,-1.9545381189944557
--- a/test/joint_trajectories/trajectory_0007.csv
+++ b/test/joint_trajectories/trajectory_0007.csv
@@ -0,0 +1,49 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018839.9870331,-2.6565675913485474,-2.3712857992447525,-0.9255559078539157,-2.0442195724473806,1.677191409598639,-1.9509867943370052
+1746018840.043153,-2.652120857280843,-2.3514240719614046,-0.96081719414931,-2.0284312315864717,1.6798071825274352,-1.9473698629721563
+1746018840.0973828,-2.6475949702273307,-2.3320899377081545,-0.9948907878594264,-2.0132860698846207,1.6824665734944126,-1.9436853434702956
+1746018840.1498814,-2.642988279158342,-2.313236694798178,-1.0278764921815773,-1.9987304370507137,1.6851719017566502,-1.9399327881358737
+1746018840.2008076,-2.638298735315647,-2.294814885433705,-1.0598744170804624,-1.984712928170956,1.6879242480214423,-1.9361103501227963
+1746018840.250297,-2.633524194858901,-2.2767822452979947,-1.0909696618664526,-1.9711899177215821,1.6907247392681217,-1.9322160918875388
+1746018840.298467,-2.6286624444814133,-2.2591021211719298,-1.1212356609614642,-1.9581237957700224,1.6935745337534254,-1.9282480057032545
+1746018840.3454187,-2.623711198269035,-2.2417424638586576,-1.1507362663155507,-1.9454818902536595,1.6964748221383763,-1.924204010085342
+1746018840.391241,-2.618668094938318,-2.2246750354050175,-1.1795273865911182,-1.9332356181869748,1.6994268283317515,-1.9200819464537484
+1746018840.436013,-2.6135306949506067,-2.2078747824687426,-1.2076582824800979,-1.921359814465153,1.7024318103456428,-1.9158795756110172
+1746018840.4798033,-2.6082964775157835,-2.1913193351437776,-1.2351726024131435,-1.9098321946797634,1.7054910611489644,-1.9115945740398341
+1746018840.5226743,-2.602962837480355,-2.1749886019263873,-1.2621092193549082,-1.8986329205545092,1.7086059095158914,-1.907224530007793
+1746018840.564681,-2.597527082095311,-2.158864439255521,-1.288502913335254,-1.8877442449032362,1.711777720865319,-1.9027669394671458
+1746018840.6058736,-2.591986427659406,-2.142930379547983,-1.3143849330085464,-1.8771502188832048,1.7150078980867605,-1.8982192017368305
+1746018840.6462967,-2.586337996033707,-2.1271714055894106,-1.3397834613776034,-1.8668364485315452,1.7182978823473665,-1.8935786149534903
+1746018840.6859908,-2.5805788110234267,-2.1115737620113757,-1.3647240048810532,-1.8567898906408586,1.7216491538739778,-1.888842371277539
+1746018840.7249928,-2.5747057946232905,-2.0961247967011114,-1.3892297206643045,-1.8469986802916203,1.7250632327031976,-1.8840075518395907
+1746018840.7633364,-2.568715763122914,-2.080812826569625,-1.4133216935874768,-1.8374519840458203,1.728541679391494,-1.8790711214117943
+1746018840.801052,-2.5626054230689332,-2.0656270232955447,-1.4370191720594814,-1.8281398740778985,1.7320860956762694,-1.874029922787756
+1746018840.838168,-2.5563713670810064,-2.0505573155703636,-1.4603397699098797,-1.8190532194872113,1.7356981250775654,-1.8688806708538452
+1746018840.8747098,-2.5500100695191747,-2.035594305069767,-1.4832996400660297,-1.810183591780205,1.7393794534287235,-1.8636199463337475
+1746018840.910701,-2.5435178820005877,-2.020729193918573,-1.5059136246824552,-1.8015231820869366,1.7431318093227937,-1.8582441891871508
+1746018840.9461634,-2.536891028764185,-2.005953721841944,-1.5281953854927215,-1.7930647281267673,1.7469569644597502,-1.8527496916424473
+1746018840.981242,-2.5301256018826224,-1.9912601115311623,-1.5501575174630116,-1.7848014492920683,1.75085673387769,-1.8471325908422993
+1746018841.0161424,-2.523217556321584,-1.9766410210193348,-1.5718116482777529,-1.7767269884990602,1.7548329760490322,-1.8413888610798816
+1746018841.0508816,-2.5161627048476034,-1.9620895020763978,-1.5931685257489863,-1.7688353596779463,1.7588875928203578,-1.835514305602535
+1746018841.0854752,-2.5089567127866803,-1.9475989638056517,-1.6142380948881407,-1.7611208999530463,1.7630225291718924,-1.829504547958538
+1746018841.1199381,-2.5015950926373596,-1.9331631407645378,-1.6350295660932637,-1.7535782257070403,1.7672397727696323,-1.8233550228616648
+1746018841.154285,-2.494073198543518,-1.918776065047465,-1.655551475672208,-1.7462021918391264,1.7715413532798507,-1.8170609665472082
+1746018841.1885288,-2.4863862206339724,-1.904432041863381,-1.6758117397321064,-1.7389878536203982,1.7759293414120219,-1.8106174065922302
+1746018841.2226827,-2.478529179238171,-1.8901256282196597,-1.6958177023090106,-1.7319304306255023,1.780405847652105,-1.8040191511719863
+1746018841.2567594,-2.470496918989714,-1.8758516143898851,-1.7155761784824244,-1.7250252722810013,1.784973020643612,-1.7972607777237521
+1746018841.2907708,-2.462284102832323,-1.86160500789886,-1.7350934931122435,-1.7182678246205565,1.7896330451688005,-1.790336620988805
+1746018841.324728,-2.4538852059461935,-1.8473810198056249,-1.754375515746359,-1.7116535978772094,1.7943881396767472,-1.7832407604030007
+1746018841.358642,-2.445294509616421,-1.8331750531060622,-1.7734276921725305,-1.7051781345753345,1.7992405532988585,-1.7759670068064304
+1746018841.392524,-2.436506095069578,-1.8189826931121282,-1.7922550730254687,-1.69883697781073,1.8041925622855022,-1.7685088884430336
+1746018841.4263835,-2.427513837309443,-1.8047996996958646,-1.8108623398073533,-1.6926256394278254,1.8092464657898573,-1.7608596362219362
+1746018841.46023,-2.418311398988563,-1.790622001314058,-1.8292538286354278,-1.6865395678191908,1.8144045809167348,-1.7530121682137474
+1746018841.4940736,-2.40889222435877,-1.7764456907543673,-1.8474335519925673,-1.6805741150850988,1.8196692369448968,-1.744959073357284
+1746018841.5279226,-2.39924953335109,-1.7622670225665948,-1.8654052187245902,-1.6747245033006095,1.8250427686213335,-1.7366925943552969
+1746018841.561786,-2.3893763158437915,-1.748082412163935,-1.8831722525006964,-1.6689857896451026,1.8305275084148604,-1.7282046097420172
+1746018841.5956717,-2.379265326186706,-1.7338884365990461,-1.9007378089298923,-1.6633528301549456,1.836125777604341,-1.7194866151109052
+1746018841.6295874,-2.3689090780605793,-1.7196818370388856,-1.9181047915060365,-1.657820241864649,1.8418398760636912,-1.710529703498208
+1746018841.6635406,-2.3582998397621493,-1.7054595229808347,-1.9352758665365535,-1.6523823631059535,1.847672070591579,-1.7013245449271
+1746018841.6975384,-2.3474296300191164,-1.6912185782709448,-1.9522534771944489,-1.6470332117384563,1.8536245816183596,-1.6918613651287444
+1746018841.7315872,-2.336290214454239,-1.6769562690034054,-1.9690398568195848,-1.6417664410902078,1.859699568106305,-1.6821299234710119
+1746018841.765693,-2.3248731028346676,-1.6626700533987864,-1.9856370415828184,-1.6365752933929596,1.865899110441612,-1.6721194901433825
+1746018841.7998614,-2.313169547261485,-1.6483575937774173,-2.0020468826151743,-1.6314525505051996,1.8722251910980745,-1.6618188226684314
--- a/test/joint_trajectories/trajectory_0008.csv
+++ b/test/joint_trajectories/trajectory_0008.csv
@@ -0,0 +1,49 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018842.5402875,-2.324867488892618,-1.6626638643050697,-1.9856481674287725,-1.6365686872386849,1.8659028757307166,-1.6721139169687684
+1746018842.5743926,-2.336285336847922,-1.6769497403275242,-1.9690513540169463,-1.6417602831704612,1.8597028749042845,-1.6821250658227473
+1746018842.6084414,-2.34742500994231,-1.6912121029455849,-1.952265112023755,-1.6470268222984992,1.8536277184471475,-1.6918567062637508
+1746018842.6424391,-2.3582954583576528,-1.70545307936971,-1.9352876507993915,-1.652375753968947,1.8476750537315745,-1.7013200743219217
+1746018842.6763926,-2.368904917330334,-1.719675412415731,-1.918116736112199,-1.6578134131794136,1.8418427163257363,-1.7105254071905907
+1746018842.7103083,-2.3792613695392113,-1.7338820187088817,-1.9007499248090844,-1.6633457817890964,1.836128485052602,-1.7194824803340931
+1746018842.7441938,-2.389372548011029,-1.7480759892384194,-1.8831845507077034,-1.6689785210597952,1.8305300923665309,-1.7282006248243642
+1746018842.778057,-2.399245940270436,-1.7622605832544262,-1.8654177104972995,-1.6747170135130687,1.8250452377037503,-1.7366887486048386
+1746018842.811906,-2.408888793065886,-1.776439224055793,-1.8474462488098538,-1.680566402634909,1.8196715991491086,-1.7449553569600713
+1746018842.8457494,-2.418308117519668,-1.7906154965185248,-1.8292667422745,-1.68653163073772,1.814406843647519,-1.7530085721433006
+1746018842.879596,-2.4275106946121183,-1.804793146322379,-1.8108754824011035,-1.6926174752096972,1.8092486359123248,-1.7608561521584685
+1746018842.9134552,-2.4365030809213355,-1.8189760808518962,-1.7922684571203384,-1.6988285833864205,1.804194646168435,-1.7685055087012589
+1746018842.947337,-2.445291614550306,-1.83316837176703,-1.773441330787354,-1.705169506284085,1.7992425568549697,-1.7759637242709054
+1746018842.981251,-2.453882421182761,-1.8473742592585363,-1.7543894224326417,-1.7116447314395444,1.794390068400034,-1.7832375684700812
+1746018843.015208,-2.462281420217367,-1.8615981580244894,-1.7351076820162925,-1.7182587151097757,1.7896349041691835,-1.7903335135143934
+1746018843.0492191,-2.4704943309371865,-1.875844665027125,-1.715590664409718,-1.7250159140930839,1.7849748146790532,-1.7972577489761261
+1746018843.0832956,-2.47852667867777,-1.8901185691141786,-1.695832500791148,-1.731920817446927,1.7804075811583937,-1.80401619578906
+1746018843.1174493,-2.486383800962901,-1.904424862616587,-1.6758268670966427,-1.7389779783934214,1.77593101853041,-1.8106145195426782
+1746018843.1516929,-2.4940708535819773,-1.9187687550655812,-1.6555669491155875,-1.746192046724132,1.771542977882727,-1.8170581430949386
+1746018843.186039,-2.5015928165873476,-1.9331556892076327,-1.6350454037569808,-1.75356780204161,1.767241348484406,-1.8233522585331812
+1746018843.2205017,-2.5089545001936986,-1.947591359536563,-1.6142543159381102,-1.7611101882074818,1.7630240594032731,-1.8295018385127595
+1746018843.255095,-2.5161605505649063,-1.9620817336095804,-1.5931851504579162,-1.7688243494070508,1.7588890807711808,-1.8355116470026929
+1746018843.2898338,-2.5232154554766004,-1.976633076469821,-1.5718286981100367,-1.7767156682900942,1.7548344247398155,-1.8413862494671194
+1746018843.3247337,-2.530123549845209,-1.9912519785640157,-1.550175015161265,-1.7847898067089647,1.7508581461650792,-1.8471300225106297
+1746018843.3598118,-2.536889021116365,-2.0059453876228623,-1.528213355164565,-1.7930527496508992,1.7469583430540085,-1.8527471630147285
+1746018843.3952734,-2.543515914507428,-2.020720645066686,-1.5059320918853654,-1.8015108530550576,1.7431331568045234,-1.8582416967917432
+1746018843.431264,-2.55000813810045,-2.0355855276141934,-1.4833186318870422,-1.8101708963206287,1.7393807722649646,-1.8636174867815025
+1746018843.4678047,-2.5563694677832736,-2.0505482949128515,-1.460359315035404,-1.8190401404559557,1.7356994176374743,-1.8688782408150693
+1746018843.5049198,-2.5626035520376185,-2.0656177441825485,-1.437039300828411,-1.8281263929974354,1.73208736424656,-1.8740275189687523
+1746018843.5426345,-2.5687139165739636,-2.0808032730786503,-1.4133424380177668,-1.8374380810504354,1.728542926191846,-1.8790687405305482
+1746018843.580977,-2.574703968813886,-2.0961149522482367,-1.389251114438977,-1.8469843340853005,1.7250644599018843,-1.8840051906001005
+1746018843.619978,-2.5805770022211805,-2.1115636093898282,-1.3647460832735314,-1.8567750784764343,1.7216503636039722,-1.8888400263422143
+1746018843.6596708,-2.586336200483651,-2.127160927053329,-1.3398062610875832,-1.8668211462238793,1.718299076723243,-1.8935762829129181
+1746018843.7000928,-2.59198464154793,-2.14291955696164,-1.3144084918670436,-1.8771344008751694,1.715009079222785,-1.8982168790760696
+1746018843.741284,-2.597525301510031,-2.15885325433725,-1.2885272698190924,-1.887727884411981,1.711778890895176,-1.9027646225275003
+1746018843.7832894,-2.6029610583646225,-2.1749770366296417,-1.2621344118238726,-1.898615989838548,1.7086070706146312,-1.907222214942744
+1746018843.826159,-2.608294695616196,-2.1913073722373317,-1.2351986679427869,-1.9098146654869097,1.7054922155579035,-1.9115922567634516
+1746018843.869948,-2.613528905755412,-2.2078624064075445,-1.2076852551025856,-1.921341658750917,1.7024329604011337,-1.9158772517366682
+1746018843.9147182,-2.6186662936039466,-2.224662233628094,-1.179555294664146,-1.9332168092383715,1.699427976499047,-1.9200796112202423
+1746018843.960539,-2.6237093795310957,-2.2417292287198323,-1.150765128596573,-1.9454624044238837,1.6964759710521848,-1.9242016582667052
+1746018844.0074894,-2.6286606025452244,-2.2590884528179984,-1.1212654807402322,-1.9581036151406634,1.6935756862672857,-1.9282456314970036
+1746018844.0556576,-2.6335223232628375,-2.276768155982648,-1.0910004181509518,-1.9711690341873827,1.6907258985155078,-1.9322136887744374
+1746018844.105146,-2.6382968267575286,-2.2948004060436342,-1.0599060512365686,-1.9846913497400405,1.6879254174928588,-1.9361079106879855
+1746018844.156071,-2.642986325290239,-2.3132218846188746,-1.027908887935713,-1.9987081973841945,1.6851730853871425,-1.9399303038527773
+1746018844.2085686,-2.647592960920918,-2.3320749078831513,-0.9949237227429526,-2.013263252386578,1.682467776055919,-1.9436828040336234
+1746018844.262797,-2.652118807999548,-2.351408754603147,-0.9608508897531411,-2.028407650512558,1.679808394220622,-1.9473672790949055
+1746018844.3189442,-2.6565658755308927,-2.3712814222056484,-0.9255726297006897,-2.0442018666333395,1.677193874683371,-1.9509855317761713
+1746018844.3772337,-2.660936109401195,-2.391761991854846,-0.8889481963044275,-2.0607182448089048,1.6746231815756811,-1.9545393022863669
--- a/test/joint_trajectories/trajectory_0009.csv
+++ b/test/joint_trajectories/trajectory_0009.csv
@@ -0,0 +1,42 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018844.4323194,-2.660877261862579,-2.3915612336261627,-0.8892745731176053,-2.060581756344425,1.6746560467702452,-1.954490040833097
+1746018844.5018663,-2.6557016253685237,-2.367151732073812,-0.9329722265719926,-2.0408663006710657,1.6777023299366034,-1.950284122335776
+1746018844.568479,-2.650338301479531,-2.3435216649692228,-0.9748262064865951,-2.022170034928338,1.6808551155733973,-1.9459197479035828
+1746018844.6324482,-2.6448595231275824,-2.3206302212549903,-1.0150192676341385,-2.00437125890477,1.6840735359108852,-1.9414581925452332
+1746018844.6940737,-2.6392626596402247,-2.2983849561821654,-1.053739712316787,-1.987373145463973,1.6873590182698432,-1.936897110448579
+1746018844.753599,-2.6335440482396093,-2.2767099849378747,-1.091140526454904,-1.971097246079571,1.6907134915419153,-1.9322331955150607
+1746018844.8112278,-2.627699877042689,-2.255541403189485,-1.1273497901716167,-1.9554779579368455,1.6941389515028558,-1.9274629912990284
+1746018844.8671327,-2.6217261835824344,-2.2348247780796133,-1.1624758691154253,-1.9404598376249336,1.6976374611766571,-1.9225828884404097
+1746018844.921461,-2.615618848050387,-2.2145132424777683,-1.1966113549198447,-1.925995560355681,1.7012111528542908,-1.917589116436357
+1746018844.9743397,-2.6093735860153804,-2.1945660523232307,-1.229836047098761,-1.9120443742740236,1.7048622302288887,-1.9124777347253512
+1746018845.0258791,-2.6029859408132183,-2.1749474761822234,-1.262219247527254,-1.898570909569995,1.7085929705191631,-1.9072446232255862
+1746018845.076176,-2.596451275585984,-2.15562592839206,-1.293821550991666,-1.8855442474723783,1.7124057265704336,-1.901885472280265
+1746018845.1253145,-2.589764764954471,-2.1365732837297275,-1.3246962603079382,-1.8729371826298127,1.7163029289176146,-1.896395771963805
+1746018845.1733701,-2.5829213863081004,-2.117764329375401,-1.3548905175963548,-1.8607256314646146,1.7202870877915686,-1.8907708007007797
+1746018845.2204094,-2.575915910697711,-2.0991763221294404,-1.3844462180779842,-1.8488881521187457,1.7243607950467474,-1.885005613146809
+1746018845.2664921,-2.568742893317628,-2.080788627324052,-1.41340075520735,-1.8374055506774118,1.728526725984139,-1.879095027277583
+1746018845.3116713,-2.5613966635646004,-2.062582421872802,-1.441787633541303,-1.8262605547651598,1.7327876410391179,-1.8730336106288294
+1746018845.3559947,-2.5538713146626058,-2.0445404482129246,-1.4696369768295674,-1.8154375402081122,1.7371463872986794,-1.8668156656263457
+1746018845.399506,-2.5461606928442797,-2.026646809036475,-1.496975952322328,-1.8049222998003287,1.7416058998068351,-1.8604352139413074
+1746018845.4422443,-2.5382583860818433,-2.008886795024565,-1.523829127504841,-1.794701845674274,1.746169202610286,-1.8538859798019078
+1746018845.4843569,-2.5301577123631076,-1.991246739530718,-1.5502187718989133,-1.7847642386079776,1.7508394094890196,-1.8471613721880802
+1746018845.5262134,-2.521851707511356,-1.9737138954678772,-1.5761651138787571,-1.7750984389791293,1.755619724307854,-1.8402544658315958
+1746018845.5678425,-2.5133331125519365,-1.9562763306531217,-1.6016865603977386,-1.76569417512119,1.7605134409151653,-1.8331579809393252
+1746018845.6092708,-2.504594360633246,-1.9389228386356796,-1.6267998859455726,-1.7565418256350511,1.7655239425038518,-1.8258642615529352
+1746018845.650524,-2.4956275635156593,-1.9216428626354147,-1.6515203958325457,-1.7476323128238214,1.770654700336853,-1.8183652524538716
+1746018845.691625,-2.4864244976490446,-1.9044264306929433,-1.6758620679411436,-1.7389570048929837,1.7759092717250482,-1.8106524745182977
+1746018845.732597,-2.4769765898679537,-1.887264100509744,-1.6998376763324217,-1.7305076249262603,1.7812912971288517,-1.8027169984228417
+1746018845.7734613,-2.4672749027436818,-1.8701469127601613,-1.7234588994971436,-1.7222761649335785,1.7868044962361145,-1.794549416598781
+1746018845.814238,-2.4573101196443767,-1.853066351904166,-1.74673641556505,-1.7142548034896372,1.7924526628476727,-1.7861398133299125
+1746018845.8549469,-2.447072529568564,-1.8360143137332672,-1.7696799864028452,-1.7064358256536563,1.7982396583778275,-1.7774777328881506
+1746018845.895606,-2.4365520118341872,-1.8189830790519497,-1.7922985322225422,-1.6988115439934046,1.8041694037498326,-1.7685521456012738
+1746018845.936233,-2.4257380207249533,-1.8019652930411643,-1.8146001980711162,-1.6913742196378094,1.8102458694357564,-1.7593514117497224
+1746018845.9768445,-2.4146195702188327,-1.7849539499747735,-1.836592413368029,-1.684115982358782,1.8164730633555406,-1.749863243194603
+1746018846.0174568,-2.403185218950566,-1.7679423830691017,-1.8582819454898074,-1.67702874873967,1.8228550163112764,-1.7400746626478572
+1746018846.0580845,-2.391423055591449,-1.7509242593437726,-1.8796749482630837,-1.6701041375293006,1.8293957645893064,-1.729971960508938
+1746018846.0987422,-2.3793206848662733,-1.7338935794618204,-1.900777006113524,-1.663333381310824,1.8360993293143677,-1.719540649211549
+1746018846.1394434,-2.3668652144696614,-1.71684468260119,-1.921593174523144,-1.656707233637037,1.8429696920862773,-1.7087654150505553
+1746018846.1802008,-2.35404324319303,-1.6997722564903195,-1.9421280173687696,-1.6502158708020003,1.8500107663703318,-1.6976300674949925
+1746018846.2210262,-2.3408408506298817,-1.6826713528192683,-1.9623856416465784,-1.6438487874362184,1.8572263640475164,-1.6861174860404806
+1746018846.2619305,-2.3272435888919,-1.665537408316354,-1.982369730028953,-1.6375946851332603,1.8646201564597407,-1.674209564716127
+1746018846.3029237,-2.3132364768425053,-1.6483662718597736,-2.0020835716477787,-1.631441353343968,1.8721956292088429,-1.6618871544406322
--- a/test/joint_trajectories/trajectory_0010.csv
+++ b/test/joint_trajectories/trajectory_0010.csv
@@ -0,0 +1,24 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018846.3560357,-2.3130085956938866,-1.6483233526243926,-2.001977206248431,-1.6314733113507236,1.8723087568514263,-1.6616594225035375
+1746018846.4305744,-2.3388358476021205,-1.6795460542007503,-1.9664368180404543,-1.6425485632303423,1.8583739130646464,-1.684427078831277
+1746018846.5047834,-2.362647087264697,-1.710630663521463,-1.929485668203533,-1.654140630995307,1.8453295054735044,-1.70515310496932
+1746018846.5787566,-2.38524072066141,-1.7416164876426439,-1.8916255347633935,-1.6661784706090492,1.832857910617216,-1.7246866713100308
+1746018846.6526036,-2.4066879165837656,-1.772549426654369,-1.8528149107566543,-1.6787097382051663,1.8209319155061339,-1.7431089883505848
+1746018846.7264087,-2.4270634867744083,-1.803464910887023,-1.8130208451561034,-1.6917873807028299,1.8095274743428993,-1.7605076413015845
+1746018846.8002665,-2.4464370770556814,-1.8344023750415324,-1.7722041374105049,-1.70546001075909,1.7986202070321387,-1.7769618250610493
+1746018846.8742797,-2.464873352229402,-1.8654049090770946,-1.7303183448807542,-1.7197740207063121,1.7881860302351003,-1.7925432489101956
+1746018846.9485602,-2.4824323075854995,-1.8965195152315921,-1.6873086821866494,-1.734775167169783,1.7782014140406837,-1.8073169303173995
+1746018847.023231,-2.499169576884385,-1.9277975484986474,-1.6431106682832322,-1.750510123619308,1.7686435631331445,-1.8213418982473633
+1746018847.0984266,-2.515136735413988,-1.9592953707943308,-1.5976484503980861,-1.7670280594852574,1.7594905384681407,-1.834671820492901
+1746018847.1742954,-2.530381592906062,-1.9910752586345577,-1.5508326974006406,-1.7843823234114629,1.7507213334694898,-1.8473555619151956
+1746018847.2511272,-2.544948473012585,-2.02320662620703,-1.502557914652279,-1.8026323258921155,1.7423159157986694,-1.8594376805662984
+1746018847.33048,-2.5588784773562345,-2.0557676569134933,-1.4526989717652803,-1.8218457450095547,1.7342552433420195,-1.8709588683013811
+1746018847.4125922,-2.572209733115841,-2.0888474824947716,-1.4011065426442688,-1.8421012240361587,1.7265212611556757,-1.881956341987868
+1746018847.4977489,-2.5849776237714557,-2.1225491192448254,-1.3476010141333377,-1.8634918013750936,1.7190968846095789,-1.8924641908525368
+1746018847.5862975,-2.597215003075061,-2.156993482156479,-1.2919641913282793,-1.8861294293305844,1.7119659728025542,-1.9025136849151718
+1746018847.6786654,-2.608952392569415,-2.1923249801243347,-1.2339277521284209,-1.9101511305001087,1.7051132954222599,-1.9121335488517688
+1746018847.7753854,-2.62021816305776,-2.2287195051033524,-1.1731567639384495,-1.935727669516739,1.6985244955594485,-1.9213502049969469
+1746018847.8771353,-2.631038700292097,-2.266396176996222,-1.1092254406221964,-1.9630762026428175,1.692186050547594,-1.9301879884749291
+1746018847.984796,-2.6414385546579795,-2.3056352264240374,-1.041580206816695,-1.9924794566165864,1.6860852327341735,-1.9386693364873135
+1746018848.0995455,-2.6514405733959023,-2.346806404631134,-0.9694809829908353,-2.0243161316700506,1.6802100723780216,-1.9468149521890472
+1746018848.2230148,-2.6610660107953903,-2.3904165342649235,-0.891902860265656,-2.0591117367270884,1.6745493261558808,-1.9546439402665394
--- a/test/joint_trajectories/trajectory_0011.csv
+++ b/test/joint_trajectories/trajectory_0011.csv
@@ -0,0 +1,18 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018848.2643304,-2.6609347487505555,-2.3904755025187687,-0.8916116138109151,-2.0593167577101767,1.6746293736765256,-1.9545390845923662
+1746018848.4278781,-2.6475531493072775,-2.332318534806963,-0.9943716968370002,-2.013565313574511,1.682490414580407,-1.9436497516464375
+1746018848.5865493,-2.6339045194406596,-2.2754722799061518,-1.0940677285933433,-1.969542426340629,1.6905077553408931,-1.932547644666357
+1746018848.726085,-2.6190663880240668,-2.2238026054586078,-1.181740610454354,-1.9320343919110845,1.6992022371674427,-1.9204314114830487
+1746018848.8568728,-2.6033934070997264,-2.174329564014844,-1.2639170623258666,-1.8976314190106425,1.7083642728307726,-1.9076037456955157
+1746018848.9800177,-2.5868032934933214,-2.1266751279832836,-1.341291313785015,-1.8659825536647,1.718038444620415,-1.8939904458004968
+1746018849.0967052,-2.5692198702647198,-2.080442419046562,-1.4146081969793198,-1.8367071108395678,1.7282627815289686,-1.8795199449258921
+1746018849.2078116,-2.5505574657807384,-2.0353250128181197,-1.4844184670314229,-1.8095222375768334,1.7390793755610483,-1.8641106993373677
+1746018849.3140135,-2.530721284543266,-1.9910746347012829,-1.5511470373904463,-1.7842063812936106,1.7505336738296329,-1.8476709341994808
+1746018849.4180765,-2.509606267481844,-1.9474848560071414,-1.615126653025983,-1.760580635828015,1.7626746293723121,-1.830097009816757
+1746018849.5209816,-2.487095896203111,-1.9043800875809487,-1.6766208845549073,-1.7384952286674875,1.7755547320918614,-1.8112715515031887
+1746018849.623092,-2.463060886353499,-1.8616081514239577,-1.7358400165117254,-1.717819218717114,1.789229913233929,-1.7910612483308577
+1746018849.7247272,-2.437357783499739,-1.819035280519161,-1.7929523367620606,-1.698431976491794,1.8037592566058942,-1.7693142652982765
+1746018849.8261704,-2.4098274864327496,-1.776542884785589,-1.8480923649767727,-1.680215488641479,1.819204424966241,-1.7458572061753823
+1746018849.9276726,-2.3802937443161665,-1.7340257146298896,-1.9013670084679408,-1.663046774310319,1.8356286737151244,-1.7204915624652455
+1746018850.029455,-2.348561706300859,-1.6913912489620513,-1.9528603051450268,-1.6467898270707944,1.8530952749049048,-1.6929895894622942
+1746018850.1317062,-2.3144166497039267,-1.648560279501294,-2.0026372117318076,-1.6312865552694753,1.8716651091213938,-1.6630895702386406
--- a/test/joint_trajectories/trajectory_0012.csv
+++ b/test/joint_trajectories/trajectory_0012.csv
@@ -0,0 +1,4 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018850.109841,-2.313011843897503,-1.6483278638043584,-2.001973134912872,-1.631473607524726,1.8723022286671673,-1.6616605676847982
+1746018850.9285998,-2.530057868362053,-1.9912887235928973,-1.5500285523322554,-1.7848794748435552,1.7508934604482302,-1.8470722114886884
+1746018851.9807262,-2.66090844815948,-2.391741808932408,-0.8889580877977425,-2.060722047953615,1.6746397246401257,-1.9545166867765127
--- a/test/joint_trajectories/trajectory_0013.csv
+++ b/test/joint_trajectories/trajectory_0013.csv
@@ -0,0 +1,6 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018851.5973852,-2.6608992184765543,-2.391989626851453,-0.8884322127188211,-2.0610011947501348,1.6746442142862108,-1.9545087957586498
+1746018852.1919017,-2.6029044107903507,-2.175002713505088,-1.2619779734894523,-1.8987579475592,1.7086796971987361,-1.9072056912717912
+1746018852.650539,-2.5300530074321905,-1.9911976499513728,-1.5501482326683962,-1.7848832159339922,1.750936604029861,-1.847101460421629
+1746018853.0620122,-2.4364126458090603,-1.8188401607040356,-1.792335579468164,-1.6989204345833993,1.8043023971112344,-1.7684754807829304
+1746018853.4696677,-2.313041265082593,-1.6480818588616595,-2.002234897954106,-1.6315915489852604,1.872411311265667,-1.661800398910474
--- a/test/joint_trajectories/trajectory_0014.csv
+++ b/test/joint_trajectories/trajectory_0014.csv
@@ -0,0 +1,5 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018853.1196609,-2.3130113520279294,-1.648325580267437,-2.0019753776066005,-1.6314736381361497,1.8723036414963827,-1.6616607582297658
+1746018853.6607533,-2.4699891120194684,-1.8749778101254129,-1.7165904026027747,-1.724776644488748,1.7853302453653583,-1.7968751622153594
+1746018854.222654,-2.58022921110272,-2.11034624955184,-1.3666577963427213,-1.8560060626683874,1.7218522810006531,-1.8885548842815478
+1746018854.9840958,-2.660900393826002,-2.3920882685349434,-0.8882298622663658,-2.061105495482815,1.674643586760336,-1.9545098252724087
--- a/test/joint_trajectories/trajectory_0015.csv
+++ b/test/joint_trajectories/trajectory_0015.csv
@@ -0,0 +1,6 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018854.5857203,-2.6608992364924906,-2.391998742211375,-0.8884133845660367,-2.0610109650372497,1.674644204602144,-1.9545088150235257
+1746018855.1802669,-2.6029044159431307,-2.1750027107661634,-1.261977980411899,-1.8987579468806866,1.7086796970381348,-1.9072056980080838
+1746018855.638904,-2.530053007429944,-1.9911976499443473,-1.5501482326775624,-1.784883215932142,1.7509366040327956,-1.8471014604205982
+1746018856.0503771,-2.4364126458090523,-1.8188401607040505,-1.7923355794681415,-1.698920434583396,1.804302397111229,-1.768475480782915
+1746018856.4580326,-2.313041265082593,-1.648081858861659,-2.0022348979541063,-1.6315915489852604,1.872411311265667,-1.6618003989104737
--- a/test/joint_trajectories/trajectory_0016.csv
+++ b/test/joint_trajectories/trajectory_0016.csv
@@ -0,0 +1,4 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018856.150761,-2.313009953566788,-1.6483215379860596,-2.0019795627446983,-1.631472905032882,1.8723066215453645,-1.6616604891281928
+1746018856.9695346,-2.5300578687393553,-1.9912887230961742,-1.5500285541171726,-1.7848794736403348,1.7508934602761297,-1.847072211839675
+1746018858.021661,-2.6609084481595286,-2.3917418089286335,-0.8889580878056473,-2.060722047949449,1.6746397246401052,-1.9545166867765533
--- a/test/joint_trajectories/trajectory_0017.csv
+++ b/test/joint_trajectories/trajectory_0017.csv
@@ -0,0 +1,7 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018857.5495617,-2.660899302230865,-2.392025404827116,-0.8883582428013344,-2.0610396015402404,1.6746441699032264,-1.9545088818412941
+1746018858.03965,-2.615528788576293,-2.214614992975414,-1.1962898699669853,-1.9262010237551856,1.7012759762388958,-1.9175228174280992
+1746018858.4300761,-2.561292881365791,-2.0626085021640415,-1.4416017879509189,-1.8264012161759362,1.7328581622598236,-1.8729535840622131
+1746018858.7666776,-2.4954979559493378,-1.921613228789207,-1.6514139228439317,-1.747741091546823,1.770741105680469,-1.8182630675361218
+1746018859.0931258,-2.414450811594105,-1.784870900410647,-1.8365413167359037,-1.6842092245652904,1.8165860531760938,-1.7497284587716226
+1746018859.419361,-2.3130102016373804,-1.6482177569989882,-2.0020828136446647,-1.6315268038763553,1.872352510197027,-1.6617054021835982
--- a/test/joint_trajectories/trajectory_0018.csv
+++ b/test/joint_trajectories/trajectory_0018.csv
@@ -0,0 +1,4 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018859.1601691,-2.313008228565413,-1.6483158278494643,-2.0019854696581354,-1.631472115415696,1.872310711194097,-1.6616604176314222
+1746018859.9789567,-2.5300578690813227,-1.9912887226431317,-1.5500285557418627,-1.7848794725457204,1.750893460120368,-1.847072212157987
+1746018861.0310829,-2.6609084481595735,-2.391741808925192,-0.8889580878128558,-2.06072204794565,1.6746397246400875,-1.9545166867765904
--- a/test/joint_trajectories/trajectory_0019.csv
+++ b/test/joint_trajectories/trajectory_0019.csv
@@ -0,0 +1,7 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018860.5458684,-2.6608993141666497,-2.3920293955061878,-0.8883499955898984,-2.0610438801437265,1.6746441636646425,-1.9545088935264285
+1746018861.03597,-2.615528787978891,-2.2146149908948014,-1.1962898729589728,-1.926201023315805,1.7012759778885602,-1.917522817898188
+1746018861.4263961,-2.561292881365768,-2.0626085021656806,-1.441601787948662,-1.8264012161758152,1.7328581622588528,-1.8729535840613691
+1746018861.7629974,-2.495497955949338,-1.9216132287892056,-1.6514139228439335,-1.7477410915468232,1.7707411056804698,-1.8182630675361227
+1746018862.0894456,-2.414450811594105,-1.784870900410647,-1.8365413167359037,-1.6842092245652904,1.8165860531760938,-1.7497284587716226
+1746018862.4156811,-2.3130102016373804,-1.6482177569989882,-2.0020828136446647,-1.6315268038763553,1.872352510197027,-1.6617054021835982
--- a/test/joint_trajectories/trajectory_0020.csv
+++ b/test/joint_trajectories/trajectory_0020.csv
@@ -0,0 +1,4 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018862.1515627,-2.313008521539318,-1.6483168722231125,-2.0019843899417324,-1.631472238564406,1.872309973057284,-1.6616604031041338
+1746018862.970348,-2.5300578690196116,-1.9912887227241693,-1.5500285554485365,-1.7848794727441384,1.7508934601485429,-1.8470722121005971
+1746018864.022474,-2.6609084481595655,-2.391741808925812,-0.8889580878115577,-2.0607220479463337,1.6746397246400901,-1.9545166867765837
--- a/test/joint_trajectories/trajectory_0021.csv
+++ b/test/joint_trajectories/trajectory_0021.csv
@@ -0,0 +1,7 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018863.5637312,-2.6608992903786857,-2.392021009144001,-0.8883673324424155,-2.0610348828617324,1.674644176109414,-1.9545088700368323
+1746018864.053805,-2.6155287892350185,-2.2146149952669463,-1.1962898666711363,-1.926201024239328,1.701275974419156,-1.9175228169091953
+1746018864.4442313,-2.561292881365816,-2.062608502162236,-1.4416017879534062,-1.8264012161760694,1.732858162260893,-1.8729535840631433
+1746018864.7808328,-2.4954979559493373,-1.9216132287892083,-1.65141392284393,-1.7477410915468223,1.770741105680468,-1.8182630675361207
+1746018865.107281,-2.4144508115941044,-1.7848709004106473,-1.8365413167359033,-1.6842092245652907,1.8165860531760938,-1.7497284587716229
+1746018865.4335163,-2.3130102016373804,-1.6482177569989882,-2.002082813644665,-1.6315268038763553,1.872352510197027,-1.6617054021835982
--- a/test/joint_trajectories/trajectory_0022.csv
+++ b/test/joint_trajectories/trajectory_0022.csv
@@ -0,0 +1,4 @@
+timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
+1746018865.1454973,-2.3130094134797368,-1.6483198465856737,-2.001981312549549,-1.6314726439594216,1.8723078443112016,-1.6616604315564611
+1746018865.9642751,-2.5300578688417263,-1.9912887229596485,-1.550028554603313,-1.7848794733138196,1.7508934602295874,-1.8470722119350325
+1746018867.0164015,-2.660908448159542,-2.391741808927602,-0.8889580878078083,-2.06072204794831,1.6746397246401,-1.9545166867765644