From f8f8c621e9ffc2b1e84eaa9cfb11026f4f06001d Mon Sep 17 00:00:00 2001 From: m-guberina <gubi.guberina@gmail.com> Date: Fri, 3 Nov 2023 16:31:14 +0100 Subject: [PATCH] fixed the python clik. theres now actually more things to put in the cpp clik (the signal handling for safe stopping, better goal definitions and a few more goodies). there also exists freedrive access from the computer in the utils folder --- .gitignore | 2 + clik/clik.py | 61 +++++++++++++++--- dmp/motion_planning/dmp/.dmp.py.swp | Bin 16384 -> 0 bytes dmp/my_sol/.test_traj_w_speedj.py.swp | Bin 12288 -> 0 bytes .../__pycache__/create_dmp.cpython-310.pyc | Bin 4349 -> 4362 bytes dmp/my_sol/create_dmp.py | 2 +- dmp/my_sol/run_dmp.py | 22 ++++--- dmp/my_sol/test_traj_w_speedj.py | 21 ++++-- util/freedrive.py | 3 + 9 files changed, 86 insertions(+), 25 deletions(-) delete mode 100644 dmp/motion_planning/dmp/.dmp.py.swp delete mode 100644 dmp/my_sol/.test_traj_w_speedj.py.swp diff --git a/.gitignore b/.gitignore index f5743f4..0f103fc 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ clik/build/ build/ +**/.build +**/build diff --git a/clik/clik.py b/clik/clik.py index af7ab74..55a899d 100644 --- a/clik/clik.py +++ b/clik/clik.py @@ -10,6 +10,8 @@ from rtde_control import RTDEControlInterface as RTDEControl from rtde_receive import RTDEReceiveInterface as RTDEReceive from robotiq_gripper import RobotiqGripper import os +import copy +import signal #SIMULATION = True SIMULATION = False @@ -55,6 +57,7 @@ if not SIMULATION: rtde_receive = RTDEReceive("192.168.1.102") #NOTE: socket_timeout is the third argument, check what it does gripper.connect("192.168.1.102", 63352) + # this is a blocking call gripper.activate() else: rtde_control = RTDEControl("127.0.0.1") @@ -63,18 +66,45 @@ else: #gripper.connect("127.0.0.1", 63352) # define goal -Mgoal = pin.SE3(np.eye(3), np.array([0.5, 0.3, 0.5])) -eps = 1e-4 -IT_MAX = 10000000 -DT = 1e-3 + +JOINT_ID = 6 +q = rtde_receive.getActualQ() +q.append(0.0) +q.append(0.0) +pin.forwardKinematics(model, data, np.array(q)) +Mtool = data.oMi[JOINT_ID] +print("pos", Mtool) +#SEerror = pin.SE3(np.zeros((3,3)), np.array([0.0, 0.0, 0.1]) +Mgoal = copy.deepcopy(Mtool) +Mgoal.translation = Mgoal.translation + np.array([-0.2, 0.2, 0.1]) +print("goal", Mgoal) +eps = 1e-3 +IT_MAX = 100000 +update_rate = 500 +dt = 1/update_rate damp = 1e-6 +acceleration = 1.0 + +# if you just stop it normally, it will continue running +# the last speedj lmao +def handler(signum, frame): + print('sending 100 speedjs full of zeros') + for i in range(100): + rtde_control.endFreedriveMode() + vel_cmd = np.zeros(6) + rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500) + exit() + +signal.signal(signal.SIGINT, handler) success = False -JOINT_ID = 6 for i in range(IT_MAX): + start = time.time() q = rtde_receive.getActualQ() if not SIMULATION: gripper_pos = gripper.get_current_position() + # all 3 are between 0 and 255 + #gripper.move(i % 255, 100, 100) # just padding to fill q, which is only needed for forward kinematics #q.append(gripper_pos) #q.append(gripper_pos) @@ -96,15 +126,26 @@ for i in range(IT_MAX): # this does the whole thing unlike the C++ version lel J = pin.computeJointJacobian(model, data, q, JOINT_ID) J = J + np.eye(J.shape[0], J.shape[1]) * 10**-4 - v = np.matmul(np.linalg.pinv(J), pin.log(SEerror.inverse() * Mgoal).vector) - v = np.array(v[:6]) + # idk what i was thinking here lol + #v = np.matmul(np.linalg.pinv(J), pin.log(SEerror.inverse() * Mgoal).vector) + v = J.T @ err_vector + v_cmd = v[:6] + v_cmd = v_cmd * 5 + v_cmd = np.clip(v_cmd, -2, 2) if not SIMULATION: - rtde_control.speedJ(v, 0.1, 1.0 / 500) + rtde_control.speedJ(v_cmd, acceleration, dt) else: - q = pin.integrate(model, q, v * DT) - if not i % 5000: + q = pin.integrate(model, q, v * dt) + if not i % 1000: print(" error = ", err_vector.transpose()) print("pos", data.oMi[JOINT_ID]) + end = time.time() + diff = end - start + if dt < diff: + print("missed deadline by", diff - dt) + continue + else: + time.sleep(dt - diff) if success: print("Convergence achieved!") diff --git a/dmp/motion_planning/dmp/.dmp.py.swp b/dmp/motion_planning/dmp/.dmp.py.swp deleted file mode 100644 index a3910b56929b4cb2d4e3a6d1fde51c388d51c8a8..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 16384 zcmYc?2=nw+u+TGNU|?VnU|=}R>X<6~ri$TICIdryesMv5N@5X63Lnl*EY8l%!>0lw zR|hjtKO;Xk)kq)C%_+@G(Jv@UtjH|ZFD;5M&de>yNsUj=&nqd)&(TlGEzr-+FUic$ zi!aDY%*)HnO9zXk<QC`^R6>j%B}YSGGz18S055~Fk)Z)dnX;0ig0N61h&hT!Ltr!n zMnhmU1V%$(Gz3ONU^E0qLtr!nMo0*h6fiN=GcYhPLH!HsPcuUKFq#?4*M-tmP#Pu= z<&ILLAut*OqaiRF0;3@?8UmvsFd71*Aut*OqaiRF0;3@?8Ulkb1QJsi7^E2(7^Fc1 z00ImQu>Sumeg=k@{0t1&_!$_k@-r}8;b&kt$<M$riJyU?mY;zki=TlZoS%WgfuDgv zm7jq@g`a_enV*5-1s?;$aXtoyb$kpAYxx)$difX_s`wZflKB`Iy!aRxO!ycWWcU~u zr1=;a-tjUp+~8$k*u%@fu#lI5VF51#!vtOihALhLh6G*)1`l2a1_NFO1_@pU260{n z1_oXRhUYvC49|EN7%uWKFihZKU?}EcV2I&iU~uGNU{K;=U|{B9U|`~5V0g*Rz;KzH zfng6f1H(dY28IRP3=Hku3=D<b3=C1+3=C%63=F2+3=A^d3=C}C3=HqN7#J>bF)&=@ zVqn<D#lWzbi-BPg7Xw2Z7Xw2+7Xw2c7Xw287XyPD7X!n2P6mdZoD2*ZoD2-<oD2+p zoD2*WoD2+#oD2-lI2ah(IT#pRI2ag=IT#p>I2aftIT#q2IT#q;urn|$VP{}i%+A2j z$j-nJ!_L4E&CbAJ%+A2@lZ}Dl3L68%dNu}zE;a^+95x09Q8osK-K-1@k*o|10;~)S z{HzQNFIX5D_OUQ9B(N|rsIf3GsIo9HTw`Wn=wfDIuwiCkkYZ+FkYr|Hc+SMYaE*z9 z;VKgY!(Ju^hGk3)48=?g4E9V640cSA_?802Iqo#600qhlt`#LkiOD4jB}Iu@smUez zMU@K0C5a`e3=B{u3dN~8X?i6YsU?XDw#Xc91w#dWs6b+=f|f!`396n7nEDC@ZHT%w z4NaU{lodh?QW8s2q1J<JPf1NvC@x7Y&;U6>M<Jy|(+c6NqSTVoqCABHxSb#!AY7oJ zZL3gFmYAbglv<pTSda?Vu2+y>tOw?5BFq3OhhdNo1zUvz1uX>wJq$xL(-iUw^l~!u z5_8h^^74yvHS!g76)H3p?9jzQZq-Xy(1rN8Air4C3d1yIg_4X^g_O+VlEl2^RE3g! zFux?RC_S}AM<Fv!A+tmwvsgzVGd(ZAC>1Q3pHy6un4FV}#jNCv)Z}cCA;qbwAkEm! zhG|#GELKP>Eh@=KEmA1SNX)|w3urjx73k$9=Eaxf$LE#iYUCB@rR0}r1nDT0X@X+9 zAR|)~!zDoqwxGaBEGkN@)QFBY(9tc5Hj33LiZ+am)lrBBa}0Efq7A@YU644Kr(+Ns ztEr$3(wkpel3Juuq*H`xdXa*yLXm<#NELE4;82-YpqE^fUtA1{CXll+eVS5`kqJwp z>GAnV#R^&qAR#S~9jO%s8oJ=POoB=!CKYRd{Eg`}kS3_tlaup{H9-17!39ntAiEXx z6+l6V>~63eC@j&ERhg!y0;Ubh3I#=(c_kW3E}6w8Rtid>3{_Hu-AqtWDkYXE6yz6Y zf>u@`lvik?nv9vK!5%J4%>kur!&F@hg~YrRkZ-WK+y;`s6G5San!=SqiLg9Xy(m>7 zH@_@1FC8VNltEHSabam{Y85nZf^0-l1S&f63iMDEfwB^cqI^`7U`ZU+kID+ki8;xo zIiQlI3|zp0a!P&@sIXHgO3g_u$t(lsz@+??O1SZ9`9%sa^As}kAYO$D6eE>F1rRq> zr55EEW0W(Xq6Jij!8FDvmgQ%rfU-PTDVEYKuRyO1l+Uv@z$)}IG%7T8AhuV5nUExz z7!OLT;K(Z0sL(`<R#4!9<tpI*$xKtI0A+o@{Jd01WP#k32DU^;p~4m<j>S0{a04=6 z86{gmO98A6QN~q*oB%8EQWUfh?p4rMD1#dTig<|5a&Tc%P@Jg=st)oB^l}p`GIJrt zOmS(h21rIn0hIng83dvSUZPeSfCRz*1Nj|pd_gfhGZz$RLNtLCXu_)jh*(~HQd+T& zf+4wqmH{>fTcClW8(J&n73hHr5Ks-NVFU}3^30TyjA8{XEd?WxA2f<#ftH+_SCU#( ztO=2VdkIt#pvD@=3J@m3n&`|}h;iTuL$fJ5GZvR+dHE#@pyU9~`Z=k28pWClwzdj} zh-3o`UL6Hc6@`&@z)@3@SgK&FP?A^*_6#Vo6{nWOmn4=#L$4&U6gj<t^`tA<DnQB# znELem#2l!GbY$gF?^i^_gElV~Y!N7IK$z$(0Lir2ViRr$*ll@M6kAmZ^$E`0RuOGr z1+k1w&s5>G0c>nya<YQ0f&nZwRvN&A3@lxinuAZeAio$^2B7PNH4f1Xgrotm*(Hgk zXp*29LMrX@3iKdlDA)^ndEg8RE%_=9bQBErp!pS)G>TG-Q%k@KkrM^jqJqQ{P*DX+ zz;NFwE2N|rXBMTVD1c-U?EtU_NT8so2a7{O15+Fl44C4uFu;@n`4mM|8QR2!I}mIk zv~aKmXFgOZXz`$cO|BeWR9PW2FS8^wF(<PsRUs*{7~J5?O9r(BiWNZhnooETS{Ohg z3dN0JCxJ2~rYI;b(JciluQb3aoCiu(a1SXX_cRI;ixP8FK}8>$i@>c4kUl*lxK6Mg zNucJ2f~|s)fgYqSSdfXT0Md}NRWLL#Kx)P-gMALGhZW%UFq*aCx&c(9gX#!Wb3s+E ztwLgaF{*Hif~`Uds!+CqtwJ_jNLc~Nlc1srlz`*oL2(ox4^G263fZ=X1_n9`DYixi zIv^`-4RsX2P63H)S}`Q&Bo-GdxcCNGF)%P><`(1^l_=zu<`z^cBo-^=6)-R;E9#dP z7wIQu=IN*Al_?ZdmSp7TF@XC2r`Z`8W^+K+|HJ0@pYSs<oZx3*Sjf-7P|VN3P{hx` z;1BKRf8k?bxXj1Eu!E0*p@oluA%Ks8L647tL6?t#fuE0o;TSIiLn|)>LkP6rF2>8i z@QsIoVF3>VLnsde!yj%2hV|SG4C}ZV7^ZMDFvM~*Fz9kKFz|9SFx=u|U^vLdz|hUb zz~Iis!0>^Sf#C!v1H)WS28J|F1_nb;1_oYE28ItD3=GFP7#NOmFfeT3fVh7a$Pc*t zv!gPjAut*OqaiRF0;3@?8Ulkq1V9ZT5C*sDAkBHiu#K`p5U7EV-bx0k2Vt<D^5{$* zg{)XmTdg!#L0=&yH4on80x5!F&_=_|yi&|z)66sl(12b_YM!Qo4QQ;v4AQ}cX#~+J zsi3xNT6}t9W*&IR0yHuapQWIs01?yF3qiOVqydEUN^=!#p$gD7K}LA7YAYztjL(8~ zu)sa+_~O(;9fd4Z8$b>CEYOHlQDR;?bo30|RKzmM3~~(!g99ubG#;R<P#K?+nwt*_ zJS*^+D_9ObW&>{8YbjKwAgMwL*0gwt7hn#BXwXssb&e}jkhQ@E9Khp6Xl8)I6J!F& zhv3l(&<q3AfnWz!8h{2UP;`Mjf;BRs5ee3ZX$B;2O%ZVm3cyVC07MErusNVk6=shV zECmT{M5GjhJpms5O@$8`gE|X{sS!|*y96Af2yMy=&iT0or6s8f<*AwJ870Mto(L=) zL20`{FF8LiIk6-)FR>(5LjxwI0~(XX>@RETD1i3a+8QG4f$31NRnUM%tCfy|p;fGc zE<B#CbQG*~4P!xraT*YZMuWQCU^NInSP48%poU>}B-pQ{xDl)Y>_jjt8sS1XA8b2l z939~xh$j$yuoAcfl@*-f1HUP$MVV#bkzS;521h5n*A1F5Kn`k%gpNWqgb}T47#j;6 zf&e8&h$zJOpmGE>6rYlr1{qP+gLc@F2WG*xLB%23@Fy3jQbbIa#6yK3x^pt~iVG5x zQ#A~rqa>iL2xWrPsE$HvUP?iJW?qSHNKt7j!XRaZ;M5ZIkpV>V13MIC7-;+s68r`_ z3c7}fF)6SL(AWU9FtGwlqYtx!M{wY=0oDT^AV6p`h$TV~Xvjvv7Fuqh8V4$zA*Mkl z_M#0CZiJ)?xEJ6S8R{s2#xx<)Nbv~?6lmdwG#84c^PZLo8GF)!I!Z?YY8ukC0NA^b zp(Su1Uk@~#q^nStniF48lv)PLY3Q@1AQjO0)hd)}QdCbv#-G6ES85<46=pK1SOt}) IWvMv~0I0oJHvj+t diff --git a/dmp/my_sol/.test_traj_w_speedj.py.swp b/dmp/my_sol/.test_traj_w_speedj.py.swp deleted file mode 100644 index b0a2e39c26f4e81689ada873f61d6c521e61172c..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 12288 zcmYc?2=nw+u+TGNU|?VnU|`sC(<L>`wua$UCIdryesMv5N@5X63Lnl*EY8l%!>0lw zR|hjtKO;Xk)kq)C%_+@G(Jv@UtjH|ZFD;5M&de>yNsUj=&nqd)&(TlGEzr-cj4#g5 z(Jx6YE{QKGO3aEck1sArO-;$tE2xARK1z;;z-S2I4uR5=G+hf`24f>b1CSzRB}D~c zp->QW6px0$Xb6mkz-S1JhQMeDjE2By2#kinXb6mu5GW~NWT<CgU|@p!R|`rrqR~+9 zC^Z@aqaiRF0;3@?8UmvsFd71*Aut*OqaiRF0;3@?8UmvsFa$#&F@=G_mWzRbksC7q z59|Lw<!4}6!Oy^u&CkH#z|X*-!Oy@T%Fn>S&CkH_pO1mzBOe39MLq_G3w#U==lK{I zYWNr!ocI_Rr1%&ZUh^_AT;OG3n8VA!(96rfP|C}|kj~4%kjBfvV8zS8z{ktLz{|_P zaFd6DVF3>V!$ckih6)}Ah8P|OhG-rJ1|J><1|c2>hAZ3*3`e*b7*=vKFtl?sFywPH zFgSBFFz9kKFo<zO-294b2nPD7lSe~fGz3ONU^E0qLtr!nMnhmU1n3X~3JMAeC7HRY zdc`@ZsRbG-nQ3X73?M-q!XO0-whF0vDGIs@#U+VFC1BMczAeNckZBqaGm1)5;0spu zz$;U|G%BO5bTVUg6b$qXbrcNsO$`hnYQbhHpqi1EU!;(!keR1Yl$e*Es!^E|pOTuJ zuUDLrSdbcR7^`W;fMP>#epxES0)tph2IYdH%)AnqCQSyYZUtL~yaK)C{JiAElGMD! zl2i?t5*?64RccXwv4%#Tj)I}4rlyVpX!WhFA+m0;4-^coVij~1K%P;s(lw0LRM1z@ zD2X=Efl7dxU<rmi1zUwmsGAL985k-dE=VjYO03jKNi0c>wgT(Y)C*xK0R@0wQ7YJn z8iqOwIjMOXC7POG37A@gSOyRiVoZ8!URqIpZb?On251FvYH~?_QDuBiequ^NVo8RM zLP}~*W^QIlYLTtFj=ClT7BvdC3hH|LrA3AY@%g!_>51T##1M1zl8ejK86bgNl$xBH zS(d6`s}L08;u-|uX($<58tECDS?C$+85$TVX+qRO*C<2PIzv`0W2$9PhQ<-dJDLp2 zAWFejp&&C)&n2-WQ6o1$B{fGAt_sEl(@=FurI|S?z94mOMftg*MJZ_-r9~-e@gUE~ zCngo==aiPDA{midoDrXrS%fT=i!P>MtB_x;2Qon~F{v0tYrxcKGAP5C3bqPLdV2bL zdiq8AN%<x5DXGQDMVSR9nfZCe`brGSn9jgrE<#aJYEEKFW*Nw>n1Xoi0x8!oEiz4w zFV8H=hzGkSvoJm*F)t-m4<xC?z`&51TaaH=qL7<dQjn8hl9QRFS5OIJDI^vv6y%h^ z<cl-Y^AdAlJWx)CvGa>zti00Pf=ZCSyaJd|L1tcla&ksyK1jGAGmin3Bo$yTNiWJQ zC`c_*fVv?F6kM5w?hsL^QcytwOJOi|h-BuOSCU$kmYAFhG9M}VA!&vsIV8={bcdlC YDh|<{T$Gwvk{X|qTL5#Pi*En}089)1IsgCw diff --git a/dmp/my_sol/__pycache__/create_dmp.cpython-310.pyc b/dmp/my_sol/__pycache__/create_dmp.cpython-310.pyc index 6624a8482e4f4610de7960c76fea932f0bb877bb..3ced647a15e91d71a1f56852c1755b0f84070bf1 100644 GIT binary patch delta 98 zcmeyX*rmjq&&$ijz`(#z_1h(N{YKumOpFPWe={iyq_B!G)G)*|)i7i+EnrSzo2<nw zEzXw0(8`p?B+0OlF^jE+A)Y;jA(%mveR2%5F=sl+6b6PL_LKXWs~N>MbF+Nn0RY+D B7uo;- delta 84 zcmeBD`m4yB&&$ijz`(#@Tj-p+awG3sCdQb_znPSIY8c{~Y8bMZ7BHu<PS#_V7H3Ui oXk|)cl4MxOn8jAZ5YL{%5X_*-HaUUWc=8<PYDS^W5-gv10E<8r`v3p{ diff --git a/dmp/my_sol/create_dmp.py b/dmp/my_sol/create_dmp.py index c524edc..a3a7d80 100644 --- a/dmp/my_sol/create_dmp.py +++ b/dmp/my_sol/create_dmp.py @@ -42,7 +42,7 @@ class DMP: # and this is just joint positions trajectory_loadpath = './ur10_omega_trajectory.csv' data = np.genfromtxt(trajectory_loadpath, delimiter=',') - self.time = data[:, 0] + self.time = data[:, 0] * 1.5 self.time = self.time.reshape(1, len(self.time)) self.y = np.array(data[:, 1:]).T diff --git a/dmp/my_sol/run_dmp.py b/dmp/my_sol/run_dmp.py index 171fa7f..7e95e8f 100644 --- a/dmp/my_sol/run_dmp.py +++ b/dmp/my_sol/run_dmp.py @@ -9,6 +9,10 @@ import time import signal import matplotlib.pyplot as plt +# TODO +# add scaling (point of albin's paper) +# generate this from other trajectories + #urdf_path_relative = "../../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf" #urdf_path_absolute = os.path.abspath(urdf_path_relative) @@ -25,7 +29,7 @@ rtde_receive = RTDEReceive("192.168.1.102") #rtde_control = RTDEControl("127.0.0.1") #rtde_receive = RTDEReceive("127.0.0.1") -N_ITER = 10000 +N_ITER = 20000 qs = np.zeros((N_ITER, 6)) dmp_poss = np.zeros((N_ITER, 6)) dqs = np.zeros((N_ITER, 6)) @@ -74,8 +78,8 @@ update_rate = 500 dt = 1.0 / update_rate JOINT_ID = 6 -kp = 100 -acceleration = 0.15 +kp = 10 +acceleration = 1.5 print("starting the trajectory") for i in range(N_ITER): @@ -93,7 +97,6 @@ for i in range(N_ITER): vel_cmd = np.clip(vel_cmd, -2.0, 2.0) if np.isnan(vel_cmd[0][0]): break - exit() #rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500) rtde_control.speedJ(vel_cmd, acceleration, dt) @@ -103,11 +106,14 @@ for i in range(N_ITER): dmp_vels[i] = dmp.vel.reshape((6,)) end = time.time() diff = end - start - print(diff) - try: + if dt < diff: + print("missed deadline by", diff - dt) + continue + else: time.sleep(dt - diff) - except ValueError: - print("it took", dt - (end - start), "more than", dt, "seconds") + # TODO do this with something sensible + if (np.linalg.norm(dmp.vel) < 0.0001) and (i > 5000): + break handler(None, None) diff --git a/dmp/my_sol/test_traj_w_speedj.py b/dmp/my_sol/test_traj_w_speedj.py index c00dc8e..c275f1d 100644 --- a/dmp/my_sol/test_traj_w_speedj.py +++ b/dmp/my_sol/test_traj_w_speedj.py @@ -29,14 +29,23 @@ t = data[:, 0] t = t.reshape(1, len(t)) y = np.array(data[:, 1:]).T +update_rate = 500 +dt = 1.0 / update_rate +kp = 2 + n = y.shape[0] -yd_demo = (y[:, 1:] - y[:, :-1]) / (t[0, 1:] - t[0, :-1]) -yd_demo = np.concatenate((yd_demo, np.zeros((n, 1))), axis=1) -#print(yd_demo) +yd = (y[:, 1:] - y[:, :-1]) / (t[0, 1:] - t[0, :-1]) +yd = np.concatenate((yd, np.zeros((n, 1))), axis=1) +print(yd.shape) rtde_control.moveJ(y[:,0]) -for i in range(yd_demo.shape[1]): +for i in range(yd.shape[1]): start = time.time() - rtde_control.speedJ(y[:,i], 0.1, 1/500) + q = np.array(rtde_receive.getActualQ()) + vel_cmd = yd[:,i] + kp * (y[:,i] - q.reshape((6,1))) + rtde_control.speedJ(yd[:,i], 0.1, 1/500) end = time.time() diff = end - start - time.sleep(diff) + if dt < diff: + continue + else: + time.sleep(dt - diff) diff --git a/util/freedrive.py b/util/freedrive.py index c0f7534..aa33bbf 100644 --- a/util/freedrive.py +++ b/util/freedrive.py @@ -11,6 +11,9 @@ def handler(signum, frame): rtde_control = RTDEControl("192.168.1.102") rtde_receive = RTDEReceive("192.168.1.102") +while not rtde_control.isConnected(): + continue +print("connected") rtde_control.freedriveMode() signal.signal(signal.SIGINT, handler) -- GitLab