AS: upload log recordings

2025-04-30 13:28:00 +02:00 · 2025-04-30 13:28:00 +02:00 · cd1ae17d41
commit cd1ae17d41
parent ff3b2a03bf
124 changed files with 150040 additions and 157 deletions
--- a/osc_recieve.py
+++ b/osc_recieve.py
@ -2,6 +2,7 @@ from osc4py3.as_eventloop import *
 from osc4py3 import oscmethod as osm
 import time
 def handler(*args):
    print('recieved')
    print(args)
 osc_startup()
--- a/test/joint_states_cart_logs/cartesian.csv
+++ b/test/joint_states_cart_logs/cartesian.csv
--- a/test/joint_states_logs/elbow_joint.csv
+++ b/test/joint_states_logs/elbow_joint.csv
--- a/test/joint_states_logs/shoulder_lift_joint.csv
+++ b/test/joint_states_logs/shoulder_lift_joint.csv
--- a/test/joint_states_logs/shoulder_pan_joint.csv
+++ b/test/joint_states_logs/shoulder_pan_joint.csv
--- a/test/joint_states_logs/wrist_1_joint.csv
+++ b/test/joint_states_logs/wrist_1_joint.csv
--- a/test/joint_states_logs/wrist_2_joint.csv
+++ b/test/joint_states_logs/wrist_2_joint.csv
--- a/test/joint_states_logs/wrist_3_joint.csv
+++ b/test/joint_states_logs/wrist_3_joint.csv
--- a/test/joint_trajectories/trajectory_0001.csv
+++ b/test/joint_trajectories/trajectory_0001.csv
@ -1,66 +1,2 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
-1746007895.631715,0.34789084579519747,-1.9303232409378663,2.4531551499040436,-2.0442310079927557,-1.5266919925384135,-1.2986713702047135
+1746011500.6981866,-0.2584434767928996,-1.5593503626194987,1.918429404588129,-1.6944083250439956,-1.6324038286536853,-1.8219460785849033
 1746007895.6482904,0.34789071292771734,-1.9430107452460468,2.4398062433934307,-2.0181945459258777,-1.5266919948685196,-1.2986715068961738
 1746007895.664526,0.34789059114114007,-1.9549182732647898,2.426210832832817,-1.992691563590373,-1.5266919963045205,-1.2986716303458004
 1746007895.680433,0.347890469009442,-1.9660724370784124,2.412378225451552,-1.9677047513122585,-1.5266919976624596,-1.2986717540212458
 1746007895.6960235,0.3478903477169517,-1.9765001943259384,2.3983167564079633,-1.9432154866680131,-1.5266919989396688,-1.2986718767390126
 1746007895.711309,0.34789022794986924,-1.9862279086393635,2.3840341149992756,-1.9192050951739712,-1.5266920001383708,-1.2986719978209438
 1746007895.7263014,0.34789011027011085,-1.995281278992278,2.36953738450567,-1.8956549609930118,-1.5266920012611176,-1.2986721167126836
 1746007895.7410126,0.3478899951251999,-2.003685262513461,2.3548330689403674,-1.8725466308710357,-1.5266920023107295,-1.2986722329737916
 1746007895.755454,0.34788988286316025,-2.0114640165651374,2.3399271195049085,-1.8498618985148652,-1.526692003290204,-1.2986723462628438
 1746007895.7696373,0.3478897737461759,-2.018640857097043,2.32482496021555,-1.827582871865184,-1.5266920042026413,-1.298672456323774
 1746007895.7835736,0.3478896679629502,-2.02523823073822,2.3095315124979514,-1.8056920255997955,-1.5266920050511816,-1.2986725629735267
 1746007895.7972734,0.34788956563977314,-2.0312776983833114,2.294051218600062,-1.7841722409603784,-1.5266920058389577,-1.2986726660910082
 1746007895.8107479,0.34788946685034094,-2.0367799283024826,2.2783880637159015,-1.7630068347663652,-1.526692006569055,-1.2986727656072838
 1746007895.8240068,0.3478893716244009,-2.0417646970563603,2.2625455967474872,-1.7421795792618433,-1.5266920072444816,-1.2986728614969603
 1746007895.8370607,0.34788927995530106,-2.0462508967274355,2.2465269496595086,-1.7216747142386455,-1.5266920078681463,-1.2986729537706567
 1746007895.8499188,0.3478891918065443,-2.050256547186641,2.2303348554026217,-1.7014769526928448,-1.5266920084428401,-1.2986730424684771
 1746007895.8625913,0.34788910711743615,-2.0537988122985964,2.213971664397395,-1.68157148110318,-1.5266920089712261,-1.2986731276543866
 1746007895.8750865,0.3478890258079246,-2.056894019132308,2.1974393595827584,-1.6619439552685238,-1.5266920094558305,-1.298673209411392
 1746007895.8874135,0.3478889477827254,-2.0595576793872965,2.1807395700410908,-1.6425804925065342,-1.5266920098990386,-1.2986732878374436
 1746007895.8995812,0.3478888729348073,-2.061804512369776,2.163873583217299,-1.6234676608962553,-1.526692010303092,-1.2986733630419678
 1746007895.9115975,0.34788880114832477,-2.0636484689613557,2.146842355752031,-1.6045924661423367,-1.52669201067009,-1.2986734351429532
 1746007895.9234705,0.34788873230106754,-2.0651027561154383,2.1296465229498613,-1.58594233654653,-1.5266920110019893,-1.2986735042645208
 1746007895.935208,0.3478886662664884,-2.0661798614957307,2.112286406902202,-1.5675051064918182,-1.5266920113006073,-1.2986735705349128
 1746007895.9468176,0.34788860291536494,-2.066891577938595,2.0947620232821933,-1.549268998774708,-1.5266920115676255,-1.2986736340848435
 1746007895.9583063,0.34788854211715403,-2.0672490274777435,2.0770730868250187,-1.5312226060606062,-1.5266920118045932,-1.29867369504616
 1746007895.9696813,0.3478884837410736,-2.0672626847174285,2.0592190155021637,-1.5133548716846823,-1.5266920120129321,-1.2986737535507717
 1746007895.9811532,0.3478884276569545,-2.0669423993797817,2.041198933392213,-1.4956550699751308,-1.5266920121939394,-1.2986738097298078
 1746007895.9927316,0.34788837373589887,-2.0662974178845652,2.023011672243916,-1.478112786236312,-1.526692012348793,-1.2986738637129709
 1746007896.0044172,0.3478883218507671,-2.0653364038459934,2.0046557717194027,-1.460717896495001,-1.526692012478556,-1.2986739156280531
 1746007896.0162115,0.34788827187652593,-2.064067457392431,1.9861294782967231,-1.4434605470831,-1.5266920125841787,-1.2986739656005986
 1746007896.0281155,0.3478882236904717,-2.0624981332312133,1.9674307428010955,-1.4263311341039633,-1.5266920126665053,-1.2986740137536823
 1746007896.0401306,0.34788817717234943,-2.0606354573931918,1.948557216523433,-1.4093202828062983,-1.5266920127262733,-1.2986740602077966
 1746007896.052259,0.3478881322043854,-2.058485942600362,1.9295062458726964,-1.3924188268688278,-1.5266920127641195,-1.2986741050808206
 1746007896.064502,0.3478880886712403,-2.0560556022053786,1.9102748654951593,-1.3756177875800133,-1.5266920127805803,-1.2986741484880688
 1746007896.076862,0.347888046459893,-2.053349962654308,1.8908597897787516,-1.358908352879629,-1.526692012776094,-1.298674190542407
 1746007896.0893412,0.3478880054594673,-2.0503740744237353,1.8712574026437414,-1.3422818562123562,-1.5266920127510017,-1.2986742313544242
 1746007896.1019423,0.34788796556100454,-2.047132521380514,1.851463745502084,-1.3257297551274225,-1.5266920127055488,-1.2986742710326582
 1746007896.114668,0.3478879266571857,-2.04362942850708,1.8314745032461888,-1.309243609542127,-1.526692012639883,-1.2986743096838709
 1746007896.1275208,0.34788788864200315,-2.039868467927316,1.8112849881033242,-1.2928150595704844,-1.5266920125540542,-1.298674347413368
 1746007896.1405046,0.34788785141039114,-2.0358528631573765,1.7908901211637511,-1.2764358028006608,-1.526692012448013,-1.2986743843253659
 1746007896.1536226,0.34788781485780174,-2.0315853914924853,1.7702844113582374,-1.2600975708858706,-1.5266920123216075,-1.2986744205234044
 1746007896.1668787,0.34788777887973676,-2.027068384424188,1.7494619316231237,-1.2437921052923921,-1.5266920121745786,-1.2986744561108052
 1746007896.1802766,0.3478877433712224,-2.022303725962573,1.7284162919474246,-1.2275111320247103,-1.5266920120065568,-1.2986744911911856
 1746007896.1938212,0.34788770822622395,-2.0172928487139554,1.707140608945414,-1.2112463351207257,-1.5266920118170548,-1.2986745258690278
 1746007896.207517,0.34788767333699333,-2.0120367275358757,1.6856274715381767,-1.1949893286786706,-1.5266920116054594,-1.2986745602503138
 1746007896.2213688,0.34788763859333915,-2.006535870557089,1.6638689022567785,-1.178731627140726,-1.5266920113710227,-1.2986745944432359
 1746007896.2353826,0.34788760388180284,-2.000790307309445,1.6418563135957331,-1.1624646135151206,-1.5266920111128508,-1.298674628558995
 1746007896.2495637,0.3478875690847305,-1.9947995736698036,1.6195804587452898,-1.1461795051671047,-1.5266920108298896,-1.2986746627127
 1746007896.2639189,0.3478875340792178,-1.9885626932516767,1.5970313759111363,-1.1298673167477213,-1.5266920105209079,-1.298674697024391
 1746007896.278455,0.3478874987358962,-1.9820781548158712,1.5741983252856606,-1.113518819755262,-1.5266920101844792,-1.2986747316202107
 1746007896.293179,0.34788746291754613,-1.975343885184266,1.5510697175601624,-1.0971244981346882,-1.5266920098189571,-1.2986747666337475
 1746007896.3080993,0.3478874264774783,-1.9683572170373258,1.5276330326547054,-1.0806744992111383,-1.526692009422448,-1.2986748022075973
 1746007896.3232243,0.34788738925765017,-1.961114850849448,1.5038747270821453,-1.0641585791199923,-1.5266920089927776,-1.2986748384951818
 1746007896.3385634,0.34788735108645374,-1.9536128100606636,1.479780128042627,-1.0475660417313133,-1.526692008527451,-1.2986748756628859
 1746007896.3541267,0.3478873117760912,-1.945846388390841,1.4553333119484133,-1.0308856698624465,-1.5266920080236042,-1.2986749138925853
 1746007896.3699253,0.3478872711194536,-1.937810087962969,1.4305169645849123,-1.0141056473181265,-1.526692007477948,-1.2986749533846595
 1746007896.3859713,0.34788722888637036,-1.9294975466018247,1.4053122194940268,-0.997213469978008,-1.526692006886695,-1.2986749943616074
 1746007896.4022775,0.34788718481907965,-1.9209014522951477,1.3796984703829134,-0.9801958437477167,-1.5266920062454759,-1.2986750370724152
 1746007896.4188583,0.3478871386267013,-1.912013442321961,1.3536531523644229,-0.9630385666751806,-1.526692005549235,-1.2986750817978843
 1746007896.4357297,0.3478870899784585,-1.9028239839337202,1.327151485556005,-0.9457263918735825,-1.5266920047920989,-1.2986751288571656
 1746007896.4529092,0.3478870384952839,-1.893322232672989,1.3001661729077645,-0.9282428670371528,-1.5266920039672176,-1.2986751786158535
 1746007896.4704158,0.34788698373933835,-1.883495863368153,1.2726670419664634,-0.9105701452184616,-1.526692003066563,-1.2986752314960903
 1746007896.4882708,0.34788692520080255,-1.8733308674625937,1.2446206174272616,-0.8926887600610418,-1.5266920020806745,-1.2986752879893018
 1746007896.5064976,0.3478868622810758,-1.8628113084965976,1.2159896075175087,-0.8745773567140409,-1.5266920009983362,-1.2986753486723996
 1746007896.5251236,0.347886794271171,-1.8519190250783573,1.1867322821208344,-0.8562123670016613,-1.5266919998061619,-1.2986754142286157
--- a/test/joint_trajectories/trajectory_0002.csv
+++ b/test/joint_trajectories/trajectory_0002.csv
@ -0,0 +1,2 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011502.5537577,-0.2308989147082774,-1.5576496867458662,1.9268478330411956,-1.7397192673604973,-1.7839696265599596,-1.800062792954931
--- a/test/joint_trajectories/trajectory_0003.csv
+++ b/test/joint_trajectories/trajectory_0003.csv
@ -0,0 +1,2 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011503.7946618,-0.1686252647757187,-1.5472390563625094,1.9591905880121567,-1.8458983617027747,-2.1567869317617543,-1.7676185896498668
--- a/test/joint_trajectories/trajectory_0004.csv
+++ b/test/joint_trajectories/trajectory_0004.csv
@ -0,0 +1,2 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011506.1262758,-0.37964591217805577,-1.5433421257233508,1.898245007183581,-1.3844446013798872,-0.9486654069655223,-2.0294127243158604
--- a/test/joint_trajectories/trajectory_0005.csv
+++ b/test/joint_trajectories/trajectory_0005.csv
@ -0,0 +1,2 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011508.8176053,-0.12860039709478688,-1.5327987457296275,1.998946192099603,-1.9460947967968842,-2.4571289985280873,-1.769060918772316
--- a/test/joint_trajectories/trajectory_0006.csv
+++ b/test/joint_trajectories/trajectory_0006.csv
@ -0,0 +1,2 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011511.4248803,-0.32851231244735324,-1.556318860706303,1.9054772242742963,-1.5542632600057393,-1.2513477173722232,-1.906920178150602
--- a/test/joint_trajectories/trajectory_0007.csv
+++ b/test/joint_trajectories/trajectory_0007.csv
@ -0,0 +1,2 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011513.0201094,-0.2811586958622918,-1.5170442353198785,1.842088326077329,-1.4186114407746067,-1.510089919486052,-2.4769183223554463
--- a/test/joint_trajectories/trajectory_0008.csv
+++ b/test/joint_trajectories/trajectory_0008.csv
@ -0,0 +1,2 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011514.4977942,-0.2598094630044301,-1.5109851246905668,1.835455896600081,-1.4172144983489905,-1.6251002087604622,-2.6978004634140476
--- a/test/joint_trajectories/trajectory_0009.csv
+++ b/test/joint_trajectories/trajectory_0009.csv
@ -0,0 +1,2 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011516.3413794,-0.35378337935391757,-1.6016681749948434,2.001576650922292,-1.8398676754403294,-1.1068121709485519,-1.3089895738273505
--- a/test/joint_trajectories/trajectory_0010.csv
+++ b/test/joint_trajectories/trajectory_0010.csv
@ -0,0 +1,2 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011516.9457223,-0.3556423347154132,-1.6207376464189907,2.0674685662627326,-2.094571991443367,-1.0957598017613526,-0.8558302161593532
--- a/test/joint_trajectories/trajectory_0011.csv
+++ b/test/joint_trajectories/trajectory_0011.csv
@ -0,0 +1,2 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011518.62554,-0.22730261986774725,-1.490045675183208,2.2190438871192217,-3.301392142549558,-1.803855577475699,-1.1463284162240381
--- a/test/joint_trajectories/trajectory_0012.csv
+++ b/test/joint_trajectories/trajectory_0012.csv
@ -0,0 +1,2 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011520.7094712,-0.4317759969545931,-1.5878768142143662,2.192443620570412,-2.521846200611928,-0.5524424819151541,-2.4849698587736393
--- a/test/joint_trajectories/trajectory_0013.csv
+++ b/test/joint_trajectories/trajectory_0013.csv
@ -0,0 +1,2 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011523.1514468,-0.24750520595890402,-1.4000131090083312,1.6968945996898945,-0.9602205280305407,-1.6922387549044173,-2.0162200057746915
--- a/test/joint_trajectories/trajectory_0014.csv
+++ b/test/joint_trajectories/trajectory_0014.csv
@ -0,0 +1,2 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011524.1556451,-0.3182135655214777,-1.560055437175354,1.9097863748307429,-1.5859980050857687,-1.3082834080649495,-2.3680372330882276
--- a/test/joint_trajectories/trajectory_0015.csv
+++ b/test/joint_trajectories/trajectory_0015.csv
@ -0,0 +1,47 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011532.3775527,-0.30894014150806504,-1.5669624274668106,1.9042846745908033,-1.5759250186099247,-1.3052212682010704,-2.358970457919635
 1746011532.3865535,-0.29951462028540377,-1.5746827072638354,1.8991631799853055,-1.5655223084690406,-1.302157934148812,-2.349729009901037
 1746011532.3956466,-0.28999236793102323,-1.5821327904556532,1.893768031785319,-1.5551728647012082,-1.299085502736366,-2.3403768923063355
 1746011532.4048228,-0.2803828434536557,-1.5893147777944368,1.888113149667931,-1.5448870120534992,-1.29600789151451,-2.3309228625769896
 1746011532.4140818,-0.27068697279341025,-1.5962264257299066,1.8822006724905993,-1.534669627012995,-1.292926323254532,-2.321367305293071
 1746011532.423422,-0.2609057730729365,-1.6028655394437732,1.8760326119996122,-1.5245253966227839,-1.2898420809661382,-2.3117106837056016
 1746011532.432843,-0.25104034908015027,-1.6092301605114112,1.8696109643373768,-1.5144587139776953,-1.2867564873577304,-2.3019535517744245
 1746011532.442343,-0.2410918924967751,-1.615318561806938,1.862937699764272,-1.5044736710581663,-1.2836709029096613,-2.2920965549764483
 1746011532.4519212,-0.23106168250726977,-1.6211292423777088,1.8560147544355132,-1.494574053100561,-1.280586724529396,-2.2821404324939154
 1746011532.461576,-0.22095108620581394,-1.6266609215774772,1.8488440222206304,-1.4847633337661217,-1.277505384032303,-2.27208601926141
 1746011532.4713063,-0.2107615587926004,-1.63191253248257,1.8414273465564799,-1.475044671095092,-1.274428346452043,-2.261934247853489
 1746011532.4811106,-0.20049464355107371,-1.6368832146500345,1.833766512343601,-1.4654209042097546,-1.2713571081804027,-2.251686150198327
 1746011532.490987,-0.19015197159825803,-1.6415723062762058,1.825863237892543,-1.4558945507280288,-1.2682931949372982,-2.241342859102626
 1746011532.5009344,-0.17973526140124552,-1.6459793358131505,1.8177191669238457,-1.446467804847744,-1.265238159572765,-2.230905609573229
 1746011532.5109506,-0.16924631805390744,-1.650104013098832,1.8093358606224275,-1.437142536060682,-1.2621935797038921,-2.220375739921232
 1746011532.5210338,-0.15868703230898706,-1.6539462200547095,1.8007147897441387,-1.4279202884548707,-1.259161055190833,-2.20975469263481
 1746011532.5311825,-0.14805937936189872,-1.6575060010018712,1.7918573267692857,-1.4188022805633207,-1.2561422054572369,-2.199044015007587
 1746011532.5413945,-0.1373654173838048,-1.6607835526437718,1.78276473809483,-1.409789405717405,-1.2531386666616569,-2.188245359510044
 1746011532.5516677,-0.12660728580284886,-1.66377921376024,1.773438176253931,-1.4008822328632593,-1.2501520887277258,-2.1773604838923286
 1746011532.5620003,-0.11578720333378723,-1.6664934546537367,1.7638786721483601,-1.3920810077999068,-1.2471841322421073,-2.166391251007756
 1746011532.5723896,-0.10490746575767496,-1.6689268663848584,1.7540871272760539,-1.383385654798164,-1.244236465230428,-2.155339628347411
 1746011532.5828335,-0.09397044345468464,-1.6710801498299162,1.744064305932791,-1.3747957785597347,-1.2413107598225561,-2.144207687277447
 1746011532.5933301,-0.08297857869462044,-1.6729541045890408,1.733810827363511,-1.3663106664761655,-1.2384086888197157,-2.1329976019720163
 1746011532.6038766,-0.07193438269113583,-1.6745496177687471,1.723327157835163,-1.3579292911474354,-1.2355319221769676,-2.121711648036216
 1746011532.6144705,-0.060840432427123226,-1.6758676526582372,1.7126136025991432,-1.3496503131198552,-1.2326821234155756,-2.1103522008149636
 1746011532.6251094,-0.04969936726019375,-1.6769092373139614,1.701670297707322,-1.3414720838025957,-1.229860945980645,-2.0989217333853714
 1746011532.6357908,-0.038513885318542496,-1.6776754530620488,1.6904972016412279,-1.3333926485214538,-1.22707002956023,-2.087422814231895
 1746011532.6465118,-0.027286739698854756,-1.6781674229232575,1.6790940867092523,-1.3254097496674189,-1.224310996382744,-2.0758581046053415
 1746011532.6572702,-0.016020734479192722,-1.6783862999599461,1.6674605301614935,-1.317520829896112,-1.2215854475100847,-2.0642303555686525
 1746011532.6680627,-0.00471872056099798,-1.6783332555393113,1.6555959049661695,-1.3097230353321918,-1.2188949591442615,-2.052542404734268
 1746011532.6788871,0.006616408644537053,-1.6780094675017159,1.6434993701852099,-1.3020132187303681,-1.216241078965607,-2.0407971726997913
 1746011532.6897402,0.017981721669467987,-1.6774161082172339,1.6311698608795444,-1.294387942541572,-1.2136253225207554,-2.028997659190574
 1746011532.7006192,0.02937425356430845,-1.6765543325076628,1.6186060774667759,-1.2868434818291583,-1.2110491696785375,-2.0171469389197525
 1746011532.7115214,0.040791010577527764,-1.6754252654049557,1.6058064744449787,-1.279375826975629,-1.2085140611717575,-2.0052481571781136
 1746011532.7224438,0.05222897492076184,-1.6740299897103676,1.5927692483863591,-1.2719806861152065,-1.2060213952424548,-1.9933045251679966
 1746011532.7333837,0.06368510960067075,-1.6723695333114297,1.5794923250930877,-1.264653487221584,-1.2035725244077626,-1.9813193150971644
 1746011532.7443378,0.07515636329801145,-1.6704448562060403,1.565973345794613,-1.2573893797732356,-1.2011687523628145,-1.969295855050236
 1746011532.7553036,0.08663967527428262,-1.6682568371743745,1.5522096522508715,-1.2501832359106326,-1.1988113310363462,-1.9572375236568027
 1746011532.766278,0.09813198028623438,-1.6658062600298131,1.538198270608726,-1.2430296509904695,-1.1965014578137083,-1.9451477445767797
 1746011532.777258,0.10963021348861535,-1.663093799369412,1.5239358938392282,-1.2359229434313594,-1.1942402729409318,-1.9330299808247997
 1746011532.7882407,0.12113131530575494,-1.6601200057324186,1.5094188625604934,-1.2288571537331794,-1.1920288571223079,-1.920887728956595
 1746011532.7992232,0.13263223625295328,-1.656885290061631,1.4946431440245043,-1.2218260425380787,-1.1898682293226504,-1.908724513141237
 1746011532.8102028,0.14412994168914262,-1.6533899073466933,1.4796043090152429,-1.214823087584744,-1.1877593447840336,-1.8965438791438975
 1746011532.8211763,0.15562141648292682,-1.649633939310259,1.4642975063694816,-1.2078414793884473,-1.1857030932653418,-1.8843493882443578
 1746011532.8321412,0.16710366957485334,-1.6456172759768606,1.4487174347891267,-1.2008741154571434,-1.1837002975114548,-1.8721446111168845
 1746011532.843094,0.1785737384196322,-1.6413395959396315,1.4328583115640452,-1.1939135928278035,-1.1817517119573333,-1.8599331216972814
