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{*iZ)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