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