--- a/test/joint_trajectories/trajectory_0016.csv
+++ b/test/joint_trajectories/trajectory_0016.csv
@ -0,0 +1,35 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011532.9093502,0.17852975827604078,-1.6413620961711513,1.432904773258274,-1.1939113868562339,-1.181767285751261,-1.8599814060470266
 1746011532.9121642,0.1791423661533531,-1.6443089751156363,1.4356220642759387,-1.1939515104006904,-1.1816828704790572,-1.8593172214226008
 1746011532.9151247,0.17972115967504276,-1.6474092781856875,1.4385175490404558,-1.1939806557195587,-1.1815860008009254,-1.858700491823218
 1746011532.9180827,0.18030354334605692,-1.6505069821021467,1.4413999620349998,-1.1940008540491647,-1.1814886645821547,-1.858079887453538
 1746011532.9210384,0.18088957698129748,-1.6536021263655902,1.4442698847790432,-1.1940126728906337,-1.181390854382581,-1.8574553437045864
 1746011532.9239917,0.18147929373640448,-1.6566947257343672,1.447127360905741,-1.1940161558519595,-1.181292567278401,-1.8568268243560773
 1746011532.9269423,0.18207272710781508,-1.659784794352331,1.4499724333863702,-1.1940113466541604,-1.1811938003380604,-1.8561942928092718
 1746011532.9298909,0.1826699109819847,-1.6628723458155636,1.4528051445531496,-1.193998289107684,-1.1810945506150201,-1.8555577120342512
 1746011532.9328368,0.1832709657561904,-1.6659574275662148,1.4556255880432118,-1.193977371439152,-1.180995296770786,-1.854916754332894
 1746011532.9357805,0.18387575443649595,-1.66903998307905,1.4584337006203159,-1.1939479487353288,-1.1808950726864766,-1.8542719617580978
 1746011532.9387217,0.1844843974179926,-1.6721200590454641,1.461229575321501,-1.1939104097402413,-1.1807943569348007,-1.8536230064904415
 1746011532.9416606,0.18509693046078723,-1.6751976668171398,1.4640132518871063,-1.1938647987709128,-1.1806931464676582,-1.8529698494381184
 1746011532.9445972,0.1857133894861458,-1.6782728172902541,1.4667847696327403,-1.19381116026532,-1.1805914382652711,-1.8523124513206506
 1746011532.9475315,0.18633381083856237,-1.6813455208285344,1.469544167305414,-1.1937495388220596,-1.1804892292938265,-1.8516507723891948
 1746011532.9504633,0.18695830953270587,-1.6844158204106683,1.472291533340563,-1.1936803006659134,-1.180386946009585,-1.8509845077222598
 1746011532.953393,0.18758676681114927,-1.6874836588207487,1.475026804464255,-1.1936028475611375,-1.1802837264473127,-1.850314145543893
 1746011532.95632,0.18821929781741575,-1.6905490782820654,1.4777500685985512,-1.1935175463754872,-1.1801799969715037,-1.8496393806758762
 1746011532.9592452,0.18885594089033386,-1.6936120869753721,1.4804613621837062,-1.1934244424978357,-1.1800757544544436,-1.8489601711712187
 1746011532.9621677,0.18949673458640826,-1.6966726926291358,1.4831607212883053,-1.193323581513419,-1.1799709957942388,-1.8482764748321703
 1746011532.9650881,0.1901417179225695,-1.6997309024502292,1.4858481814770812,-1.193215009239971,-1.1798657178761764,-1.8475882489509616
 1746011532.9680064,0.19079101680450838,-1.7027867569755777,1.488523828751278,-1.1930991126530077,-1.179760383096819,-1.8468951652101107
 1746011532.970922,0.19144449892397386,-1.7058401944209909,1.49118759539485,-1.1929752559509001,-1.1796540573836447,-1.846197749524356
 1746011532.9738357,0.19210229030174641,-1.7088912546130657,1.4938395670399442,-1.1928438268893742,-1.1795472030379548,-1.8454956733185028
 1746011532.976747,0.19276443213246397,-1.7119399426074104,1.4964797771935654,-1.192704872380535,-1.1794398168524336,-1.8447888914825854
 1746011532.979656,0.19343096583993402,-1.7149862630202437,1.4991082590526998,-1.192558439589233,-1.1793318956515038,-1.8440773586415828
 1746011532.9825628,0.19410193334911385,-1.7180302199533106,1.5017250453702289,-1.1924045759852178,-1.1792234362484706,-1.843361028864793
 1746011532.9854672,0.1947774727047218,-1.7210718515596957,1.5043302201296918,-1.1922436916924262,-1.1791149406731267,-1.842639548030878
 1746011532.9883695,0.19545743628220258,-1.724111091507644,1.50692371142953,-1.1920751098490152,-1.1790053953900768,-1.8419134837444424
 1746011532.9912696,0.19614196222257707,-1.7271479772824292,1.509505603021963,-1.1918992414966079,-1.1788953023303348,-1.8411824816285436
 1746011532.9941673,0.19683109483122152,-1.7301825108505824,1.5120759258325727,-1.1917161355098762,-1.1787846582173813,-1.8404464931245368
 1746011532.997063,0.19752487865420054,-1.7332146937496575,1.5146347105345308,-1.1915258410713998,-1.1786734598134356,-1.83970546939417
 1746011532.9999561,0.198223446155954,-1.736244556431033,1.5171820311847277,-1.1913287277480447,-1.178562158133051,-1.8389590858199736
 1746011533.0028472,0.19892666883640464,-1.7392720403097979,1.5197178297387968,-1.1911242052172775,-1.1784498415153513,-1.8382078421536026
 1746011533.0057359,0.19963467940373159,-1.7422971751413838,1.522242180031375,-1.190912644040474,-1.1783369608870868,-1.8374514133672655
--- a/test/joint_trajectories/trajectory_0017.csv
+++ b/test/joint_trajectories/trajectory_0017.csv
@ -0,0 +1,17 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011532.96937,0.1996149312400659,-1.7423370505584117,1.5222788837364885,-1.1908851791808488,-1.1783330130878016,-1.837479699291939
 1746011532.9718719,0.200262293922445,-1.7449571274045572,1.5244528104277881,-1.1907199694402422,-1.1782373586883028,-1.8367807027588579
 1746011532.9744103,0.2008931564315537,-1.747615467765933,1.5266560784260381,-1.1905227839594892,-1.1781371178583053,-1.836106578625262
 1746011532.9769473,0.2015277961552968,-1.7502720018734532,1.528850436185305,-1.190320122937042,-1.1780364384300057,-1.8354283619633922
 1746011532.9794822,0.2021662778847655,-1.7529267134383824,1.5310360091458515,-1.1901121559955627,-1.1779353130981651,-1.834745982589347
 1746011532.9820156,0.20280863501156254,-1.755579600886335,1.5332128164305345,-1.1898989189173264,-1.1778337396089285,-1.8340594038536864
 1746011532.9845471,0.20345490122239562,-1.7582306623196367,1.535380876891213,-1.1896804476804461,-1.177731715716572,-1.8333685887760118
 1746011532.987077,0.20410520724689096,-1.7608799214426603,1.5375402472621884,-1.1894570994255063,-1.1776297282264434,-1.8326732118309557
 1746011532.9896052,0.20475939476397587,-1.7635273236812863,1.5396908695255505,-1.1892282687595537,-1.1775267968862857,-1.831973810950857
 1746011532.9921315,0.2054175943909744,-1.7661728924422135,1.541832800498801,-1.1889943135600172,-1.1774234084352213,-1.8312700605915737
 1746011532.994656,0.20607984159963522,-1.7688166245270907,1.5439660582046333,-1.188755271101918,-1.1773195605752425,-1.8305619218000004
 1746011532.9971788,0.2067461719769157,-1.7714585165012662,1.546090660591009,-1.188511178893645,-1.177215251050271,-1.8298493554830682
 1746011532.9996998,0.20741662150373985,-1.7740985646221508,1.5482066254380156,-1.188262074770003,-1.1771104776033998,-1.829132322109461
 1746011533.0022192,0.2080913294251041,-1.776736792853624,1.5503140117063676,-1.1880083396604972,-1.1770057492004449,-1.8284104769517562
 1746011533.0047367,0.20877012743980305,-1.7793731405421336,1.5524127537926482,-1.1877493263213408,-1.1769000412582562,-1.8276843885161107
 1746011533.0072525,0.20945315467114156,-1.7820076313710644,1.5545029108163382,-1.1874854165077162,-1.1767938626692256,-1.8269537120545105
--- a/test/joint_trajectories/trajectory_0018.csv
+++ b/test/joint_trajectories/trajectory_0018.csv
@ -0,0 +1,15 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011533.0190654,0.20944771696269537,-1.7820177940736373,1.5545083280916252,-1.1874720958297715,-1.1767925896703122,-1.8269622054696948
 1746011533.0219567,0.210245905361218,-1.7850454316261009,1.5569053783895868,-1.187175817805035,-1.176671119450416,-1.8261054823207006
 1746011533.0248547,0.2110450542958917,-1.7880804262863563,1.5592950996813135,-1.1868588857222866,-1.1765473680943883,-1.8252504213329301
 1746011533.0277505,0.21184999518463377,-1.7911129266971686,1.5616734328935804,-1.1865355720157902,-1.176422980931931,-1.824389074522891
 1746011533.030644,0.21266079469762644,-1.794142922123413,1.5640404365631593,-1.1862059759939099,-1.1762979534657665,-1.8235213687671945
 1746011533.033535,0.21347761875867644,-1.7971704281462575,1.5663961735538372,-1.1858704943165908,-1.1761727928965247,-1.8226469338055695
 1746011533.0364237,0.2143003194568478,-1.8001953832839868,1.5687405936583678,-1.185528522743659,-1.176046474595747,-1.821766307990132
 1746011533.03931,0.2151290631707501,-1.803217802394025,1.5710737597926219,-1.1851804594664614,-1.1759195056931606,-1.820879120362124
 1746011533.0421937,0.21596391344958787,-1.8062376738889752,1.5733956965680616,-1.1848263696451777,-1.175791882698632,-1.8199853009958653
 1746011533.045075,0.21680504618685292,-1.8092550120048323,1.5757064674215364,-1.1844666701147677,-1.1756641380196526,-1.8190844600557678
 1746011533.0479538,0.21765230333496177,-1.8122697511928618,1.5780060183988258,-1.184100725064824,-1.1755351967317593,-1.8181771641344706
 1746011533.0508304,0.21850586127095273,-1.815281904992074,1.5802944130961434,-1.1837289529993977,-1.1754055911687653,-1.8172630226499327
 1746011533.0537043,0.2193657869623351,-1.8182914595537316,1.5825716753640862,-1.1833514222061552,-1.175275317883291,-1.8163419618666665
 1746011533.0565755,0.220232268502357,-1.8212984270540684,1.5848378683386573,-1.1829685706481636,-1.175144943129931,-1.8154135687062034
--- a/test/joint_trajectories/trajectory_0019.csv
+++ b/test/joint_trajectories/trajectory_0019.csv
@ -0,0 +1,13 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011533.0714974,0.2202332353178278,-1.8212996301723934,1.5848342024508142,-1.1829621556169063,-1.175144355746336,-1.8154133099458098
 1746011533.074887,0.22126328798173267,-1.8248491996348302,1.5875039325546112,-1.1825096443583636,-1.1749896280960084,-1.8143089575371847
 1746011533.0782738,0.22230472178020166,-1.8283957826933195,1.5901523832884195,-1.1820417306737023,-1.174833029745834,-1.8131930984142417
 1746011533.081657,0.2233554823119892,-1.8319386319426068,1.5927854181635261,-1.1815662073996251,-1.1746754758575133,-1.8120670987319112
 1746011533.0850365,0.22441581318597592,-1.8354777584890876,1.5954031287131762,-1.1810836032409828,-1.1745176056412858,-1.8109304583431374
 1746011533.0884125,0.2254855588838094,-1.8390130757667882,1.5980054670147057,-1.1805932127270944,-1.174358128510822,-1.809783811391106
 1746011533.091785,0.22656507512691482,-1.8425446020134872,1.6005925429064334,-1.1800958596893751,-1.1741981299978,-1.808626374313122
 1746011533.0951536,0.22765429252059555,-1.846072267047234,1.6031643358006935,-1.1795910969756798,-1.1740367082782175,-1.8074585435167199
 1746011533.0985186,0.22875353476201177,-1.849596077349526,1.6057209413159574,-1.1790796342405936,-1.1738747715782,-1.8062796372673278
 1746011533.1018798,0.22986272720900525,-1.8531159602907912,1.608262338048145,-1.178561009760029,-1.1737113843384208,-1.8050900673871466
 1746011533.1052372,0.23098220692453797,-1.8566319201108015,1.6107886216874618,-1.1780359556031286,-1.173547489722104,-1.8038891275548088
 1746011533.108591,0.23211189379753172,-1.8601438818435339,1.6132997702703946,-1.1775039955219195,-1.1733821165937317,-1.8026772454473654
--- a/test/joint_trajectories/trajectory_0020.csv
+++ b/test/joint_trajectories/trajectory_0020.csv
@ -0,0 +1,10 @@
 timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint
 1746011533.1262534,0.23295061628362745,-1.8606528281904156,1.6141227767097774,-1.1781085973591856,-1.1728280870610868,-1.801873625010036
 1746011533.1290176,0.2332226910494013,-1.8635476231107664,1.6157203731285588,-1.1769826557055938,-1.1732207615523518,-1.8014851303147161
 1746011533.1311831,0.2339647451029263,-1.865815387841045,1.6173258046554624,-1.1766286232338377,-1.173112816304153,-1.800688864191614
 1746011533.133347,0.234712410472262,-1.8680814424376027,1.6189269138120093,-1.176274417870966,-1.1730042828415328,-1.7998865040982508
 1746011533.1355095,0.23546458382048918,-1.8703457783271995,1.6205217491102424,-1.1759176402196132,-1.1728953262414372,-1.7990792326724827
 1746011533.13767,0.23622130022515941,-1.8726083846896246,1.6221103199469766,-1.175558326846458,-1.1727859457007208,-1.7982670110643595
 1746011533.139829,0.2369827067106569,-1.8748692255930655,1.6236926018532278,-1.1751966191861274,-1.1726766192773124,-1.7974495944339322
 1746011533.1419864,0.23774862589655887,-1.8771283385948851,1.625268673324137,-1.1748323517178003,-1.1725663871748784,-1.7966273437185916
 1746011533.144142,0.2385192047065856,-1.8793856876854476,1.6268385099631235,-1.1744656664579487,-1.1724557275369645,-1.7958000139980583
--- a/test/joint_trajectories_cart/trajectory_0001_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0001_cart.csv
@ -1,66 +1,2 @@
 timestamp,x,y,z,roll,pitch,yaw
-1746007895.631715,0.3088828058922495,0.3027269165946418,0.34661821114314917,0.029199972563886116,0.05943114972318121,0.07554418605895213
+1746011500.6981866,0.6532147996609244,2.910667943093163e-05,0.45143072952175217,-1.3204229019021049e-05,0.24324840834544145,-6.09777640597976e-06
 1746007895.6482904,0.3088855911078177,0.30272787071202345,0.3568255033119091,0.029199964638559815,0.05943119446554579,0.07554418922775448
 1746007895.664526,0.3088882837882884,0.3027287960216436,0.3670331335094078,0.029199958806133082,0.05943123261893196,0.07554419034480084
 1746007895.680433,0.30889099465575065,0.30272972779103025,0.3772407440158607,0.02919995378263869,0.05943126821229602,0.07554419138624326
 1746007895.6960235,0.30889370223480245,0.3027306587285221,0.38744834908754394,0.029199949515663983,0.059431301225840144,0.07554419235226106
 1746007895.711309,0.3088963972896752,0.30273158577189635,0.39765595910243334,0.029199945922609605,0.05943133182217294,0.07554419324758674
 1746007895.7263014,0.3088990714824175,0.3027325061324509,0.4078635824717611,0.02919994292931875,0.059431360155208356,0.0755441940767172
 1746007895.7410126,0.30890171745379064,0.30273341732848624,0.41807122595075796,0.029199940469544758,0.05943138637048122,0.07554419484390476
 1746007895.755454,0.3089043287766588,0.3027343171746473,0.4282788948436116,0.02919993848438856,0.05943141060476556,0.07554419555314677
 1746007895.7696373,0.30890689989847614,0.3027352037667931,0.4384865931929611,0.029199936921723103,0.05943143298590282,0.0755441962081792
 1746007895.7835736,0.3089094260781315,0.30273607546427433,0.44869432395158765,0.02919993573561735,0.05943145363282832,0.07554419681247815
 1746007895.7972734,0.3089119033198907,0.30273693087061665,0.45890208913631175,0.029199934885769586,0.05943147265574899,0.07554419736926588
 1746007895.8107479,0.3089143283066322,0.30273776881342374,0.46910988996467684,0.029199934336967468,0.05943149015642133,0.07554419788151615
 1746007895.8240068,0.3089166983340241,0.30273858832412953,0.47931772697531755,0.02919993405858135,0.05943150622849517,0.0755441983519675
 1746007895.8370607,0.3089190112468673,0.30273938861807653,0.489525600133156,0.02919993402408845,0.059431520957914236,0.07554419878313261
 1746007895.8499188,0.3089212653784559,0.30274016907527135,0.49973350892067,0.029199934210642927,0.05943153442332451,0.07554419917731087
 1746007895.8625913,0.3089234594935124,0.3027409292220562,0.509941452416514,0.029199934598676914,0.05943154669650706,0.07554419953660217
 1746007895.8750865,0.3089255927350384,0.3027416687138618,0.5201494293627695,0.029199935171546437,0.05943155784279278,0.07554419986291581
 1746007895.8874135,0.30892766457521814,0.30274238731913083,0.5303574382220267,0.02919993591520902,0.05943156792147708,0.07554420015798692
 1746007895.8995812,0.30892967477040084,0.30274308490445184,0.5405654772254435,0.029199936817939084,0.05943157698621335,0.0755442004233848
 1746007895.9115975,0.3089316233200682,0.30274376142090714,0.5507735444128308,0.029199937870074962,0.059431585085376895,0.07554420066052397
 1746007895.9234705,0.3089335104296283,0.30274441689160636,0.5609816376657,0.029199939063795685,0.059431592262417864,0.07554420087067526
 1746007895.935208,0.30893533647683225,0.3027450514003559,0.5711897547341369,0.029199940392925974,0.05943159855617638,0.07554420105497582
 1746007895.9468176,0.3089371019815646,0.3027456650813995,0.581397893258247,0.02919994185276755,0.059431604001173,0.0755442012144349
 1746007895.9583063,0.30893880757875797,0.3027462581101627,0.5916060507848278,0.02919994343995118,0.059431608627875464,0.07554420134994387
 1746007895.9696813,0.30894045399415543,0.30274683069491576,0.6018142247798426,0.029199945152311862,0.05943161246293387,0.07554420146228316
 1746007895.9811532,0.3089420420226653,0.3027473830692811,0.6120224126371856,0.02919994698878389,0.05943161552938765,0.07554420155212573
 1746007895.9927316,0.3089435725090348,0.30274791548549934,0.6222306116841378,0.029199948949309745,0.05943161784685309,0.07554420162004695
 1746007896.0044172,0.30894504633060055,0.3027484282083779,0.6324388191838727,0.02919995103476924,0.05943161943167465,0.07554420166652441
 1746007896.0162115,0.308946464381864,0.3027489215098425,0.6426470323352657,0.02919995324692094,0.05943162029705933,0.07554420169194481
 1746007896.0281155,0.3089478275606671,0.30274939566401804,0.6528552482702403,0.029199955588356648,0.059431620453180414,0.07554420169660579
 1746007896.0401306,0.3089491367557536,0.30274985094276635,0.6630634640488007,0.029199958062472265,0.059431619907257614,0.07554420168071893
 1746007896.052259,0.3089503928354988,0.30275028761161277,0.6732716766518473,0.029199960673444023,0.05943161866361672,0.07554420164441025
 1746007896.064502,0.30895159663762256,0.30275070592599673,0.6834798829718505,0.029199963426223594,0.05943161672371424,0.07554420158772354
 1746007896.076862,0.3089527489596979,0.30275110612778017,0.6936880798013536,0.029199966326538533,0.05943161408614957,0.07554420151061769
 1746007896.0893412,0.30895385055027114,0.3027514884419554,0.7038962638192854,0.029199969380906004,0.05943161074664109,0.07554420141296699
 1746007896.1019423,0.3089549021004305,0.30275185307349173,0.7141044315749581,0.029199972596657218,0.059431606697980005,0.0755442012945609
 1746007896.114668,0.3089559042356463,0.3027522002042608,0.724312579469608,0.029199975981973235,0.05943160192995359,0.07554420115510091
 1746007896.1275208,0.308956857507723,0.3027525299899805,0.7345207037352596,0.029199979545933345,0.05943159642923779,0.07554420099419647
 1746007896.1405046,0.3089577623866825,0.30275284255711743,0.7447288004106216,0.02919998329857209,0.0594315901792604,0.07554420081136332
 1746007896.1536226,0.30895861925241597,0.3027531379996822,0.7549368653136748,0.029199987250955482,0.05943158316002108,0.07554420060601655
 1746007896.1668787,0.3089594283859099,0.3027534163758522,0.7651448940104941,0.029199991415267128,0.05943157534788118,0.07554420037746386
 1746007896.1802766,0.3089601899598486,0.3027536777043444,0.7753528817797725,0.029199995804913316,0.059431566715297736,0.07554420012489954
 1746007896.1938212,0.3089609040283793,0.3027539219604599,0.7855608235723841,0.029200000434645736,0.05943155723051351,0.07554419984739473
 1746007896.207517,0.30896157051579043,0.3027541490717056,0.79576871396518,0.029200005320706098,0.059431546857189016,0.07554419954388544
 1746007896.2213688,0.30896218920382756,0.3027543589128905,0.8059765471080536,0.029200010480993372,0.059431535553965895,0.07554419921316151
 1746007896.2353826,0.3089627597173225,0.30275455130057066,0.8161843166630853,0.0292000159352609,0.0594315232739523,0.07554419885385112
 1746007896.2495637,0.3089632815077614,0.302754725986703,0.8263920157343401,0.02920002170534537,0.05943150996412084,0.07554419846440165
 1746007896.2639189,0.30896375383434194,0.3027548826513375,0.836599636786554,0.02920002781543515,0.05943149556460065,0.07554419804306192
 1746007896.278455,0.308964175741984,0.30275502089413564,0.8468071715505788,0.029200034292383475,0.05943148000783994,0.07554419758785429
 1746007896.293179,0.30896454603563994,0.30275514022447814,0.8570146109129313,0.029200041166077693,0.05943146321762438,0.07554419709654864
 1746007896.3080993,0.3089648632501172,0.3027552400498464,0.8672219447861991,0.029200048469873403,0.05943144510791264,0.07554419656662707
 1746007896.3232243,0.3089651256144225,0.30275531966210817,0.877429161956241,0.029200056241107695,0.05943142558146048,0.07554419599524276
 1746007896.3385634,0.3089653310094254,0.3027553782212435,0.8876362499011259,0.029200064521707824,0.059431404528183324,0.07554419537917491
 1746007896.3541267,0.3089654769173044,0.30275541473591944,0.8978431945754486,0.029200073358920633,0.05943138182319465,0.07554419471476809
 1746007896.3699253,0.308965560360866,0.3027554280401851,0.9080499801519605,0.029200082806182356,0.05943135732445806,0.07554419399786509
 1746007896.3859713,0.3089655778302918,0.30275541676534584,0.9182565887102463,0.02920009292417168,0.05943133086995622,0.0755441932237239
 1746007896.4022775,0.30896552519417725,0.30275537930581714,0.9284629998592813,0.02920010378208407,0.05943130227424764,0.0755441923869175
 1746007896.4188583,0.30896539759083397,0.3027553137774056,0.938669190276764,0.02920011545918448,0.059431271324283094,0.07554419148121018
 1746007896.4357297,0.30896518929456074,0.3027552179659922,0.9488751331429803,0.029200128046722867,0.05943123777424447,0.07554419049940657
 1746007896.4529092,0.3089648935499541,0.3027550892639529,0.9590807974397872,0.029200141650296274,0.05943120133917503,0.07554418943316638
 1746007896.4704158,0.3089645023650244,0.302754924590772,0.9692861470756484,0.029200156392801088,0.059431161687030276,0.07554418827277107
 1746007896.4882708,0.3089640062507063,0.3027547202930812,0.9794911397841307,0.02920017241814515,0.05943111842868737,0.07554418700683074
 1746007896.5064976,0.30896339388991495,0.302754472017653,0.9896957257243316,0.029200189895963125,0.059431071105262925,0.07554418562191367
 1746007896.5251236,0.3089626517129394,0.30275417454843034,0.9998998456846993,0.029200209027662124,0.05943101917186524,0.0755441841020697
