From 4289c04e45fba80fa1124ace08792ef41ec50845 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Mon, 6 Nov 2023 17:19:43 +0100
Subject: [PATCH] added temporal coupling back

---
 TODOS                                         |   4 +
 clik/clik.py                                  |   4 +-
 dmp/my_sol/.run_dmp.py.swp                    | Bin 0 -> 20480 bytes
 .../__pycache__/create_dmp.cpython-310.pyc    | Bin 4362 -> 4349 bytes
 .../temporal_coupling.cpython-310.pyc         | Bin 0 -> 2508 bytes
 dmp/my_sol/create_dmp.py                      |   4 +-
 dmp/my_sol/run_dmp.py                         |  40 ++++++---
 dmp/my_sol/temporal_coupling.py               |  80 ++++++++++++++++++
 8 files changed, 116 insertions(+), 16 deletions(-)
 create mode 100644 dmp/my_sol/.run_dmp.py.swp
 create mode 100644 dmp/my_sol/__pycache__/temporal_coupling.cpython-310.pyc
 create mode 100644 dmp/my_sol/temporal_coupling.py

diff --git a/TODOS b/TODOS
index 2efde79..c68c61e 100644
--- a/TODOS
+++ b/TODOS
@@ -7,6 +7,10 @@ IMPORTANT
 - do test to see if the calibration is ok
 - recalibrate if necessary
 
+2. servoj 
+- try to use this instead of speedj and see what you get 
+- you need to set up tests inteligently to actually measure what you want
+
 NICE TO HAVE
 ---------------
 1. other inverse kinematics algorithms
diff --git a/clik/clik.py b/clik/clik.py
index 55a899d..30b38e9 100644
--- a/clik/clik.py
+++ b/clik/clik.py
@@ -76,7 +76,7 @@ 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])
+Mgoal.translation = Mgoal.translation + np.array([-0.2, -0.2, -0.1])
 print("goal", Mgoal)
 eps = 1e-3
 IT_MAX = 100000
@@ -104,7 +104,7 @@ for i in range(IT_MAX):
     if not SIMULATION:
         gripper_pos = gripper.get_current_position()
         # all 3 are between 0 and 255
