From d4240bc4deb84f5dbe4bc886f81e271e3e1fca02 Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Wed, 30 Apr 2025 09:44:12 +0200 Subject: [PATCH] AS: joint and cartesian limits --- osc_recieve.py | 2 +- test/joint_states_recording.py | 4 +- test/joint_trajectory_recording.py | 4 +- workspace/src/joint_angles.py | 13 +- .../__pycache__/plugdata.cpython-310.pyc | Bin 4509 -> 7402 bytes .../plugdata_cart_fix.cpython-310.pyc | Bin 6423 -> 10240 bytes .../trajectory_server_new.cpython-310.pyc | Bin 7033 -> 7047 bytes ...trajectory_server_new_cart.cpython-310.pyc | Bin 6508 -> 7007 bytes .../joint_control/joint_control/plugdata.py | 91 +++++++++++++- .../joint_control/plugdata_cart_fix.py | 114 +++++++++++++++++- .../joint_control/trajectory_server_new.py | 6 +- .../trajectory_server_new_cart.py | 57 +++++---- 12 files changed, 241 insertions(+), 50 deletions(-) diff --git a/osc_recieve.py b/osc_recieve.py index 4051558..b9ec47e 100644 --- a/osc_recieve.py +++ b/osc_recieve.py @@ -6,7 +6,7 @@ def handler(*args): osc_startup() osc_udp_server("0.0.0.0", 8000, "osc_server") -osc_method("/coordinates", handler, argscheme=osm.OSCARG_DATAUNPACK) +osc_method("/joint_angles", handler, argscheme=osm.OSCARG_MESSAGE) while True: diff --git a/test/joint_states_recording.py b/test/joint_states_recording.py index 84fbd92..ccea413 100644 --- a/test/joint_states_recording.py +++ b/test/joint_states_recording.py @@ -56,8 +56,8 @@ class JointStateLogger(Node): writer.writerow(["timestamp", "x", "y", "z", "roll", "pitch", "yaw"]) with open(file_path_cart, mode='a', newline='') as f: writer = csv.writer(f) - [x,y,z] = self.robot.fkine(list([msg.position[-1]])+list(msg.position[:4])).t - [roll, pitch, yaw] = self.robot.fkine(list([msg.position[-1]])+list(msg.position[:4])).rpy() + [x,y,z] = self.robot.fkine(list([msg.position[-1]])+list(msg.position[:5])).t + [roll, pitch, yaw] = self.robot.fkine(list([msg.position[-1]])+list(msg.position[:5])).rpy() writer.writerow([timestamp, x, y, z, roll, pitch, yaw]) diff --git a/test/joint_trajectory_recording.py b/test/joint_trajectory_recording.py index 3ee8c0b..12b08f6 100644 --- a/test/joint_trajectory_recording.py +++ b/test/joint_trajectory_recording.py @@ -59,9 +59,7 @@ class JointTrajectoryLogger(Node): for point in msg.points: timestamp = recive_time + point.time_from_start.sec + point.time_from_start.nanosec * 1e-9 T = self.robot.fkine(point.positions) - x = T.t[0] - y = T.t[1] - z = T.t[2] + [x,y,z] = T.t roll, pitch, yaw = T.rpy() row = [timestamp, x, y, z, roll, pitch, yaw] writer.writerow(row) diff --git a/workspace/src/joint_angles.py b/workspace/src/joint_angles.py index f6a801c..42c70cf 100644 --- a/workspace/src/joint_angles.py +++ b/workspace/src/joint_angles.py @@ -10,15 +10,14 @@ def main(): # Make client channels to send packets osc_udp_client("172.18.0.3", 8000, "osc_client") - robot = rtb.Robot.URDF("/BA/ur10e.urdf") # Example joint positions to send - joint_positions1 = [-0.4,0.6, 0.4, 0.0, 0.0, 0.0] - joint_positions2 = [-0.4,-0.6, 0.4, 0.0,0.0, 0.0] - joint_positions3 = [-0.4,-0.6, 0.6, 0.0, 0.0, 0.0] - joint_positions4 = [-0.4,-0.6, 0.2, 0.0, 0.0, 0.0] - joint_positions5 = [-0.4,0.6, 0.2, 0.0, 0.0, 0.0] + joint_positions1 = [0.0,0.0, 0.0, 0.0, 0.0, 0.0] + joint_positions2 = [-0.4,-1, 0.4, 0.0,0.0, 0.0] + joint_positions3 = [-0.4,-1, 0.2, 0.0, 0.0, 0.0]#, 6.0] + joint_positions4 = [-0.4,1, 0.2, 0.0, 0.0, 0.0]#, 6.0] + joint_positions5 = [-0.4,1, 0.4, 0.0, 0.0, 0.0]#, 6.0] - msg = oscbuildparse.OSCMessage("/joint_trajectory", None, [joint_positions1, joint_positions2])#, joint_positions3, joint_positions4, joint_positions5]) + msg = oscbuildparse.OSCMessage("/joint_angles", None, [joint_positions1])#, joint_positions3])#, joint_positions4, joint_positions5]) osc_send(msg, "osc_client") osc_process() print("Sending joint positions") diff --git a/workspace/src/joint_control/joint_control/__pycache__/plugdata.cpython-310.pyc b/workspace/src/joint_control/joint_control/__pycache__/plugdata.cpython-310.pyc index f032c6c4812182e18bd74f9539fd7606ce224233..93228bb199934f1df17dc5b1eef0d7a00518d878 100644 GIT binary patch literal 7402 zcmcgx%X1vZd7tO*>|*gC@gP7-k7ss%{hXrwI}JuZ1_p29iGP8HDNGHNCjDxG+SZzy zN_{=h+eXu9n@v-WjiA&nH_Nhb2G$1Ql!8jT+MJLuHK?^GHAPk0Q_blDGSi%qyPs>G zm%p>k*#`a{OGp|Vz2g|QYf zuda#dA4GnbY;bqiYb6oyiMcVryEKmlRS92D+{D93eI89Eid$_j*@;+-wjKTG#UOqS z9ZzYhOlfLNZR#H>TPo9-{-M=0n88dCYqAn65!iBgIgf*1A%!ip%RQH}Kdv zP2J7Fk9Rzt-lSvLNo03&kiARf+R5NNz8Bk@ZtO8T3hj5+@7irIj@@l9ZnOp@5~Xy% z6+8rsL$rxXAAJe5abJ zXeTlaZEb9Ba%?=sN`0M``$jTNca_X!dS)=|NJsx1s~mxX35wZ-IZ2X5`!QQI0R6dnW%aXuB|=4Sk$P@^6V~_*D5!E5;Y;%_HMWwDWMhz@7qc zE}baHPn{@zGc(!Kd)g-|ul`sGuV8dpj^?oc71}Q}(6jTmhEs-;JpGB%`CzwS#;v}O9iPd{pDOGc;Fou=WaT9Vqh~)e ze1GdlswSy^SzBpBXN&#C@I-G4o^06R+T%p&<2 zypt`gDp>_HpI`=3I5MNJLIT2>;=I2cp7;6F^J3psJU5|=3+&rR&LrqmXHKMTgrdK&Ca%U((;cb6&QO7SPNCUQ_E&Fw6;3M(6CMSD$8U_U|3ZaMr;`Od zCi@j{10+9bd8Hx0{+m}6MO0zXoY>(SaXsqsQBBa?3tT9glkUPjh-ZQS2vv6viLeQHMHiaLZ<0 z?o!`&V>@u;#CG|1x9x?=@HlR`9e6RBVob%Y9k1o?xIbK@Z5 zdvV8Yc{et@e!z-T6_+qJ>IB_w<|b~V(-YNInC+w$wL5RTOau{iwvxX7^+vwoIDY6S zj?-8TA_z(R20DX%()aF%iN__s!EE&0klEc%2fe^=`$>lHeC(uj7KN9(%zC zBZ0JhxGXWkD95kgvcHmpd=donDQae@IZq9-C2tm}cLB{S)LBIH4XNw>zf$+=A4y&M z$58hL-D&#H_rr&7;4^4Kr<*kFcLNVjxZt>V=(dsonsX4@!0N(9U&j>z1v<1DxZz%1 z-iYV6cJe!EaH+%(#&&&M-nYDEyWNF;YEmmBHI*CB?m0^Dy6xEF;%uaUkmN;D{q zy~GZ^gCZiz2^i9vJ~Lp!!15F%N>=4T)N-mXgBb>n$aHRV`;d>DOSzwPksi z9ZuI!EQ#zr&+FK&oabQF_+}&8dgk>v$9V9K@4J!yXl&^jK995SjO@`RkJ3B5y5~4XFhRl^*e)^Cg1<<-Q+LKG*c6qadlRz- zM%>+uTilm!fSZIgpF4@%c{wnF!KT~V6SYxHp$A?lE)-lY-00wPV=<9$5xNL~2m3wr zU{C#|=j6g6E)?hby~Q)Q&xd~0jhzBROh`{juCdz@Q#9zZPT@L*+2IJnb8njG2VyRN zeeSOus3(K37J3x7#W{rKciwy8+`F@J=l!*J@7(>NFs0c54?BUM2OB&NF^cqd#-;ee5I7_k>@ z@n?b0k?mm-wl_KvPed6mZp-IwF^DQ+hjc~88M*XCc*hH5cHOiP?T9I(-6p)Gaap!8EBC((rO8n zYvqhwiC*=yNtu3Xln`cSR?5oTC4|uk?GZ9htes)^XrLF*f#)*NEA`c!;dyYEOWoIo zE&-OxxNJ^IAN+xPf09l=DMpjtn^TU>m(Zj)?-lk_ZeR~oFScE_3pWnK3LCuTC-#A# z?7-LNT6W7myqy{^*ge^%x6>(dHU73QW97r!zfBz3m>N@k1`-F$#M` zb=ymvAllxB>Kg|x5BV~{^$NdDvj#=coTQdtq_LU`l?$~a^qzYl)Iw`zz$at97lGvC ziC59s`V`ddNzXN?msI7)P-pp+@S8uS$HwCYGkxVxV0DY`eDEP>LV>=lV632%sk=&F zOH>FnDGwCkNc~um9XJqj2K+TdeDucwsy2c4p803^-9cOcS8i<3#{8lnzCn>Q02oEQVl78K6ZK1n#&>L%ZiWYGJBp86erjb1|zpsweFp||7h+#|@#f1l>11YAY$2v1x>Q-aaG zsHU&}DWsiYq=n7Oc<$29zd@5I#|jD(l!Vixp|n>cE5KA-v9yJBop?;LHAgzi2@8Lv z=hihTC*@2_EV3etR3b7jDMuP3p{P)=uk9~oC~n;(aj70Deb|`YiLB&IBsGl67^zv= znM|f&Yth@Cc4nxbj~5kY%+am{KbMtu&mYfb<-BaRJ9j*vm5M$U05oa2%(YX4u>JXO z==0@ww{c3v`~34e^#x%_*Srh>zfR3xQX}s~`r}fbH1Hn)EOd7>=C=reBtO3+(gOrm zgOpn66pc*Nrp^|R+Kx==g&un?Q4ZZOqE4wpg~M2sT%?X6<0uCxqFe-Lbt@MGllFK? zScAk|Xq}hvye!HvH4efl{u5ee%Dr;YJvA0&BR<9x+h{C!Vgp}Wy#{|>(#ii4Y*DM} zHMM4>jXxkn<6%pMLQPU6#}QS>X-BLZP`~Cl`&~CEX3CDkq87pr3OYEMFHRE7i8e7= zL?~B>z64NO32@tl^2Nu@pnCX;#y^I8)*@)50_Skhv$Il;PLl|0|GU1_|mb ziMp#DLuyY{z5wqCPko}s^9lSa6_ECo&_rLByY|(jbPNeRQK@2aY(WRKyjq-T#}#Is zsAR(Wy05@f6Qn|r53W$@?zqaTSXV-Cf=!@T&dRKYV&z6w#vFVrVh$^ZBsE$5GP2UD zvj3fA@_6cmN_f+xGtiJq`chg3bSA_8RQ?PnQT#gBi;L2i$)<|Z*Yp8qv&!*#xqk9^ zR{qY(B7ANAYv-Q4>OiSar*Hka3pY4wl;Oq-Lo6eDS@e)%Eej#F9XHt_A4{3(`|sWRGdSD8LtbV!x}0sLs~c3j0?0;=8zS#@PqD^wd6#lf zYf%aK`QbQY~sjBhlNCJ zhB@P;sBG0u7?xhg6Bd%0DCj1hs8Y$w54*^0U@y2Iqe$nsq&ujqLI;xvKFXB7Xg^nRG;Da;|6(u9Wz{dmg(3FAli$V+WUj zi$Zxg7UcrW$)EFv#5`;V4cwXMHSPzf*wWu`;GQW#Vqyfw`?RP@i%b+PaEDsh;XMM9 xX_e>Ud(@M5`m`ZqEv(ht3%mj2TX-Ghmos1xG4(uJ`p#&|-&l**i^vln{~rj#{Rsd7 delta 1523 zcmaJ>&2Jk;6rY*hwa4p^IEmvpPSiLJi7=u0Xe-*HkRqj0BU-hBKvAQXmE&2PZS7q% zyH1Ik>+YSn0ie`+o0hc78KI zU3xT|@zQA%!E^W9-|O+QgUl5E;$Xo!eHF1dqx*NNriJKUVG@BZ9ukZg+C=5_5i{Pw zB9=OXCy&mjF5^_K;Z*`@IBu&Qh#%6glntr-cB97gP@Rv1lEi6Az2))XF_sC-sx~UV zZ&{B;GQO7N8HiBgpA}AKg=94GN9N)l0goE*szT;L1&HAge}x{A9)?*6)AmsSo*uq} zqL~TPVLqdCcBA65hV7Rex9T+=w_durxn648zF(<>x90KEJn8d>vGV6maL9Sjo+a>EVp{a^$=8albO* zXT+V+nOx*i&G{0jHDxS|gVA>{p953s%&7)a>o_ewrXaN+Fj6#t*7WOx4T@^0wmd2{ z0@j;HGr3#%4K*;IQ0jtGQH^LjV>^{b+xDf_tb8i9%C^sERe;(qo)=HXHiolc%6O~2 z4J-e+O!>Ts<>yNmmDgygpMP@ga{#?9uIC@_DJn$2f~!%u0%ZCHXR3CiVOcNbmQ_eJ zHw7yR;-^4Sq~jE!^E8%?afMVtjvsY!lye=#H}Qa4J=_I4^~qj{4sjQF)z=PDmkf9o zlD^LL!-#2J%|Tr~@)*i0K+`8M|ckG5&b%I@q%ESD$_`EoidS4aQYmUqK zjQGB|Xhf$*{9Jr(&*a{=7g%l1YElolZA+bdUhvP=!c{YAMx89T(zN+4<;eWq#)|q% z-SwD#Vq&Q`D~`Jb;l4UKl$e;}9M<6mGO?+_HxXY?E{xn)1*v7JXv-?+`A6c%Ne@qp z>r+d$+W_)QD#CEv&DO3YZYyFOS84p3QVai9>TgN9+VohvVXp!GkgskDic&a>Q!qmc RkAS8)OXy8HXJY-&zW^MiThjml diff --git a/workspace/src/joint_control/joint_control/__pycache__/plugdata_cart_fix.cpython-310.pyc b/workspace/src/joint_control/joint_control/__pycache__/plugdata_cart_fix.cpython-310.pyc index 1942a3ccdb9c35170e5d851ba87d12cf32114043..ded605cd0ce92d17075d0aa9bf52070d91534df7 100644 GIT binary patch literal 10240 zcmcgyU2GiJb)Mhd*&i-Rk)lY+mPVE=YGryw%B~$NvTOZWacoPDB*zGCWiZ@3Bxkuh zv$``YatFJJ0lFZGB7lOTC<@mAiJ~b4v;|tAMbU@$p$~mZUy8QCpeRt}r2-1HKoGPr z(x~6LGrL?ev}5O?OU~W%hU} zJK`x9AA>^aW?7E4n?7^ON<92c-}S<~+^#v*(C6)Vd|$`AG>!>*(SIkjLkBJS9EzMD zR2xpX?6WGN9sHkivbQ){8_2FnhQUZG`}p>H;mF0)4DnqhA}t`nFGcHl76_sm;& zu9^)e2<#;%s8saz{c$nq!{)zBN4LkjP8ku6nb$R>fjzp@JTCyKhDsb!k_5F z{}k{iXhgg%X(jNYcSYwv1#MG3+E70#I6BUb5e=V6pDJnlDSGoj`)R`5YqL_<=osMd z%it<_(C2)vlS?^&+8aatgxG~yyvHQG>}1#QG_;Do&^%mwX8RP* zL=kU&7VSc(FeQB=u`|gSaW3tdPJ6(;wCC)nS;37dRvwsn?o&0Pp*sY6o;z@))ENSI z{<2e|GCbWGT9;9l*5wsl^o+9^!u-DUP`+Okuz#CCb&3f^mn1Rw?~_^R6;z?ONK{(S zU^UMVy!orFW}H=$cY7m(+e#Z!I_38HU2cOihQ|(1W0kYSSKJwJaQh6yovl54j85(J zj*##WS93SxlYx*|mJmZ$UktUaR6apuy?FlfV5+_sa=%`8`1$?wAIp$T(NNkSkQ5G< z&?P=mcD5q^+}BS?Qk;iawgQJgz{@_lBfd?{;9(*KQ`n1No1L9C^Cov})+PzcovPzL zaQYI`bUia%cFaXkgJJKU1me+1Rl4E_b?%N@nN@X^d-A59i!1T|qoxgJV{Ho7af~H+{&M&mwI!jrR z?%cjsv))=_c4%8wFyC5q*DB3+oWBIs6;}O5^R<_VG9uHIqOX6kqN`Yz>$#z2RW8+i z9I@avRC;)#+tjc&c^ zJ+SL8!#!%Y!ixEB-GL#Ja@<+7t6?4YZq+vd)qj3MhmxV!2y;az+tex!85Rn&R%naN;G`6#W}V=Js}q(b?u@VsckAE z6u&lj0dW(#$^8Xi+{K0uB87Bt1C!(SIs>=Ri;kr$<<9fT@ggaboWsf+(e%Cr>|x7v zd)fpWroHJ(H2Xz>9wZ2Nt3BTNUcFt3W(G-4WrpXhrZ!4DBB1vo}BA`d#h3CKWC;HYtGWKlT{{7fLjo;QFn=N+Duh+^;xS^@GrA*ue@G0vHP~bmD^0>7>M4 z&A3F37Hg(TKGvHYPEVru6MrloPiRXts|8WOU{%DbA2i}oxIve1->|M-zI*w-o9|w} z`c|w95d{W^;8CoLQ;k-$?uN0k)?16wUzHK{`tsi<;dPyJpJM?e3S#X()*fqBzuD%8 zh}Ft+6c^Kt2wmLUc(~PsUc|8_0c$emc1znkeQUA3!q^=_t{ zh4UlT6sDza)~S#TV$tp>>q-}nFIsv!dLbldS+x73dVlU^F;}!ZD&`UK}EZb4w zBpDqQWv-*4gj-LuMX#7EM#N0i3ZD*%aDf8M8oBJ9>{12}Lyo})Ef;S_hf@=qrFtT{ zoqKj;kI_1gEfGd*rYk4LYGSA+$Mp-XuQsL%({fyjZv2vCageAu$jlT52&YgOKZ+uH z2onFG^iHCQ4L=8*e~u-I4Ka2e)8ayc)WYp+Hc>~~Q{uSO+OmF|)&oQ1Vl zKaY#2Q(+oJ_S?ZP;rlT#vs(@4rZW`6Utu{?Za8ineNVkDo<%ImfTis7Ag7 zA0g-=YRge9^H^@iYTI6o<@C-A!&TUH5}g>uE@H)C5b>9LTMk<}) zUbgGY+@q$|@9VoZzfE04)6KFGYeaOcMsAaT7hSQ@TTPU@0L%yjZ;R8Q^$`CooVPQYN4qj>^%CzY0J0teq|g-3@Wd{v9Pw5U?$UkLz$x99yEo5rL<2#0#5ocvz6I0wu)Qomb^vLPS9p)VLmL>irYhBVY{@M z*)%qbOX?2MI6%jwgdSxaN^48oM38w!;!UP+X&cgpyrFEU8`_4xk=e*@7#q2b{6=A; zxHu$&&_+1CsU_;JtM%#+<$E`}dPip}R=h)Rs*R9<7ct8$+M`gF2wi_)_lEY25?}gb zA|jiI+RBS~;9nEu_8~-R5!Kp}g*qK$G)_wBA38?OV0q#RQy0N2K#S284e<)%?^qJy z1nlDjVS#l85kpqELUYG^m=SrC2jKH5ZZV$VPT?`1C;|M`f$)3rBO2X@pBsSJQ~Y9l z0RQ10^}zq;f$#(PyE*LjL?^c+1NuX#=Oa+hldMR}HixL4w}vyBPxc28i&$^glRKGi zA)HG1+aZn}rMURIipc2bhibRj9qN|Cr#kuYX~YwUCMh0XIwrw{$RpwxgvXPa>vA`P z2rG6Tn168pH^URS5yQKqongzwxc-U+3i2KGW%1orL$rW#xONh8O4O6~q{N2kREqvn zkmaYhpY06OI)M2MDakb4`N@C{G&G?GhF<;LZv^Z`!7`b9_c_jsDOuLd+ZY=P zd{f$#H-UTK8*-cZ&D{Q8!Zn_1o3boSl(HHtaCX!?*I$U$+wXoy#Hk()!B=v_>T;YB zUvsfW@m6xZ>h(GDf1GAwGsMc;Tw*sU?KwV=)yEp_hB^K|Rh4LNpPG6^Od^i`P>Qbn z62ZLB`8h~M!F_@fE^D!Je>T>HGgwS?Wnx6dnZ(Z%OMN3cbIWrmZqo2MMOwU-0M!Q+ z?rnvpzi2M_Esq5!MZnnk(T^@i&%ODUS@m0WW_k$eEV>96+GgO_TfzjONXFTWb?*TxdTT>X9Szsgjeb zc+EHi67BjD%m4(GW8LQ5Zu3iGn!94X{Z8Wd>5Oz2zC*KuYUL3CK5I6!uz$xVXIrcsFUh*0c&kva?F zgjon!||smF{KP*xdt>nOxH1ka!+_UX zlq)cUuLx84=kjqtmo&f`nmjI>fGi3xHiL0P;3&fDEy-jglb1V=@uD16zS@;2=P1fU zMxtfKdCO|}tW~Fa(X#Hh?0PzqwJhdW;l7j8&dGr1&rm_Di-%J0zUtKLJr7@)c5f4i z-~yRbMLYo&hNQ_w#aJ?qj$}EFRPppb34|nFMNjY-eJ{BSvwR!AE)+QYf|ic*WrK(h2|$G{j= zUzgH~q6ex3DPZ`Q*-O&>(@62X92OXoen=G-Cnf zgknPfip*ES61CU}44W0C9!+2vr8(%4c#CQHO+O_voI636}I z+D@FO6qM_=5K*MKp&KAm=T?PF zg2bANHNwLX&X+C}$$*<9?gIH*G0{g#>~$%cY=BJ%sV4r%0Empr`6StuI6b;hOE)>g z*xl%N?}8Z=#xzNDnan|s363Ok9IMMN@)=d%=L{+UIZPx29kWHh&mg5kOnmdLig}%~ z9!?#3K;$Q&zo>r~U)4wAB53+hX~cQktP&;{>5g5*<_DA_oH3&r^EPU?QN11At3>u* z(mt4*F_Zt<8I%6+A^mB4B71^Vhad;xi+UYDA3?STJPuq4dis5d7u3_VCX^=DaP2Usx@6(bb zVG37!8;_iJvpK{NabRj7m;|Y#n~e68FvuVCV~?ihs>KKL`@eV z&L?IWNe)gSzN=PAmMD+p^ z*P(0IA=H++OFhG4kKAey`hI=EU*ms{UU2}#a{2F5bC=}v delta 1837 zcmZ8hO>7fK6yDiguQ!hE1QQ24PHZQ{Bo;J@Qz$=$lr}_AQ4CcIQW12!csFK~^{z9! z2JEgyE7Knpfr>^|FGx*##Y*yso4+7L*x8&c=43?8b+&b11rGySh(9mQ!!|{^bWue+Y*Tdw0xk)M-E@?AJYnu+wx6qGL_JPv<%L@G|r zP-J1|?5b(7Xyl-6c|Me{&mACNCl(Ksiz1bo4>NQ{5f@pQtmCTpOfDXUoqZr4yHrXf zrGb-Z7kO`Br_#Lup#vu-yaCv(!YiCt+UO&=W?HxkINTQpR}6&n>TPl3ufY_Gp1;`+1tCLP5%K3Fo=RQ4Av;lERcN_C zkb_WK2ozXtF+@YXA-BUp*yu?MM$d})3Bn0OYO9ZvzlEr-4-oBG~jE#ff4A3 z?4v#u$)7{NqZ0WcGYNFOLd7d3ikehxmZ;R!FH;#5cv@6!m7dU)^Wiip441DKB`U7Y zP`RdiD>Taauh~}3@;%I^L={9Z@85s9HS+55e8p|pM&5D#{F3Duc|GsBc7t`wXmNt` z8>b;0Gh|)z3^R*0It?wYSdNJqnrT~#aj~Mkc7o3Ye2`_b zIYE_6y4^56D%JEWRMIba_yuxzWFF;+lHEDj&qBiW#s%oHi^Vu^FFZ$%W)G3u*~xAT zEU;Rftdn5PgLtRNgKS|-6wRSHl2HQX5ik>}WHh%`Qbm=#n5&**?Bz{~8OJfprLo83 zsbbo;rajfcyI2T6Lj@}e;xVNPGKz|7Q4RGg8;^27iBqh{iik>8(}%X2g%u6)&;C@g z|9jRae~%Rk??X^aotmER#_=u<@kc6C^^$4370YjKOm9!4=@|&+geCEE4f|lcww{}z#U$9J7)0juoifQ~Q`DKTT z#>o7{bdDjXA*WHRH}P3!_j+~Rw{*Lv`xo&mnI{`_DFexHE=P8jY=$D=6qd>Jx#W#mQ`lNK zN|-xXni*Xfni*@^YFJa)vzdyrYZ$V)YT2Q@Vu*@7mJ;R^=E)zqWW703ID1)YIcqpm zm?arfn1MRx)-WyLE8z!TFA)AfZ)}#7gm%oq;NF@MGGqmB~!R-xl(v)xq)K5 zlOws6`T2nEU;x@F1ad_R|Kx7&UPgh*l04SVf*?KN3@O6F3@i*K$`cqX-542a`4}0h z_<*{FYWQX|%w>Y8i3F;u<*VZZsfDo08Os?oMJ7+@@ne0-0t}GNPkBBva-@U8hJoS1 z~!hH~aBtGcvkN-X);O76z0o3ZMK?K$6jQ^KXGrHntd$i1X$!ac4%6 z6d?N+n`=;hQhv!Tme3#vq!v`L5KXl43nEecfFrtsF(zj=D@M7qiIAX( z!WI@HY9*GE($>Pi!BVVj#L~`PBe=!z=J9(o!`6qthYZ&+FcL5OtZq4vMn49H58{NV zOL#mKC?GmZ0{jN|4yq)09A<%rT7xvIfvCfxy`|IQDUs|=AyO}OQ1*(TV>MDcau>3msFSW$5Y z5%4~sKd~R<4-yJHPBE7+IZm>hUh>(loB4BT zTU18#8x`idJL_(h{!Y{@yE&H6?G$rti;hVySlTP@9?&(ZRhBPq(o-R6SQa^-9`Vuc FnIF9=fM);z diff --git a/workspace/src/joint_control/joint_control/__pycache__/trajectory_server_new_cart.cpython-310.pyc b/workspace/src/joint_control/joint_control/__pycache__/trajectory_server_new_cart.cpython-310.pyc index 976cd890a6766f577c9075204f0f55b06cbcfca7..02821aa9bf82300a8d848146dd266c5b33a6c8ea 100644 GIT binary patch delta 3027 zcmcguU5r~t6~1%-eEqxrU++(nn>5?RwAoD?*rFx0O_MZ5O_nM@4M7U#+H=>wj(x9p z?re7LUdIh?B|xM?y*%_+vL%F|LIsp3c;FF)L?tA6;R*re0THP@z#|fYbFOzC5b%nv zIdkUBnQzX_o$t(i^!V>qQ%*7&6X5rY`Y2Y!(HHHmA`l%Tj@ApT#;+JHx>;t5Ul zZQ)&UNQeM!73-+bD{_TuL*fY)4Lmd|-4NL)X%ySG%As!V_Ffh2q%81Q_dF5s4;V4T zxF&2K-QGirhe&Z46%UcF(*z(5;=540-BYN||A4x%EqKBx!IKy+O^ziPo&w7ZPgAIy z85$kSV9R`Px zGjS|p4;3y6=oCt>pFKjR!z5!wz;PN?d4dk&6Oe|p zt#24EM>Kugu=x~#J32FsS_2FQ*Ke9Fr)KePi0hV@_1HzTZ6ee`{WFtUjatoYnatpp zW9!^%o4Vz~=IL(3VG~&Pyi;|!-nQxuuG^qhP2H%rAYezIjy5Opn6|T3Z$R`SM1K#D zb}zZobS#@!TyAjFy=zJ*2wC*sl&>98It)4jCESHT*=RWi4`MGGtu6B`V-5=xUV#bx zzsRrqUscYf4*?u2pva@h`5!9P3t3QSptw~K38IQBQAv!%piUEwi13FNXbDj!38-Zv z-WR3+W|h^uUsN9^>N27)`(KM3DTe9_w1e2mmgVxA)9yUE?*A-e8^=Shsdx;uG6`~% z#qMo$IQb z*-;KHUUE*LDvi7<&Ix0}v&~#HKea=yLOa!H^p2Pp`hYfQD$RnI@^GRejnViWxj*C0 z&;+<8Ac4KlXCdS?FM$o9At|LRKzV@Fa4Kv!7b=SnEDy9HDG4-{-}Bk$NzK$~zn30k z+T6n%kbYuwoeOjZGsosSB&Y*pVWdMjIEEEB4^B5JSiL~&@`+apoV0cg9S)b zF#0O2{E`5f_2KTHiO|mwp_c;Wte4sIyb3v%s{PEZ40@ub-O6l}ZK0g=e;mJk9FGPZ z8yOGRcSQC$Gy`HyPRzd_o2$^pR(&UNUaP;9fB)Qvy;uC-CrWec_wcp|q@8XLfE0&O zlU*)L?6jXu&gNJGhhZ+MLlMjmJ~-uHNggQlaxK$FLc`Ho?jS+kHQm3JJT^GOO5hlX zb|+8_#*8jQd^RN?5cP&-o9t8QMd0kRP8S5P25Q^o%xJPj^q<3Sb%Goy%&_ZbAi0>qh1Lg$bu+*i3;v?H^862m28r(%GuQ-ZX;f*`1o%L1vV* zEM$dju(}&WZ(7D=7WQNq(M~|R0qF&V1*8*5U1OV_#ts={1AZ~{X2o?{?3vK}av(t? zP(p}+K#m1-D4v@T>?pJ^L2*xl5JipTA-`y(NJ@}jN}#JGOID!`xrd}cKMYz5GEN@q zJV}WuzcJG~hx81|$`CL}Rw}iY;kuA}Tb0U#N!g?57cv{)0ZAw>vRV_u+AD%5j-oW`eel52l}d~j*>JEz7fz; zGy$6CX*9V-Sk=>@F9nvU7oAx6j28vADPH8Huz8xNN0~9))bQ+h0-hZqd7w^**KY=A z3=h8C%3n!`JYS=f1p* z=^6+p)lCj*IV7$UCjWo=n&F4UoPXQ@V{U2RZP0q!^2+2!oFsguWwkB7&Q9JvnqMF* zPk~DaP$uj;>ZkS$6WA%@J7{0^S>ajnp6dUo@X|B&x8B6x51)dl;&_ugTX5@|HHHVM zyq^`J3Pi^Z1E5dctC8R&{mth2t8~8g#X*(&gZA;g5}VTD-U9myAj`H>`Q1!O|Qvt z6T%b_$hHF+Y)ZqB41wG>EZc=K?vDTc*@CP^W5oaI>?-il|NHDC*)hUtl?u95D&=|h zb$_kokh1??Y32G?uycmtf~=a$9j8@wc38O1xC?t>8Lc+#2>TrRDe%;qllWkG=NVUT3qLX1BzIWH(zife@uc$u~! zXPi5ek9DqD!L}5NRw~?90;&l636M}E$P2vi7mz=I;UV;?SOW3!gm|EwGfq~amhPEz zf9H44y>sV$_|fsdu314oZxZBtbIqcd%+ z2;yV_F{3Qg0n4%USOqLEC`gL|&nZOq4xuC}GGKNJ%-m66R$|#`2Io8UL&9?RlpP9a zndJeiM6;bLvv#LEU{J!yI%(P$;{)=tGOkbXkc0e6C-D5p2_rXZg%b+cgI{USQCgFK z)~`r6{X<>rb707SreCxhVCFL@N+^o*E5@~X8`J`fa1BI`>M5P-)TAanRpr6Ijc2KT z2-%0^lKGk0L~X)6HcxfEFxm)u{nO7&*YxerCagViSUxfv$M^Vr1uQ4fx(ecA(}-2( zZA7s$@cR4+K(WyeLND?HpEu-c_RGu52r9j>&Cy9S#gD>+A47o$;B^pd^2gcrN)a@? zjteltS5t(%M+XJ@-|YM{VQO-z$_^PS5n&q6;CfY|I4aC_oXbPGBiou#g(h^77KYH; z`Y;35bh6g$WJOA3n88$*Y11f&>vw3GFmqQMnh_CZC!a70%i^<5z`F(Ob2t;3VKyqj zs)V&e&yuU(8Rjq`hI!yjR~ENNB_xPkoiK~(Z>Q>HOrv6_G@9ANb!JWRG8tNM2(42V zmdIkXumZDp)nNe~7L!d2Oa)KsW{Up}uu^ioSy5m!b+SW6-l+mwPS6_oH^eqh$)Y0pPxZYO}01symiY7gDyXXpq8wP=_|1U zSWH`={t(Xq3pWp62cf16TBZinF5@M57<87_s146WC}s&vRa!zbT?3mzb1*CzB^XPO z=Q%8VprfN4wNkdcQ0yMXYfh>S#~tIm<7{-@Fx+T$yN>hJp`JxvQ+`ye(S!0o#RF8A z`${M0^-)?tG7qVsBV$a4DWln=G-hEEz;oI z%|UoP@iiy^G;@&V!?kd1PKHsp!&U*uBddq=TrG-<4~TM=}JBwMwl9X|C<_A>Ui zj4R9Y--RUuaasxZ2I^mY0!-cRdHfOLtMYpF ze2Sl!_p5j3`Ii8K6J;Z>*9vYzTW)P|Y)sAlyewa;Ez$+KSv#=wDnPM*?ox0qh+_2{ zm(HH&ZzH5$_k0Fv+6~-D-mlHtuOVuF)9v1Do#Q;<@?q`HYkLXJG4(mVQfyBD2Ka8T z#os}|y4C6i8(y^KByX2wYOMMJ`*aq?YR~oj4y*|y`LlW@WoG1W>&N9k>W51|M^A?1 zAj5H*bNmB&WG;Z@&0 ii?4&uzmEb7AV>dPpuqcnN;GA&v68-OoHnWY$^QUoR7L#& diff --git a/workspace/src/joint_control/joint_control/plugdata.py b/workspace/src/joint_control/joint_control/plugdata.py index 7560a51..accb35d 100644 --- a/workspace/src/joint_control/joint_control/plugdata.py +++ b/workspace/src/joint_control/joint_control/plugdata.py @@ -7,6 +7,7 @@ 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.""" @@ -44,15 +45,87 @@ class ScaledJointTrajectoryPublisher(Node): # 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): - """Handles incoming OSC messages for joint positions.""" - print(f"Received joint angles: {args}") - self.desired_joint_positions = [float(i) for i in list(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): @@ -89,8 +162,16 @@ class ScaledJointTrajectoryPublisher(Node): 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) + 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'] diff --git a/workspace/src/joint_control/joint_control/plugdata_cart_fix.py b/workspace/src/joint_control/joint_control/plugdata_cart_fix.py index a86d62f..a045cce 100644 --- a/workspace/src/joint_control/joint_control/plugdata_cart_fix.py +++ b/workspace/src/joint_control/joint_control/plugdata_cart_fix.py @@ -47,22 +47,106 @@ class ScaledJointTrajectoryPublisher(Node): 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): - """Handles incoming OSC messages for joint positions.""" - self.desired_joint_positions = [float(i) for i in list(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: @@ -84,8 +168,24 @@ class ScaledJointTrajectoryPublisher(Node): 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 j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True) + 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()): @@ -102,8 +202,10 @@ class ScaledJointTrajectoryPublisher(Node): point.time_from_start.nanosec = int((duration - int(duration)) * 1e9) msg.points.append(point) else: - print('IK could not find a solution!') + 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 diff --git a/workspace/src/joint_control/joint_control/trajectory_server_new.py b/workspace/src/joint_control/joint_control/trajectory_server_new.py index 162bcd9..bcdeb07 100644 --- a/workspace/src/joint_control/joint_control/trajectory_server_new.py +++ b/workspace/src/joint_control/joint_control/trajectory_server_new.py @@ -63,7 +63,7 @@ class ScaledJointTrajectoryPublisher(Node): viapoints = [] msg = JointTrajectory() msg.joint_names = self.joint_names - steps_per_m = 1 + 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] @@ -79,7 +79,7 @@ class ScaledJointTrajectoryPublisher(Node): prev_sol = list(sol[0]) else: print('IK could not find a solution!') dt = 0.01 - tacc = 0.1 + 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)) @@ -91,7 +91,7 @@ class ScaledJointTrajectoryPublisher(Node): 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.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() diff --git a/workspace/src/joint_control/joint_control/trajectory_server_new_cart.py b/workspace/src/joint_control/joint_control/trajectory_server_new_cart.py index f4a8263..9c8a5fc 100644 --- a/workspace/src/joint_control/joint_control/trajectory_server_new_cart.py +++ b/workspace/src/joint_control/joint_control/trajectory_server_new_cart.py @@ -22,6 +22,31 @@ class ScaledJointTrajectoryPublisher(Node): 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( @@ -60,44 +85,28 @@ class ScaledJointTrajectoryPublisher(Node): try: print("Received joint positions") viapoints = np.array([list(i) for i in args]) - print(1) msg = JointTrajectory() - print(2) msg.joint_names = self.joint_names - print(3) x,y,z = self.robot.fkine(self.current_joint_positions).t - r,p,y = self.robot.fkine(self.current_joint_positions).rpy() - q0 = [x, y, z, r, p, y] - print(4) - traj = rtb.mstraj(viapoints, q0 = q0 ,dt=0.01, tacc=1, qdmax=[0.1]*len(self.joint_names)) - print(traj.q) - print(5) + 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 = [] - print(6) - prev_sol = q0 + prev_sol = self.current_joint_positions for i in range(len(traj.q)): - point = JointTrajectoryPoint() - print(8) - T = sm.SE3(traj.q[i][:3]) * sm.SE3.RPY(traj.q[i][3:]) + 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) - print(9) if sol[1] == 1: - print(10) + point = JointTrajectoryPoint() point.positions = list(sol[0]) - print(11) point.time_from_start.sec = int(traj.t[i]) - print(12) point.time_from_start.nanosec = int((traj.t[i] - int(traj.t[i])) * 1e9) - print(13) msg.points.append(point) - print(14) prev_sol = list(sol[0]) - print(15) else: print('IK could not find a solution!') - print(16) msg.header.stamp = self.get_clock().now().to_msg() - print(17) self.publisher.publish(msg) + print(f'lenght msg.points: {len(msg.points)}') print('published') except Exception as e: @@ -114,6 +123,7 @@ def main(): 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'] @@ -151,6 +161,7 @@ def main(): 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