--- a/test/joint_trajectories_cart/trajectory_0002_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0002_cart.csv
@ -0,0 +1,2 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011502.5537577,0.6532209075758113,1.0756815914218787e-06,0.4516153142154451,-0.16215949666928248,0.24324442726272735,3.5922113093537217e-06
--- a/test/joint_trajectories_cart/trajectory_0003_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0003_cart.csv
@ -0,0 +1,2 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011503.7946618,0.6529296662848618,0.0001171952325476136,0.4518345764749449,-0.5540031529568706,0.24331773342964108,4.69086289888107e-05
--- a/test/joint_trajectories_cart/trajectory_0004_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0004_cart.csv
@ -0,0 +1,2 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011506.1262758,0.6531841589826776,7.594775719196867e-06,0.4516327471980498,0.7704351038290599,0.24324551392018412,8.825426257712039e-05
--- a/test/joint_trajectories_cart/trajectory_0005_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0005_cart.csv
@ -0,0 +1,2 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011508.8176053,0.6519301378527343,0.000246380607491159,0.4518166615560665,-0.8648432191117436,0.24331385935128844,6.84897539465569e-05
--- a/test/joint_trajectories_cart/trajectory_0006_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0006_cart.csv
@ -0,0 +1,2 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011511.4248803,0.6532211249765901,-4.640926830890013e-07,0.4516099103343386,0.41893370022393944,0.24324392159472094,6.9169210304312415e-06
--- a/test/joint_trajectories_cart/trajectory_0007_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0007_cart.csv
@ -0,0 +1,2 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011513.0201094,0.6532056398505591,-2.430523687325936e-05,0.4516035248995389,0.41890080872762575,0.24328409798273318,0.6621515721264102
--- a/test/joint_trajectories_cart/trajectory_0008_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0008_cart.csv
@ -0,0 +1,2 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011514.4977942,0.6531535873906201,3.226099854712443e-05,0.451621695542192,0.4189365351282477,0.24345255768684354,0.9324309523005815
--- a/test/joint_trajectories_cart/trajectory_0009_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0009_cart.csv
@ -0,0 +1,2 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011516.3413794,0.6532228899076257,-8.734005357835528e-07,0.4516109408555056,0.4189245598540158,0.24324870008972216,-0.5945868340929674
--- a/test/joint_trajectories_cart/trajectory_0010_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0010_cart.csv
@ -0,0 +1,2 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011516.9457223,0.6532259816635906,8.687875099125386e-06,0.4516023101187117,0.41892289323625903,0.24323021257171426,-0.9999976543455035
--- a/test/joint_trajectories_cart/trajectory_0011_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0011_cart.csv
@ -0,0 +1,2 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011518.62554,0.6526460598071762,0.00016233659881639828,0.4505174009244495,0.41888295418843996,-0.9593977816362218,-1.0000019508430145
--- a/test/joint_trajectories_cart/trajectory_0012_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0012_cart.csv
@ -0,0 +1,2 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011520.7094712,0.6532244274421134,-1.7968454893851904e-06,0.45161139839620745,0.41892026617979816,-0.9999989236882725,0.4459443492535846
--- a/test/joint_trajectories_cart/trajectory_0013_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0013_cart.csv
@ -0,0 +1,2 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011523.1514468,0.6532232229354377,1.4005465547722862e-07,0.4516135063656488,0.4189189404036424,0.8378378326699625,0.44594595564446193
--- a/test/joint_trajectories_cart/trajectory_0014_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0014_cart.csv
@ -0,0 +1,2 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011524.1556451,0.6526165258198791,0.00022251508158053351,0.45185868733414647,0.41889425028065813,0.05405305802733039,0.44594760180151755
--- a/test/joint_trajectories_cart/trajectory_0015_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0015_cart.csv
@ -0,0 +1,47 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011532.3775527,0.6512881887268702,0.007050389956288726,0.45883384215193174,0.41893680554671126,0.05405493303024958,0.4459506377683439
 1746011532.3865535,0.6494210525767303,0.014096258365284144,0.4660516807919667,0.4189372137893637,0.054054771598264925,0.44595076116694754
 1746011532.3956466,0.6475584462486482,0.021146586588911994,0.4732735009024223,0.418937639540948,0.05405480256408068,0.4459508826488975
 1746011532.4048228,0.6456959611371006,0.028196877359326522,0.4804952394461889,0.4189380351066616,0.0540548330734993,0.4459509956294648
 1746011532.4140818,0.6438334973021466,0.03524716963469923,0.48771693403395694,0.41893843164739697,0.0540548650884272,0.4459511084203038
 1746011532.423422,0.6419710560271976,0.042297464827151354,0.49493858493285725,0.4189388286968493,0.054054898322699184,0.44595122073758114
 1746011532.432843,0.6401086376831812,0.049347763522088896,0.5021601917919647,0.4189392257087538,0.054054932762312984,0.4459513323331494
 1746011532.442343,0.6382462425006378,0.05639806633340971,0.5093817543760966,0.41893962212438607,0.05405496838826595,0.4459514429505996
 1746011532.4519212,0.6363838705878709,0.06344837388949373,0.5166032725507721,0.41894001736476555,0.0540550051753644,0.4459515523234504
 1746011532.461576,0.6345215219313194,0.07049868682557954,0.5238247462731221,0.41894041083193806,0.05405504309195317,0.4459516601758356
 1746011532.4713063,0.6326591963969299,0.07754900577618078,0.5310461755827307,0.41894080191046257,0.05405508209958807,0.4459517662232993
 1746011532.4811106,0.6307968937323406,0.08459933136745729,0.5382675605923894,0.4189411899690717,0.05405512215273253,0.44595187017368965
 1746011532.490987,0.6289346135698085,0.09164966420963258,0.5454889014788349,0.4189415743625144,0.05405516319849011,0.4459519717281652
 1746011532.5009344,0.6270723554298027,0.09870000488953307,0.5527101984735424,0.41894195443357113,0.05405520517637865,0.44595207058230596
 1746011532.5109506,0.6252101187251904,0.10575035396332563,0.5599314518536389,0.4189423295152421,0.054055248018150905,0.44595216642733093
 1746011532.5210338,0.6233479027659393,0.11280071194951939,0.5671526619329876,0.4189426989330866,0.0540552916476569,0.4459522589514128
 1746011532.5311825,0.6214857067642444,0.11985107932229533,0.5743738290534699,0.4189430620077248,0.0540553359807797,0.4459523478410905
 1746011532.5413945,0.619623529840002,0.12690145650521492,0.58159495357651,0.4189434180574653,0.05405538092541573,0.44595243278276564
 1746011532.5516677,0.6177613710265463,0.13395184386535455,0.5888160358748533,0.41894376640105996,0.05405542638152815,0.44595251346428266
 1746011532.5620003,0.6158992292765568,0.1410022417079041,0.5960370763245991,0.41894410636056634,0.05405547224127168,0.445952589576575
 1746011532.5723896,0.6140371034680612,0.1480526502712529,0.6032580752974984,0.418944437264291,0.05405551838918558,0.4459526608153687
 1746011532.5828335,0.6121749924104495,0.15510306972259164,0.610479033153504,0.41894475844980317,0.05405556470245956,0.4459527268829339
 1746011532.5933301,0.6103128948504163,0.16215350015403363,0.6176999502335496,0.41894506926698644,0.05405561105127686,0.44595278748986317
 1746011532.6038766,0.6084508094777588,0.16920394157926164,0.6249208268525323,0.4189453690811145,0.054055657299229766,0.4459528423568681
 1746011532.6144705,0.6065887349309571,0.17625439393069608,0.63214166329247,0.4189456572759171,0.054055703303813776,0.4459528912165734
 1746011532.6251094,0.6047266698024585,0.183304857057166,0.6393624597957832,0.4189459332566162,0.05405574891698129,0.4459529338152956
 1746011532.6357908,0.6028646126436084,0.19035533072206456,0.6465832165586606,0.41894619645290165,0.05405579398577318,0.4459529699147775
 1746011532.6465118,0.6010025619691528,0.19740581460195988,0.6538039337244439,0.418946446321826,0.05405583835300709,0.4459529992938798
 1746011532.6572702,0.599140516261253,0.20445630828562025,0.6610246113769753,0.4189466823505827,0.054055881858017275,0.4459530217501919
 1746011532.6680627,0.59727847397296,0.21150681127341242,0.6682452495338352,0.4189469040591534,0.05405592433745181,0.44595303710155226
 1746011532.6788871,0.5954164335310711,0.21855732297702138,0.675465848139387,0.41894711100279125,0.054055965626099434,0.44595304518746165
 1746011532.6897402,0.5935543933383355,0.22560784271943266,0.6826864070575521,0.4189473027743174,0.054056005557748234,0.44595304587036744
 1746011532.7006192,0.5916923517749302,0.23265836973511314,0.689906926064208,0.41894747900621887,0.0540560439660668,0.4459530390368058
 1746011532.7115214,0.589830307199159,0.23970890317031993,0.6971274048391137,0.41894763937251167,0.05405608068548591,0.44595302459838476
 1746011532.7224438,0.5879682579473172,0.24675944208345774,0.7043478429572336,0.4189477835903714,0.05405611555207773,0.4459530024925937
 1746011532.7333837,0.5861062023326524,0.25380998544540284,0.7115682398793376,0.41894791142150195,0.05405614840441865,0.44595297268342715
 1746011532.7443378,0.5842441386433538,0.26086053213969906,0.7187885949417235,0.41894802267323883,0.05405617908441479,0.4459529351618146
 1746011532.7553036,0.582382065139506,0.2679110809625296,0.7260089073448978,0.4189481171993768,0.05405620743808544,0.4459528899458427
 1746011532.766278,0.5805199800489084,0.27496163062235307,0.7332291761410191,0.41894819490071916,0.05405623331628642,0.4459528370807702
 1746011532.777258,0.578657881561679,0.28201217973909004,0.7404494002198939,0.41894825572534605,0.0540562565753638,0.44595277663882227
 1746011532.7882407,0.5767957678235212,0.28906272684272516,0.7476695782932722,0.41894829966860114,0.05405627707771135,0.445952708718774
 1746011532.7992232,0.5749336369275324,0.2961132703711883,0.7548897088771448,0.41894832677281046,0.0540562946922452,0.4459526334453102
 1746011532.8102028,0.5730714869043956,0.30316380866735004,0.7621097902717207,0.41894833712673335,0.05405630929474836,0.44595255096817504
 1746011532.8211763,0.5712093157107727,0.31021433997495534,0.7693298205386735,0.4189483308647651,0.05405632076810223,0.4459524614611087
 1746011532.8321412,0.5693471212156821,0.3172648624332917,0.7765497974751978,0.41894830816590123,0.05405632900237528,0.4459523651205818
 1746011532.843094,0.5674849011845903,0.324315374070359,0.7837697185843172,0.4189482692524881,0.05405633389476564,0.44595226216433215
--- a/test/joint_trajectories_cart/trajectory_0016_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0016_cart.csv
@ -0,0 +1,35 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011532.9093502,0.5674853499674306,0.3242862163047914,0.7837520505570519,0.41894275091874245,0.05406598619636892,0.4459546385124911
 1746011532.9121642,0.5655203340044299,0.3243232139343958,0.7837811453682008,0.4189190125640916,0.05405405274687227,0.4459459617567151
 1746011532.9151247,0.5634728777509015,0.32432323296121107,0.7837808640921244,0.41891908771875325,0.054054065093887944,0.4459459801674847
 1746011532.9180827,0.56142538376228,0.32432322767354205,0.7837808693979561,0.41891916379478217,0.05405407750870588,0.4459459987475455
 1746011532.9210384,0.5593778897507958,0.32432322230419774,0.7837808746566378,0.4189192408141165,0.05405408999558747,0.44594601750175694
 1746011532.9239917,0.5573303956605581,0.324323216870689,0.783780879898348,0.4189193187915494,0.05405410255651824,0.4459460364329057
 1746011532.9269423,0.5552829014923439,0.32432321137257836,0.7837808851241632,0.4189193977421289,0.054054115193513566,0.4459460555438191
 1746011532.9298909,0.5532354072469263,0.32432320580930507,0.7837808903350302,0.4189194776811745,0.05405412790861878,0.44594607483736665
 1746011532.9328368,0.5511879129572292,0.3243232000219463,0.783780895473564,0.4189189999602915,0.054054066528201355,0.4459459653154673
 1746011532.9357805,0.5491404185276059,0.324323194483961,0.7837809007161218,0.4189190819233664,0.054054079405795256,0.44594598498304555
 1746011532.9387217,0.5470929240551087,0.3243231887222405,0.7837809058875711,0.4189191649224788,0.054054092367817566,0.44594600484211916
 1746011532.9416606,0.5450454295083246,0.32432318289251283,0.7837809110479097,0.4189192489741611,0.0540541054164527,0.44594602489575275
 1746011532.9445972,0.5429979348879981,0.32432317699467256,0.7837809161978866,0.4189193340951758,0.054054118553908466,0.44594604514704433
 1746011532.9475315,0.5409504401948257,0.3243231710281024,0.7837809213383895,0.4189194203025934,0.05405413178243131,0.4459460655991395
 1746011532.9504633,0.5389029454608026,0.32432316484919305,0.7837809264149049,0.4189190063301625,0.05405406707655031,0.44594596650076956
 1746011532.953393,0.5368554505927587,0.3243231588853115,0.7837809315949152,0.4189190947628819,0.054054080494091196,0.4459459873640832
 1746011532.95632,0.5348079556851494,0.32432315270920914,0.7837809367121189,0.41891918433505476,0.05405409400964625,0.44594600843792265
 1746011532.9592452,0.5327604607073285,0.3243231464613392,0.7837809418234594,0.41891927506511256,0.05405410762561882,0.44594602972564984
 1746011532.9621677,0.5307129656599798,0.32432314014149904,0.7837809469296537,0.41891936697176096,0.054054121344443117,0.44594605123066533
 1746011532.9650881,0.5286654705437356,0.3243231337490123,0.7837809520315433,0.4189194600740538,0.0540541351685951,0.44594607295642474
 1746011532.9680064,0.526617975395055,0.3243231271261544,0.7837809570743026,0.4189190133465497,0.054054067649949834,0.4459459677813797
 1746011532.970922,0.5245704801071371,0.324323120742288,0.7837809622262041,0.4189191088987512,0.054054081692357064,0.4459459899591836
 1746011532.9738357,0.5225229847879679,0.32432311412821635,0.7837809673200242,0.41891920570584323,0.054054095847763634,0.44594601236841264
 1746011532.976747,0.5204754894023057,0.32432310743816406,0.7837809724129858,0.41891930378842596,0.05405411011882692,0.44594603501276386
 1746011532.979656,0.5184279939507895,0.3243231006719233,0.7837809775057574,0.41891940316740844,0.05405412450824138,0.44594605789597763
 1746011532.9825628,0.5163804984340025,0.32432309382875696,0.7837809825991414,0.41891950386409554,0.05405413901874815,0.4459460810218539
 1746011532.9854672,0.5143330028933008,0.3243230867351692,0.7837809876380952,0.4189190210770627,0.054054068310297194,0.44594596918846346
 1746011532.9883695,0.5122855072069994,0.32432307990746007,0.7837809927914396,0.4189191244747295,0.05405408307141227,0.4459459928112815
 1746011532.9912696,0.5102380114978926,0.32432307282956124,0.7837809978912779,0.4189192292563803,0.05405409796212598,0.44594601668851686
 1746011532.9941673,0.5081905157257325,0.3243230656710888,0.7837810029950474,0.4189193354450786,0.05405411298538767,0.44594604082424105
 1746011532.997063,0.5061430198911258,0.3243230584318355,0.7837810081033706,0.41891944306423795,0.054054128144191756,0.4459460652225732
 1746011532.9999561,0.5040955240325694,0.32432305095389435,0.783781013170239,0.4189190281039311,0.05405406900093136,0.4459459704899269
 1746011533.0028472,0.5020480280368114,0.32432304370668014,0.783781018337226,0.41891913865609154,0.05405408444002193,0.4459459954260739
 1746011533.0057359,0.5000005320181706,0.324323036220767,0.7837810234636979,0.4189192507116003,0.054054100023951286,0.44594602063757643
--- a/test/joint_trajectories_cart/trajectory_0017_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0017_cart.csv
@ -0,0 +1,17 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011532.96937,0.4999799111984431,0.3243081283037476,0.7837763382146322,0.41893036666045197,0.05406520342098361,0.4459520469720618
 1746011532.9718719,0.498198598246477,0.3243232995029089,0.7837817133511014,0.41891902146016785,0.054054059442134345,0.4459459650099033
 1746011532.9744103,0.49639681535927316,0.32432331591000985,0.7837816523350581,0.4189191103759429,0.05405407144821279,0.44594598464693813
 1746011532.9769473,0.4945950175357502,0.3243233106893685,0.7837816558185404,0.4189192003558497,0.05405408356026246,0.4459460044781496
 1746011532.9794822,0.492793219698817,0.32432330538032317,0.7837816593398205,0.418919291424881,0.05405409578156464,0.44594602450820553
 1746011532.9820156,0.49099142182681615,0.3243233000189337,0.7837816628672264,0.4189193835992854,0.054054108114165125,0.44594604473988947
 1746011532.9845471,0.4891896239200019,0.32432329460485615,0.7837816664011746,0.41891947689557,0.054054120560140924,0.44594606517602114
 1746011532.987077,0.48738782601544817,0.3243232889859099,0.7837816699068272,0.41891901345481813,0.054054066267745,0.4459459664667732
 1746011532.9896052,0.4855860280030464,0.32432328361579815,0.7837816734906478,0.41891910904556084,0.05405407894685183,0.4459459873204016
 1746011532.9921315,0.4837842299934453,0.3243232780412712,0.7837816770464489,0.4189192058093559,0.054054091745767,0.4459460083871717
 1746011532.994656,0.4819824319500601,0.32432327241166403,0.7837816806104704,0.4189193037639644,0.05405410466671982,0.4459460296700891
 1746011532.9971788,0.48018063387319726,0.3243232667269524,0.7837816841829807,0.4189194029273745,0.05405411771196841,0.44594605117218783
 1746011532.9996998,0.47837883576312107,0.3243232609866381,0.7837816877643675,0.4189195033178884,0.054054130883808164,0.4459460728965463
 1746011533.0022192,0.4765770376606043,0.3243232550298062,0.783781691317646,0.418919020670677,0.054054066976875564,0.44594596776270296
 1746011533.0047367,0.47477523944441624,0.3243232493361584,0.7837816949557206,0.41891912357162425,0.05405408040895131,0.4459459899409692
 1746011533.0072525,0.47297344123632357,0.32432324342649127,0.7837816985659193,0.41891922775643586,0.05405409397473383,0.44594601235098746
--- a/test/joint_trajectories_cart/trajectory_0018_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0018_cart.csv
@ -0,0 +1,15 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011533.0190654,0.4729675835676446,0.32431919158166256,0.783782219687678,0.418922884054828,0.05405868097341197,0.4459483222897532
 1746011533.0219567,0.47089459816249213,0.32432287238180096,0.7837810330193966,0.4189190657577595,0.054054069850432804,0.44594597634878497
 1746011533.0248547,0.46881560483196083,0.3243228675515081,0.7837810199457409,0.41891920842841107,0.05405408868042115,0.4459460073194099
 1746011533.0277505,0.4667366080950193,0.3243228581755897,0.7837810255312853,0.41891935317054213,0.05405410772450029,0.44594603866277943
 1746011533.030644,0.46465761131310124,0.32432284868702294,0.7837810311435567,0.41891950002348666,0.05405412698769483,0.4459460703855337
 1746011533.033535,0.46257861452455523,0.3243228389036736,0.7837810367397424,0.4189190680570318,0.05405407308603246,0.4459459778929188
 1746011533.0364237,0.4604996175853681,0.32432282938741386,0.7837810424341896,0.4189192192454925,0.054054092801804524,0.4459460103931869
 1746011533.03931,0.45842062064063405,0.32432281957623044,0.7837810481133953,0.41891937266003854,0.054054112751202324,0.4459460432918138
 1746011533.0421937,0.4563416236426001,0.324322809654284,0.7837810538166952,0.41891952834086704,0.05405413293927991,0.445946076595379
 1746011533.045075,0.4542626266438649,0.3243227994218713,0.783781059506461,0.4189190770570686,0.054054073994403255,0.4459459794864965
 1746011533.0479538,0.45218362948898905,0.3243227894739724,0.7837810652987756,0.4189192373940573,0.05405409467538531,0.44594601362000447
 1746011533.0508304,0.4501046323345144,0.3243227792154759,0.7837810710783286,0.41891940012244544,0.05405411561077476,0.44594604817876654
 1746011533.0537043,0.44802563512886656,0.3243227688413813,0.7837810768850887,0.41891956528588714,0.05405413680605373,0.4459460831698299
 1746011533.0565755,0.44594663792894845,0.32432275813736583,0.7837810826820362,0.41891908672518635,0.05405407497758083,0.44594598118475093
--- a/test/joint_trajectories_cart/trajectory_0019_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0019_cart.csv
@ -0,0 +1,13 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011533.0714974,0.44594479370791107,0.3243229113593311,0.7837834733719453,0.41891998721071555,0.05405555708968378,0.445946672040322
 1746011533.074887,0.4434899234293596,0.3243221284925224,0.7837800252479045,0.4189191578719105,0.05405408390802372,0.4459459966004781
 1746011533.0782738,0.4410329262832197,0.3243221032886653,0.7837800326064785,0.4189194002158503,0.054054115351261894,0.4459460483515074
 1746011533.081657,0.43857593149369867,0.32432208526580847,0.7837800424032673,0.4189196469167678,0.054054147258543545,0.4459461008721819
 1746011533.0850365,0.43611893669538565,0.324322066742555,0.7837800521930054,0.41891917029277026,0.05405408569634271,0.4459459989852153
 1746011533.0884125,0.4336619416506908,0.32432204855344715,0.7837800621389451,0.41891942598366166,0.054054118566797926,0.44594605308849516
 1746011533.091785,0.431204946646144,0.3243220296329888,0.7837800720717742,0.4189191794136409,0.054054086899736536,0.44594600067556533
 1746011533.0951536,0.42874795144320704,0.32432201086746965,0.7837800821331815,0.4189194444943332,0.05405412078389863,0.44594605642453433
 1746011533.0985186,0.4262909562628118,0.3243219914405286,0.7837800922016209,0.41891918902008396,0.05405408791048293,0.44594600234282633
 1746011533.1018798,0.4238339608814064,0.3243219721801857,0.7837801024013001,0.41891946391135065,0.054054122861514,0.4459460598032368
 1746011533.1052372,0.421376965530512,0.3243219522339363,0.783780112612744,0.4189191990581946,0.05405408897376629,0.4459460040763636
 1746011533.108591,0.4189199699757924,0.32432193246638513,0.7837801229578205,0.4189194842024383,0.054054125047544234,0.44594606331668346
--- a/test/joint_trajectories_cart/trajectory_0020_cart.csv
+++ b/test/joint_trajectories_cart/trajectory_0020_cart.csv
@ -0,0 +1,10 @@
 timestamp,x,y,z,roll,pitch,yaw
 1746011533.1262534,0.41842447692181745,0.3246813746559968,0.783571645193747,0.4193546159992642,0.05398112551456086,0.4459945230852728
 1746011533.1290176,0.4165345722897328,0.3243238880878223,0.7837812215209303,0.4189189150143966,0.05405392159912383,0.4459458795883365
 1746011533.1311831,0.4149447971824438,0.32432330924252983,0.783782256231408,0.4189190379373616,0.05405393659333914,0.44594590418307367
 1746011533.133347,0.4133549745090436,0.32432330222803823,0.7837822593086075,0.4189191627286618,0.05405395180376138,0.4459459291070972
 1746011533.1355095,0.4117651512087156,0.32432329658904135,0.7837822621834206,0.4189192890305639,0.054053967185446875,0.44594595428888806
 1746011533.13767,0.41017532789440103,0.3243232909068814,0.7837822650719903,0.41891941686407186,0.054053982741013626,0.4459459797315213
 1746011533.139829,0.4085855045878922,0.32432328502953667,0.7837822680046664,0.418919048294228,0.05405406941982729,0.4459459715020369
 1746011533.1419864,0.4069956812244997,0.3243232793969047,0.7837822708929729,0.41891917925855926,0.05405408533160594,0.44594599747635805
 1746011533.144142,0.4054058578692489,0.32432327356954554,0.7837822738253393,0.41891931182272635,0.05405410142575136,0.4459460237215898