-        #gripper.move(i % 255, 100, 100)
+        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)
diff --git a/dmp/my_sol/.run_dmp.py.swp b/dmp/my_sol/.run_dmp.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..362c6bac7ccaa4eac41286b03a9e5bfb24526142
GIT binary patch
literal 20480
zcmYc?2=nw+u+TGNU|?VnU|@J6=$X1Yu!f=6pMfDgzqlYjC9w!3g%9T@7H4PX;Zp&T
ztAiP+pOK%NYNQY6=9K28=ob_vR%90ImlnkrXXX~<q{b)b=am%Y=jf;87U<_z#uw-3
z=ogjd#i!&J=oM5#OdKUgLtr!nhzfzyk~CclUIt?$Lj#a{WhF%gVWChEa}<w;z-S1J
zhQMeDjE2By2#kinXb6mkz-S1JkPs*-U}mUiU|?W^`Zpg+GosNfP(BBgE`!oAb<9w{
z5R|Tg(lB`_ca$0pfzc2c4S~@R7!85Z5Eu=C(GVC7fzc2c4S~@R7!85Z5Ey_ENK9d1
zILOGrFcZ{)7hqt3_5YvpGcc^+XJE+YXJByPXJFvqXJGii$G~uxkAdMb9|Oa2J_d%J
zd<+aT`4|{x@G&q<;A3FO<YQoP<YQoP;A3F0=VM@a!^^<1iI;()i<f~RnwNpWftP`S
zhnIojFAoF5QyvC}>pTn$*LWBhR`D<})bTJdgz_*j=<qNwX!9^I$nY>QF!C@kJm6+v
zIKs`qu$Y^HVG%b2LmM{(LpV1BgB>>mgBUjh!w)V7hQnM842QTF7?yJ}Fm!P-FywJD
zFa&ZjFc@+%FbHrlF#P0XU^vCez%YxGfgypDfkB0nfkBv)f#EI(1H)+!28PKT3=9n%
z3=GyB3=FCq3=DVK85oYSGcastXJA;v&cIO5&cKk*&cKkz&cNWo&cI;9&cGnV&cN`1
zje+4J8w0}yHU@_6Yzz!b*%%o5*cccZ*%%la*cce<*%%mn*ccdG*%%m%*cccL*%%na
z*%%nu*%%lY*%%n^vobJTVr5|1#md02f|Y?`1}g(YEh_^<0xJW91S<oBI4c8#7%KzA
zR~80_yDSV0M_CvccC#=rtYTqcn8d=skjBEm5Xi#7pvuC)@ST}~VJ|Zz9-EjM7($sD
z7#x`y7(|#E7@jjRFkE6{VA#OKz_5skfgzuXfgz8Hfgy;AfkBIjfq@4)_@khpppa5n
z9Gw}fV5^W)s8^I)oRL_Ns-a<~qp8UNmd!1QFUT*3DFLm-$S=mC4rU0t8DK+-N>bp9
zJM@YRQd3jBG|EzQ;*)bzbQBVklT&k2ixNvR^Ye5RQc589g6vKzN=?iL8<3f%kXN9W
zS)7-c2h$jB5F2d}t7!$6S5PP@%FHX#&^1)hDvQretbiDz3=vg=+NGqU0A-<ifC`F1
z&Lzn<s6!NN;Z6b>0plPEV@3%=D`;U%S!#}gwnBD+f|h~?h+mLjte~ra5)Fo$u*fV_
zumuHVVo_0Ir3N@wic*tPGs{x-(o;(ulS@hya{^N|psI;c3{wlTxj3~Xz9g|!qa?8u
z5~3xErJ!V$Tc8JGX)BZ@>y;LyB$lLt914zV1ucb?5^xMEE0m<>7UUNt<|riRmlot?
z=B0zh!3LM47HGf{hO$CRZUI=TxFoTtM8Q^}Br`Wv4@78aGNk1fDP$^S<|z~<=B1}<
z_{DpMxCUujF+dXnSW9MJx<W}tszOOoVpeK$Nq$kKk|qNK*vOJ(1zUv>=djcq$K+(^
z{Ji3lqQuO+)D(^M#N6D(_`Lkw%)G=L9R&zCQAYus*mM*UL1|7$A+?|w;&U(`64v>7
zsl^&*nhIJ9hI-~;OTc>2C5^yRsRhLfwh9J%1_p*;KGYttm?@G7)FBGC3Wj<HVDXZ~
zQUe8B1&AVLg%E!ie}$aX#G*W~S923fG87VXic%9(Dj8C9ic=xk2JBtG{19giO$JaF
z%P&y~arF)G4|4R0clHkr@bUC>w_;#G&Y@r{%^9)_6l@iY7?c$X5{nXZQ%h2diWSm|
z@^cj`6LUc^n3tBBu8@|QlM0H9#GIteJoRFQqSOLVW#i@V=@%03>7rn(V8)OFia|p?
z0|k8rNZN=mN-RlbKw^V@WnjR7;V@846_utkFeroKUavSOHMKy)Sd#(j`Q(h$<ZOkK
zjKmU!%KTFGqEt{o6oK4RT#}lYqL81akf;FG!~o6+(6wiJx%p+OUZ4c3SAbs7X)>hb
zg8c2`8=#@dP@I{bmzbjmp)??rUa+UTr(cMULPla<N=|B#CIf>qIA1F$D5O?omS}*S
zr=Xw!4Qv#PGm8U?^3#hFbAw9r@-p+%HK1i1R3FHgf}9e);*9)q4QMK>04<G+Pbt*P
zNlj19OTiE>$jL9!0ELbYDAUB3rREezTj`j_>L?`V=j0dJs^=GhvWdEmLQY~*YL2aX
zN^U`XN})Q6@z_l-)I&2ITuGo9o?euinulsOve8(~MK!b#&9OzPDM+Rv8HH&Qib3e+
zB;}N*A`F6=qpScbJHX*k49-!Y08fO6Fq&_nL79mWl#rl9(u?90Q20Uv3P~#@pip(9
zY6E)#Ne3t>P}HL+M^_E@Fhn`Xk4WXCDKw42{i$H908V73Nnj5f8JR%zK^1_i1XOur
zggmCC5nK{e&JZF8N}Z6hFcF*<VMR7L3LvGndSXs-z5>WSC7F5Y>Y9+E8WhAR`3JpS
zVW4NI11cUB^c74E3?R0FG$E?byaK(d)S`S)@eH;OB#UBXYF>(4QEF;RQD#}HZ+=QD
zG=rf9qM?BSG#H`oEl$l#0aY@F1_laXS7j9|q?P96fXW=Oh3c9NDXD3&LIPZNl;-Lv
zq=AZ3O)CaaF$2!5kbnVe(SQbwj)Iw{CPPXgsGfkkA5{+8V#BE!k6uW92`b|a3=9ky
z7?i=m0c|5F*eV2txVQ#ES_~RWhDPRk26_g1hDw@Hh0rn-s?Zs-nio|eUIQ(S^bE}`
z^bGY34UCjDQOacuGttyCC_|fHDTyVCnheSy3Y3^L^YmO2OA<A5^HWlDG~udXTrdq)
zmsFaWlLCt79JiwU+|Z(wG>y`tl(hJQ#FC8o#H8Z<oYIn1BqLIbGvZS+i;$&q(Zv*O
z74nPqKqlxVCKZEd4VW5D24xsi!B#;@PfuS@ProQXDZeB>CAB!YD6;_6A}-cfVo=6(
z1{QM>ii%Qm5=%16KyJkp#A_EwxqfMpX=;3VW=Tdo*fp7j@u1o)RSzVo!~m+%(o^$5
z4R%nKnhCA<^Gh;Pix5q!%+z8AWrf6)6oumCL{MW+L8Bl)Gp_`c1z^pAg2Vz)4GFEU
z85lA_?Y|O*+{6-4UeC!)(krM0u@n-EL4^`b9#Yf7c%bqF#?CK>vGPiD3o1eS@(N%=
z1(|vI$;la+`5@tf%sd8ga|PDujE6Nk6`-yFwTg7GH3cF1U?G!Ulvz-aS_IP-1d6`Q
zLU)KLR4J(Q1It2C(~<I#XI@EaQCebhD#%?(;z*id*$PQBG;d*OhKfTpCl{qAmZXB(
z2r&1#_y#b5`v1y&3=HjjkoEtt`THCE3=D_(85macGce5LXJE+UXJClrXJGK>XJ9bo
zXJBCCXJ9zP$H1_UkAYz?9|OZmJ_d#rd<+bId<+b|d<+a7P&3;=<_^HP#;BE}Aut*O
zqaiRF0;3@?8UmvsFd71*Aut*OqaiRF0)sOIK%;Gtp*-lw4Rl}*I<d#V02+13NiEXw
z%g;;IQ2>!hLw_i9gBp1SdO4t(nRLCp{Gwd&=zUpgj;4YQcrL}jP*Wi>FGWEkQ^5{2
z76==zgN{6;<SUe9WELxchCCFC^K(;6GC-qu#i@D4nMpaR@S#D_Py=+DA*Dn?S0N=c
zEe$fPh?r3Uc?#5B&&(@DXoL(4D&=Mt7pJBuq@*UM<YeZhDkN1Z>3|IZnFXEp%uG{A
zDN(Qi3q$5;K(;8@Dx~J6DCjB_gQmQ|4g&E}r@lZ|gT|M^L(`B6-P{7bveX>Z$xd+p
z{~{j)11~?M{|{T=e~zDlVIw~S!(x60h5~*DhJ1bohG2dM25o+b+V6Y}42MSh_JcDs
zjk;<y1V%$(Gz3ONU^E0qLtr!nMnhmU1V%$(Gz5la2!MOKum#7&u3E;T2_!V67eGUo
f9YF_9APatyb21BHYov7)KntQ_i&VkuPBj?-C*ZOR

literal 0
HcmV?d00001

diff --git a/dmp/my_sol/__pycache__/create_dmp.cpython-310.pyc b/dmp/my_sol/__pycache__/create_dmp.cpython-310.pyc
index 3ced647a15e91d71a1f56852c1755b0f84070bf1..3758f9217ef8902406dbbdc7eeffcf674a23ca91 100644
GIT binary patch
delta 158
zcmeBD`m4yB&&$ijz`(#D#_O5-aU<_rCdQb_znPSIY8c{~Y8bMZ7BHu<PS#_V7H3Ui
zXk|)cl4MxOn8jAZ5YL{%5X_*-HaUUWc=8<PY9=A($&xJp8I31PvUW0BPhQMg%jhs!
zk*$Q$b@DW}G)Aw<f7#L)eJ7`|e`gHX9K#XL$QU+x3+E=rxXI~UKNyoYCvb<dFcwVS
N#{Y+rd-7(1Z~(yWET;ef

delta 172
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_LKXWtC_?YC-boUXEdD5
z!`jJcIe8juEu-CJ5w;RW=gFOHX^b9|Kd_}SdQT2#|IX;Q*`FhvkuhZQa?VYRF_WXX
ZelR9(4(1MJVa%JnlK&4Q=j3Gq;Q;JZFAV?y

diff --git a/dmp/my_sol/__pycache__/temporal_coupling.cpython-310.pyc b/dmp/my_sol/__pycache__/temporal_coupling.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..31d5b4d59ffa1b2859605ad73e9336eadddcd992
GIT binary patch
literal 2508
zcmd1j<>g{vU|@L6;+e|J&A{*&#6iZ)3=9ko3=9m#8Vn2!DGVu$ISf&Z?hGkRDa<Vl
zDa_4GQB3X(DJ&_hEet8F%}i0uDU87knrtsYX80w8NMy_eGfjknfgzP4iZO*DiYbLL
zg{g%hiaCWjm_d`}7K>kgh;uT?UYJf0n~{Nm!5QQZ7LYqYb};!Bfdp4FXfoeoDNfBv
zyTzQ6TX2ghrKAX?0Zjag*3Zb#P1R4&FD}SWNh|{Mj1b(M(!3P?f}+HV%wql0qWI#>
z+=86c_~iV&lA`<^{gm7S{oKm<;`|)_lGNOS{G!C1_~iW3f}G5}biIPgTWqBTDTyVi
z>>y`>e8$39#SL=29z@(vljRmie0*MFZfbn|Ew1?Z-29Z%91xo)KEALtF$W@3#KFM8
z0Cp`B0Wz})WF{NPaFCxl7<d@LA{c=R@+~-u)Q|&}6C6d1QOuCYiDF4%OJQ$eh+<9Q
z2xidax+NUq9G05nn4IjKpI2N`l$e>9n)3Y5%8!?PlLG9MLDs^8AH)W^037^w3=9l4
zj5Q4L3^hzO4DpOLtThbrOf}34m`hk{n41|>7}J;*g5+6iSioW|NMdX$48aVVOnxsx
z5vR$1i#I(nH#adpFF!XkFEQs9JA|8fi#fHR_!fInYH>zlLFz5$oYcHqtYz`Ji50h4
z6G4<FTM@`XMf{+U695sQ^i(7YVhMr>P>?`E6BLl3Sjf!FEQycj1%)Ok!Lu=fpa`P`
zW0ee+fY6JNhiE`ahhUAcba;S)fuV$90b>p00;Yuw+zb#J<oQ}=usA~&GdDvGm;{Tm
zfJGS>uq*_rWdZ2{i?f2o8M0Wxs#&=iz@lt5j9F|oj0@O7^g_lO#w-qLh8iXj262X3
zcCZe{1xz*U3mG9cgIHjF98ftHkVp;V0x%6Vle31ohNXtJhOLI9hBJ*Vm_d`#Pm}W&
zQ(nO>wzA@q#N_N-j8))Jyv3MN1Panyj1{*S({3@BB$jG&LBgCB6uzJ&af{LM7NgTG
zM(10MF1Oe!;`34~N^Y?^LTP6(4T%GAY=9#|Fg-OdwJ5P9H9j}7q$o2vwHRbeF(?Wp
z7{wStScVY<1sJPTvBe99ev}B3W?*38fkjXi69YpH!vauhWm?D-!&b{&%Tmi)%T~)?
z%TWRfJmwU}Y=(u5wVXAa3z!!&6zP_Lf{UewQ<9;EF^e^YNti)|p@tR27Y9W!Gl(S(
z(!rI-Qo@+U#?4U61viNW$)unXP{a{q5_cXm$fR2C5{?qi8un(!X2x1hushg6wsAAm
zuz_f>tvoe6Aa_YH)G$di)bf@vF5rUbS;z=>1v|t)k_@$cSmjt2GD<Sk@`LQ-W~ddY
z;d5b#b*Sa6;dfz(wXPKeB??J~67CxI8bL{hW=0W)8V(Q(EW!y95of3ssu4<I%w{P%
zU&37j@gZ2f2tzX?SVT~gp$6=Bkcc=#t#AqB0-hS78ey;>L~2B81VFlLL}~>JrxZ@8
z5kT^jXboc)FA`r291>hLyfvaVVrifd7O!Dkzz51ekoc)#$l@1akY=crsA0$w0J&=d
zW05R~UBb9PutowDhb4>)gi@FxYza_?6sTdy5*A@7VO$_m!dWBH%$ULq$~qzp3z<M}
z6k({9%wtMntd*=`$U<_F2sl;LNP@GV6gY;ZYPdl<Ye77g8mSs_2(Ly8RFOfdr<b5Y
zyois1fk9KONEMWK#2FYEZm|~^mZqjwrQTvr%&WY`oSRrtBnuK}&df`@#hja&cZ;<s
zF)ux}NEjr_mI&tDVoppdzQs~pSX6S0r6jS`phya2Ah_5AWuaTl#ihBo*mDyrGIL9F
zi$EpeEtdSe)M9W!sVR+?fs5ooy5&KH0*Fuq5y~J!1!O5><}I$0#L|@b+|0cAL>N1<
zB0ljJXGvlyShx%(oS7G2c8e)9^A=N97D77h7Gu^e=DgC}Tg)k`dAFELatn|&q}>vL
zn-8l3IA9{T*eg>~;*%1KZm|~Rm&7N6J)DA8o`9=PkS~fo7#J8NnE04E7<CvqnD`h$
zG#e8SlLR9lqYRS_BL^62Fmf<zFoI-R7?~QFe(|vjFbOe9F!M0FFtRYoF!3?-F|sgK
zNntB_V0AiU5vUT;WGmtXg%qetDB=dO#6Sc%f^pX8prQ$+wiu*>gMo{QgPn(~2qdb>
ra*H*uG`FC#2$VX&)jU`n$-js~l*0znkgx;QqQ#)1n1hLrS%?(?_Y^%>

literal 0
HcmV?d00001

diff --git a/dmp/my_sol/create_dmp.py b/dmp/my_sol/create_dmp.py
index a3a7d80..74282cf 100644
--- a/dmp/my_sol/create_dmp.py
+++ b/dmp/my_sol/create_dmp.py
@@ -42,7 +42,9 @@ class DMP:
         # and this is just joint positions
         trajectory_loadpath = './ur10_omega_trajectory.csv'
         data = np.genfromtxt(trajectory_loadpath, delimiter=',')
-        self.time = data[:, 0] * 1.5
+        # this one is slow enough
+        #self.time = data[:, 0] * 1.5
+        self.time = data[:, 0]
         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 7e95e8f..e9577e5 100644
--- a/dmp/my_sol/run_dmp.py
+++ b/dmp/my_sol/run_dmp.py
@@ -2,6 +2,7 @@ from create_dmp import DMP
 from rtde_control import RTDEControlInterface as RTDEControl
 from rtde_receive import RTDEReceiveInterface as RTDEReceive
 from robotiq_gripper import RobotiqGripper
+from temporal_coupling import NoTC, TCVelAccConstrained
 import pinocchio as pin
 import numpy as np
 import os
@@ -29,7 +30,7 @@ rtde_receive = RTDEReceive("192.168.1.102")
 #rtde_control = RTDEControl("127.0.0.1")
 #rtde_receive = RTDEReceive("127.0.0.1")
 
-N_ITER = 20000
+N_ITER = 10000
 qs = np.zeros((N_ITER, 6))
 dmp_poss = np.zeros((N_ITER, 6))
 dqs = np.zeros((N_ITER, 6))
@@ -73,31 +74,44 @@ rtde_control.moveJ(dmp.pos.reshape((6,)))
 # TODO check that you're there instead of a sleep
 #time.sleep(3)
 
-#exit()
+TEMPORAL_COUPLING = True
 update_rate = 500
 dt = 1.0 / update_rate
 JOINT_ID = 6
 
-kp = 10
-acceleration = 1.5
+
+# parameters from yaml config file in albin's repo
+kp = 2
+acceleration = 1.7
+
+if not TEMPORAL_COUPLING:
+    tc = NoTC()
+else:
+    # TODO learn the math already
+    tau0 = 5
+    gamma_nominal = 1.0
+    gamma_a = 0.5
+    eps = 0.001
+    v_max = np.ones(6) * 2
+    a_max = np.ones(6) * 1.7
+    tc = TCVelAccConstrained(gamma_nominal, gamma_a, v_max, a_max, eps)
+
 
 print("starting the trajectory")
 for i in range(N_ITER):
     start = time.time()
-#while True:
+    # dmp
     dmp.step(dt)
+    # temporal coupling
+    tau = dmp.tau + tc.update(dmp, dt) * dt
+    dmp.set_tau(tau)
     q = np.array(rtde_receive.getActualQ())
     dq = np.array(rtde_receive.getActualQd())
-    #print("dq:", dq)
     vel_cmd = dmp.vel + kp * (dmp.pos - q.reshape((6,1)))
-    #print("dmp.vel", dmp.vel.reshape((6,)))
-    #print("dmp.pos", dmp.pos.reshape((6,)))
-    #print("q:", q)
-    #print("vel_cmd", vel_cmd.reshape((6,)))
-    vel_cmd = np.clip(vel_cmd, -2.0, 2.0)
-    if np.isnan(vel_cmd[0][0]):
+    vel_cmd = vel_cmd.reshape((6,))
+    vel_cmd = np.clip(vel_cmd, -1 * v_max, v_max)
+    if np.isnan(vel_cmd[0]):
         break
-    #rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
     rtde_control.speedJ(vel_cmd, acceleration, dt)
 
     qs[i] = q.reshape((6,))
diff --git a/dmp/my_sol/temporal_coupling.py b/dmp/my_sol/temporal_coupling.py
new file mode 100644
index 0000000..5d9107a
--- /dev/null
+++ b/dmp/my_sol/temporal_coupling.py
@@ -0,0 +1,80 @@
+#!/usr/bin/env python
+
+import numpy as np
+
+
+class NoTC:
+    def update(self, dmp, dt):
+        return 0
+
+
+class TCVelAccConstrained:
+
+    def __init__(self, gamma_nominal, gamma_a, v_max, a_max, eps=0.001):
+        self.gamma_nominal = gamma_nominal
+        self.gamma_a = gamma_a
+        self.eps = eps
+        self.v_max = v_max.reshape((len(v_max), 1))
+        self.a_max = a_max.reshape((len(a_max), 1))
+
+    def generate_matrices(self, dmp, dt):
+        A = np.vstack((-dmp.z(), dmp.z()))
+        B = np.vstack((-self.a_max, -self.a_max))
+        C = np.vstack((dmp.h(), -dmp.h()))
+        D = np.vstack((-self.v_max, -self.v_max))
+        x_next = dmp.x + dmp.f(dmp.x) / dmp.tau * dt
+        A_next = np.vstack((-dmp.z(x_next), dmp.z(x_next)))
+        C_next = np.vstack((dmp.h(x_next), -dmp.h(x_next)))
+        return A, B, C, D, A_next, C_next
+
+    def update(self, dmp, dt):
+
+        A, B, C, D, A_next, C_next = self.generate_matrices(dmp, dt)
+
+        # Acceleration bounds
+        i = np.squeeze(A < 0)
+        if i.any():
+            taud_min_a = np.max(- (B[i] * dmp.tau ** 2 + C[i]) / A[i])
+        else:
+            taud_min_a = -np.inf
+        i = np.squeeze(A > 0)
+        if i.any():
+            taud_max_a = np.min(- (B[i] * dmp.tau ** 2 + C[i]) / A[i])
+        else:
+            taud_max_a = np.inf
+        # Velocity bounds
+        i = range(len(A_next))
+        tau_min_v = np.max(-A_next[i] / D[i])
+        taud_min_v = (tau_min_v - dmp.tau) / dt
+        # Feasibility bounds
+        ii = np.arange(len(A_next))[np.squeeze(A_next < 0)]
+        jj = np.arange(len(A_next))[np.squeeze(A_next > 0)]
+        tau_min_f = -np.inf
+        for i in ii:
+            for j in jj:
+                num = C_next[i] * abs(A_next[j]) + C_next[j] * abs(A_next[i])
+                if num > 0:
+                    den = abs(B[i] * A_next[j]) + abs(B[j] * A_next[i])
+                    tmp = np.sqrt(num / den)
+                    if tmp > tau_min_f:
+                        tau_min_f = tmp
+        taud_min_f = (tau_min_f - dmp.tau) / dt
+        # Nominal bound
+        taud_min_nominal = (dmp.tau0 - dmp.tau) / dt
+
+        taud_min = np.max((taud_min_a, taud_min_v, taud_min_f, taud_min_nominal))
+
+        # Base update law
+        ydd_bar = dmp.h() / (dmp.tau**2 * self.a_max)
+        if self.gamma_a > 0:
+            pot_a = self.gamma_a * np.sum(ydd_bar ** 2 / np.maximum(1 - ydd_bar ** 2, self.gamma_a * self.eps * np.ones((len(ydd_bar), 1))))
+        else:
+            pot_a = 0
+        #pot_a = self.gamma_a * np.amax(ydd_bar ** 2 / np.maximum(1 - ydd_bar ** 2, self.gamma_a * self.eps * np.ones((len(ydd_bar), 1))))
+        taud = self.gamma_nominal * (dmp.tau0 - dmp.tau) + dmp.tau * pot_a
+
+        # Saturate
+        taud = np.min((taud, taud_max_a))
+        taud = np.max((taud, taud_min))
+
+        return taud
-- 
GitLab