AS: upload log recordings
This commit is contained in:
cha2080
parent
ff3b2a03bf
commit
cd1ae17d41
@ -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()
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -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
|
||||
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
|
||||
1746011500.6981866,-0.2584434767928996,-1.5593503626194987,1.918429404588129,-1.6944083250439956,-1.6324038286536853,-1.8219460785849033
|
||||
|
||||
|
2
test/joint_trajectories/trajectory_0002.csv
Normal file
2
test/joint_trajectories/trajectory_0002.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories/trajectory_0003.csv
Normal file
2
test/joint_trajectories/trajectory_0003.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories/trajectory_0004.csv
Normal file
2
test/joint_trajectories/trajectory_0004.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories/trajectory_0005.csv
Normal file
2
test/joint_trajectories/trajectory_0005.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories/trajectory_0006.csv
Normal file
2
test/joint_trajectories/trajectory_0006.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories/trajectory_0007.csv
Normal file
2
test/joint_trajectories/trajectory_0007.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories/trajectory_0008.csv
Normal file
2
test/joint_trajectories/trajectory_0008.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories/trajectory_0009.csv
Normal file
2
test/joint_trajectories/trajectory_0009.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories/trajectory_0010.csv
Normal file
2
test/joint_trajectories/trajectory_0010.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories/trajectory_0011.csv
Normal file
2
test/joint_trajectories/trajectory_0011.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories/trajectory_0012.csv
Normal file
2
test/joint_trajectories/trajectory_0012.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories/trajectory_0013.csv
Normal file
2
test/joint_trajectories/trajectory_0013.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories/trajectory_0014.csv
Normal file
2
test/joint_trajectories/trajectory_0014.csv
Normal file
@ -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
|
||||
|
47
test/joint_trajectories/trajectory_0015.csv
Normal file
47
test/joint_trajectories/trajectory_0015.csv
Normal file
@ -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
|
||||
|
35
test/joint_trajectories/trajectory_0016.csv
Normal file
35
test/joint_trajectories/trajectory_0016.csv
Normal file
@ -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
|
||||
|
17
test/joint_trajectories/trajectory_0017.csv
Normal file
17
test/joint_trajectories/trajectory_0017.csv
Normal file
@ -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
|
||||
|
15
test/joint_trajectories/trajectory_0018.csv
Normal file
15
test/joint_trajectories/trajectory_0018.csv
Normal file
@ -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
|
||||
|
13
test/joint_trajectories/trajectory_0019.csv
Normal file
13
test/joint_trajectories/trajectory_0019.csv
Normal file
@ -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
|
||||
|
10
test/joint_trajectories/trajectory_0020.csv
Normal file
10
test/joint_trajectories/trajectory_0020.csv
Normal file
@ -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
|
||||
|
@ -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
|
||||
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
|
||||
1746011500.6981866,0.6532147996609244,2.910667943093163e-05,0.45143072952175217,-1.3204229019021049e-05,0.24324840834544145,-6.09777640597976e-06
|
||||
|
||||
|
2
test/joint_trajectories_cart/trajectory_0002_cart.csv
Normal file
2
test/joint_trajectories_cart/trajectory_0002_cart.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories_cart/trajectory_0003_cart.csv
Normal file
2
test/joint_trajectories_cart/trajectory_0003_cart.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories_cart/trajectory_0004_cart.csv
Normal file
2
test/joint_trajectories_cart/trajectory_0004_cart.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories_cart/trajectory_0005_cart.csv
Normal file
2
test/joint_trajectories_cart/trajectory_0005_cart.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories_cart/trajectory_0006_cart.csv
Normal file
2
test/joint_trajectories_cart/trajectory_0006_cart.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories_cart/trajectory_0007_cart.csv
Normal file
2
test/joint_trajectories_cart/trajectory_0007_cart.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories_cart/trajectory_0008_cart.csv
Normal file
2
test/joint_trajectories_cart/trajectory_0008_cart.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories_cart/trajectory_0009_cart.csv
Normal file
2
test/joint_trajectories_cart/trajectory_0009_cart.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories_cart/trajectory_0010_cart.csv
Normal file
2
test/joint_trajectories_cart/trajectory_0010_cart.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories_cart/trajectory_0011_cart.csv
Normal file
2
test/joint_trajectories_cart/trajectory_0011_cart.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories_cart/trajectory_0012_cart.csv
Normal file
2
test/joint_trajectories_cart/trajectory_0012_cart.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories_cart/trajectory_0013_cart.csv
Normal file
2
test/joint_trajectories_cart/trajectory_0013_cart.csv
Normal file
@ -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
|
||||
|
2
test/joint_trajectories_cart/trajectory_0014_cart.csv
Normal file
2
test/joint_trajectories_cart/trajectory_0014_cart.csv
Normal file
@ -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
|
||||
|
47
test/joint_trajectories_cart/trajectory_0015_cart.csv
Normal file
47
test/joint_trajectories_cart/trajectory_0015_cart.csv
Normal file
@ -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
|
||||
|
35
test/joint_trajectories_cart/trajectory_0016_cart.csv
Normal file
35
test/joint_trajectories_cart/trajectory_0016_cart.csv
Normal file
@ -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
|
||||
|
17
test/joint_trajectories_cart/trajectory_0017_cart.csv
Normal file
17
test/joint_trajectories_cart/trajectory_0017_cart.csv
Normal file
@ -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
|
||||
|
15
test/joint_trajectories_cart/trajectory_0018_cart.csv
Normal file
15
test/joint_trajectories_cart/trajectory_0018_cart.csv
Normal file
@ -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
|
||||
|
13
test/joint_trajectories_cart/trajectory_0019_cart.csv
Normal file
13
test/joint_trajectories_cart/trajectory_0019_cart.csv
Normal file
@ -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
|
||||
|
10
test/joint_trajectories_cart/trajectory_0020_cart.csv
Normal file
10
test/joint_trajectories_cart/trajectory_0020_cart.csv
Normal file
@ -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
|
||||
|
@ -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()
|
||||
@ -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()
|
||||
@ -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()
|
||||
@ -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()
|
||||
@ -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)
|
||||
@ -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()
|
||||
@ -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()
|
||||
@ -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()
|
||||
@ -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()
|
||||
@ -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()
|
||||
@ -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()
|
||||
@ -1 +1 @@
|
||||
0
|
||||
1
|
||||
|
||||
@ -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
|
||||
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
|
||||
PWD=/ws/src/ba-alexanderschaefer/workspace/build/joint_control
|
||||
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
|
||||
|
||||
@ -1 +1 @@
|
||||
/BA/workspace/src/joint_control/joint_control
|
||||
/ws/src/ba-alexanderschaefer/workspace/src/joint_control/joint_control
|
||||
@ -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
|
||||
joint_control.egg-info/SOURCES.txt
|
||||
joint_control.egg-info/dependency_links.txt
|
||||
joint_control.egg-info/entry_points.txt
|
||||
joint_control.egg-info/requires.txt
|
||||
joint_control.egg-info/top_level.txt
|
||||
joint_control.egg-info/zip-safe
|
||||
resource/joint_control
|
||||
test/test_copyright.py
|
||||
test/test_flake8.py
|
||||
test/test_pep257.py
|
||||
@ -1 +1 @@
|
||||
/BA/workspace/src/joint_control/package.xml
|
||||
/ws/src/ba-alexanderschaefer/workspace/src/joint_control/package.xml
|
||||
Binary file not shown.
@ -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'
|
||||
|
||||
@ -1 +1 @@
|
||||
/BA/workspace/src/joint_control/resource/joint_control
|
||||
/ws/src/ba-alexanderschaefer/workspace/src/joint_control/resource/joint_control
|
||||
@ -1 +1 @@
|
||||
/BA/workspace/src/joint_control/setup.cfg
|
||||
/ws/src/ba-alexanderschaefer/workspace/src/joint_control/setup.cfg
|
||||
@ -1 +0,0 @@
|
||||
/BA/workspace/src/joint_control/setup.py
|
||||
@ -1,2 +0,0 @@
|
||||
/BA/workspace/build/joint_control
|
||||
.
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -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()
|
||||
@ -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()
|
||||
@ -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()
|
||||
@ -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()
|
||||
@ -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()
|
||||
@ -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()
|
||||
@ -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)
|
||||
@ -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()
|
||||
@ -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()
|
||||
@ -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()
|
||||
@ -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()
|
||||
@ -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()
|
||||
@ -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()
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue
Block a user