--- a/workspace/build/joint_control/build/lib/joint_control/plugdata.py
+++ b/workspace/build/joint_control/build/lib/joint_control/plugdata.py
@ -0,0 +1,209 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from sensor_msgs.msg import JointState
 from osc4py3.as_allthreads import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import time
 import numpy as np
 import os
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, joint_velocity_limits):
        super().__init__('scaled_joint_trajectory_publisher')
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            '/scaled_joint_trajectory_controller/joint_trajectory', 
            1
        )
        self.subscription = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_states_callback,
            1  # Increased queue size for joint states
        )
        # Store received joint positions
        self.current_joint_positions = [0.0] * len(joint_names)
        self.joint_names = joint_names
        self.joint_velocity_limits = joint_velocity_limits
        self.desired_joint_positions = [0.0] * len(joint_names)
        self.previous_desired = [0.0] * len(joint_names)
        ip = "0.0.0.0"  # Listen on all network interfaces
        port = 8000     # Must match the sender's port in `joint_state_osc.py`
        osc_startup()
        osc_udp_server(ip, port, "osc_server")
        print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [joint_positions] optional: duration as last argument")
        # Register OSC handler
        osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
        while True:
            try:
                self.x_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for x (space-separated, enter 'x' for no limit): ").split()]
                self.y_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for y (space-separated, enter 'x' for no limit): ").split()]
                self.z_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for z (space-separated, enter 'x' for no limit): ").split()]
                if len(self.x_limits) != 2 or len(self.y_limits) != 2 or len(self.z_limits) != 2:
                    print("Invalid input. Please enter exactly two values (or leave blank) for each limit.")
                    continue
                if (self.x_limits[0] is not None and self.x_limits[1] is not None and self.x_limits[0] >= self.x_limits[1]) or \
                (self.y_limits[0] is not None and self.y_limits[1] is not None and self.y_limits[0] >= self.y_limits[1]) or \
                (self.z_limits[0] is not None and self.z_limits[1] is not None and self.z_limits[0] >= self.z_limits[1]):
                    print("Invalid input. Lower limit must be less than upper limit for each axis.")
                    continue
                print(f"Current limits:")
                print(f"x: {self.x_limits}")
                print(f"y: {self.y_limits}")
                print(f"z: {self.z_limits}")
                confirm = input("Do you want your robot to move in this range? (y/n): ").strip().lower()
                if confirm == 'y':
                    break
                elif confirm == 'n':
                    print("Please re-enter the limits.")
                else:
                    print("Invalid input. Please enter 'y' or 'n'.")
            except ValueError:
                print("Invalid input. Please enter numeric values only.")
        # Ask the user if they want to set new joint limits
        update_limits = input("Do you want to set new joint limits? (y/n): ").strip().lower()
        if update_limits == 'y':
            for joint_name in self.joint_names:
                while True:
                    try:
                        print(f"Current position limits for joint '{joint_name}': {self.robot.links[joint_name]} rad")
                        lower_limit = input(f"Enter the new lower limit for joint '{joint_name}' (or press Enter to keep current): ").strip()
                        upper_limit = input(f"Enter the new upper limit for joint '{joint_name}' (or press Enter to keep current): ").strip()
                        if lower_limit is not None and upper_limit is not None and lower_limit >= upper_limit:
                            print("Invalid input. Lower limit must be less than upper limit.")
                            continue
                        if lower_limit:
                            self.robot.links[joint_name][0] = float(lower_limit)
                        if upper_limit:
                            self.robot.links[joint_name][1] = float(upper_limit)
                        break
                    except ValueError:
                        print("Invalid input. Please enter numeric values or leave blank to keep current limits.")
        self.hz = float(input("Enter the desired refresh frequency (Hz): "))
        # Start the OSC server in a separate thread to avoid blocking the ROS 2 event loop
        self.create_timer(1/self.hz, self.update_position)  # Timer to call osc_process periodically
    def joint_angles_handler(self, *args):
    # Ensure the desired joint positions are within the specified limits
        x, y, z, r, p, yaw, *_ = [float(i) for i in list(args)]
        if self.x_limits[0] is not None:
            x = max(self.x_limits[0], x)
        if self.x_limits[1] is not None:
            x = min(self.x_limits[1], x)
        if self.y_limits[0] is not None:
            y = max(self.y_limits[0], y)
        if self.y_limits[1] is not None:
            y = min(self.y_limits[1], y)
        if self.z_limits[0] is not None:
            z = max(self.z_limits[0], z)
        if self.z_limits[1] is not None:
            z = min(self.z_limits[1], z)
        if x != args[0] or y != args[1] or z != args[2]:
            self.get_logger().warn(
            f"Desired joint positions adjusted to fit within limits: "
            f"x={x}, y={y}, z={z} (original: x={args[0]}, y={args[1]}, z={args[2]})"
            )
        self.desired_joint_positions = [x, y, z, r, p, yaw]
    def joint_states_callback(self, msg):
        """Callback function to handle incoming joint states."""
        joint_names = msg.name  # List of joint names
        joint_position_dict = dict(zip(joint_names, self.current_joint_positions))
        self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
    def update_position(self):
        time1 = time.time()
        if self.desired_joint_positions == self.previous_desired:
            return
        duration = 0
        for p1, p2, max_vel in zip(self.desired_joint_positions, self.current_joint_positions, self.joint_velocity_limits.values()):
            duration = max(max(duration, abs(p1 - p2) / max_vel),1/self.hz)# as minimun
        duration *= 1.8
        #print(f"Duration: {duration}")
        #print(f'vel: {max(np.array(self.desired_joint_positions_joint_positions) - np.array(self.joint_positions)/duration)}')
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        point = JointTrajectoryPoint()
        point.positions = self.desired_joint_positions  # Updated joint positions
        point.time_from_start.sec = int(duration)
        #point.time_from_start.sec = 10
        point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
        #point.time_from_start.nanosec = 0
        #point.velocities = [i*0.95 for i in self.joint_velocity_limits.values()]
        msg.points.append(point)
        self.publisher.publish(msg)
        #print(f"desired: {self.desired_joint_positions}")
        #print(f"desired: {[180/3.141*i for i in self.desired_joint_positions]}")
        #print(f"current: {[180/3.141*i for i in self.current_joint_positions]}")
        self.previous_desired = self.desired_joint_positions
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    while True:
        path_to_urdf = input("Enter the path to the URDF file: ")
        if os.path.isfile(path_to_urdf):
            if not path_to_urdf.endswith('.urdf'):
                print("The file is not a URDF file. Please enter a valid URDF file.")
                continue
            break
        else:
            print("Invalid path. Please enter a valid path to the URDF file.")
    tree = ET.parse(path_to_urdf)
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    joint_velocity_limits = {}
    # Iterate over all joints in the URDF
    for joint in root.findall('.//joint'):
        joint_name = joint.get('name')  # Get the name of the joint
        # Look for the <limit> tag under each joint
        limit = joint.find('limit')
        if limit is not None:
            # Extract the velocity limit (if it exists)
            velocity_limit = limit.get('velocity')
            if velocity_limit is not None:
                joint_velocity_limits[joint_name] = float(velocity_limit)
    rclpy.init()
    node = ScaledJointTrajectoryPublisher(joint_names, joint_velocity_limits)
    # Run ROS 2 spin, and osc_process will be handled by the timer
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/build/joint_control/build/lib/joint_control/plugdata_cart.py
+++ b/workspace/build/joint_control/build/lib/joint_control/plugdata_cart.py
@ -0,0 +1,158 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from sensor_msgs.msg import JointState
 from osc4py3.as_allthreads import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import time
 import numpy as np
 import spatialmath as sm
 import roboticstoolbox as rtb
 import os
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, joint_velocity_limits, robot, cost_mask):
        super().__init__('scaled_joint_trajectory_publisher')
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            '/scaled_joint_trajectory_controller/joint_trajectory', 
            1
        )
        self.subscription = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_states_callback,
            1  # Increased queue size for joint states
        )
        # Store received joint positions
        self.current_joint_positions = [0.0] * len(joint_names)
        self.joint_names = joint_names
        self.joint_velocity_limits = joint_velocity_limits
        self.desired_joint_positions = [0.0] * len(joint_names)
        self.previous_desired = [0.0] * len(joint_names)
        self.robot = robot
        self.cost_mask = cost_mask
        ip = "0.0.0.0"  # Listen on all network interfaces
        port = 8000     # Must match the sender's port in `joint_state_osc.py`
        osc_startup()
        osc_udp_server(ip, port, "osc_server")
        print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [joint_positions] optional: duration as last argument")
        # Register OSC handler
        osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
        self.hz = float(input("Enter the desired refresh frequency (Hz): "))
        # Start the OSC server in a separate thread to avoid blocking the ROS 2 event loop
        self.create_timer(1/self.hz, self.update_position)  # Timer to call osc_process periodically
    def joint_angles_handler(self, *args):
        """Handles incoming OSC messages for joint positions."""
        self.desired_joint_positions = [float(i) for i in list(args)]
    def joint_states_callback(self, msg):
        """Callback function to handle incoming joint states."""
        joint_position_dict = dict(zip(msg.name, msg.position))
        self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
    def update_position(self):
        if self.desired_joint_positions == self.previous_desired:
            return
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        steps_per_m = 30
        if True: #len(args[0]) == len(self.joint_names):
            prev_duration = 0
            T1 = self.robot.fkine(self.current_joint_positions)
            [x,y,z] = T1.t
            [roll, pitch, yaw] = T1.rpy()
            x1, y1, z1, roll1, pitch1, yaw1 = self.desired_joint_positions
            steps = int(np.linalg.norm(np.array([x1-x, y1-y, z1-z])) * steps_per_m)
            if steps < 2: steps = 2
            cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)]
            for j in range(steps):
                print(cart_traj[j])
                sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True)
                if sol[1] == 1:
                    duration = 0
                    prev = self.current_joint_positions if j == 0 else prev_sol
                    for p1, p2, max_vel in zip(sol[0], prev, self.joint_velocity_limits.values()):
                        duration = max(duration, abs(p1 - p2) / max_vel)#, 1/self.hz) # as minimun
                    point = JointTrajectoryPoint()
                    point.positions = list(sol[0])
                    #duration *= 2
                    duration += prev_duration
                    prev_duration = duration
                    point.time_from_start.sec = int(duration)
                    point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
                    msg.points.append(point)
                    prev_sol = list(sol[0])
                else:
                    print('IK could not find a solution!')
                    prev_sol = self.current_joint_positions
        msg.header.stamp = self.get_clock().now().to_msg()
        self.publisher.publish(msg)
        self.previous_desired = self.desired_joint_positions
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    robot_urdf = input("Enter the path to the URDF file: ")
    tree = ET.parse(robot_urdf)
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    robot = rtb.ERobot.URDF(robot_urdf)
    joint_velocity_limits = {}
    # Iterate over all joints in the URDF
    for joint in root.findall('.//joint'):
        joint_name = joint.get('name')  # Get the name of the joint
        # Look for the <limit> tag under each joint
        limit = joint.find('limit')
        if limit is not None:
            # Extract the velocity limit (if it exists)
            velocity_limit = limit.get('velocity')
            if velocity_limit is not None:
                joint_velocity_limits[joint_name] = float(velocity_limit)
    while True:
        try:
            print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
            print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
            cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
            if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
                break
            else:
                print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
        except ValueError:
            print("Invalid input. Please enter integers only.")
    print(f"Cost mask: {cost_mask}")
    rclpy.init()
    node = ScaledJointTrajectoryPublisher(joint_names, joint_velocity_limits, robot, cost_mask)
    # Run ROS 2 spin, and osc_process will be handled by the timer
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/build/joint_control/build/lib/joint_control/plugdata_cart_fix.py
+++ b/workspace/build/joint_control/build/lib/joint_control/plugdata_cart_fix.py
@ -0,0 +1,264 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from sensor_msgs.msg import JointState
 from osc4py3.as_allthreads import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import numpy as np
 import spatialmath as sm
 import roboticstoolbox as rtb
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, joint_velocity_limits, robot, cost_mask):
        super().__init__('scaled_joint_trajectory_publisher')
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            '/scaled_joint_trajectory_controller/joint_trajectory', 
            1
        )
        self.subscription = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_states_callback,
            1  # Increased queue size for joint states
        )
        # Store received joint positions
        self.current_joint_positions = [0.0] * len(joint_names)
        self.joint_names = joint_names
        self.joint_velocity_limits = joint_velocity_limits
        self.desired_joint_positions = [0.0] * len(joint_names)
        self.previous_desired = [0.0] * len(joint_names)
        self.robot = robot
        self.cost_mask = cost_mask
        self.prev_pose = None
        ip = "0.0.0.0"  # Listen on all network interfaces
        port = 8000     # Must match the sender's port in `joint_state_osc.py`
        osc_startup()
        osc_udp_server(ip, port, "osc_server")
        print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [joint_positions] optional: duration as last argument")
        # Register OSC handler
        osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
        set_limits = input("Do you want to set limit for x, y and z? (y/n): ").strip().lower()
        if set_limits == 'y':
            while True:
                        try:
                            self.x_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for x (space-separated, enter 'x' for no limit): ").split()]
                            self.y_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for y (space-separated, enter 'x' for no limit): ").split()]
                            self.z_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for z (space-separated, enter 'x' for no limit): ").split()]
                            if len(self.x_limits) != 2 or len(self.y_limits) != 2 or len(self.z_limits) != 2:
                                print("Invalid input. Please enter exactly two values (or leave blank) for each limit.")
                                continue
                            if (self.x_limits[0] is not None and self.x_limits[1] is not None and self.x_limits[0] >= self.x_limits[1]) or \
                            (self.y_limits[0] is not None and self.y_limits[1] is not None and self.y_limits[0] >= self.y_limits[1]) or \
                            (self.z_limits[0] is not None and self.z_limits[1] is not None and self.z_limits[0] >= self.z_limits[1]):
                                print("Invalid input. Lower limit must be less than upper limit for each axis.")
                                continue
                            print(f"Current limits:")
                            print(f"x: {self.x_limits}")
                            print(f"y: {self.y_limits}")
                            print(f"z: {self.z_limits}")
                            con = True
                            while con:
                                confirm = input("Do you want your robot to move in this range? (y/n): ").strip().lower()
                                if confirm == 'y':
                                    break
                                elif confirm == 'n':
                                    print("Please re-enter the limits.")
                                    con = False
                                else:
                                    print("Invalid input. Please enter 'y' or 'n'.")
                            if con: break
                        except ValueError:
                            print("Invalid input. Please enter numeric values only.")
                # Ask the user if they want to set new joint limits
        update_limits = input("Do you want to set new joint limits? (y/n): ").strip().lower()
        if update_limits == 'y':
            for i in range(len(self.joint_names)):
                while True:
                    try:
                        lim = self.robot.qlim.copy()
                        # Find the link corresponding to the joint name
                        print(f"Current position limits for joint '{self.joint_names[i]}': [{self.robot.qlim[0][i]} {self.robot.qlim[1][i]}] rad")
                        lower_limit = input(f"Enter the new lower limit for joint '{self.joint_names[i]}' (or press Enter to keep current): ").strip()
                        upper_limit = input(f"Enter the new upper limit for joint '{self.joint_names[i]}' (or press Enter to keep current): ").strip()
                        if lower_limit and upper_limit and float(lower_limit) >= float(upper_limit):
                            print("Invalid input. Lower limit must be less than upper limit.")
                            continue
                        if lower_limit:
                            lim[0][i] = float(lower_limit)
                        if upper_limit:
                            lim[1][i] = float(upper_limit)
                        self.robot.qlim = lim
                        print(f"New limits for joint '{self.joint_names[i]}': [{self.robot.qlim[0][i]} {self.robot.qlim[1][i]}] rad")
                        print("-" * 50)
                        break
                    except ValueError:
                        print("Invalid input. Please enter numeric values or leave blank to keep current limits.")
        self.hz = float(input("Enter the desired refresh frequency (Hz): "))
        # Start the OSC server in a separate thread to avoid blocking the ROS 2 event loop
        self.create_timer(1/self.hz, self.update_position)  # Timer to call osc_process periodically
    def joint_angles_handler(self, *args):
    # Ensure the desired joint positions are within the specified limits
        print("received joint angles")
        x, y, z, r, p, yaw, *_ = [float(i) for i in list(args)]
        if self.x_limits[0] is not None:
            x = max(self.x_limits[0], x)
        if self.x_limits[1] is not None:
            x = min(self.x_limits[1], x)
        if self.y_limits[0] is not None:
            y = max(self.y_limits[0], y)
        if self.y_limits[1] is not None:
            y = min(self.y_limits[1], y)
        if self.z_limits[0] is not None:
            z = max(self.z_limits[0], z)
        if self.z_limits[1] is not None:
            z = min(self.z_limits[1], z)
        if x != args[0] or y != args[1] or z != args[2]:
            self.get_logger().warn(
            f"Desired joint positions adjusted to fit within limits: "
            f"x={x}, y={y}, z={z} (original: x={args[0]}, y={args[1]}, z={args[2]})"
            )
        self.desired_joint_positions = [x, y, z, r, p, yaw]
    def joint_states_callback(self, msg):
        """Callback function to handle incoming joint states."""
        joint_position_dict = dict(zip(msg.name, msg.position))
        self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
        joint_position_dict = dict(zip(msg.name, msg.velocity))
        self.current_joint_velocities = [joint_position_dict[name] for name in self.joint_names]
    def update_position(self):
        if self.desired_joint_positions == self.previous_desired:
            return
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        steps_per_m = 100
        if True: #len(args[0]) == len(self.joint_names):
            prev_duration = 0
            if self.prev_pose == None: 
                [x,y,z] = self.robot.fkine(self.current_joint_positions).t
                [roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
            else:
                [x,y,z] = self.prev_pose[:3]
                [roll, pitch, yaw] = self.prev_pose[3:]
            x1, y1, z1, roll1, pitch1, yaw1 = self.desired_joint_positions
            self.prev_pose = self.desired_joint_positions
            steps = int(np.linalg.norm(np.array([x1, y1, z1])- self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
            if steps < 2: steps = 2
            cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)]
            for j in range(steps):
                sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
                if sol[1] == 1:
                    fowards = self.robot.fkine_all(sol[0])
                    out_of_bounds = (fowards.t[1:,0] > self.x_limits[1] if self.x_limits[1] != None else False) | (fowards.t[1:,0] < self.x_limits[0] if self.x_limits[0] != None else False) | (fowards.t[1:,1] > self.y_limits[1] if self.y_limits[1] != None else False) | (fowards.t[1:,1] < self.y_limits[0] if self.y_limits[0] != None else False) | (fowards.t[1:,2] > self.z_limits[1] if self.z_limits[1] != None else False) | (fowards.t[1:,2] < self.z_limits[0] if self.z_limits[0] != None else False)
                    if np.any(out_of_bounds):
                        #print(fowards.t)
                        #indices = np.where(out_of_bounds)[0]
                        #print(f"indices: {indices}")
                        self.get_logger().warn("One or more links moved out of bounds!")
                        '''
                        for i in indices:
                            try:
                                print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}")
                            except IndexError:
                                print(f"index {i} is out of bounds, but no corresponding joint found.")
                        self.previous_desired = self.desired_joint_positions
                        '''
                        break
                    duration = 0
                    prev = self.current_joint_positions if j == 0 else prev_sol
                    for p1, p2, max_vel in zip(sol[0], prev, self.joint_velocity_limits.values()):
                        duration = max(duration, abs(p1 - p2) / max_vel)#, 1/self.hz) # as minimun
                    prev_sol = list(sol[0])
                    if duration == 0:
                        continue
                    point = JointTrajectoryPoint()
                    point.positions = list(sol[0])
                    duration *= 2
                    duration += prev_duration
                    prev_duration = duration
                    point.time_from_start.sec = int(duration)
                    point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
                    msg.points.append(point)
                else:
                    print(f'IK could not find a solution for (x,y,z) = {cart_traj[j].t} and (r,p,y) = {cart_traj[j].rpy()}!')
                    prev_sol = self.current_joint_positions
        if len(msg.points) == 0:
            return
        msg.header.stamp = self.get_clock().now().to_msg()
        self.publisher.publish(msg)
        self.previous_desired = self.desired_joint_positions
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    robot_urdf = input("Enter the path to the URDF file: ")
    tree = ET.parse(robot_urdf)
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    robot = rtb.ERobot.URDF(robot_urdf)
    joint_velocity_limits = {}
    # Iterate over all joints in the URDF
    for joint in root.findall('.//joint'):
        joint_name = joint.get('name')  # Get the name of the joint
        # Look for the <limit> tag under each joint
        limit = joint.find('limit')
        if limit is not None:
            # Extract the velocity limit (if it exists)
            velocity_limit = limit.get('velocity')
            if velocity_limit is not None:
                joint_velocity_limits[joint_name] = float(velocity_limit)
    while True:
        try:
            print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
            print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
            cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
            if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
                break
            else:
                print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
        except ValueError:
            print("Invalid input. Please enter integers only.")
    print(f"Cost mask: {cost_mask}")
    rclpy.init()
    node = ScaledJointTrajectoryPublisher(joint_names, joint_velocity_limits, robot, cost_mask)
    # Run ROS 2 spin, and osc_process will be handled by the timer
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/build/joint_control/build/lib/joint_control/plugdata_cart_smooth.py
+++ b/workspace/build/joint_control/build/lib/joint_control/plugdata_cart_smooth.py
@ -0,0 +1,232 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from sensor_msgs.msg import JointState
 from osc4py3.as_allthreads import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import time
 import numpy as np
 import spatialmath as sm
 import roboticstoolbox as rtb
 import os
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, joint_velocity_limits, robot, cost_mask):
        super().__init__('scaled_joint_trajectory_publisher')
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            '/scaled_joint_trajectory_controller/joint_trajectory', 
            1
        )
        self.subscription = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_states_callback,
            1  # Increased queue size for joint states
        )
        # Store received joint positions
        self.current_joint_positions = [0.0] * len(joint_names)
        self.joint_names = joint_names
        self.joint_velocity_limits = joint_velocity_limits
        self.desired_joint_positions = [0.0] * len(joint_names)
        self.previous_desired = [0.0] * len(joint_names)
        self.robot = robot
        self.cost_mask = cost_mask
        ip = "0.0.0.0"  # Listen on all network interfaces
        port = 8000     # Must match the sender's port in `joint_state_osc.py`
        osc_startup()
        osc_udp_server(ip, port, "osc_server")
        print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [joint_positions] optional: duration as last argument")
        # Register OSC handler
        osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
        self.hz = float(input("Enter the desired refresh frequency (Hz): "))
        # Start the OSC server in a separate thread to avoid blocking the ROS 2 event loop
        self.create_timer(1/self.hz, self.update_position)  # Timer to call osc_process periodically
        while True:
            try:
                self.x_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for x (space-separated, enter 'x' for no limit): ").split()]
                self.y_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for y (space-separated, enter 'x' for no limit): ").split()]
                self.z_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for z (space-separated, enter 'x' for no limit): ").split()]
                if len(self.x_limits) != 2 or len(self.y_limits) != 2 or len(self.z_limits) != 2:
                    print("Invalid input. Please enter exactly two values (or leave blank) for each limit.")
                    continue
                if (self.x_limits[0] is not None and self.x_limits[1] is not None and self.x_limits[0] >= self.x_limits[1]) or \
                (self.y_limits[0] is not None and self.y_limits[1] is not None and self.y_limits[0] >= self.y_limits[1]) or \
                (self.z_limits[0] is not None and self.z_limits[1] is not None and self.z_limits[0] >= self.z_limits[1]):
                    print("Invalid input. Lower limit must be less than upper limit for each axis.")
                    continue
                print(f"Current limits:")
                print(f"x: {self.x_limits}")
                print(f"y: {self.y_limits}")
                print(f"z: {self.z_limits}")
                confirm = input("Do you want your robot to move in this range? (y/n): ").strip().lower()
                if confirm == 'y':
                    break
                elif confirm == 'n':
                    print("Please re-enter the limits.")
                else:
                    print("Invalid input. Please enter 'y' or 'n'.")
            except ValueError:
                print("Invalid input. Please enter numeric values only.")
    def joint_angles_handler(self, *args):
    # Ensure the desired joint positions are within the specified limits
        x, y, z, r, p, yaw, *_ = [float(i) for i in list(args)]
        if self.x_limits[0] is not None:
            x = max(self.x_limits[0], x)
        if self.x_limits[1] is not None:
            x = min(self.x_limits[1], x)
        if self.y_limits[0] is not None:
            y = max(self.y_limits[0], y)
        if self.y_limits[1] is not None:
            y = min(self.y_limits[1], y)
        if self.z_limits[0] is not None:
            z = max(self.z_limits[0], z)
        if self.z_limits[1] is not None:
            z = min(self.z_limits[1], z)
        if x != args[0] or y != args[1] or z != args[2]:
            self.get_logger().warn(
            f"Desired joint positions adjusted to fit within limits: "
            f"x={x}, y={y}, z={z} (original: x={args[0]}, y={args[1]}, z={args[2]})"
            )
        self.desired_joint_positions = [x, y, z, r, p, yaw]
    def joint_states_callback(self, msg):
        """Callback function to handle incoming joint states."""
        joint_position_dict = dict(zip(msg.name, msg.position))
        self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
    def rampfunction(self, startvalue, blendtime, currenttime):
        """
        Ramp function to create a smooth transition from startvalue to 1 over blendtime seconds.
        """
        if currenttime < blendtime:
            return startvalue + (1 - startvalue) * (currenttime / blendtime)
        else:
            return 1
    def update_position(self):
        if self.desired_joint_positions == self.previous_desired:
            return
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        steps_per_m = 30
        if True: #len(args[0]) == len(self.joint_names):
            prev_duration = 0
            T1 = self.robot.fkine(self.current_joint_positions)
            [x,y,z] = T1.t
            [roll, pitch, yaw] = T1.rpy()
            x1, y1, z1, roll1, pitch1, yaw1 = self.desired_joint_positions
            steps = int(np.linalg.norm(np.array([x1-x, y1-y, z1-z])) * steps_per_m)
            if steps < 2: steps = 2
            cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)]
            for j in range(steps):
                sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True)
                if sol[1] == 1:
                    duration = 0
                    prev = self.current_joint_positions if j == 0 else prev_sol
                    for p1, p2, max_vel in zip(sol[0], prev, self.joint_velocity_limits.values()):
                        duration = max(duration, abs(p1 - p2) / max_vel)#, 1/self.hz) # as minimun
                    point = JointTrajectoryPoint()
                    point.positions = list(sol[0])
                    duration /= self.rampfunction(0.1, 2, prev_duration)
                    duration += prev_duration
                    prev_duration = duration
                    point.time_from_start.sec = int(duration)
                    point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
                    msg.points.append(point)
                    prev_sol = list(sol[0])
                else:
                    print(f'IK could not find a solution for (x,y,z) = ({cart_traj[j].t}), (roll,pitch,yaw) = ({cart_traj[j].rpy()})!')
                    prev_sol = self.current_joint_positions
        msg.header.stamp = self.get_clock().now().to_msg()
        self.publisher.publish(msg)
        self.previous_desired = self.desired_joint_positions
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    while True:
        path_to_urdf = input("Enter the path to the URDF file: ")
        if os.path.isfile(path_to_urdf):
            if not path_to_urdf.endswith('.urdf'):
                print("The file is not a URDF file. Please enter a valid URDF file.")
                continue
            break
        else:
            print("Invalid path. Please enter a valid path to the URDF file.")
    tree = ET.parse(path_to_urdf)
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    robot = rtb.ERobot.URDF(path_to_urdf)
    joint_velocity_limits = {}
    # Iterate over all joints in the URDF
    for joint in root.findall('.//joint'):
        joint_name = joint.get('name')  # Get the name of the joint
        # Look for the <limit> tag under each joint
        limit = joint.find('limit')
        if limit is not None:
            # Extract the velocity limit (if it exists)
            velocity_limit = limit.get('velocity')
            if velocity_limit is not None:
                joint_velocity_limits[joint_name] = float(velocity_limit)
    print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a Cartesian coordinate [x, y, z, roll, pitch, yaw].")
    print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotation.")
    while True:
        try:
            cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
            if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
                considered_coords = [coord for coord, use in zip(['x', 'y', 'z', 'roll', 'pitch', 'yaw'], cost_mask) if use == 1]
                print(f"The following coordinates will be considered for IK: {', '.join(considered_coords)}")
                confirm = input("Are you sure you want to proceed with this cost mask? (y/n): ").strip().lower()
                if confirm == 'y':
                    break
                elif confirm == 'n':
                    print("Please re-enter the cost mask.")
                else:
                    print("Invalid input. Please enter 'y' or 'n'.")
            else:
                print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
        except ValueError:
            print("Invalid input. Please enter integers only.")
    print(f"Cost mask: {cost_mask}")
    rclpy.init()
    node = ScaledJointTrajectoryPublisher(joint_names, joint_velocity_limits, robot, cost_mask)
    # Run ROS 2 spin, and osc_process will be handled by the timer
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/build/joint_control/build/lib/joint_control/test.py
+++ b/workspace/build/joint_control/build/lib/joint_control/test.py
@ -0,0 +1,36 @@
 import numpy as np
 from roboticstoolbox.tools.trajectory import mstraj
 # Define via points (each row is a joint configuration)
 viapoints = np.array([
    [0, 0, 0],         # Start
    [0.5, 0.2, -0.1],  # Intermediate
    [1.0, 0.4, 0.2]    # End
 ])
 # Time step
 dt = 0.01  # seconds
 # Acceleration time
 tacc = 0.2  # seconds
 # Maximum joint velocity per joint (same length as number of joints)
 qdmax = [0.5, 0.3, 0.4]  # radians per second
 # Optional: starting position (otherwise uses first viapoint)
 q0 = viapoints[0]
 # Generate the trajectory
 traj = mstraj(
    viapoints=viapoints,
    dt=dt,
    tacc=tacc,
    qdmax=qdmax,
 )
 # Extract trajectory
 time = traj.t          # Time vector
 positions = traj.q     # Joint angles (shape: K x N)
 print("Time vector:", time)
 print("Joint positions:\n", positions)
--- a/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast.py
+++ b/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast.py
@ -0,0 +1,141 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from osc4py3.as_eventloop import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import roboticstoolbox as rtb
 import spatialmath as sm
 import numpy as np
 import time
 import os
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, robot, cost_mask, joint_velocity_limits):
        super().__init__('scaled_joint_trajectory_publisher')
        self.joint_velocity_limits = joint_velocity_limits
        self.cost_mask = cost_mask
        self.robot = robot
        self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
        if self.trajectroy_topic_name == "":
            self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            self.trajectroy_topic_name, 
            10
        )
        # Store received joint positions
        self.joint_names = joint_names
        self.port = 8000 # UDP port
        osc_startup()
        osc_udp_server("0.0.0.0", self.port, "osc_server")
        print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
        # Register OSC handler
        osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
        print("OSC method registered for /joint_trajectory")
    def joint_angles_handler(self, *args):
        """Handles incoming OSC messages for joint positions."""
        print("Received joint positions")
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        steps = 50
        if True: #len(args[0]) == len(self.joint_names):
            prev_duration = 0
            for i in range(len(args)-1):
                x, y, z, roll, pitch, yaw = args[i]
                x1, y1, z1, roll1, pitch1, yaw1 = args[i+1]
                cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i], order='xyz') for i in range(steps)]
                prev_sol = [0.0,0.0,0.0,0.0,0.0,0.0] if i == 0  else sol[0]
                for j in (range(steps) if i == 0 else range(1,steps)):
                    #print(cart_traj[j])
                    sol = self.robot.ik_LM(cart_traj[j], q0=[0.0] * len(self.joint_names), mask = self.cost_mask, joint_limits = True) if i == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True)
                    if sol[1] == 1:
                        if list(sol[0])==list(prev_sol): continue
                        duration = 0
                        for p1, p2, max_vel in zip(sol[0], prev_sol, self.joint_velocity_limits.values()):
                            duration = max(duration, abs(p1 - p2) / max_vel) # as minimun
                        point = JointTrajectoryPoint()
                        point.positions = list(sol[0])
                        duration *= 1.6
                        duration += prev_duration
                        prev_duration = duration
                        point.time_from_start.sec = int(duration)
                        point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
                        msg.points.append(point)
                        prev_sol = list(sol[0])
                    else: print('IK could not find a solution!')
        msg.header.stamp = self.get_clock().now().to_msg()
        self.publisher.publish(msg)
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    while True:
        path_to_urdf = input("Enter the path to the URDF file: ")
        if os.path.isfile(path_to_urdf):
            if not path_to_urdf.endswith('.urdf'):
                print("The file is not a URDF file. Please enter a valid URDF file.")
                continue
            break
        else:
            print("Invalid path. Please enter a valid path to the URDF file.")
    tree = ET.parse(path_to_urdf)
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    robot = rtb.ERobot.URDF(path_to_urdf)
    print(robot)
    joint_velocity_limits = {}
    # Iterate over all joints in the URDF
    for joint in root.findall('.//joint'):
        joint_name = joint.get('name')  # Get the name of the joint
        # Look for the <limit> tag under each joint
        limit = joint.find('limit')
        if limit is not None:
            # Extract the velocity limit (if it exists)
            velocity_limit = limit.get('velocity')
            if velocity_limit is not None:
                joint_velocity_limits[joint_name] = float(velocity_limit)
    rclpy.init()
    while True:
        try:
            print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
            print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
            cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
            if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
                break
            else:
                print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
        except ValueError:
            print("Invalid input. Please enter integers only.")
    print(f"Cost mask: {cost_mask}")
    node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask, joint_velocity_limits)
    # Run both ROS 2 and OSC Server together
    try:
        while rclpy.ok():
            osc_process()  # Handle one OSC request at a time
            rclpy.spin_once(node, timeout_sec=0.1)  # Process ROS callbacks
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast_max_acc.py
+++ b/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast_max_acc.py
@ -0,0 +1,166 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from sensor_msgs.msg import JointState
 from osc4py3.as_allthreads import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import roboticstoolbox as rtb
 import spatialmath as sm
 import numpy as np
 import time
 import os
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, robot, cost_mask, joint_velocity_limits):
        super().__init__('scaled_joint_trajectory_publisher')
        self.joint_velocity_limits = joint_velocity_limits
        self.cost_mask = cost_mask
        self.robot = robot
        self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
        if self.trajectroy_topic_name == "":
            self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            self.trajectroy_topic_name, 
            10
        )
        self.subscription = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_states_callback,
            1  # Increased queue size for joint states
            )
        self.maximum_acceleration = [0.0] * len(joint_names)
        # Store received joint positions
        self.joint_names = joint_names
        for joint in joint_names:
            self.maximum_acceleration[joint_names.index(joint)] = float(input(f"Enter the maximum acceleration for joint {joint}: "))
        self.port = 8000 # UDP port
        osc_startup()
        osc_udp_server("0.0.0.0", self.port, "osc_server")
        print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
        # Register OSC handler
        osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
        print("OSC method registered for /joint_trajectory")
    def joint_states_callback(self, msg):
        """Callback function to handle incoming joint states."""
        joint_position_dict = dict(zip(msg.name, msg.position))
        self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
        joint_velocity_dict = dict(zip(msg.name, msg.velocity))
        self.current_joint_velocities = [joint_velocity_dict[name] for name in self.joint_names]
    def joint_angles_handler(self, *args):
        """Handles incoming OSC messages for joint positions."""
        print("Received joint positions")
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        steps = 50
        if True: #len(args[0]) == len(self.joint_names):
            prev_duration = 0
            for i in range(len(args)-1):
                x, y, z, roll, pitch, yaw = args[i]
                x1, y1, z1, roll1, pitch1, yaw1 = args[i+1]
                cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i], order='xyz') for i in range(steps)]
                prev_sol = self.current_joint_positions if i == 0  else sol[0]
                for j in (range(steps) if i == 0 else range(1,steps)):
                    #print(cart_traj[j])
                    sol = self.robot.ik_LM(cart_traj[j], q0=[0.0] * len(self.joint_names), mask = self.cost_mask, joint_limits = True) if i == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True)
                    if sol[1] == 1:
                        if list(sol[0])==list(prev_sol): continue
                        duration = 0
                        for i, (p1, p2, max_vel) in enumerate(zip(sol[0], prev_sol, self.joint_velocity_limits.values())):
                            print(f'joint {i}, p1: {p1}, p2: {p2}, max_vel: {max_vel}')
                            if len(msg.points) == 0: v = self.current_joint_velocities[i]
                            max_acc_duration = np.sqrt((v/self.maximum_acceleration[i])**2 + 2*(abs(p1 - p2)/self.maximum_acceleration[i]))- v/self.maximum_acceleration[i]
                            duration = max(duration, abs(p1 - p2) / max_vel, max_acc_duration) # as minimun
                            v = abs(p1 - p2) / duration
                            print(f'duration: {duration}, max_acc_duration: {max_acc_duration}, max_vel_duration: { abs(p1 - p2) / max_vel}, v: {v}')
                        point = JointTrajectoryPoint()
                        point.positions = list(sol[0])
                        duration += prev_duration
                        prev_duration = duration
                        point.time_from_start.sec = int(duration)
                        point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
                        msg.points.append(point)
                        prev_sol = list(sol[0])
                    else: print('IK could not find a solution!')
        msg.header.stamp = self.get_clock().now().to_msg()
        self.publisher.publish(msg)
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    while True:
        path_to_urdf = input("Enter the path to the URDF file: ")
        if os.path.isfile(path_to_urdf):
            if not path_to_urdf.endswith('.urdf'):
                print("The file is not a URDF file. Please enter a valid URDF file.")
                continue
            break
        else:
            print("Invalid path. Please enter a valid path to the URDF file.")
    tree = ET.parse(path_to_urdf)
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    robot = rtb.ERobot.URDF(path_to_urdf)
    print(robot)
    joint_velocity_limits = {}
    # Iterate over all joints in the URDF
    for joint in root.findall('.//joint'):
        joint_name = joint.get('name')  # Get the name of the joint
        # Look for the <limit> tag under each joint
        limit = joint.find('limit')
        if limit is not None:
            # Extract the velocity limit (if it exists)
            velocity_limit = limit.get('velocity')
            if velocity_limit is not None:
                joint_velocity_limits[joint_name] = float(velocity_limit)
    rclpy.init()
    while True:
        try:
            print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
            print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
            cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
            if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
                break
            else:
                print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
        except ValueError:
            print("Invalid input. Please enter integers only.")
    print(f"Cost mask: {cost_mask}")
    node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask, joint_velocity_limits)
    # Run both ROS 2 and OSC Server together
    try:
        while rclpy.ok():
            osc_process()  # Handle one OSC request at a time
            rclpy.spin_once(node, timeout_sec=0.1)  # Process ROS callbacks
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast_smooth.py
+++ b/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast_smooth.py
@ -0,0 +1,152 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from osc4py3.as_eventloop import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import roboticstoolbox as rtb
 import spatialmath as sm
 import numpy as np
 import time
 import os
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, robot, cost_mask, joint_velocity_limits):
        super().__init__('scaled_joint_trajectory_publisher')
        self.joint_velocity_limits = joint_velocity_limits
        self.cost_mask = cost_mask
        self.robot = robot
        self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
        if self.trajectroy_topic_name == "":
            self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            self.trajectroy_topic_name, 
            10
        )
        # Store received joint positions
        self.joint_names = joint_names
        self.port = 8000 # UDP port
        osc_startup()
        osc_udp_server("0.0.0.0", self.port, "osc_server")
        print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
        # Register OSC handler
        osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
        print("OSC method registered for /joint_trajectory")
    def rampfunction(self, startvalue, blendtime, currenttime):
            """
            Ramp function to create a smooth transition from startvalue to 1 over blendtime seconds.
            """
            if currenttime < blendtime:
                return startvalue + (1 - startvalue) * (currenttime / blendtime)
            else:
                return 1
    def joint_angles_handler(self, *args):
        """Handles incoming OSC messages for joint positions."""
        print("Received joint positions")
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        steps = 50
        if True: #len(args[0]) == len(self.joint_names):
            prev_duration = 0
            for i in range(len(args)-1):
                x, y, z, roll, pitch, yaw = args[i]
                x1, y1, z1, roll1, pitch1, yaw1 = args[i+1]
                cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i], order='xyz') for i in range(steps)]
                prev_sol = [0.0,0.0,0.0,0.0,0.0,0.0] if i == 0  else sol[0]
                for j in (range(steps) if i == 0 else range(1,steps)):
                    #print(cart_traj[j])
                    sol = self.robot.ik_LM(cart_traj[j], q0=[0.0] * len(self.joint_names), mask = self.cost_mask, joint_limits = True) if i == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True)
                    if sol[1] == 1:
                        if list(sol[0])==list(prev_sol): continue
                        duration = 0
                        for p1, p2, max_vel in zip(sol[0], prev_sol, self.joint_velocity_limits.values()):
                            duration = max(duration, abs(p1 - p2) / max_vel) # as minimun
                        point = JointTrajectoryPoint()
                        point.positions = list(sol[0])
                        duration /= self.rampfunction(0.1, 2, prev_duration)
                        duration += prev_duration
                        prev_duration = duration
                        point.time_from_start.sec = int(duration)
                        point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
                        msg.points.append(point)
                        prev_sol = list(sol[0])
                    else: print('IK could not find a solution!')
        msg.header.stamp = self.get_clock().now().to_msg()
        self.publisher.publish(msg)
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    while True:
        path_to_urdf = input("Enter the path to the URDF file: ")
        if os.path.isfile(path_to_urdf):
            if not path_to_urdf.endswith('.urdf'):
                print("The file is not a URDF file. Please enter a valid URDF file.")
                continue
            break
        else:
            print("Invalid path. Please enter a valid path to the URDF file.")
    tree = ET.parse(path_to_urdf)
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    robot = rtb.ERobot.URDF(path_to_urdf)
    print(robot)
    joint_velocity_limits = {}
    # Iterate over all joints in the URDF
    for joint in root.findall('.//joint'):
        joint_name = joint.get('name')  # Get the name of the joint
        # Look for the <limit> tag under each joint
        limit = joint.find('limit')
        if limit is not None:
            # Extract the velocity limit (if it exists)
            velocity_limit = limit.get('velocity')
            if velocity_limit is not None:
                joint_velocity_limits[joint_name] = float(velocity_limit)
    rclpy.init()
    while True:
        try:
            print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
            print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
            cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
            if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
                break
            else:
                print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
        except ValueError:
            print("Invalid input. Please enter integers only.")
    print(f"Cost mask: {cost_mask}")
    node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask, joint_velocity_limits)
    # Run both ROS 2 and OSC Server together
    try:
        while rclpy.ok():
            osc_process()  # Handle one OSC request at a time
            rclpy.spin_once(node, timeout_sec=0.1)  # Process ROS callbacks
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/build/joint_control/build/lib/joint_control/trajectory_server_new.py
+++ b/workspace/build/joint_control/build/lib/joint_control/trajectory_server_new.py
@ -0,0 +1,166 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from sensor_msgs.msg import JointState
 from osc4py3.as_allthreads import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import roboticstoolbox as rtb
 import spatialmath as sm
 import numpy as np
 import time
 import os
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, robot, cost_mask, joint_velocity_limits):
        super().__init__('scaled_joint_trajectory_publisher')
        self.joint_velocity_limits = [joint_velocity_limits[joint] for joint in joint_names]
        self.cost_mask = cost_mask
        self.robot = robot
        self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
        if self.trajectroy_topic_name == "":
            self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            self.trajectroy_topic_name, 
            10
        )
        self.subscription = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_states_callback,
            1  # Increased queue size for joint states
        )
        # Store received joint positions
        self.joint_names = joint_names
        self.port = 8000 # UDP port
        osc_startup()
        osc_udp_server("0.0.0.0", self.port, "osc_server")
        print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
        # Register OSC handler
        osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
        print("OSC method registered for /joint_trajectory")
    def joint_states_callback(self, msg):
        """Callback function to handle incoming joint states."""
        joint_position_dict = dict(zip(msg.name, msg.position))
        self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
        joint_velocity_dict = dict(zip(msg.name, msg.velocity))
        self.current_joint_velocities = [joint_velocity_dict[name] for name in self.joint_names]
    def joint_angles_handler(self, *args):
        """Handles incoming OSC messages for joint positions."""
        try:
            print("Received joint positions")
            viapoints = []
            msg = JointTrajectory()
            msg.joint_names = self.joint_names
            steps_per_m = 4
            for i in range(len(args)-1):
                x, y, z, roll, pitch, yaw = args[i]
                x1, y1, z1, roll1, pitch1, yaw1 = args[i+1]
                steps = int(np.linalg.norm(np.array([x1-x, y1-y, z1-z])) * steps_per_m)
                if steps < 2: steps = 2
                cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i], order='xyz') for i in range(steps)]
                if i == 0: prev_sol = self.current_joint_positions
                for j in (range(steps) if i == 0 else range(1,steps)):
                    #print(cart_traj[j])
                    sol = self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True)
                    if sol[1] == 1:
                        viapoints.append(list(sol[0]))
                        prev_sol = list(sol[0])
                    else: print('IK could not find a solution!')
            dt = 0.01
            tacc = 0.5
            print(f'length viapoints: {len(viapoints)}')
            traj = rtb.mstraj(np.array(viapoints), q0 = self.current_joint_positions ,dt=dt, tacc=tacc, qdmax=[1 * i for i in self.joint_velocity_limits])
            print(len(traj.q))
            print(len(traj.t))
            print(traj.t)
            print(traj.arrive)
            msg.points = []
            for i in range(len(traj.q)):
                point = JointTrajectoryPoint()
                point.positions = list(traj.q[i])
                point.time_from_start.sec = int(traj.t[i])
                point.time_from_start.nanosec = int(((traj.t[i] - int(traj.t[i])) * 1e9))
                #point.time_from_start = rclpy.duration.Duration(seconds=traj.t[i]).to_msg()
                msg.points.append(point)
            msg.header.stamp = self.get_clock().now().to_msg()
            self.publisher.publish(msg)
            print('published')
        except Exception as e:
            print(f'Error in joint angles handler: {e}')
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    while True:
        path_to_urdf = input("Enter the path to the URDF file: ")
        if os.path.isfile(path_to_urdf):
            if not path_to_urdf.endswith('.urdf'):
                print("The file is not a URDF file. Please enter a valid URDF file.")
                continue
            break
        else:
            print("Invalid path. Please enter a valid path to the URDF file.")
    tree = ET.parse(path_to_urdf)
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    robot = rtb.ERobot.URDF(path_to_urdf)
    print(robot)
    joint_velocity_limits = {}
    # Iterate over all joints in the URDF
    for joint in root.findall('.//joint'):
        joint_name = joint.get('name')  # Get the name of the joint
        # Look for the <limit> tag under each joint
        limit = joint.find('limit')
        if limit is not None:
            # Extract the velocity limit (if it exists)
            velocity_limit = limit.get('velocity')
            if velocity_limit is not None:
                joint_velocity_limits[joint_name] = float(velocity_limit)
    rclpy.init()
    while True:
        try:
            print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
            print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
            cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
            if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
                break
            else:
                print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
        except ValueError:
            print("Invalid input. Please enter integers only.")
    print(f"Cost mask: {cost_mask}")
    node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask, joint_velocity_limits)
    # Run both ROS 2 and OSC Server together
    try:
        while rclpy.ok():
            osc_process()  # Handle one OSC request at a time
            rclpy.spin_once(node, timeout_sec=0.1)  # Process ROS callbacks
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/build/joint_control/build/lib/joint_control/trajectory_server_new_cart.py
+++ b/workspace/build/joint_control/build/lib/joint_control/trajectory_server_new_cart.py
@ -0,0 +1,180 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from sensor_msgs.msg import JointState
 from osc4py3.as_allthreads import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import roboticstoolbox as rtb
 import spatialmath as sm
 import numpy as np
 import os
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, robot, cost_mask, joint_velocity_limits):
        super().__init__('scaled_joint_trajectory_publisher')
        self.joint_velocity_limits = joint_velocity_limits
        self.cost_mask = cost_mask
        self.robot = robot
        self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
        if self.trajectroy_topic_name == "":
            self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
        print(f"Using topic name: {self.trajectroy_topic_name}")
        print("--------------------------------------------------------------------------------------------------------------------------------")
        while True:
            try:
                self.speed = input("Enter your desired speed of the tcp (in m/s): ")
                if self.speed == '':
                    self.speed = 1
                else:
                    self.speed = float(self.speed)
                break
            except ValueError:
                print("Invalid input. Please enter a number.")
                continue
        while True:
            try:
                self.t_acc = input("Enter how fast you want the tcp to reach that velocity (in s). \nRemember! If the acceleration time is to short the robot might not be able to accelerate fast enough: ")
                if self.t_acc == '':
                    self.t_acc = 2
                else:
                    self.t_acc = float(self.t_acc)
                break
            except ValueError:
                print("Invalid input. Please enter a number.")
                continue
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            self.trajectroy_topic_name, 
            10
        )
        self.subscription = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_states_callback,
            1  # Increased queue size for joint states
        )
        # Store received joint positions
        self.joint_names = joint_names
        self.port = 8000 # UDP port
        osc_startup()
        osc_udp_server("0.0.0.0", self.port, "osc_server")
        print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
        # Register OSC handler
        osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
        print("OSC method registered for /joint_trajectory")
    def joint_states_callback(self, msg):
        """Callback function to handle incoming joint states."""
        joint_position_dict = dict(zip(msg.name, msg.position))
        self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
        joint_velocity_dict = dict(zip(msg.name, msg.velocity))
        self.current_joint_velocities = [joint_velocity_dict[name] for name in self.joint_names]
    def joint_angles_handler(self, *args):
        """Handles incoming OSC messages for joint positions."""
        try:
            print("Received joint positions")
            viapoints = np.array([list(i) for i in args])
            msg = JointTrajectory()
            msg.joint_names = self.joint_names
            x,y,z = self.robot.fkine(self.current_joint_positions).t
            r,p,yaw = self.robot.fkine(self.current_joint_positions).rpy()
            q0 = [x, y, z, r, p, yaw]
            traj = rtb.mstraj(viapoints, q0 = q0 ,dt=0.01, tacc=self.t_acc, qdmax=self.speed)
            msg.points = []
            prev_sol = self.current_joint_positions
            for i in range(len(traj.q)):
                T = sm.SE3(traj.q[i][:3]) * sm.SE3.RPY(traj.q[i][3:], order='xyz')
                sol = self.robot.ik_LM(T, q0=prev_sol, mask = self.cost_mask, joint_limits = True)
                if sol[1] == 1:
                    point = JointTrajectoryPoint()
                    point.positions = list(sol[0])
                    point.time_from_start.sec = int(traj.t[i])
                    point.time_from_start.nanosec = int((traj.t[i] - int(traj.t[i])) * 1e9)
                    msg.points.append(point)
                    prev_sol = list(sol[0])
                else: print('IK could not find a solution!')
            msg.header.stamp = self.get_clock().now().to_msg()
            self.publisher.publish(msg)
            print(f'lenght msg.points: {len(msg.points)}')
            print('published')
        except Exception as e:
            print(f'Error in joint_angles_handler: {e}')
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    while True:
        path_to_urdf = input("Enter the path to the URDF file: ")
        if os.path.isfile(path_to_urdf):
            if not path_to_urdf.endswith('.urdf'):
                print("The file is not a URDF file. Please enter a valid URDF file.")
                continue
            break
        else:
            print("Invalid path. Please enter a valid path to the URDF file.")
    print("--------------------------------------------------------------------------------------------------------------------------------")
    tree = ET.parse(path_to_urdf)
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    robot = rtb.ERobot.URDF(path_to_urdf)
    print(robot)
    joint_velocity_limits = {}
    # Iterate over all joints in the URDF
    for joint in root.findall('.//joint'):
        joint_name = joint.get('name')  # Get the name of the joint
        # Look for the <limit> tag under each joint
        limit = joint.find('limit')
        if limit is not None:
            # Extract the velocity limit (if it exists)
            velocity_limit = limit.get('velocity')
            if velocity_limit is not None:
                joint_velocity_limits[joint_name] = float(velocity_limit)
    rclpy.init()
    while True:
        try:
            print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
            print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
            cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
            if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
                break
            else:
                print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
        except ValueError:
            print("Invalid input. Please enter integers only.")
    print(f"Cost mask: {cost_mask}")
    print("--------------------------------------------------------------------------------------------------------------------------------")
    node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask, joint_velocity_limits)
    # Run both ROS 2 and OSC Server together
    try:
        while rclpy.ok():
            osc_process()  # Handle one OSC request at a time
            rclpy.spin_once(node, timeout_sec=0.1)  # Process ROS callbacks
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/build/joint_control/build/lib/joint_control/trajectory_server_trapezoidal.py
+++ b/workspace/build/joint_control/build/lib/joint_control/trajectory_server_trapezoidal.py
@ -0,0 +1,185 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from sensor_msgs.msg import JointState
 from osc4py3.as_allthreads import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import roboticstoolbox as rtb
 import spatialmath as sm
 import numpy as np
 import time
 import os
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, robot, cost_mask, joint_velocity_limits):
        super().__init__('scaled_joint_trajectory_publisher')
        self.joint_velocity_limits = [joint_velocity_limits[joint] for joint in joint_names]
        self.cost_mask = cost_mask
        self.robot = robot
        self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
        if self.trajectroy_topic_name == "":
            self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            self.trajectroy_topic_name, 
            10
        )
        self.subscription = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_states_callback,
            1  # Increased queue size for joint states
            )
        self.maximum_acceleration = [0.0] * len(joint_names)
        # Store received joint positions
        self.joint_names = joint_names
        for joint in joint_names:
            self.maximum_acceleration[joint_names.index(joint)] = float(input(f"Enter the maximum acceleration for joint {joint}: "))
        self.port = 8000 # UDP port
        osc_startup()
        osc_udp_server("0.0.0.0", self.port, "osc_server")
        print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
        # Register OSC handler
        osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
        print("OSC method registered for /joint_trajectory")
    def joint_states_callback(self, msg):
        """Callback function to handle incoming joint states."""
        joint_position_dict = dict(zip(msg.name, msg.position))
        self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
        joint_velocity_dict = dict(zip(msg.name, msg.velocity))
        self.current_joint_velocities = [joint_velocity_dict[name] for name in self.joint_names]
    def joint_angles_handler(self, *args):
        """Handles incoming OSC messages for joint positions."""
        try:
            print("Received joint positions")
            msg = JointTrajectory()
            msg.joint_names = self.joint_names
            steps_per_m = 50
            prev_duration = 0
            for i in range(len(args)-1):
                x, y, z, roll, pitch, yaw = args[i]
                x1, y1, z1, roll1, pitch1, yaw1 = args[i+1]
                steps = int(np.linalg.norm([x1-x, y1-y, z1-z])*steps_per_m)
                if steps <= 1: steps = 2
                cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i], order='xyz') for i in range(steps)]
                prev_sol = self.current_joint_positions if i == 0  else sol[0]
                sol_set = []
                for j in (range(steps) if i == 0 else range(1,steps)):
                    sol = self.robot.ik_LM(cart_traj[j], q0=[0.0] * len(self.joint_names), mask = self.cost_mask, joint_limits = True) if i == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True)
                    if sol[1] == 1:
                        sol_set.append(sol[0])
                        prev_sol = list(sol[0])
                    else: print(f'IK could not find a solution for (x,y,z) = ({cart_traj[j].t}), (roll,pitch,yaw) = ({cart_traj[j].rpy()})!')
                distance = abs(sol_set[0]-sol_set[-1])
                ts= distance/np.array(self.joint_velocity_limits)+2*np.array(self.joint_velocity_limits)/np.array(self.maximum_acceleration)
                t = max(ts)
                idx = list(ts).index(t)
                s_acc = self.joint_velocity_limits[idx]**2/(2*self.maximum_acceleration[idx])
                print(f"t: {t}, idx: {idx}, s_acc: {s_acc}")
                print(f"sol_set: {sol_set}")
                for sol in sol_set:
                    print(f"sol: {sol}")
                    s = abs(sol[idx]-sol_set[0][idx])
                    print(f"sol_set[0][idx]: {sol_set[0][idx]}, sol[idx]: {sol[idx]}, s: {s}")
                    if s <= s_acc:
                        duration = np.sqrt(s/self.maximum_acceleration[idx])
                        print(f"acceleration phase, duration: {duration}")
                    elif s <= sol_set[-1][idx]-s_acc:
                        duration = self.joint_velocity_limits[idx]/self.maximum_acceleration[idx] + (s-s_acc)/self.joint_velocity_limits[idx]
                        print(f"constant velocity phase, duration: {duration}")
                    else:
                        duration = t-np.sqrt((sol_set[-1][idx]-s)/self.maximum_acceleration[idx])
                        print(f"deceleration phase, duration: {duration}")
                    point = JointTrajectoryPoint()
                    point.positions = list(sol)
                    duration += prev_duration
                    point.time_from_start.sec = int(duration)
                    point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
                    msg.points.append(point)
                prev_duration = duration
            msg.header.stamp = self.get_clock().now().to_msg()
            self.publisher.publish(msg)
        except Exception as e:
            print(f"Error in joint angles handler: {e}")
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    while True:
        path_to_urdf = input("Enter the path to the URDF file: ")
        if os.path.isfile(path_to_urdf):
            if not path_to_urdf.endswith('.urdf'):
                print("The file is not a URDF file. Please enter a valid URDF file.")
                continue
            break
        else:
            print("Invalid path. Please enter a valid path to the URDF file.")
    tree = ET.parse(path_to_urdf)
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    robot = rtb.ERobot.URDF(path_to_urdf)
    print(robot)
    joint_velocity_limits = {}
    # Iterate over all joints in the URDF
    for joint in root.findall('.//joint'):
        joint_name = joint.get('name')  # Get the name of the joint
        # Look for the <limit> tag under each joint
        limit = joint.find('limit')
        if limit is not None:
            # Extract the velocity limit (if it exists)
            velocity_limit = limit.get('velocity')
            if velocity_limit is not None:
                joint_velocity_limits[joint_name] = float(velocity_limit)
    rclpy.init()
    while True:
        try:
            print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
            print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
            cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
            if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
                break
            else:
                print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
        except ValueError:
            print("Invalid input. Please enter integers only.")
    print(f"Cost mask: {cost_mask}")
    node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask, joint_velocity_limits)
    # Run both ROS 2 and OSC Server together
    try:
        while rclpy.ok():
            osc_process()  # Handle one OSC request at a time
            rclpy.spin_once(node, timeout_sec=0.1)  # Process ROS callbacks
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/build/joint_control/colcon_build.rc
+++ b/workspace/build/joint_control/colcon_build.rc
@ -1 +1 @@
-0
+1
--- a/workspace/build/joint_control/colcon_command_prefix_setup_py.sh.env
+++ b/workspace/build/joint_control/colcon_command_prefix_setup_py.sh.env
@ -1,16 +1,15 @@
-AMENT_PREFIX_PATH=/BA/workspace/install/painting_robot_control:/BA/workspace/install/mock_robot:/BA/workspace/install/joint_info:/BA/workspace/install/joint_control:/opt/ros/humble
+AMENT_PREFIX_PATH=/opt/ros/humble
 COLCON=1
 COLCON_PREFIX_PATH=/BA/workspace/install
 HOME=/root
-HOSTNAME=0e38e264ac6b
+HOSTNAME=hapticslab2
 LANG=C.UTF-8
 LC_ALL=C.UTF-8
 LD_LIBRARY_PATH=/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib
 LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.webp=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36:
-OLDPWD=/BA/workspace/src/joint_control/joint_control
+OLDPWD=/ws/src/ba-alexanderschaefer
 PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin
-PWD=/BA/workspace/build/joint_control
+PWD=/ws/src/ba-alexanderschaefer/workspace/build/joint_control
-PYTHONPATH=/BA/workspace/build/painting_robot_control:/BA/workspace/install/painting_robot_control/lib/python3.10/site-packages:/BA/workspace/build/mock_robot:/BA/workspace/install/mock_robot/lib/python3.10/site-packages:/BA/workspace/build/joint_info:/BA/workspace/install/joint_info/lib/python3.10/site-packages:/BA/workspace/build/joint_control:/BA/workspace/install/joint_control/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages
+PYTHONPATH=/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages
 ROS_DISTRO=humble
 ROS_LOCALHOST_ONLY=0
 ROS_PYTHON_VERSION=3
--- a/workspace/build/joint_control/joint_control
+++ b/workspace/build/joint_control/joint_control
@ -1 +1 @@
-/BA/workspace/src/joint_control/joint_control
+/ws/src/ba-alexanderschaefer/workspace/src/joint_control/joint_control
--- a/workspace/build/joint_control/joint_control.egg-info/SOURCES.txt
+++ b/workspace/build/joint_control/joint_control.egg-info/SOURCES.txt
@ -24,11 +24,7 @@ joint_control/trajectory_server_cart_fast_smooth.py
 joint_control/trajectory_server_new.py
 joint_control/trajectory_server_new_cart.py
 joint_control/trajectory_server_trapezoidal.py
-joint_control.egg-info/PKG-INFO
+resource/joint_control
-joint_control.egg-info/SOURCES.txt
+test/test_copyright.py
-joint_control.egg-info/dependency_links.txt
+test/test_flake8.py
-joint_control.egg-info/entry_points.txt
+test/test_pep257.py
 joint_control.egg-info/requires.txt
 joint_control.egg-info/top_level.txt
 joint_control.egg-info/zip-safe
 resource/joint_control
--- a/workspace/build/joint_control/package.xml
+++ b/workspace/build/joint_control/package.xml
@ -1 +1 @@
-/BA/workspace/src/joint_control/package.xml
+/ws/src/ba-alexanderschaefer/workspace/src/joint_control/package.xml
--- a/workspace/build/joint_control/prefix_override/pycache/sitecustomize.cpython-310.pyc
+++ b/workspace/build/joint_control/prefix_override/pycache/sitecustomize.cpython-310.pyc
--- a/workspace/build/joint_control/prefix_override/sitecustomize.py
+++ b/workspace/build/joint_control/prefix_override/sitecustomize.py
@ -1,4 +1,4 @@
 import sys
 if sys.prefix == '/usr':
    sys.real_prefix = sys.prefix
-    sys.prefix = sys.exec_prefix = '/BA/workspace/install/joint_control'
+    sys.prefix = sys.exec_prefix = '/ws/src/ba-alexanderschaefer/workspace/install/joint_control'
--- a/workspace/build/joint_control/resource/joint_control
+++ b/workspace/build/joint_control/resource/joint_control
@ -1 +1 @@
-/BA/workspace/src/joint_control/resource/joint_control
+/ws/src/ba-alexanderschaefer/workspace/src/joint_control/resource/joint_control
--- a/workspace/build/joint_control/setup.cfg
+++ b/workspace/build/joint_control/setup.cfg
@ -1 +1 @@
-/BA/workspace/src/joint_control/setup.cfg
+/ws/src/ba-alexanderschaefer/workspace/src/joint_control/setup.cfg
--- a/workspace/build/joint_control/setup.py
+++ b/workspace/build/joint_control/setup.py
@ -1 +0,0 @@
 /BA/workspace/src/joint_control/setup.py
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint-control.egg-link
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint-control.egg-link
@ -1,2 +0,0 @@
 /BA/workspace/build/joint_control
 .
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/init.py
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/init.py
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/init.cpython-310.pyc
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/init.cpython-310.pyc
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/cart_tcp_server.cpython-310.pyc
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/cart_tcp_server.cpython-310.pyc
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/joint_angles_server.cpython-310.pyc
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/joint_angles_server.cpython-310.pyc
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/plugdata.cpython-310.pyc
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/plugdata.cpython-310.pyc
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/plugdata_cart.cpython-310.pyc
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/plugdata_cart.cpython-310.pyc
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/plugdata_cart_fix.cpython-310.pyc
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/plugdata_cart_fix.cpython-310.pyc
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/plugdata_cart_smooth.cpython-310.pyc
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/plugdata_cart_smooth.cpython-310.pyc
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/test.cpython-310.pyc
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/test.cpython-310.pyc
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/trajectory_server.cpython-310.pyc
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/trajectory_server.cpython-310.pyc
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/trajectory_server_cart.cpython-310.pyc
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/trajectory_server_cart.cpython-310.pyc
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/trajectory_server_cart_fast.cpython-310.pyc
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/trajectory_server_cart_fast.cpython-310.pyc
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/trajectory_server_cart_fast_max_acc.cpython-310.pyc
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/trajectory_server_cart_fast_max_acc.cpython-310.pyc
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/trajectory_server_cart_fast_smooth.cpython-310.pyc
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/trajectory_server_cart_fast_smooth.cpython-310.pyc
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/trajectory_server_new.cpython-310.pyc
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/trajectory_server_new.cpython-310.pyc
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/trajectory_server_new_cart.cpython-310.pyc
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/trajectory_server_new_cart.cpython-310.pyc
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/trajectory_server_trapezoidal.cpython-310.pyc
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/pycache/trajectory_server_trapezoidal.cpython-310.pyc
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/cart_tcp_server.py
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/cart_tcp_server.py
@ -0,0 +1,108 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from osc4py3.as_eventloop import *
 from osc4py3 import oscmethod as osm
 import roboticstoolbox as rtb
 import spatialmath as sm
 import xml.etree.ElementTree as ET
 import time
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, robot, joint_names):
        super().__init__('scaled_joint_trajectory_publisher')
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            '/scaled_joint_trajectory_controller/joint_trajectory', 
            10
        )
        # Store received joint positions
        self.joint_positions = [0.0] * len(joint_names)
        self.joint_names = joint_names
        self.robot = robot
        osc_startup()
        osc_udp_server("0.0.0.0", 8000, "osc_server")
        print("Server started on 0.0.0.0:8000 \nready to receive messages in the following format: /tcp_coordinates [x, y, z, roll, pitch, yaw] optional: duration as last argument: /tcp_coordinates [x, y, z, roll, pitch, yaw, duration]")
        # Register OSC handler
        osc_method("/tcp_coordinates", self.tcp_coordinates_handler, argscheme=osm.OSCARG_DATAUNPACK)
    def tcp_coordinates_handler(self, *args):
        """Handles incoming OSC messages for tcp position."""
        time1 = time.time()
        if len(args) == len(self.joint_positions):
            x, y, z, roll, pitch, yaw = args
            duration = 4.0  # Default duration
        elif len(args) == len(self.joint_positions) + 1:
            x, y, z, roll, pitch, yaw, duration = args
        else:
            print("Invalid number of arguments")
            return
        # Create the desired end-effector pose
        Tep = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
        # Compute the inverse kinematics to get the joint angles
        #time1 = time.time()
        #sol = self.robot.ikine_LM(Tep)
        #print(f"Time taken for ERobot: {time.time() - time1}")
        #time1 = time.time()
        #sol = self.robot.ikine_LM(Tep, q0=self.joint_positions)
        #print(f"Time taken for ERobot with initial guess: {time.time() - time1}")
        #time1 = time.time()
        #sol = self.robot.ets().ikine_LM(Tep)
        #print(f"Time taken for ETS: {time.time() - time1}")
        #time1 = time.time()
        sol = self.robot.ik_LM(Tep, q0=self.joint_positions)
        #print(f"Time taken for ETS with initial guess: {time.time() - time1}")
        if sol[1]:
            joint_positions = list(sol[0])
            self.send_trajectory(joint_positions, duration)
            #print(f"Computed joint positions: {joint_positions}")
        else:
            print("Inverse kinematics failed")
        print(f"Frequency: {1/(time.time() - time1)} Hz")
    def send_trajectory(self, joint_positions, duration=4.0):
        """Publish a joint trajectory command to move the robot."""
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        point = JointTrajectoryPoint()
        point.positions = joint_positions  # Updated joint positions
        point.time_from_start.sec = int(duration)
        point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
        msg.points.append(point)
        self.publisher.publish(msg)
        print(f"Updated joint positions: {joint_positions}")
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    tree = ET.parse('/BA/robot.urdf')
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    robot = rtb.ERobot.URDF('/BA/robot.urdf')
    rclpy.init()
    node = ScaledJointTrajectoryPublisher(robot.ets(), joint_names)
    # Run both ROS 2 and OSC Server together
    try:
        while rclpy.ok():
            osc_process()  # Handle one OSC request at a time
            rclpy.spin_once(node, timeout_sec=0.1)  # Process ROS callbacks
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/joint_angles_server.py
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/joint_angles_server.py
@ -0,0 +1,80 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from osc4py3.as_eventloop import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names):
        super().__init__('scaled_joint_trajectory_publisher')
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            '/scaled_joint_trajectory_controller/joint_trajectory', 
            10
        )
        # Store received joint positions
        self.joint_positions = [0.0] * len(joint_names)
        self.joint_names = joint_names
        osc_startup()
        osc_udp_server("0.0.0.0", 8000, "osc_server")
        print("Server started on 0.0.0.0:8000 \nready to receive messages in the following format: \n   /joint_angles [joint_positions]; optional: duration as last element, default is 3sec")
        # Register OSC handler
        osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
    def joint_angles_handler(self, *args):
        """Handles incoming OSC messages for joint positions."""
        if len(args) == len(self.joint_positions): 
            self.joint_positions = args
            self.send_trajectory(self.joint_positions)
        elif len(args) == len(self.joint_positions) + 1:
            self.joint_positions = args[:-1]
            self.send_trajectory(self.joint_positions, args[-1])
            print(f'Duration: {args[-1]}')
    def send_trajectory(self, joint_positions, duration=3.0):
        """Publish a joint trajectory command to move the robot."""
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        point = JointTrajectoryPoint()
        point.positions = joint_positions  # Updated joint positions
        point.time_from_start.sec = int(duration)
        point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
        msg.points.append(point)
        self.publisher.publish(msg)
        print(f"Updated joint positions: {self.joint_positions}")
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    tree = ET.parse('/BA/robot.urdf')
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    rclpy.init()
    node = ScaledJointTrajectoryPublisher(joint_names)
    # Run both ROS 2 and OSC Server together
    try:
        while rclpy.ok():
            osc_process()  # Handle one OSC request at a time
            rclpy.spin_once(node, timeout_sec=0.1)  # Process ROS callbacks
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata.py
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata.py
@ -0,0 +1,209 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from sensor_msgs.msg import JointState
 from osc4py3.as_allthreads import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import time
 import numpy as np
 import os
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, joint_velocity_limits):
        super().__init__('scaled_joint_trajectory_publisher')
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            '/scaled_joint_trajectory_controller/joint_trajectory', 
            1
        )
        self.subscription = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_states_callback,
            1  # Increased queue size for joint states
        )
        # Store received joint positions
        self.current_joint_positions = [0.0] * len(joint_names)
        self.joint_names = joint_names
        self.joint_velocity_limits = joint_velocity_limits
        self.desired_joint_positions = [0.0] * len(joint_names)
        self.previous_desired = [0.0] * len(joint_names)
        ip = "0.0.0.0"  # Listen on all network interfaces
        port = 8000     # Must match the sender's port in `joint_state_osc.py`
        osc_startup()
        osc_udp_server(ip, port, "osc_server")
        print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [joint_positions] optional: duration as last argument")
        # Register OSC handler
        osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
        while True:
            try:
                self.x_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for x (space-separated, enter 'x' for no limit): ").split()]
                self.y_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for y (space-separated, enter 'x' for no limit): ").split()]
                self.z_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for z (space-separated, enter 'x' for no limit): ").split()]
                if len(self.x_limits) != 2 or len(self.y_limits) != 2 or len(self.z_limits) != 2:
                    print("Invalid input. Please enter exactly two values (or leave blank) for each limit.")
                    continue
                if (self.x_limits[0] is not None and self.x_limits[1] is not None and self.x_limits[0] >= self.x_limits[1]) or \
                (self.y_limits[0] is not None and self.y_limits[1] is not None and self.y_limits[0] >= self.y_limits[1]) or \
                (self.z_limits[0] is not None and self.z_limits[1] is not None and self.z_limits[0] >= self.z_limits[1]):
                    print("Invalid input. Lower limit must be less than upper limit for each axis.")
                    continue
                print(f"Current limits:")
                print(f"x: {self.x_limits}")
                print(f"y: {self.y_limits}")
                print(f"z: {self.z_limits}")
                confirm = input("Do you want your robot to move in this range? (y/n): ").strip().lower()
                if confirm == 'y':
                    break
                elif confirm == 'n':
                    print("Please re-enter the limits.")
                else:
                    print("Invalid input. Please enter 'y' or 'n'.")
            except ValueError:
                print("Invalid input. Please enter numeric values only.")
        # Ask the user if they want to set new joint limits
        update_limits = input("Do you want to set new joint limits? (y/n): ").strip().lower()
        if update_limits == 'y':
            for joint_name in self.joint_names:
                while True:
                    try:
                        print(f"Current position limits for joint '{joint_name}': {self.robot.links[joint_name]} rad")
                        lower_limit = input(f"Enter the new lower limit for joint '{joint_name}' (or press Enter to keep current): ").strip()
                        upper_limit = input(f"Enter the new upper limit for joint '{joint_name}' (or press Enter to keep current): ").strip()
                        if lower_limit is not None and upper_limit is not None and lower_limit >= upper_limit:
                            print("Invalid input. Lower limit must be less than upper limit.")
                            continue
                        if lower_limit:
                            self.robot.links[joint_name][0] = float(lower_limit)
                        if upper_limit:
                            self.robot.links[joint_name][1] = float(upper_limit)
                        break
                    except ValueError:
                        print("Invalid input. Please enter numeric values or leave blank to keep current limits.")
        self.hz = float(input("Enter the desired refresh frequency (Hz): "))
        # Start the OSC server in a separate thread to avoid blocking the ROS 2 event loop
        self.create_timer(1/self.hz, self.update_position)  # Timer to call osc_process periodically
    def joint_angles_handler(self, *args):
    # Ensure the desired joint positions are within the specified limits
        x, y, z, r, p, yaw, *_ = [float(i) for i in list(args)]
        if self.x_limits[0] is not None:
            x = max(self.x_limits[0], x)
        if self.x_limits[1] is not None:
            x = min(self.x_limits[1], x)
        if self.y_limits[0] is not None:
            y = max(self.y_limits[0], y)
        if self.y_limits[1] is not None:
            y = min(self.y_limits[1], y)
        if self.z_limits[0] is not None:
            z = max(self.z_limits[0], z)
        if self.z_limits[1] is not None:
            z = min(self.z_limits[1], z)
        if x != args[0] or y != args[1] or z != args[2]:
            self.get_logger().warn(
            f"Desired joint positions adjusted to fit within limits: "
            f"x={x}, y={y}, z={z} (original: x={args[0]}, y={args[1]}, z={args[2]})"
            )
        self.desired_joint_positions = [x, y, z, r, p, yaw]
    def joint_states_callback(self, msg):
        """Callback function to handle incoming joint states."""
        joint_names = msg.name  # List of joint names
        joint_position_dict = dict(zip(joint_names, self.current_joint_positions))
        self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
    def update_position(self):
        time1 = time.time()
        if self.desired_joint_positions == self.previous_desired:
            return
        duration = 0
        for p1, p2, max_vel in zip(self.desired_joint_positions, self.current_joint_positions, self.joint_velocity_limits.values()):
            duration = max(max(duration, abs(p1 - p2) / max_vel),1/self.hz)# as minimun
        duration *= 1.8
        #print(f"Duration: {duration}")
        #print(f'vel: {max(np.array(self.desired_joint_positions_joint_positions) - np.array(self.joint_positions)/duration)}')
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        point = JointTrajectoryPoint()
        point.positions = self.desired_joint_positions  # Updated joint positions
        point.time_from_start.sec = int(duration)
        #point.time_from_start.sec = 10
        point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
        #point.time_from_start.nanosec = 0
        #point.velocities = [i*0.95 for i in self.joint_velocity_limits.values()]
        msg.points.append(point)
        self.publisher.publish(msg)
        #print(f"desired: {self.desired_joint_positions}")
        #print(f"desired: {[180/3.141*i for i in self.desired_joint_positions]}")
        #print(f"current: {[180/3.141*i for i in self.current_joint_positions]}")
        self.previous_desired = self.desired_joint_positions
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    while True:
        path_to_urdf = input("Enter the path to the URDF file: ")
        if os.path.isfile(path_to_urdf):
            if not path_to_urdf.endswith('.urdf'):
                print("The file is not a URDF file. Please enter a valid URDF file.")
                continue
            break
        else:
            print("Invalid path. Please enter a valid path to the URDF file.")
    tree = ET.parse(path_to_urdf)
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    joint_velocity_limits = {}
    # Iterate over all joints in the URDF
    for joint in root.findall('.//joint'):
        joint_name = joint.get('name')  # Get the name of the joint
        # Look for the <limit> tag under each joint
        limit = joint.find('limit')
        if limit is not None:
            # Extract the velocity limit (if it exists)
            velocity_limit = limit.get('velocity')
            if velocity_limit is not None:
                joint_velocity_limits[joint_name] = float(velocity_limit)
    rclpy.init()
    node = ScaledJointTrajectoryPublisher(joint_names, joint_velocity_limits)
    # Run ROS 2 spin, and osc_process will be handled by the timer
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart.py
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart.py
@ -0,0 +1,158 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from sensor_msgs.msg import JointState
 from osc4py3.as_allthreads import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import time
 import numpy as np
 import spatialmath as sm
 import roboticstoolbox as rtb
 import os
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, joint_velocity_limits, robot, cost_mask):
        super().__init__('scaled_joint_trajectory_publisher')
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            '/scaled_joint_trajectory_controller/joint_trajectory', 
            1
        )
        self.subscription = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_states_callback,
            1  # Increased queue size for joint states
        )
        # Store received joint positions
        self.current_joint_positions = [0.0] * len(joint_names)
        self.joint_names = joint_names
        self.joint_velocity_limits = joint_velocity_limits
        self.desired_joint_positions = [0.0] * len(joint_names)
        self.previous_desired = [0.0] * len(joint_names)
        self.robot = robot
        self.cost_mask = cost_mask
        ip = "0.0.0.0"  # Listen on all network interfaces
        port = 8000     # Must match the sender's port in `joint_state_osc.py`
        osc_startup()
        osc_udp_server(ip, port, "osc_server")
        print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [joint_positions] optional: duration as last argument")
        # Register OSC handler
        osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
        self.hz = float(input("Enter the desired refresh frequency (Hz): "))
        # Start the OSC server in a separate thread to avoid blocking the ROS 2 event loop
        self.create_timer(1/self.hz, self.update_position)  # Timer to call osc_process periodically
    def joint_angles_handler(self, *args):
        """Handles incoming OSC messages for joint positions."""
        self.desired_joint_positions = [float(i) for i in list(args)]
    def joint_states_callback(self, msg):
        """Callback function to handle incoming joint states."""
        joint_position_dict = dict(zip(msg.name, msg.position))
        self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
    def update_position(self):
        if self.desired_joint_positions == self.previous_desired:
            return
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        steps_per_m = 30
        if True: #len(args[0]) == len(self.joint_names):
            prev_duration = 0
            T1 = self.robot.fkine(self.current_joint_positions)
            [x,y,z] = T1.t
            [roll, pitch, yaw] = T1.rpy()
            x1, y1, z1, roll1, pitch1, yaw1 = self.desired_joint_positions
            steps = int(np.linalg.norm(np.array([x1-x, y1-y, z1-z])) * steps_per_m)
            if steps < 2: steps = 2
            cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)]
            for j in range(steps):
                print(cart_traj[j])
                sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True)
                if sol[1] == 1:
                    duration = 0
                    prev = self.current_joint_positions if j == 0 else prev_sol
                    for p1, p2, max_vel in zip(sol[0], prev, self.joint_velocity_limits.values()):
                        duration = max(duration, abs(p1 - p2) / max_vel)#, 1/self.hz) # as minimun
                    point = JointTrajectoryPoint()
                    point.positions = list(sol[0])
                    #duration *= 2
                    duration += prev_duration
                    prev_duration = duration
                    point.time_from_start.sec = int(duration)
                    point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
                    msg.points.append(point)
                    prev_sol = list(sol[0])
                else:
                    print('IK could not find a solution!')
                    prev_sol = self.current_joint_positions
        msg.header.stamp = self.get_clock().now().to_msg()
        self.publisher.publish(msg)
        self.previous_desired = self.desired_joint_positions
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    robot_urdf = input("Enter the path to the URDF file: ")
    tree = ET.parse(robot_urdf)
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    robot = rtb.ERobot.URDF(robot_urdf)
    joint_velocity_limits = {}
    # Iterate over all joints in the URDF
    for joint in root.findall('.//joint'):
        joint_name = joint.get('name')  # Get the name of the joint
        # Look for the <limit> tag under each joint
        limit = joint.find('limit')
        if limit is not None:
            # Extract the velocity limit (if it exists)
            velocity_limit = limit.get('velocity')
            if velocity_limit is not None:
                joint_velocity_limits[joint_name] = float(velocity_limit)
    while True:
        try:
            print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
            print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
            cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
            if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
                break
            else:
                print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
        except ValueError:
            print("Invalid input. Please enter integers only.")
    print(f"Cost mask: {cost_mask}")
    rclpy.init()
    node = ScaledJointTrajectoryPublisher(joint_names, joint_velocity_limits, robot, cost_mask)
    # Run ROS 2 spin, and osc_process will be handled by the timer
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart_fix.py
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart_fix.py
@ -0,0 +1,264 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from sensor_msgs.msg import JointState
 from osc4py3.as_allthreads import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import numpy as np
 import spatialmath as sm
 import roboticstoolbox as rtb
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, joint_velocity_limits, robot, cost_mask):
        super().__init__('scaled_joint_trajectory_publisher')
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            '/scaled_joint_trajectory_controller/joint_trajectory', 
            1
        )
        self.subscription = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_states_callback,
            1  # Increased queue size for joint states
        )
        # Store received joint positions
        self.current_joint_positions = [0.0] * len(joint_names)
        self.joint_names = joint_names
        self.joint_velocity_limits = joint_velocity_limits
        self.desired_joint_positions = [0.0] * len(joint_names)
        self.previous_desired = [0.0] * len(joint_names)
        self.robot = robot
        self.cost_mask = cost_mask
        self.prev_pose = None
        ip = "0.0.0.0"  # Listen on all network interfaces
        port = 8000     # Must match the sender's port in `joint_state_osc.py`
        osc_startup()
        osc_udp_server(ip, port, "osc_server")
        print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [joint_positions] optional: duration as last argument")
        # Register OSC handler
        osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
        set_limits = input("Do you want to set limit for x, y and z? (y/n): ").strip().lower()
        if set_limits == 'y':
            while True:
                        try:
                            self.x_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for x (space-separated, enter 'x' for no limit): ").split()]
                            self.y_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for y (space-separated, enter 'x' for no limit): ").split()]
                            self.z_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for z (space-separated, enter 'x' for no limit): ").split()]
                            if len(self.x_limits) != 2 or len(self.y_limits) != 2 or len(self.z_limits) != 2:
                                print("Invalid input. Please enter exactly two values (or leave blank) for each limit.")
                                continue
                            if (self.x_limits[0] is not None and self.x_limits[1] is not None and self.x_limits[0] >= self.x_limits[1]) or \
                            (self.y_limits[0] is not None and self.y_limits[1] is not None and self.y_limits[0] >= self.y_limits[1]) or \
                            (self.z_limits[0] is not None and self.z_limits[1] is not None and self.z_limits[0] >= self.z_limits[1]):
                                print("Invalid input. Lower limit must be less than upper limit for each axis.")
                                continue
                            print(f"Current limits:")
                            print(f"x: {self.x_limits}")
                            print(f"y: {self.y_limits}")
                            print(f"z: {self.z_limits}")
                            con = True
                            while con:
                                confirm = input("Do you want your robot to move in this range? (y/n): ").strip().lower()
                                if confirm == 'y':
                                    break
                                elif confirm == 'n':
                                    print("Please re-enter the limits.")
                                    con = False
                                else:
                                    print("Invalid input. Please enter 'y' or 'n'.")
                            if con: break
                        except ValueError:
                            print("Invalid input. Please enter numeric values only.")
                # Ask the user if they want to set new joint limits
        update_limits = input("Do you want to set new joint limits? (y/n): ").strip().lower()
        if update_limits == 'y':
            for i in range(len(self.joint_names)):
                while True:
                    try:
                        lim = self.robot.qlim.copy()
                        # Find the link corresponding to the joint name
                        print(f"Current position limits for joint '{self.joint_names[i]}': [{self.robot.qlim[0][i]} {self.robot.qlim[1][i]}] rad")
                        lower_limit = input(f"Enter the new lower limit for joint '{self.joint_names[i]}' (or press Enter to keep current): ").strip()
                        upper_limit = input(f"Enter the new upper limit for joint '{self.joint_names[i]}' (or press Enter to keep current): ").strip()
                        if lower_limit and upper_limit and float(lower_limit) >= float(upper_limit):
                            print("Invalid input. Lower limit must be less than upper limit.")
                            continue
                        if lower_limit:
                            lim[0][i] = float(lower_limit)
                        if upper_limit:
                            lim[1][i] = float(upper_limit)
                        self.robot.qlim = lim
                        print(f"New limits for joint '{self.joint_names[i]}': [{self.robot.qlim[0][i]} {self.robot.qlim[1][i]}] rad")
                        print("-" * 50)
                        break
                    except ValueError:
                        print("Invalid input. Please enter numeric values or leave blank to keep current limits.")
        self.hz = float(input("Enter the desired refresh frequency (Hz): "))
        # Start the OSC server in a separate thread to avoid blocking the ROS 2 event loop
        self.create_timer(1/self.hz, self.update_position)  # Timer to call osc_process periodically
    def joint_angles_handler(self, *args):
    # Ensure the desired joint positions are within the specified limits
        print("received joint angles")
        x, y, z, r, p, yaw, *_ = [float(i) for i in list(args)]
        if self.x_limits[0] is not None:
            x = max(self.x_limits[0], x)
        if self.x_limits[1] is not None:
            x = min(self.x_limits[1], x)
        if self.y_limits[0] is not None:
            y = max(self.y_limits[0], y)
        if self.y_limits[1] is not None:
            y = min(self.y_limits[1], y)
        if self.z_limits[0] is not None:
            z = max(self.z_limits[0], z)
        if self.z_limits[1] is not None:
            z = min(self.z_limits[1], z)
        if x != args[0] or y != args[1] or z != args[2]:
            self.get_logger().warn(
            f"Desired joint positions adjusted to fit within limits: "
            f"x={x}, y={y}, z={z} (original: x={args[0]}, y={args[1]}, z={args[2]})"
            )
        self.desired_joint_positions = [x, y, z, r, p, yaw]
    def joint_states_callback(self, msg):
        """Callback function to handle incoming joint states."""
        joint_position_dict = dict(zip(msg.name, msg.position))
        self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
        joint_position_dict = dict(zip(msg.name, msg.velocity))
        self.current_joint_velocities = [joint_position_dict[name] for name in self.joint_names]
    def update_position(self):
        if self.desired_joint_positions == self.previous_desired:
            return
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        steps_per_m = 100
        if True: #len(args[0]) == len(self.joint_names):
            prev_duration = 0
            if self.prev_pose == None: 
                [x,y,z] = self.robot.fkine(self.current_joint_positions).t
                [roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
            else:
                [x,y,z] = self.prev_pose[:3]
                [roll, pitch, yaw] = self.prev_pose[3:]
            x1, y1, z1, roll1, pitch1, yaw1 = self.desired_joint_positions
            self.prev_pose = self.desired_joint_positions
            steps = int(np.linalg.norm(np.array([x1, y1, z1])- self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
            if steps < 2: steps = 2
            cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)]
            for j in range(steps):
                sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
                if sol[1] == 1:
                    fowards = self.robot.fkine_all(sol[0])
                    out_of_bounds = (fowards.t[1:,0] > self.x_limits[1] if self.x_limits[1] != None else False) | (fowards.t[1:,0] < self.x_limits[0] if self.x_limits[0] != None else False) | (fowards.t[1:,1] > self.y_limits[1] if self.y_limits[1] != None else False) | (fowards.t[1:,1] < self.y_limits[0] if self.y_limits[0] != None else False) | (fowards.t[1:,2] > self.z_limits[1] if self.z_limits[1] != None else False) | (fowards.t[1:,2] < self.z_limits[0] if self.z_limits[0] != None else False)
                    if np.any(out_of_bounds):
                        #print(fowards.t)
                        #indices = np.where(out_of_bounds)[0]
                        #print(f"indices: {indices}")
                        self.get_logger().warn("One or more links moved out of bounds!")
                        '''
                        for i in indices:
                            try:
                                print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}")
                            except IndexError:
                                print(f"index {i} is out of bounds, but no corresponding joint found.")
                        self.previous_desired = self.desired_joint_positions
                        '''
                        break
                    duration = 0
                    prev = self.current_joint_positions if j == 0 else prev_sol
                    for p1, p2, max_vel in zip(sol[0], prev, self.joint_velocity_limits.values()):
                        duration = max(duration, abs(p1 - p2) / max_vel)#, 1/self.hz) # as minimun
                    prev_sol = list(sol[0])
                    if duration == 0:
                        continue
                    point = JointTrajectoryPoint()
                    point.positions = list(sol[0])
                    duration *= 2
                    duration += prev_duration
                    prev_duration = duration
                    point.time_from_start.sec = int(duration)
                    point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
                    msg.points.append(point)
                else:
                    print(f'IK could not find a solution for (x,y,z) = {cart_traj[j].t} and (r,p,y) = {cart_traj[j].rpy()}!')
                    prev_sol = self.current_joint_positions
        if len(msg.points) == 0:
            return
        msg.header.stamp = self.get_clock().now().to_msg()
        self.publisher.publish(msg)
        self.previous_desired = self.desired_joint_positions
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    robot_urdf = input("Enter the path to the URDF file: ")
    tree = ET.parse(robot_urdf)
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    robot = rtb.ERobot.URDF(robot_urdf)
    joint_velocity_limits = {}
    # Iterate over all joints in the URDF
    for joint in root.findall('.//joint'):
        joint_name = joint.get('name')  # Get the name of the joint
        # Look for the <limit> tag under each joint
        limit = joint.find('limit')
        if limit is not None:
            # Extract the velocity limit (if it exists)
            velocity_limit = limit.get('velocity')
            if velocity_limit is not None:
                joint_velocity_limits[joint_name] = float(velocity_limit)
    while True:
        try:
            print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
            print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
            cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
            if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
                break
            else:
                print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
        except ValueError:
            print("Invalid input. Please enter integers only.")
    print(f"Cost mask: {cost_mask}")
    rclpy.init()
    node = ScaledJointTrajectoryPublisher(joint_names, joint_velocity_limits, robot, cost_mask)
    # Run ROS 2 spin, and osc_process will be handled by the timer
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart_smooth.py
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart_smooth.py
@ -0,0 +1,232 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from sensor_msgs.msg import JointState
 from osc4py3.as_allthreads import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import time
 import numpy as np
 import spatialmath as sm
 import roboticstoolbox as rtb
 import os
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, joint_velocity_limits, robot, cost_mask):
        super().__init__('scaled_joint_trajectory_publisher')
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            '/scaled_joint_trajectory_controller/joint_trajectory', 
            1
        )
        self.subscription = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_states_callback,
            1  # Increased queue size for joint states
        )
        # Store received joint positions
        self.current_joint_positions = [0.0] * len(joint_names)
        self.joint_names = joint_names
        self.joint_velocity_limits = joint_velocity_limits
        self.desired_joint_positions = [0.0] * len(joint_names)
        self.previous_desired = [0.0] * len(joint_names)
        self.robot = robot
        self.cost_mask = cost_mask
        ip = "0.0.0.0"  # Listen on all network interfaces
        port = 8000     # Must match the sender's port in `joint_state_osc.py`
        osc_startup()
        osc_udp_server(ip, port, "osc_server")
        print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [joint_positions] optional: duration as last argument")
        # Register OSC handler
        osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
        self.hz = float(input("Enter the desired refresh frequency (Hz): "))
        # Start the OSC server in a separate thread to avoid blocking the ROS 2 event loop
        self.create_timer(1/self.hz, self.update_position)  # Timer to call osc_process periodically
        while True:
            try:
                self.x_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for x (space-separated, enter 'x' for no limit): ").split()]
                self.y_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for y (space-separated, enter 'x' for no limit): ").split()]
                self.z_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for z (space-separated, enter 'x' for no limit): ").split()]
                if len(self.x_limits) != 2 or len(self.y_limits) != 2 or len(self.z_limits) != 2:
                    print("Invalid input. Please enter exactly two values (or leave blank) for each limit.")
                    continue
                if (self.x_limits[0] is not None and self.x_limits[1] is not None and self.x_limits[0] >= self.x_limits[1]) or \
                (self.y_limits[0] is not None and self.y_limits[1] is not None and self.y_limits[0] >= self.y_limits[1]) or \
                (self.z_limits[0] is not None and self.z_limits[1] is not None and self.z_limits[0] >= self.z_limits[1]):
                    print("Invalid input. Lower limit must be less than upper limit for each axis.")
                    continue
                print(f"Current limits:")
                print(f"x: {self.x_limits}")
                print(f"y: {self.y_limits}")
                print(f"z: {self.z_limits}")
                confirm = input("Do you want your robot to move in this range? (y/n): ").strip().lower()
                if confirm == 'y':
                    break
                elif confirm == 'n':
                    print("Please re-enter the limits.")
                else:
                    print("Invalid input. Please enter 'y' or 'n'.")
            except ValueError:
                print("Invalid input. Please enter numeric values only.")
    def joint_angles_handler(self, *args):
    # Ensure the desired joint positions are within the specified limits
        x, y, z, r, p, yaw, *_ = [float(i) for i in list(args)]
        if self.x_limits[0] is not None:
            x = max(self.x_limits[0], x)
        if self.x_limits[1] is not None:
            x = min(self.x_limits[1], x)
        if self.y_limits[0] is not None:
            y = max(self.y_limits[0], y)
        if self.y_limits[1] is not None:
            y = min(self.y_limits[1], y)
        if self.z_limits[0] is not None:
            z = max(self.z_limits[0], z)
        if self.z_limits[1] is not None:
            z = min(self.z_limits[1], z)
        if x != args[0] or y != args[1] or z != args[2]:
            self.get_logger().warn(
            f"Desired joint positions adjusted to fit within limits: "
            f"x={x}, y={y}, z={z} (original: x={args[0]}, y={args[1]}, z={args[2]})"
            )
        self.desired_joint_positions = [x, y, z, r, p, yaw]
    def joint_states_callback(self, msg):
        """Callback function to handle incoming joint states."""
        joint_position_dict = dict(zip(msg.name, msg.position))
        self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
    def rampfunction(self, startvalue, blendtime, currenttime):
        """
        Ramp function to create a smooth transition from startvalue to 1 over blendtime seconds.
        """
        if currenttime < blendtime:
            return startvalue + (1 - startvalue) * (currenttime / blendtime)
        else:
            return 1
    def update_position(self):
        if self.desired_joint_positions == self.previous_desired:
            return
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        steps_per_m = 30
        if True: #len(args[0]) == len(self.joint_names):
            prev_duration = 0
            T1 = self.robot.fkine(self.current_joint_positions)
            [x,y,z] = T1.t
            [roll, pitch, yaw] = T1.rpy()
            x1, y1, z1, roll1, pitch1, yaw1 = self.desired_joint_positions
            steps = int(np.linalg.norm(np.array([x1-x, y1-y, z1-z])) * steps_per_m)
            if steps < 2: steps = 2
            cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)]
            for j in range(steps):
                sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True)
                if sol[1] == 1:
                    duration = 0
                    prev = self.current_joint_positions if j == 0 else prev_sol
                    for p1, p2, max_vel in zip(sol[0], prev, self.joint_velocity_limits.values()):
                        duration = max(duration, abs(p1 - p2) / max_vel)#, 1/self.hz) # as minimun
                    point = JointTrajectoryPoint()
                    point.positions = list(sol[0])
                    duration /= self.rampfunction(0.1, 2, prev_duration)
                    duration += prev_duration
                    prev_duration = duration
                    point.time_from_start.sec = int(duration)
                    point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
                    msg.points.append(point)
                    prev_sol = list(sol[0])
                else:
                    print(f'IK could not find a solution for (x,y,z) = ({cart_traj[j].t}), (roll,pitch,yaw) = ({cart_traj[j].rpy()})!')
                    prev_sol = self.current_joint_positions
        msg.header.stamp = self.get_clock().now().to_msg()
        self.publisher.publish(msg)
        self.previous_desired = self.desired_joint_positions
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    while True:
        path_to_urdf = input("Enter the path to the URDF file: ")
        if os.path.isfile(path_to_urdf):
            if not path_to_urdf.endswith('.urdf'):
                print("The file is not a URDF file. Please enter a valid URDF file.")
                continue
            break
        else:
            print("Invalid path. Please enter a valid path to the URDF file.")
    tree = ET.parse(path_to_urdf)
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    robot = rtb.ERobot.URDF(path_to_urdf)
    joint_velocity_limits = {}
    # Iterate over all joints in the URDF
    for joint in root.findall('.//joint'):
        joint_name = joint.get('name')  # Get the name of the joint
        # Look for the <limit> tag under each joint
        limit = joint.find('limit')
        if limit is not None:
            # Extract the velocity limit (if it exists)
            velocity_limit = limit.get('velocity')
            if velocity_limit is not None:
                joint_velocity_limits[joint_name] = float(velocity_limit)
    print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a Cartesian coordinate [x, y, z, roll, pitch, yaw].")
    print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotation.")
    while True:
        try:
            cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
            if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
                considered_coords = [coord for coord, use in zip(['x', 'y', 'z', 'roll', 'pitch', 'yaw'], cost_mask) if use == 1]
                print(f"The following coordinates will be considered for IK: {', '.join(considered_coords)}")
                confirm = input("Are you sure you want to proceed with this cost mask? (y/n): ").strip().lower()
                if confirm == 'y':
                    break
                elif confirm == 'n':
                    print("Please re-enter the cost mask.")
                else:
                    print("Invalid input. Please enter 'y' or 'n'.")
            else:
                print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
        except ValueError:
            print("Invalid input. Please enter integers only.")
    print(f"Cost mask: {cost_mask}")
    rclpy.init()
    node = ScaledJointTrajectoryPublisher(joint_names, joint_velocity_limits, robot, cost_mask)
    # Run ROS 2 spin, and osc_process will be handled by the timer
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/test.py
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/test.py
@ -0,0 +1,36 @@
 import numpy as np
 from roboticstoolbox.tools.trajectory import mstraj
 # Define via points (each row is a joint configuration)
 viapoints = np.array([
    [0, 0, 0],         # Start
    [0.5, 0.2, -0.1],  # Intermediate
    [1.0, 0.4, 0.2]    # End
 ])
 # Time step
 dt = 0.01  # seconds
 # Acceleration time
 tacc = 0.2  # seconds
 # Maximum joint velocity per joint (same length as number of joints)
 qdmax = [0.5, 0.3, 0.4]  # radians per second
 # Optional: starting position (otherwise uses first viapoint)
 q0 = viapoints[0]
 # Generate the trajectory
 traj = mstraj(
    viapoints=viapoints,
    dt=dt,
    tacc=tacc,
    qdmax=qdmax,
 )
 # Extract trajectory
 time = traj.t          # Time vector
 positions = traj.q     # Joint angles (shape: K x N)
 print("Time vector:", time)
 print("Joint positions:\n", positions)
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server.py
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server.py
@ -0,0 +1,78 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from osc4py3.as_eventloop import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names):
        super().__init__('scaled_joint_trajectory_publisher')
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            '/scaled_joint_trajectory_controller/joint_trajectory', 
            10
        )
        # Store received joint positions
        self.joint_positions = []
        self.joint_names = joint_names
        osc_startup()
        osc_udp_server("0.0.0.0", 8000, "osc_server")
        print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [joint_positions] optional: duration as last argument")
        # Register OSC handler
        osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
    def joint_angles_handler(self, *args):
        """Handles incoming OSC messages for joint positions."""
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        n=2
        for arg in args:
            if len(arg) == len(self.joint_names): 
                point = JointTrajectoryPoint()
                point.positions = list(arg)
                point.time_from_start.sec = n
                n+=2
                point.time_from_start.nanosec = 0
                msg.points.append(point)
            elif len(arg) == len(self.joint_names) + 1:
                point = JointTrajectoryPoint()
                point.positions = list(arg[:-1])
                point.time_from_start.sec = int(arg[-1])
                point.time_from_start.nanosec = int((arg[-1] - int(arg[-1])) * 1e9)
                msg.points.append(point)
        self.publisher.publish(msg)
        print("published joint positions")
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    tree = ET.parse('/BA/robot.urdf')
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    rclpy.init()
    node = ScaledJointTrajectoryPublisher(joint_names)
    # Run both ROS 2 and OSC Server together
    try:
        while rclpy.ok():
            osc_process()  # Handle one OSC request at a time
            rclpy.spin_once(node, timeout_sec=0.1)  # Process ROS callbacks
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart.py
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart.py
@ -0,0 +1,106 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from osc4py3.as_eventloop import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import roboticstoolbox as rtb
 import spatialmath as sm
 import numpy as np
 import time
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, robot):
        super().__init__('scaled_joint_trajectory_publisher')
        self.robot = robot
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            '/scaled_joint_trajectory_controller/joint_trajectory', 
            10
        )
        # Store received joint positions
        self.joint_names = joint_names
        osc_startup()
        osc_udp_server("0.0.0.0", 6080, "osc_server")
        print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
        # Register OSC handler
        osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
    def joint_angles_handler(self, *args):
        """Handles incoming OSC messages for joint positions."""
        time1 = time.time()
        print("Received joint positions")
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        joint_positions = [0.0] * len(self.joint_names)
        steps = 30
        vel = 0.4
        if len(args[0]) == len(self.joint_names):
            n=2.0
            for i in range(len(args)-1):
                x, y, z, roll, pitch, yaw = args[i]
                Tep1 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
                x, y, z, roll, pitch, yaw = args[i+1]
                Tep2 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
                cart_traj = rtb.ctraj(Tep1, Tep2, steps)
                for j in range(steps-1):
                    sol = self.robot.ik_LM(cart_traj[j], q0=joint_positions)
                    dist = np.linalg.norm(cart_traj[j].t - cart_traj[j+1].t)
                    point = JointTrajectoryPoint()
                    point.positions = list(sol[0])
                    joint_positions = list(sol[0])
                    point.time_from_start.sec = int(n)
                    point.time_from_start.nanosec = int((n - int(n)) * 1e9)
                    n+=dist/vel
                    n+=0.1
                    msg.points.append(point)
        elif len(args[0]) == len(self.joint_names) + 1:
            for i in range(len(args)):
                x, y, z, roll, pitch, yaw, timetag = args[i]
                Tep = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
                x, y, z, roll, pitch, yaw = args[i+1][:-1]
                Tep2 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
                cart_traj = rtb.ctraj(Tep, Tep2, steps)
                for Tep in cart_traj:
                    sol = self.robot.ik_LM(Tep, q0=joint_positions)
        else:
            print("Invalid number or format of arguments")
        self.publisher.publish(msg)
        print("published joint positions")
        print(f'Frequency: {round(1/(time.time()-time1),2)} Hz')
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    tree = ET.parse('/BA/robot.urdf')
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    robot = rtb.ERobot.URDF('/BA/robot.urdf')
    rclpy.init()
    node = ScaledJointTrajectoryPublisher(joint_names, robot)
    # Run both ROS 2 and OSC Server together
    try:
        while rclpy.ok():
            osc_process()  # Handle one OSC request at a time
            rclpy.spin_once(node, timeout_sec=0.1)  # Process ROS callbacks
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast.py
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast.py
@ -0,0 +1,141 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from osc4py3.as_eventloop import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import roboticstoolbox as rtb
 import spatialmath as sm
 import numpy as np
 import time
 import os
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, robot, cost_mask, joint_velocity_limits):
        super().__init__('scaled_joint_trajectory_publisher')
        self.joint_velocity_limits = joint_velocity_limits
        self.cost_mask = cost_mask
        self.robot = robot
        self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
        if self.trajectroy_topic_name == "":
            self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            self.trajectroy_topic_name, 
            10
        )
        # Store received joint positions
        self.joint_names = joint_names
        self.port = 8000 # UDP port
        osc_startup()
        osc_udp_server("0.0.0.0", self.port, "osc_server")
        print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
        # Register OSC handler
        osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
        print("OSC method registered for /joint_trajectory")
    def joint_angles_handler(self, *args):
        """Handles incoming OSC messages for joint positions."""
        print("Received joint positions")
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        steps = 50
        if True: #len(args[0]) == len(self.joint_names):
            prev_duration = 0
            for i in range(len(args)-1):
                x, y, z, roll, pitch, yaw = args[i]
                x1, y1, z1, roll1, pitch1, yaw1 = args[i+1]
                cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i], order='xyz') for i in range(steps)]
                prev_sol = [0.0,0.0,0.0,0.0,0.0,0.0] if i == 0  else sol[0]
                for j in (range(steps) if i == 0 else range(1,steps)):
                    #print(cart_traj[j])
                    sol = self.robot.ik_LM(cart_traj[j], q0=[0.0] * len(self.joint_names), mask = self.cost_mask, joint_limits = True) if i == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True)
                    if sol[1] == 1:
                        if list(sol[0])==list(prev_sol): continue
                        duration = 0
                        for p1, p2, max_vel in zip(sol[0], prev_sol, self.joint_velocity_limits.values()):
                            duration = max(duration, abs(p1 - p2) / max_vel) # as minimun
                        point = JointTrajectoryPoint()
                        point.positions = list(sol[0])
                        duration *= 1.6
                        duration += prev_duration
                        prev_duration = duration
                        point.time_from_start.sec = int(duration)
                        point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
                        msg.points.append(point)
                        prev_sol = list(sol[0])
                    else: print('IK could not find a solution!')
        msg.header.stamp = self.get_clock().now().to_msg()
        self.publisher.publish(msg)
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    while True:
        path_to_urdf = input("Enter the path to the URDF file: ")
        if os.path.isfile(path_to_urdf):
            if not path_to_urdf.endswith('.urdf'):
                print("The file is not a URDF file. Please enter a valid URDF file.")
                continue
            break
        else:
            print("Invalid path. Please enter a valid path to the URDF file.")
    tree = ET.parse(path_to_urdf)
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    robot = rtb.ERobot.URDF(path_to_urdf)
    print(robot)
    joint_velocity_limits = {}
    # Iterate over all joints in the URDF
    for joint in root.findall('.//joint'):
        joint_name = joint.get('name')  # Get the name of the joint
        # Look for the <limit> tag under each joint
        limit = joint.find('limit')
        if limit is not None:
            # Extract the velocity limit (if it exists)
            velocity_limit = limit.get('velocity')
            if velocity_limit is not None:
                joint_velocity_limits[joint_name] = float(velocity_limit)
    rclpy.init()
    while True:
        try:
            print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
            print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
            cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
            if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
                break
            else:
                print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
        except ValueError:
            print("Invalid input. Please enter integers only.")
    print(f"Cost mask: {cost_mask}")
    node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask, joint_velocity_limits)
    # Run both ROS 2 and OSC Server together
    try:
        while rclpy.ok():
            osc_process()  # Handle one OSC request at a time
            rclpy.spin_once(node, timeout_sec=0.1)  # Process ROS callbacks
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast_max_acc.py
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast_max_acc.py
@ -0,0 +1,166 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from sensor_msgs.msg import JointState
 from osc4py3.as_allthreads import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import roboticstoolbox as rtb
 import spatialmath as sm
 import numpy as np
 import time
 import os
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, robot, cost_mask, joint_velocity_limits):
        super().__init__('scaled_joint_trajectory_publisher')
        self.joint_velocity_limits = joint_velocity_limits
        self.cost_mask = cost_mask
        self.robot = robot
        self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
        if self.trajectroy_topic_name == "":
            self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            self.trajectroy_topic_name, 
            10
        )
        self.subscription = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_states_callback,
            1  # Increased queue size for joint states
            )
        self.maximum_acceleration = [0.0] * len(joint_names)
        # Store received joint positions
        self.joint_names = joint_names
        for joint in joint_names:
            self.maximum_acceleration[joint_names.index(joint)] = float(input(f"Enter the maximum acceleration for joint {joint}: "))
        self.port = 8000 # UDP port
        osc_startup()
        osc_udp_server("0.0.0.0", self.port, "osc_server")
        print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
        # Register OSC handler
        osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
        print("OSC method registered for /joint_trajectory")
    def joint_states_callback(self, msg):
        """Callback function to handle incoming joint states."""
        joint_position_dict = dict(zip(msg.name, msg.position))
        self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
        joint_velocity_dict = dict(zip(msg.name, msg.velocity))
        self.current_joint_velocities = [joint_velocity_dict[name] for name in self.joint_names]
    def joint_angles_handler(self, *args):
        """Handles incoming OSC messages for joint positions."""
        print("Received joint positions")
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        steps = 50
        if True: #len(args[0]) == len(self.joint_names):
            prev_duration = 0
            for i in range(len(args)-1):
                x, y, z, roll, pitch, yaw = args[i]
                x1, y1, z1, roll1, pitch1, yaw1 = args[i+1]
                cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i], order='xyz') for i in range(steps)]
                prev_sol = self.current_joint_positions if i == 0  else sol[0]
                for j in (range(steps) if i == 0 else range(1,steps)):
                    #print(cart_traj[j])
                    sol = self.robot.ik_LM(cart_traj[j], q0=[0.0] * len(self.joint_names), mask = self.cost_mask, joint_limits = True) if i == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True)
                    if sol[1] == 1:
                        if list(sol[0])==list(prev_sol): continue
                        duration = 0
                        for i, (p1, p2, max_vel) in enumerate(zip(sol[0], prev_sol, self.joint_velocity_limits.values())):
                            print(f'joint {i}, p1: {p1}, p2: {p2}, max_vel: {max_vel}')
                            if len(msg.points) == 0: v = self.current_joint_velocities[i]
                            max_acc_duration = np.sqrt((v/self.maximum_acceleration[i])**2 + 2*(abs(p1 - p2)/self.maximum_acceleration[i]))- v/self.maximum_acceleration[i]
                            duration = max(duration, abs(p1 - p2) / max_vel, max_acc_duration) # as minimun
                            v = abs(p1 - p2) / duration
                            print(f'duration: {duration}, max_acc_duration: {max_acc_duration}, max_vel_duration: { abs(p1 - p2) / max_vel}, v: {v}')
                        point = JointTrajectoryPoint()
                        point.positions = list(sol[0])
                        duration += prev_duration
                        prev_duration = duration
                        point.time_from_start.sec = int(duration)
                        point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
                        msg.points.append(point)
                        prev_sol = list(sol[0])
                    else: print('IK could not find a solution!')
        msg.header.stamp = self.get_clock().now().to_msg()
        self.publisher.publish(msg)
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    while True:
        path_to_urdf = input("Enter the path to the URDF file: ")
        if os.path.isfile(path_to_urdf):
            if not path_to_urdf.endswith('.urdf'):
                print("The file is not a URDF file. Please enter a valid URDF file.")
                continue
            break
        else:
            print("Invalid path. Please enter a valid path to the URDF file.")
    tree = ET.parse(path_to_urdf)
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    robot = rtb.ERobot.URDF(path_to_urdf)
    print(robot)
    joint_velocity_limits = {}
    # Iterate over all joints in the URDF
    for joint in root.findall('.//joint'):
        joint_name = joint.get('name')  # Get the name of the joint
        # Look for the <limit> tag under each joint
        limit = joint.find('limit')
        if limit is not None:
            # Extract the velocity limit (if it exists)
            velocity_limit = limit.get('velocity')
            if velocity_limit is not None:
                joint_velocity_limits[joint_name] = float(velocity_limit)
    rclpy.init()
    while True:
        try:
            print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
            print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
            cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
            if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
                break
            else:
                print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
        except ValueError:
            print("Invalid input. Please enter integers only.")
    print(f"Cost mask: {cost_mask}")
    node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask, joint_velocity_limits)
    # Run both ROS 2 and OSC Server together
    try:
        while rclpy.ok():
            osc_process()  # Handle one OSC request at a time
            rclpy.spin_once(node, timeout_sec=0.1)  # Process ROS callbacks
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast_smooth.py
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast_smooth.py
@ -0,0 +1,152 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from osc4py3.as_eventloop import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import roboticstoolbox as rtb
 import spatialmath as sm
 import numpy as np
 import time
 import os
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, robot, cost_mask, joint_velocity_limits):
        super().__init__('scaled_joint_trajectory_publisher')
        self.joint_velocity_limits = joint_velocity_limits
        self.cost_mask = cost_mask
        self.robot = robot
        self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
        if self.trajectroy_topic_name == "":
            self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            self.trajectroy_topic_name, 
            10
        )
        # Store received joint positions
        self.joint_names = joint_names
        self.port = 8000 # UDP port
        osc_startup()
        osc_udp_server("0.0.0.0", self.port, "osc_server")
        print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
        # Register OSC handler
        osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
        print("OSC method registered for /joint_trajectory")
    def rampfunction(self, startvalue, blendtime, currenttime):
            """
            Ramp function to create a smooth transition from startvalue to 1 over blendtime seconds.
            """
            if currenttime < blendtime:
                return startvalue + (1 - startvalue) * (currenttime / blendtime)
            else:
                return 1
    def joint_angles_handler(self, *args):
        """Handles incoming OSC messages for joint positions."""
        print("Received joint positions")
        msg = JointTrajectory()
        msg.joint_names = self.joint_names
        steps = 50
        if True: #len(args[0]) == len(self.joint_names):
            prev_duration = 0
            for i in range(len(args)-1):
                x, y, z, roll, pitch, yaw = args[i]
                x1, y1, z1, roll1, pitch1, yaw1 = args[i+1]
                cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i], order='xyz') for i in range(steps)]
                prev_sol = [0.0,0.0,0.0,0.0,0.0,0.0] if i == 0  else sol[0]
                for j in (range(steps) if i == 0 else range(1,steps)):
                    #print(cart_traj[j])
                    sol = self.robot.ik_LM(cart_traj[j], q0=[0.0] * len(self.joint_names), mask = self.cost_mask, joint_limits = True) if i == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True)
                    if sol[1] == 1:
                        if list(sol[0])==list(prev_sol): continue
                        duration = 0
                        for p1, p2, max_vel in zip(sol[0], prev_sol, self.joint_velocity_limits.values()):
                            duration = max(duration, abs(p1 - p2) / max_vel) # as minimun
                        point = JointTrajectoryPoint()
                        point.positions = list(sol[0])
                        duration /= self.rampfunction(0.1, 2, prev_duration)
                        duration += prev_duration
                        prev_duration = duration
                        point.time_from_start.sec = int(duration)
                        point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
                        msg.points.append(point)
                        prev_sol = list(sol[0])
                    else: print('IK could not find a solution!')
        msg.header.stamp = self.get_clock().now().to_msg()
        self.publisher.publish(msg)
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    while True:
        path_to_urdf = input("Enter the path to the URDF file: ")
        if os.path.isfile(path_to_urdf):
            if not path_to_urdf.endswith('.urdf'):
                print("The file is not a URDF file. Please enter a valid URDF file.")
                continue
            break
        else:
            print("Invalid path. Please enter a valid path to the URDF file.")
    tree = ET.parse(path_to_urdf)
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    robot = rtb.ERobot.URDF(path_to_urdf)
    print(robot)
    joint_velocity_limits = {}
    # Iterate over all joints in the URDF
    for joint in root.findall('.//joint'):
        joint_name = joint.get('name')  # Get the name of the joint
        # Look for the <limit> tag under each joint
        limit = joint.find('limit')
        if limit is not None:
            # Extract the velocity limit (if it exists)
            velocity_limit = limit.get('velocity')
            if velocity_limit is not None:
                joint_velocity_limits[joint_name] = float(velocity_limit)
    rclpy.init()
    while True:
        try:
            print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
            print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
            cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
            if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
                break
            else:
                print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
        except ValueError:
            print("Invalid input. Please enter integers only.")
    print(f"Cost mask: {cost_mask}")
    node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask, joint_velocity_limits)
    # Run both ROS 2 and OSC Server together
    try:
        while rclpy.ok():
            osc_process()  # Handle one OSC request at a time
            rclpy.spin_once(node, timeout_sec=0.1)  # Process ROS callbacks
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_new.py
+++ b/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_new.py
@ -0,0 +1,166 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from sensor_msgs.msg import JointState
 from osc4py3.as_allthreads import *
 from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import roboticstoolbox as rtb
 import spatialmath as sm
 import numpy as np
 import time
 import os
 class ScaledJointTrajectoryPublisher(Node):
    """Node to publish joint trajectories based on OSC messages."""
    def __init__(self, joint_names, robot, cost_mask, joint_velocity_limits):
        super().__init__('scaled_joint_trajectory_publisher')
        self.joint_velocity_limits = [joint_velocity_limits[joint] for joint in joint_names]
        self.cost_mask = cost_mask
        self.robot = robot
        self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
        if self.trajectroy_topic_name == "":
            self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
        # ROS2 Publisher
        self.publisher = self.create_publisher(
            JointTrajectory, 
            self.trajectroy_topic_name, 
            10
        )
        self.subscription = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_states_callback,
            1  # Increased queue size for joint states
        )
        # Store received joint positions
        self.joint_names = joint_names
        self.port = 8000 # UDP port
        osc_startup()
        osc_udp_server("0.0.0.0", self.port, "osc_server")
        print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
        # Register OSC handler
        osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
        print("OSC method registered for /joint_trajectory")
    def joint_states_callback(self, msg):
        """Callback function to handle incoming joint states."""
        joint_position_dict = dict(zip(msg.name, msg.position))
        self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
        joint_velocity_dict = dict(zip(msg.name, msg.velocity))
        self.current_joint_velocities = [joint_velocity_dict[name] for name in self.joint_names]
    def joint_angles_handler(self, *args):
        """Handles incoming OSC messages for joint positions."""
        try:
            print("Received joint positions")
            viapoints = []
            msg = JointTrajectory()
            msg.joint_names = self.joint_names
            steps_per_m = 4
            for i in range(len(args)-1):
                x, y, z, roll, pitch, yaw = args[i]
                x1, y1, z1, roll1, pitch1, yaw1 = args[i+1]
                steps = int(np.linalg.norm(np.array([x1-x, y1-y, z1-z])) * steps_per_m)
                if steps < 2: steps = 2
                cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i], order='xyz') for i in range(steps)]
                if i == 0: prev_sol = self.current_joint_positions
                for j in (range(steps) if i == 0 else range(1,steps)):
                    #print(cart_traj[j])
                    sol = self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True)
                    if sol[1] == 1:
                        viapoints.append(list(sol[0]))
                        prev_sol = list(sol[0])
                    else: print('IK could not find a solution!')
            dt = 0.01
            tacc = 0.5
            print(f'length viapoints: {len(viapoints)}')
            traj = rtb.mstraj(np.array(viapoints), q0 = self.current_joint_positions ,dt=dt, tacc=tacc, qdmax=[1 * i for i in self.joint_velocity_limits])
            print(len(traj.q))
            print(len(traj.t))
            print(traj.t)
            print(traj.arrive)
            msg.points = []
            for i in range(len(traj.q)):
                point = JointTrajectoryPoint()
                point.positions = list(traj.q[i])
                point.time_from_start.sec = int(traj.t[i])
                point.time_from_start.nanosec = int(((traj.t[i] - int(traj.t[i])) * 1e9))
                #point.time_from_start = rclpy.duration.Duration(seconds=traj.t[i]).to_msg()
                msg.points.append(point)
            msg.header.stamp = self.get_clock().now().to_msg()
            self.publisher.publish(msg)
            print('published')
        except Exception as e:
            print(f'Error in joint angles handler: {e}')
 def main():
    """Main function to get joint names and start the ROS 2 & OSC system."""
    while True:
        path_to_urdf = input("Enter the path to the URDF file: ")
        if os.path.isfile(path_to_urdf):
            if not path_to_urdf.endswith('.urdf'):
                print("The file is not a URDF file. Please enter a valid URDF file.")
                continue
            break
        else:
            print("Invalid path. Please enter a valid path to the URDF file.")
    tree = ET.parse(path_to_urdf)
    root = tree.getroot()
    joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
    robot = rtb.ERobot.URDF(path_to_urdf)
    print(robot)
    joint_velocity_limits = {}
    # Iterate over all joints in the URDF
    for joint in root.findall('.//joint'):
        joint_name = joint.get('name')  # Get the name of the joint
        # Look for the <limit> tag under each joint
        limit = joint.find('limit')
        if limit is not None:
            # Extract the velocity limit (if it exists)
            velocity_limit = limit.get('velocity')
            if velocity_limit is not None:
                joint_velocity_limits[joint_name] = float(velocity_limit)
    rclpy.init()
    while True:
        try:
            print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
            print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
            cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
            if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
                break
            else:
                print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
        except ValueError:
            print("Invalid input. Please enter integers only.")
    print(f"Cost mask: {cost_mask}")
    node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask, joint_velocity_limits)
    # Run both ROS 2 and OSC Server together
    try:
        while rclpy.ok():
            osc_process()  # Handle one OSC request at a time
            rclpy.spin_once(node, timeout_sec=0.1)  # Process ROS callbacks
    except KeyboardInterrupt:
        print("")
    finally:
        node.destroy_node()
        rclpy.shutdown()
        osc_terminate()
 if __name__ == '__main__':
    main()
--- a/Show More
+++ b/Show More
		`@ -0,0 +1,2 @@`
							`timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint`
							`1746011502.5537577,-0.2308989147082774,-1.5576496867458662,1.9268478330411956,-1.7397192673604973,-1.7839696265599596,-1.800062792954931`
		`@ -0,0 +1,2 @@`
							`timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint`
							`1746011503.7946618,-0.1686252647757187,-1.5472390563625094,1.9591905880121567,-1.8458983617027747,-2.1567869317617543,-1.7676185896498668`
		`@ -0,0 +1,2 @@`
							`timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint`
							`1746011506.1262758,-0.37964591217805577,-1.5433421257233508,1.898245007183581,-1.3844446013798872,-0.9486654069655223,-2.0294127243158604`
		`@ -0,0 +1,2 @@`
							`timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint`
							`1746011508.8176053,-0.12860039709478688,-1.5327987457296275,1.998946192099603,-1.9460947967968842,-2.4571289985280873,-1.769060918772316`
		`@ -0,0 +1,2 @@`
							`timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint`
							`1746011511.4248803,-0.32851231244735324,-1.556318860706303,1.9054772242742963,-1.5542632600057393,-1.2513477173722232,-1.906920178150602`
		`@ -0,0 +1,2 @@`
							`timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint`
							`1746011513.0201094,-0.2811586958622918,-1.5170442353198785,1.842088326077329,-1.4186114407746067,-1.510089919486052,-2.4769183223554463`
		`@ -0,0 +1,2 @@`
							`timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint`
							`1746011514.4977942,-0.2598094630044301,-1.5109851246905668,1.835455896600081,-1.4172144983489905,-1.6251002087604622,-2.6978004634140476`
		`@ -0,0 +1,2 @@`
							`timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint`
							`1746011516.3413794,-0.35378337935391757,-1.6016681749948434,2.001576650922292,-1.8398676754403294,-1.1068121709485519,-1.3089895738273505`
		`@ -0,0 +1,2 @@`
							`timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint`
							`1746011516.9457223,-0.3556423347154132,-1.6207376464189907,2.0674685662627326,-2.094571991443367,-1.0957598017613526,-0.8558302161593532`
		`@ -0,0 +1,2 @@`
							`timestamp,shoulder_pan_joint,shoulder_lift_joint,elbow_joint,wrist_1_joint,wrist_2_joint,wrist_3_joint`
							`1746011518.62554,-0.22730261986774725,-1.490045675183208,2.2190438871192217,-3.301392142549558,-1.803855577475699,-1.1463284162240381`