From 3745796e7abd481e025adbced6f83654d62648b2 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Tue, 10 Dec 2024 16:22:28 +0100
Subject: [PATCH] cm

---
 python/examples/.cart_pulling.py.swp          | Bin 28672 -> 28672 bytes
 python/examples/cart_pulling.py               |  25 ++++++++---
 python/ur_simple_control/.managers.py.swp     | Bin 16384 -> 0 bytes
 python/ur_simple_control/clik/clik.py         |  39 ++++++++++++++++++
 .../optimal_control/.crocoddyl_mpc.py.swp     | Bin 32768 -> 0 bytes
 .../.crocoddyl_optimal_control.py.swp         | Bin 69632 -> 0 bytes
 6 files changed, 59 insertions(+), 5 deletions(-)
 delete mode 100644 python/ur_simple_control/.managers.py.swp
 delete mode 100644 python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp
 delete mode 100644 python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp

diff --git a/python/examples/.cart_pulling.py.swp b/python/examples/.cart_pulling.py.swp
index e874c004798a168923f4377f425b22573deb4629..46cded145a8cbc25199c3764296de5e366310042 100644
GIT binary patch
delta 2057
zcmZp8z}WDBQ7XwG%+puFLeGeSfq_AQfnjY-M0%xA4a1Ff8>RRJI9Wi#3=9nRlMMyL
z1KFTFK`1>LN;5)fRwx|-r58bIm^?F-uLq@nZJsHR&Cklkz`#&6nNz{O{xS~(!)_i1
zhE+Ta3_Uyy3}rkF3=%vH3_Lsx4BR{n49q+X42QWH7@D~m7@D{l7>c+V82q^z7<9QA
z7~XI(FdXM%V5sL}U@+!lV7SJ~z|hRez!1mDz+lhGz#z)W!0?lUf#C=T1H(cN28L=5
z1_mz<1_pHw1_nkB28KuM4D}3q*%=ttvNJHOVP{}yVP{|{W@lh<WM^OyV`pIa#m2y}
zf{lS8hmC>3fsKJdhmC<joQ;7&jE#Xol#PMmB`X8N4ORw*ZdL|{ELH{vX;ub?%Pb5G
zi&z*KbXXV|zA-Z}9Asu-=x1hNNMUAR@L*<O_{PM*u#1U-p^u4yp^8a?fgzTOfgy&8
zfq{{UfngIP1H&pt28LOT3=Bn#3=D;go0}DRcw}`HlJoOQit=-O^79Lvb277CN)vM&
zi*k$o^KvRD2k1#`UalL<RDYYDfnhxdBwAr1eu<xfVLd+sLl-{-LnuE3g91MT!zDfj
zhJ}0#3{iXx3_^Si48M697%uQKFwEm+U`XL*U`Xa=U@+iiV0g^Kz%Yr2fgzWNfx(N1
zfx(1_fkBanf#E(k1H*1^28J?j1_ncJ28R1w3=B)S82lN^xfmGSxEL5TxfmFDxfmE;
zaxyTy;ACL9#L2+0h?9X~AtwVvBPRnxIwu1|8YcsTGbaOsBqsxd2qy!>KMn?lTO14w
z8#o}ISjVxMk>wuyW+Cf$ESu{Clb8e(Qy3Vsm>C$FKxtF}ltcv+l<Rl%F)+;HV_=xT
z$H0)l$G{NJ$H4H7mw{n5F9SmcF9U-UF9QP$F9XAI9tMUs9tH+W9tH++9tMVM+zbqz
z+zbrm+zbrC+zbqU+zbr-+zbpqK`Dugfng^X1H(iv28L=b28LuV28KW`28IAG1_mcC
z1_li-28N%U3=BUw8R{8caWXI*;ACJ}%gMk{#mT^6&B?$Z#mT_H&dI>AgM)z~je~*V
z4?6=x13LplG&=*sOEw0Ed29>}6WACSg4q}tgxMGvF0wK(EMsM0h+<`6@L*+N5NBmz
zc+3Lv>Jb(OhHWei3=3Hp82nin7$jI27{pl^816DNFf=o(F)-*dGcf#QVqkd0#K5qZ
ziGiVjiGjhAiGkrZBLhP>BLhPUBLl-^zeKUklaoZ4Sril$6ehn)mF7W7Lw;!zeChd#
zIq@Y$iFw6o`9-;tAErqpf)s-=s+5AQLP2JpUa+gNo@YvGUP)$2rG_R0NF6d(RtWKT
z@wZZNadq?bb5#g-^$YfN@^MuN33BuccJmJcYnc2oO=|L|R3Ev_G=-x4r2G;+Fd3hh
zn47AgXse)9S(=-vWVP8at)D{{tOI04NPKyGYJ8540)$bNnVwNHc|oKES87gisuft-
z<O6w%vUm;Md?!zZu^yDDG#D5dmVk;Z0R{$GNw$fffgy#Tfgzcnfx(=gf#D(_1H%PA
z28Qi?3=C8G7#RNYGB8ZzWnh@f%fQgi%fOJy%fR5k%fRrLhk@Zb4+Fz?9tMV19tH+C
z9tH+Y9tMVAprpmk!0>~cfnhs01H&|K28L2@hI)n&ZUzQfZU%;HTnr3bxfmFVxfmEi
zxfmEkxEL57a56CTaWXLYaxyUZK$F!A4hDv891ILC91ILe91IK!91INq*%=rPvNJF&
zW@lhXV`pG6VP{}?#>T*~oQ;8D5gP-8AsYjO0UHB@92)}zCmRF9GgbzMqpS=JyIJcY
zrB@^?1A`PR1H)Su28Q`83=DBB3=DcK3=E%{85m}QvK%u5LnAW-Lk%+nLp3u4Lp(DB
zgC{csgCsKp!#^elhWktm45yeF7`8GoFl=FBV5ntcV8~`-VDM&QVDMsMV9;V>VBlh6
zV0gvIz;Kcg655MFAwKbn`sRnho0)hDqpgf$b-+=lJUKtip<Y?R&p*V~N})WXQlTUx
zRUxf3Ia{GTu^7xLNGvW<C`c^HP)JEl%gjqnQAjJw&jm}R=B4PSrlqAOm*f}Og8;<L
zkocs;;#37&h2qqlG-x8rP0UM7Pc70*Ni0d!%lFNUHjD);&dE=YpS(FNSAvAJMkoPW
zgj+C8zUQyBc~WE^+vNR;s+;Q)xf$a?$<LmLfnh!mq}Ya4wEy`T7(VhdFx-I_-NF0}
z3_<)13=;ed4Ey*P7&7=67)<yW7-abv7=A!YyAoao1~Fa+hGS6k3VAj&vOM6P9F@p5
Rd2*8G<jy4h&0R?e%m5MOj=lf@

delta 1778
zcmZp8z}WDBQ7XwG%+puFLeGeSfq_AQfx$C8B7Kce4TIw9jZ%C9oNORr1_lP-$%X>r
zfvixT3Y1<5r5T|#3zRm2(np~*Or9CaSAx<%H_sHv=4VxBU|{H<%&A~s|AB{r;RX)_
z!*L!4hLt=F43l{n7)*H>7&Lhp7&Leo7!-II7#?slFf8C^V3^O%z|halz>veuz~IHr
zz`)7P!0?2Nfng38149581H)%d28IQk3=Gwr3=A=x3=Bq`3=CqN3=9uB7#Q|(Ffh#E
zU|>k+U|?|JU|^8rU|{&q&QQ;Ai=Bbt6gvaMNp=Q?h3pIr6WAFT;@BA&jM*6&#Mv1b
zj<PW@bh0rp#Ii9kc(O4tn6NQ07_%`j7_l)hu(2^Pd}U=|Si#D`(9X)hV8P13@PUPa
zVLuB4gC`3EgD?vN!#!pOhBeF#42{eT45`cv48qI|4A+?$7*;beFidAsU|^_XVqmCb
zVqlPCVqiGW$iQ%%k%3`5BLhP}BLhPp<HU=alQ&qfY%Wv$!#z1bPh#_O-B_mjTrLI%
zL2gL=!GiV}KLf*2eg=j~{0t1P{0t0V_!t<@^D!{2;bUN!#>c?W%*Vjs$j87S%g4ZQ
zgO`C}GA{#zBQFC38!rRHO&$h@6+8?K-8>8o`8*5^-aHHpUOWs8COixbEIbSh%sdPX
zkGUBbPI5CaoZx1VXIRS3z);Q2z);4`z>vVrz+l78z#zm8ag+e}W=56*uFV@P-m%mt
zrZ6xtvNJHeWrw&HmQeQbGce5MXJCluXJByTXJBCAXJEL*$H36S$H3sq$H3qKay>5t
z!#Q3ChCRFt49j^L7`k{F7$SHX7$l)Cp2x$$;KIYepv%L+pu@w!pw7d<pvJ?%z{SJB
z@P(U!;RrWFJ;O?F28J$f28L3oOXIm27&N#U7{s_47`|~aFnr}=V7SV~z_68zfnh2a
z149WH1A{*o1A`(L1H(T~28NrQ3=B&+85r_885k5f85llrFfgp-U|>k(U|{g#U|^8v
zfOzgWI|IW$b_Ry&Adf;*I|IXJUY1TaPz3TQKtXc8g2Lo^wrZ?k{^Uj%;mK=kB{t8q
zO=8--Bycm+<c<*O$#S8ElNb5PZsrXOWdaA`GY$rZNt}=%gyqHO&<NM%XJ9zS$G~uu
zkAY!39|OZoJ_d#YJ_ZI|J_ZJMJ_d&8ybKImc^MeWco`TXc^MeAco`Tpc^Mcqco`U;
z^Dr<R<6&S}%EQ3W!o$Ge%frCH#>2qyl$(KJF*gH4D<}wExEUA(xEUDsaWOE|axpN}
za4|4=aWODRa4|4E<78mi!U=I)FDC<oJ|_diM-B#t%Nz_0(>NFyQaBhG#5fojuCp^R
zTw-TnNMvVVNML7R@MC9Suw-XokYQ(Fc*VxRu$qm5A)AeX!GMi{;TkIg!+ur<hJCCI
z3_Drt85nl3GBC_xWngGyWnc(nWni#oWnhqDWnf@uWnlQk!ocv6g@Iug3j@On76yh6
z76yiP76yiN76t})76t}&76t}376yig%nS^hnHd-+Ff%Y@Gcz!lF*7hQg2b2@7@C+E
z7_6BX7(O#HFx+8eVA#mWz)%k>!WbDg&$Ug)8ac*H`k+`$FG?&ffHUF?@{3ayY!!;~
zlfYtN0;B>YG5KPs<mQeD4>mzi#sEcVB*-2CP<boppj>}|kAWeakAZ=QkAdMNF9XA3
zUIvC#q_~>J!@y9%!@yw9!@%&0n}Oj5Hv_{KZU%<++zbo_+zbp++zbrn+zbp-+zbq#
zxEL7ja4|3(;bLIe#Kpj{k&A(0F&6_vCl>=l6c+<SBo_mNH#DX5aWOF5=47a6m;)_8
zGdLL-Bsm!vj&m?D#Bwk&JY;8J*u&1i;Kk0s@Slx=;UgOZ!xlCMh7xE|_==T*;Uy~r
z!);auhGVP@3?-}#47RKc3^uF`3{0RRl7)dGh=qYcj)j4NorQtn5i<kBRAvT-I8Y72
z%)qdLiGiV$NevPYZy6aF-Y_yS9AIQ%h+<@50EJNmD4d}AYV-LxL&nL4Ir^K=<rpzf
fj&lu}9PcJExx-a!v%H%F$K-i_iklz$$uj}~NH`d?

diff --git a/python/examples/cart_pulling.py b/python/examples/cart_pulling.py
index 8115932..3c17d05 100644
--- a/python/examples/cart_pulling.py
+++ b/python/examples/cart_pulling.py
@@ -10,7 +10,7 @@ from ur_simple_control.optimal_control.crocoddyl_mpc import *
 from ur_simple_control.basics.basics import followKinematicJointTrajP
 from ur_simple_control.util.logging_utils import LogManager
 from ur_simple_control.visualize.visualize import plotFromDict
-from ur_simple_control.clik.clik import getClikArgs, cartesianPathFollowingWithPlanner, controlLoopClik, invKinmQP, dampedPseudoinverse, controlLoopClikArmOnly
+from ur_simple_control.clik.clik import getClikArgs, cartesianPathFollowingWithPlanner, controlLoopClik, invKinmQP, dampedPseudoinverse, controlLoopClikArmOnly, controlLoopClikDualArmsOnly
 import pinocchio as pin
 import crocoddyl
 from functools import partial
@@ -88,7 +88,11 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
     """
 
     q = robot.getQ()
-    T_w_e = robot.getT_w_e()
+    if robot.robot_name != "yumi"
+        T_w_e = robot.getT_w_e()
+    else:
+        T_w_e_l, T_w_e_right = robot.getT_w_e()
+
     # we use binary as string representation (i don't want to deal with python's binary representation).
     # the reason for this is that then we don't have disgusting nested ifs
     # TODO: this has to have named entries for code to be readable
@@ -134,7 +138,12 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
             #controlLoopClik(robot, invKinmQP, i, past_data)
             clikController = partial(dampedPseudoinverse, 1e-3)
             #controlLoopClik(robot, clikController, i, past_data)
-            controlLoopClikArmOnly(robot, clikController, i, past_data)
+            if robot.robot_name != "yumi":
+                controlLoopClikArmOnly(robot, clikController, i, past_data)
+            else:
+                # TODO: DEFINE SENSIBLE TRANSFOR
+                goal_transform = pin.SE3.Identity()
+                controlLoopClikDualArmsOnly(robot, clikController, goal_transform, i, past_data)
 
     # case 2)
     # MASSIVE TODO: 
@@ -207,10 +216,16 @@ def cartPulling(args, robot : RobotManager, goal, path_planner):
 
     log_item = {}
     q = robot.getQ()
-    T_w_e = robot.getT_w_e()
+    if robot.robot_name != "yumi":
+        T_w_e = robot.getT_w_e()
+    else:
+        T_w_e_l, T_w_e_right = robot.getT_w_e()
     log_item['qs'] = q.reshape((robot.model.nq,))
     log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
-    save_past_item = {'path2D_untimed' : T_w_e.translation[:2],
+    #T_base = self.robot_manager.data.oMi[1]
+    # NOTE: why the fuck was the past path defined from the end-effector?????
+    #save_past_item = {'path2D_untimed' : T_w_e.translation[:2],
+    save_past_item = {'path2D_untimed' : q[:2],
                       'priority_register' : ['0','1','1']}
     loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_item, log_item)
     loop_manager.run()
diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp
deleted file mode 100644
index f08ae83e3367ae7983ea04cac5959c3c261c03e9..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 16384
zcmYc?2=nw+u+TGNU|?VnU|?wTi%2gKs$o!E&A^bJUtEx%l2`<i!iRGci?cKH^6)7E
z%hkaQ)X&JzO*PVob8|}bQuGUo5-T!`^$Uvfvr>~wiuFs2;)^qL3vyE9lk@XRit=;x
z3o1)8^7F7r<R<1Nrl%Ga>lIXjEgB_8Ltr!nP(z@!Bu&?Xm%-S`&;TT%tfZ(QEEEc2
zj^fb}7!85Z5Eu=C(GVC7fzc2c4S~@R7!83D5&|U!f(-Qx3=B+A|3*M*Ml||A6o5Mq
z3=9mBX!8G{@>WpsPACmC=Py(~04jbMO2g#;K;?a*;=)h~D8=v_Djy7$Pk>55DTZHA
zc~7YPW~c;|V)zM_hnfEeDgmV!en92@p!z+b0Rof%4wVmr%1=j={|1%!hswW1lm7~p
zhxNEDm>~*b=6`|8!@{Q*P5v`fJ`kdf;Vzo|C#bv^R9=q-!ho6o5h@S!e+8O6)Hnt?
zh&G0!X!20uQED^<MnhmU1V%$(Gz3ONU^E0qLtr!nMnhmU1V%$(h=xF73IhW-0|P@l
zsG}&rzyRz2zu;$J*u>AkP{+@}5X{fOz{AhL@P&_o;UOOb!&N>8hUI(=43&Hg4554s
z3`~3s3<r4`7`E~<Ff8X~VBqCtV0h2Nz|g_Nz~IBfz`(=9z;KY8fuVz&fgzilfgy;S
zfx(=cfkBa*fq|Kuf#E9`1H%_C28Ih<3=CVj7#QYrF)&neF))O2F))O3F)##jF)%oD
zF)$c&F)+O3WMEjt$-t1n$-p4a$-r=)gMp!)gMq=5gMmSngMr~HI|IW8b_Ry^><kPA
z><kQU><kQl*%%mBu`w{@vN15IvoSDyW@TXb#LB?%k(GhrF)IVZRaORuDXa_(39JkZ
z@vICCeyj`(R;&yRe_0q9&ayBt%x7U>C}d$^$Yo(*$YEh%2xMVk@MK|N&}Cs@5MW_o
zc+bqhu#=gAp`V$7A(fedA(okeL5!J!;WZNj!*V7DhC(I=25Tk;2395phHH$Fux<l|
zHzLg_Ac2h3oB~^=@{G*n427cnr2G<v%KTFGqEv;V(!9LPymW>9B8B42+|r!HlFYnx
zB^`y7)U?FXoDy55(jwDTB^`wr29Rk5iABY!MS6)TDe;L#>7}`;c_kX^y1HN!)pZm~
zDhpC=i%W`N%9Tk4U`v!06w*`kQi~FE6kwJVTPY;x=jEj)mt^MW=_nNB<d=ZlsgPQd
zte~I_(M6VBAWwmCT7FS(Vo6DAQG9YvVsWu;Vo^HSuX>J9Z@NIjqu3)gr@#%SSQBhB
zNIwV%gIAU)_$KCM7JvdWzevFuvdRQ4{1j|K9z!)h035O!DXGQDMVSR4Z`vvamF6iF
z=jW!DWPoC*JhLPN8VyQHN^qNX6$%oI5_3~aQj3Ze((;QGG?Mdk3vx0O^GY;x^UG3w
zh*s%^q%yU*I5n>%6FCYLG>S^|6f*M^ic%AEK<Onj50qpeVNjG>Qd*Q(tdIzDO>t@w
zD7s1#GeJqq3T&({4gi)+Pc4CluPe-6$D;INupG#*DXD1+>8T~YnR%JHi8+o%>Cn*9
zv|?ZY$-9;2fl~$~07~+U3QJSLE-1>+$w^Itr1)Zm)QZgF5<P~HjMSo3h0J1wM38q%
zQj5|OlT#H+@<EQzs4UJ*E(ZAv5`%gSiFqjs$wjG&C8-L@8JRgL3g85i3bCpnv8W_7
zxilxS2x^BOgL7$#LVjt9LP<s{L`=81AT>ENEi)MuG$qAg$^101Jaml_%t##uEP=$3
zlb@HaP?B1dt5B9%oSC1;0CrM-UQVS#S!zyxa%M@TE_5{$+^-60@DPD|+O;gTs1lNp
zGK&>LgB0Md0_FVDf`a^_l46B|%)I>M<c!RG1+e+B5+Oei&1nqDrA0-lc_j+PC5a`e
z3=H6yQcx(#NGwsvDlINiNXb`7tw>BR$*EK*2MJc@mnxJe=7EDH7Zlbh`A|KeBnu8J
zP^tnKBnqG)FH0=S%rAw<L}q?ou>v>;6ldn8=YV22H#ada1(HT|6*4kYQo%u*n3tDd
z3Cb>+X}YO7#i_*#`DqF%nQ3XMVD~x(cor)Zr52WE7Nw>rfSsVLkdauHs*sqI12zC=
zF;cF|OHBlMB)=#<F)y<!H3jUz%shqsyi|qc)VvZ<NtKhIoCwObP$QG_Gjmdl6bf>{
z=_WrVRUxq`zceoeYy~v76ms(O3ltz_GAK3XlqXgegB27f=B9#@sX{@1W?l)%2PGM)
z#i<I=^im8;ye0Vx8Hsr*IjL3*AsLy)3Tc_(P{{-3I#51@1SM1l*u{ykECI0?Dy7H3
zP?8U}H7T_iW|u-@X-P(Yk$SO0Vp3*KW=SPD*C?cA<|XDRq@)&SrspXnXXIxlrxq(D
z7J(cF3jTbB#N_1s+}!*Wkgp+52A5x%xuAF`D98szo<e$lVootbNMccXYKcN=F{s=E
znE?xx)V%b}yj0N2JBH+p#Ju!WP{iltKw=4$OF?!(tjH_@C8d&#)XXA8dWJ6cgSZ%E
zJSenMD+*GROHxxn>WfknAyHVGmy%jkTmnjNpcDuy@QOewD<w54u{c#BDK$B<v^bR^
zzbv&Vr&0l2tfp2dBqkM?6eU8-8<5_N#4=EV0~wJFj_G1hKqqGwrz#{CD-`4x7iT8r
zr0OvsD}by*M3GnU%mbGYkn%)Fp#&7G1*t`l2nBh(EI%_PGcTP1UT5cl;|*LMgQHtd
zAtWPJp*S@;KQ9FqO$9}ndC8dtIp7ixoM_5YK{1}5uaH)hpR0gK6^S|N`9+x}8MzAi
zAeR)R7J)(w<Z4iQ1o;WHVp0L244g#MGK)a@9g-#xmVryS%wmO{%%q~kqDoL4l;kVq
zf=ckrDo9o>$;?ev$V*L4Nlig7_!wXffV}*Y)M9WM56)O%7pJF!>cpbT_}t?3Vuj3H
zP|>B3Tb!=IkeLSM=42-6m6l}Y=%r=mrNo1ZIt}%r<eY*^bxkV<s167PQ^}yL5aRFR
zZ>5l!lA@5CUj&XjsN<BOrYaQW7b_H(l%}OAWTwFzP~b$(05i29GdVjal>zKfa3P$Z
zp96O!xVlKpfym~=h0{|@3Nlk5qM#-eT*5CuFEymH0BT%uC0ru4A~`-YFD)NtYhqD4
zBzcyk>cHC;49ajODEMJ*DNrH<=R>d?!O;uK0*OT_u*iaFgc%6do2QUkQIc8&%Kin3
z$=QkNsnCK8QZ*JUlq4oWa|oyu1T_<iGg5P)Ud=7dDakA-%1=%$2HOk?dW8Uph>k*F
zX=-Vzj)G5qa<+~_aYkZMYD#==YHof}B_vQVmhD0!Hzz+mJu@#o9u(8?pz+C1_k}Fo
z#ih3_v$!-dC$lP556*%`FsNbaR+OLXl9^nh18(SqWag#@fSQ}BMLMAJ7uK{3gBel;
zu_hVR(ts8UQ17Lr7M4Pz4u=QQQ%mA=^HWlDVD`fWbQCgDi}Lg06AKE8@+&fP!F3B(
zK^GStP}|78D6^m-wJ0aGxHvS(G*w3-H?s&`H^}tL(%ejlov^e6D$EmeV7%n~0{nrV
z4zZ|MFF!9HQs|VF=clE^d=L`u@8%8(5?r2!835LuS*Qo4<Dn+QObP;ZQ!)#o9)lQJ
zRFaY!pP3I=65`_O>F=2bZNnj{DN0RF%`8hrRu}{k!loKl^*|#oD8$7T+HyyzhRT5o
zEm&CPmF5;yg39o`0+<k@0R<}hGxI=MDK7<5ccmsKXMmDWVtQ&3D1#&_6sLk(86~OU
z${SkZXBLB^6WqHg%1i-O2KlAM49W@xr6u4t3bfiTR>;f))d?^ILG96;l++>}ctMt%
zm<{tksCvrFOinF=G@;_-GxIV_;^XxSD#12`j0Y8d3Pq`TiMf!bP(e{<8K@QqHHQ+D
za#D*ybwP1SVo?di4E1<*24#hS$Pf>Izj((WcV~a!03X*7*LZ(#22lThHWvefK4=g?
zfB`=LdzGJoVI{QxU(3(HkjBrz;K<LwAj%Kv|8L-9U})rHU~uANU=ZeGV0gmI!0?!t
zf#EPO1H(LC28LQ*28L)}1_n`H28MS$3=Ew-3=E+>3=AGT3=DES3=H4785nkQGce5J
zW?(4hW?(SjhPXiw<PJo?c$7670;3@?8UmvsFd71*Aut*OqaiRF0s|TXpvePd3>rl#
z(MVIOE>6u!(<{zQNsWhgp7lWO^IA}kJOw<6l>-{fs#JoE!NZKmOj7`BOe{(-)=NoE
zDou|En^+7Pi3cf5tw>HSC{b`MDJjZKDlJKMEh@?{LP&#@fG}8_Mw$|6^f@yx5j1{=
z%T1)a0b~K#ohTlJTcv^UR7p`Ks@uTYKqHNbc`5OQpuq#Zg3^-sy!`US%n}Wyq|~&c
z)KoMVfvki1CsQFY7u2Bw_p%_WK^-z&ep3ex=z)iu6~LB3x<dH{3YjHnmJkv2nQ00d
zaQDZjWEMdtwG<R>6^u2(<FvS>4K=ZZLUBfJd~$AzUTI!VW?nW@pn?1gwg;vnIVZn3
z6-fcit)Os3a)+(0f-%G%(5wqQI-p}UV7Gy$T|mZzFl6FK0W@r&my()V0Ahd@>4Ap6
zi%a5D5=#;hQ3=ulbrpD&2V^-Y5_9sCv-OHnb5avg+z3~WWSD}j0%VX)FDE}Sr5Iuv
z$de$O^pZ-`h_o#+89WO>N{E1*1H)inpgIdI1abv5O@hW-plK4-O_^y5dHE%fT#BSV
zHMgLoQUfWLaYPX)sUV^VG<lX-k_Zk5<d^}w20EBVAegZzjkbzaumz3ifo55vtzyB$
QuOPo;Q5z4AJw$i{0Jal()Bpeg

diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 5e5ec88..6f831d7 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -590,6 +590,45 @@ def clikCartesianPathIntoJointPath(args, robot, path, \
     np.savetxt("./joint_trajectory.csv", joint_trajectory, delimiter=',', fmt='%.5f')
     return joint_trajectory
 
+def controlLoopClikDualArmsOnly(robot : RobotManager, clik_controller, goal_transform, i, past_data)
+    """
+    controlLoopClikDualArmsOnly
+    ---------------
+    """
+    breakFlag = False
+    log_item = {}
+    q = robot.getQ()
+    T_w_e_left, T_w_e_right = robot.getT_w_e()
+    # 
+    Mgoal_left = robot.Mgoal.act(goal_transform)
+    Mgoal_right = robot.Mgoal.act(goal_transform.inverse())
+
+    SEerror_left = T_w_e_left.actInv(Mgoal_left)
+    SEerror_right = T_w_e_right.actInv(Mgoal_right)
+
+    err_vector_left = pin.log6(SEerror_left).vector 
+    err_vector_right = pin.log6(SEerror_right).vector 
+
+    if (np.linalg.norm(err_vector_left) < robot.args.goal_error) and (np.linalg.norm(err_vector_right) < robot.args.goal_error):
+        breakFlag = True
+    J_left = pin.computeFrameJacobian(robot.model, robot.data, q, robot.l_ee_frame_id)
+    J_left = J_left[:,3:]
+    J_right = pin.computeFrameJacobian(robot.model, robot.data, q, robot.r_ee_frame_id)
+    J_right = J_right[:,3:]
+
+
+    # compute the joint velocities based on controller you passed
+    qd_left = clik_controller(J_left, err_vector_left)
+    qd_right = clik_controller(J_right, err_vector_right)
+    qd = qd_left + qd_right
+    robot.sendQd(qd)
+    
+    log_item['qs'] = q.reshape((robot.model.nq,))
+    log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+    log_item['dqs_cmd'] = qd.reshape((robot.model.nv,))
+    # we're not saving here, but need to respect the API, 
+    # hence the empty dict
+    return breakFlag, {}, log_item
 
 if __name__ == "__main__": 
     args = get_args()
diff --git a/python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp b/python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp
deleted file mode 100644
index c17dbc853afbb73baf10fc8e0ad325f99980e80e..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 32768
zcmYc?2=nw+u+TGNU|?VnU|=W>i%37gTf?BZnt>rbzqlYjC9w!3g%9T@7H4PX<>6BT
zmaBsqsGpIan`)#F=jN2=rRWzFC01k>>lYN|XQd{W6zi83#TRGh7UZPHC+Fvt6y@jW
z7gUyH<mX|L$S)|#%uURJD@-oRPtH$CsmzJbElAcYs07<SN{oiU2o8bLk~CclUIt?$
zLjzDKC@U!{2n&URn4@?!1V%$(Gz3ONU^E0qLtr!nMnhmU1V%$(goHpz0UJX-0|NsS
z)W7jinh}j=gYpy5XqY;7sCYP(J^-a*@~lw48I%@>N<b+F7O1!^l=g#4Kq&@hsJJ|o
zPJ~K8DX8cuH5vk=Aut*OqaiRF0;3@?8UmvsFd71*Aut*OqaiRF0;3^-Ed&x%7#RE*
z7#N;`I?@6R46y$HWPS#QUwjM<kN6lEF7h!j?Bio#Sir}?Fqw~mp_Y$<A&`%O!HSQ8
z!IF=GL6(n!;VUl#Lpv`6LmMvxLn|)>Lnbc+Lm)2$gAOkP!%rRthCMtC3{^Y~4C*`#
z3~D?K47a%%7-n-bFcfk#Fc@<)Fi3MVFx=;2V7SM{z%YS}fuV|vfgyv7fgym4fx(}P
zfx(K4fq|Wif#D7(1H(d228L8l1_mEa1_p0V1_m`w28Q<>3=CH|7#NmuFfbHxFfhb%
zFfeFwFfeFxFfe>zXJEL@&cLvZoq=HjI|D;0I|D-rI|G9^I|G9vI|IW%HU@?VYzz#y
z*%%nsurV;KW@BKeV`E?_V`E^*VPjxOVq;*?Wn*CY&&t4Xo|S=NE-M4WR8|ItI93J*
z4ORvQc2)+4t1Ju*+gTVG+E^GET3HwvJXshRL|GUZUNSQ<>}6(PsAgthaAIa)aAam+
z_|3$?@QR6nVJ{N{Lq8J(g9Q@<0|yfW!#_qyJUj)(1-8f{TTDSgL7_OYEH%C$vA85Y
zB{R81!B(NVmH{LV!Uz_Kr(POgl31#)V5N{(pjVYzlwYh-l%JGeqL-VWlA5EJSEi{0
z*9lgiQdk_HoSTAIhk~*~d8&F*szPFMacOR5Ub;eBX-<wpVsc4oVo7Fxo<c@yQ7VS9
zxQ#4CcSK<^VZ}N5>G7E*sktEEgYAMYE%C|EFHo>mC`c?S$xO`Aa0aa(@$~i$aCU|)
zJpsw<C?po87wag1gF;84I6tQ>wMdfzYz%~>2c|SCisLi$GD~z6N?{C;M!np`ig>Vs
zB9Ie`G{G97N)&9N=IIp_<tOE&=IRxeCKhJ~mn4>CCTm1j7{n@Qp{odim;kj+!BzpP
zOao#G+@uf%Z3RP!T4jZ#(h`O8RE3oMJoOTVjKs23g_6wNRE4zsB88OHlEloMVuhkS
zu*;McGE3BpA(o~n7=hIlD<q|+<rk$Ylw_nTq-7QrmnbCX=jJBnr9cvdvO-B}az<We
za$-(Sr9yE=erZmMLK4`zRE5ku1!!pJfI>SdH94`gI8`ApzZ?>Vpp*o0x`M4jZe}ht
zu!{A9!6EG&92lSh4IPMYGSk2@0txjZ1zTGMrR3tm0wpW3`%#R8F9?Df=9FLI=Hf!w
zyrle!w3HN>kx<(eY!#A=QWHy3p~=MGIY0vx!|<e0VW6Xso}ZWlab|^qf-NW`CFkcQ
zCzhn<C6=UWM1vENUV3UtpoS(?ADEY-p&1L&rlh0<_D3Qp%YZToC`eN)N{SMbOHxx5
z(u(qP!O>7$l30?eP@z$%lTrxLnW&IbnU|QGnOv-plUkOVqoa_Rm!eRXnv<WLS(2Gr
z3`zjdXan1jmtO*k(Zmw4dEgA0qX6@3COiq`L()Z2X`VuEK{7bWBq|i-XXcgYmgMV#
z848(sWvNBQsS4Sdd8xUev|FqPwn7&Kz$|dC0<%CKOi4{sfQF)PfHP_+f<jP7p{O*^
zHl(ODRnrQRN{UiTN{jLol8RCjv)ytM({<3wH5~<59tT?jCSW4b(1I;i!B(NPINBf<
zO%_&+f#l0lb3jE}F{ToAkaUFsGzwuwq7}Mfg~c$1R*=Yssw|9!dIVge7N_Q=1g2=f
zY=cA~jHh6$P*EIh2umL@1E5xw7DLiiVo_0IB`njI7DJ4vz$RG%m4xInNWLMuv;qeN
zqPWV*EG|J<8f|D5tEr%^5DhiDv^ZMVFcuOd&;lyj01|FsbD&}hwhGARfIN$8P6d)V
z$_nL)MY+W#iA5!lQmIIxJhLQ2p`a+WEHl3poK8zYH3`@Ts6(L1SPz^DD-6I=V0K=C
z9*G%JS)n+!L;+N?XC~$-K(ZY;7Z#`H73UWzK%x~??7$PJf~^85?J_8XL=_YYiZb&`
zG?bk4LAfM7H7_|;Au%~4Gqo%=MMt41H8D9OHANvMwHV|HaGj{A1c}YeG*AJOlbM&8
zldhMSUzDqnT2vHYmYQ6WU!<vE1F4eqKxL2~DA&ZN78T_eL5e|yLQwsdnWvYNpKhiR
z><U(*sRuDb0URz+K?Pf=dFiPoA@Sw$sT!JkiOD6Nd1cU6fG@}nh{Kc>k~311v%!S{
zNE8wVxLO*pd<d$2AuSH%kaA1RDNbcTEfJ8aU*y6;!Ac<rlzn^?^Agiji(r+pjzXr6
z0;GkLSdyq|rNF?TtdOam3rZp-C6x-rrQoV75mc{b<d-XeYXyar)Usj)HE@knkeXkR
z1FEh-B^-k?xZDJ#pv0t{N`<7<l9JRSJp~3xEt--KYQtoL3eCh4h4REoh0J1w<kX^)
z#LT>$N`>6g<P3$>#Ny1<B88HC1@J=F+|;}h22e9dAulyG1teUYnyLdTb&FEfixu+n
z!N!#2D`e;8m+L|8PgE$*P0Yzr$SciFN-a{zPgBS&Ni6~=k7AJC#1e(v%=C;BP(7Ol
zvM?zpRiPw5Uk9WgT5sqe>g~i59fgvjN^lMWc>q+CgNpOS5(QHO1CJ_5o=OB&-bEz}
z8Tmz-Rrz_KaMS>KH8DH27~B#7J04UogIo`*EOfxl3y_IWeV`yI$w<vp$jQ%32L(h%
zVxB@tMrK~RCdhjrl?90<8Q|b31{s+IX^-e;=B1=oD8QNs$PJPFJcaz66or(;l0;}k
zWP%zYc_o>7rTL}BIhBwUS*%c=ky?_GTBHDyPRvtCPX)zPszP3BxdO;e24%3zp|u^f
zVWI$TnPe6#loTa`79r;sRVt+A=j7y<XXd4Y0yHllYMwecyc1z98i+=aSHb3H7DIfV
zSfY?tnwJd9&kV}oAWv1uPc8s8g^Dv%QbFOCoLXGW0LeDs!USX0vmVH`@#(2~&`8%S
z$Vtr019>dBAit;t>=`4Mcu+Gg1(f<rG6Fz#vRhGpZa`v5MnGawVlG4+q$)o?*wvUJ
zGYym=^NUJyGL!U5OEPox(lYZ>;)@GXlQq<fGZG6@b1Ky}tr(OQLi}C)trQAMOBC`!
zp;-)WrGt_e#L1ZuHz2jv@SBaK5M(}7JA{I{g#p4<D5)$+EoJ}*3OEq+^K*)!&W1JA
zVM_B#a|<dJ5{nh`3SdIuybW3c4ihNI%*#(s&dAIMsVm6LgP4pN+-Pgb_234@qshSB
zg5DO?L2ezox`HCX4VH%dodaOfPKm{-j(I7V3UE0r7gE^4!yrAi#5Xe!wC>%pC_Nyt
zs2EfbA{ChM!V$Ho3@FM6r8%f91E~M6#>K#}j|;N?A2z>!nxBDTE<Xc9E<Xc<6+Z)m
z5kCWiA#_Y&Hy;DTEItN?3_b=1FFpnaNj?S!2|fmfH@plCt9Th03V9hA3V0b9czGEZ
z{_rp`oabR+Sj5A?FoTDIA(V%KL5PQe;UqT$LoYW2LjgAfg8?@K!!<4jhW$`?>;<`n
z?7r@(#?cTM4S~@R7!85Z5Eu=C(GVC7fngj1pn(-or!5}ZI@T*H&C`I4O&|*^*eYPQ
zj3L7rI<P(*@)$qr2nJ$I5n?i`ENC=C9n?uPa)~d^1NXx~6A|E{6TOn6#Ju7haBn@@
z$|x2x>H(I*GRy)Vg@N_C%2JCm(<(u|<zm>NQ)U&|Ajq5vXgD|*+#d&xKczzTmSn*C
zo~a6{c`3T7X=&h5BK2a0qSUn1BG8}{Xt)G41`HjahmL4vRzU`Vp$6p_DS%8@NK^pz
z3NzC(lR<;+rJx}U@Muqnhi5Q&tVK6BzbrE^UAH_lFD1WRp*XV&<`zWXF*UEav?viY
z-c^#3Sb`X_1@)5i@=HMdN$4mCXd(jS2e2zZkqZei5DSFWQ}E65ltH2gq!5JF@u`JP
zA0dxPfh~m&1%OfmXv7RUBZR%TM#KaWr0X3I?X(Y_=^}7gfI<;6kUCVRWU$8*mKhpI
zQXyj~7#uL5xeHL>kTf|1nF<+XvooOne>x`v!*@<d{~xygzMG$c!IGbWL7Sg}VGADv
zgDoEegAE@8gB%|N!!%w7hGJd@h8H{x4EJ~#7y@}17>;l=FdXJ)VCdpzV94cWU<lx5
zV9?@bV9?}dVBq6sVBq9tV7Si3z%YZ0fguw*x39;=zyKP1VB~_@Ka4A?QLl}Lz-S1J
zhQMeDjE2By2#kin0EYmm@d{d#qYG}%f|ua9=B2nody6P@R=xqwU`3!dFKBiidlMUD
z`3|T@4Da@kw|EEEH;V>$`#{S_AS-D=%cfE^H1&#7!E==wkd8cjb)t?Yq;ZeX4qb~=
zh(kvqOb6JX5IZmy=78k}@5&rdghIM%@P&URsYSV<B_qDzRhuc9Y2f}pXn~_%a(;11
zF@%g(FN#l1jW5VAPF2?~E-3=9v(U?n&(6y)Db|byD*}aEW=d&djvjmv0o*q($q05e
zj?YNUOUX%1N-P4c`2hzL1&+y~x?@svic=wLVJL7GwL>T~4Z6q%oZ8~^5_3}(6m1oh
zDob-Sp$izvjLoFP;?#JgVJ9*oH6COJJTjFPGE=e@GSd`5V>h5>Dyg9P<HVv$$Pxfh
zpn`BwX<lAtUOMrKO+i}$l-x2&Ok<glRb?P6L6}Hu5y^^jYf%%FvVyN;aIj~XD|D>{
zWJO6%C1iCiJl7$H--^NGXE~{9CE)c%Mc~CdiAA~aoI`@QV9AnvPl5XX3z!)grhqo!
z3otOi_UA9>XJAO;XJ9blXJBCGXJEL%$G|X?kAYzZ9|J=$9|Hp)9|OY&UIvD>ybKI!
zybKJEybKKbybKHnc^DXKc^DYXco-O@c^DY3aWgP%=Vo9i<z`^;<7Qys<z`^G#l^rd
zlZ$~NmWzQwoQr|sCnp2L7ET6+C{6|jaZUz?PaF&k^Ent8d^i{wyg3*cez7w!{A6cf
zxW>-Fu#=sEp_rY4L7ts~L5`h);W--v!x=UPhIwoZ3|VXp3}I{x3`%Sa42o<F40l->
z7&fvpFzB)}FzjZ5_;Di(1A_+(1A`U|1H&R_1_o(n1_mi+28M@B3=Bt@7#Nl_F)%bT
zF)%bRF)*YuF)$=BF)-*cF)+MmWMDYJ$iT3lk%6I=k%6I>k%1wCk%0krP64Do2Swdz
zwF+CPIt;2+G7Di?zNA)#N~SYF<v6|T0wN1!SO{S&xj?xHgo&smLCT1<5hF0LS&g-k
z1E~*@DjsF%N?y=Rg+h5^F?cB<XdN=Blve^TS|_G1B*R6JmMzHFP&cS2rlf$@ae~ZJ
zhfX$t*wLBLeFUHxl}uD8fz}_U=9T7xRurbf7IA~txq>#HActl#Y}NfpUH6SzCxez8
z>y>2|mnP<9R;3of=Nv(O1!u@U7>#PBw4%gZ&_=703?(b@>P(b2axEeTL9qyK{t-44
z)R}^s22PAfrhwLRmZTQN=P9Tv7#V<e!5BbVyRhvsa0i3jn^{nrlUM>;91ivx$XpcX
zL7VmPrJ@=j6-F+IHh#PzWFM3sywx9{kqT<UK&Q4qp2{oG%gifDEh^9etx(PchemM;
zBypme1)DI9w$d?-Me5M$8tNEoYC`*QWE*LKX`~KhsXL~xAbTk?ic1ocvo$opfdkz_
zk&~HMT#%TYs$rmK0QEbxcch>NniDP7%TF$dPboo_gG4iEHW<9;4q`kOqrl76A#<nk
zpt(~8EojF8ZXmh}h{?(de*Ph@R*?B&$O3TCOlo<4Q8uVJC<m>&F3K;?Ov_A7Q7F!b
ztcnIV;UOEhz&=!lY$HON@`No51%)|mCK}$N1<iYdR)2#QLxbjiixoioA&N^<6H~zJ
z(m^Y@k<5bZol#Z*EwxWeEG|)~%r8{{&6s88!S$q+CTEu<7J!_bSd?Fy2ipITlUNDe
zHUwE|pOXq&uwR@An+yl-LIW+uhp!K>%r6CPEKJKUN`@?}2d$rndJwekJF^7hPs}9%
z3P?`LgzaI=Oar^LC>6Ym4YE`}GZ*B<%)GSxBGA63{5&178t4={EQFwTLJR;o2^1x{
zsi`F(%_*sgDLI*Wsc^r6YmeO2#Jpm#SrCiz6>{>+!As>+VG9!!(uz_GOH=cbD?!N(
zJOvN6C_fFfwE?pG1(c*A+p*vlAOa4u^D;R<x1bd4mi#<WJf=c+hC%yvpw;-G^r`?#
z?U{M$P&**W%*oST!7(^E)Hfi+)87v)1+ox(ZiXaw$i5_S>IJRVk49f30j-~u6*57a
z4N7xTKwA=FfsmYEP^k;jo2rmnmReK^-ZTK-UQiBNL6Dr72M)88e6UkMsTJg$_{5^*
zoYcJZk_-iH@SZ#5Ei)j~;<39VI#WSc!7x@q7nG_?a4Lej7-Sm=gEz0h?FWe<%0#eh
zAiEfB6*A$9GSk2{0+KuJKyg~E2dc8;OY-B9OaKqH6s4xbr(_nFfEG_cigj>(4ywV6
z67$kiHFS-16m$(0v=nku^EB{yQ4^$06Ji9CC5a^&@i1rV8iL&kb~+*;F~bpmFkva4
zK(UpSs^G6-sEM56{5A44!BLv2kempL{G?QcB=9za)D+0rg0g~%rUH1sA1raEC>Xhb
zgHRzeuOwd~F;5}b)fj2J2G|G63dWiWS^1fuy(XZ&IiOu)AOjL}ic%9(D#1xVMWHw~
zIX^E2yp;elGq0>*qzTG%#U(|h$t56Fpll1uD)8N|pbf$>#Zdj3#R^%a#gNS<$)!a_
zsd*&|px9I|Rsb#ffT@P;mIdht6_p^@fWjHH00z9n0kpXSv~5QL*$9P#{NhZ|<`G!Q
zWT>eC+Nc3<Zi2VHfz5$zzsv{ih=t_<kRu2sDLwG|e|-i9hSQ)u=K>5236PE~!!mvb
zhFSa!4E6jB45j=G43Yc{3=#Yc414$>>jC=t7#MQ-7#N)S7#N)R7#KeBGB9l9Wnk#!
zWngIJWnjqWWnhSejte;OGB9ZKGBEJ*GBAALVPJUA!@zKWhk;=~4+BF34+BFY4+BF2
z4+Dcf4+8@m4+Fy=(0%}J28O-RGXi3u>j>U+F)*y)VqhrbVqhrXVqgg6Vqmb~Vqh@m
zVqoCsVqiGS$-pq5lYt?ElYzm3lY!wc2Lr=i4hDuj91ILS91IM;&@lyR4hDuF><kPK
z*clk6u`@7CWM^QAVrO8mXJ=p#WM^Qw%f`TPl8u33DjNgC6gCEi05%4Or>qPNO{@$I
zjjRj|$*c?vk60KOidYyJ3RxH!ELa#Aq*)jk{xLH!++b#4xX#SLu!os}VLLMeLn|`_
zgBvpggE%t-gBUXd!$l?rhLcPT3`>|87-m7o7|fX%7}%K@7_KlfFq~n8#KkF4d_d!Z
zSPE1Gfkx>;G^n11RZ9wlwhcoP=^$H~0`di!V=08@Gr(iq&~3Fk3ZU)UpiDG6TS2#d
z#L8q2Xrvcq78Ilw1%M_ih?tsyk6}~6I<kgx;0_>S5M3F$=dA}HIL=JW(L<T8AvFw;
z#x2OOf#^Y!U{~X4s8!%5%IJ`Tf`S4W>kC1}9=H_)Vt_D_Lj>4{9ngjeNE{&mSq|$1
zx`uiBC^-7LD0nyqD}?wfIJqhWx%&Etxw<I01^N3bc(?|+LfUK~U7*qw)Gh;^s8W)F
zK9~UQC?i^-umJ>+RuEQ(?x0jiR4{e{4XHuaEi2@MwxNRNp;Ez3KvesTT#&{tGILW?
zpoKihEr^xbItrj;Sinsqq}9R*F-X@1x!V8<e#nqE^bCzkpUm9M63}{XNMje`=ES@d
zaQ|ApSRu0n9)qA`RG|GE&<^eF%)FFD1<0IFaXwf*Xel<h+mn-8TnyPB4Bz05WK4NR
zzCv<hQED+{DlQdvpa*#0IoLo*D-q-vR6j%f1nZ80;{&=*9d*RT2x+_pWI}vFehz%l
z2C75B7P9pk?l6#b;EoXJFb>#x2OvQZ&P)TV1`SFnrR3+ODnZu)faE}!#KSy5T0j^v
zu@71fPV%|{kP#q^FoxLm0U%u<3`rB9VZXo>4P0j=A<_m&3kX9;jh#b+e5@4G^GiTG
z{y_l^8lM4;6l5xtgU{v3%`Z!Zp8NpP2Ewqxyp+_W()4(+LB){H98?z6u?2;*Uw$6a
zY&S#{oLgY68_+Q`E{P?H&|MO!lbImbATlmYiw1bZf{sEa#7m&5;L3tj@X#Pki6+Qy
z@Q!UgH|UP{kjjEoXnZS!(kyrvKd9qV44UkQcH_Y52sV}lR#5=jy$;R_CHV?PsmX~s
z$)F)i(B=e4wucM|fVyX?&@(qcu7){84-_lV!Pv0O#Ndp?qSO@M)ZF}{N{s^O7$xYa
zC$N{$w`YJ|1D3(HJEIU;B2WWrAZTC=Jj{o7v=%IdfwqW1HZmY*707`Xp!kC|+LJ03
zKu0Hmx}B)ow9z6hzX;Ux1&v*S#sxqfN>t?v<r$gD84AS(nI*;GQWF$H(7_?FA0d3`
zK?0!ed1@x82MQ{o65&S{V3?8!KCK009YjAUrivk@F!Ej#jbcscsR^KtYGx5Q`Dqku
zLV6kCCOs|!>^N}i9eoKB(%2nX5h%z&tC7Hp@hn|J(g&>&z?m1v2_gzskV8R02M3@Y
z6$L2>VW~{PN&z**(T^U1gbFBbp=BKMZX~b=P%|E~G}0a>&<t2<9weh9E1}+gCeZwU
zBm)D(MNofJfPn#ae*a;928Koa3=9SQ3=Bd13=E3=3=9hV3=B{B7#KG2F)+07F);Y@
zF)%3dF)-ZcWnfsw%fPUdmw~~Wmw`cxmx19g4+Fym9tMVEJPZu$co-N$co-OLc^DXM
zco-P0c^DYBa5FGub2Bg`b2Bj5aWgQ;a5FG`;$mR<$i=|0kc)vK0ou1W;9_8q;$mRn
z;9_9-06OQ7lY!wDCj-M?P6mecoD2+EoD2*qoD2;AIT#qOaWF6}hn@l0#=*dl#=*eg
z#=*eA&cVR&f}Me30XqXj1v>*n7&`-lBRd0wGCKpq3pNIZoooyY&1?(|4r~kzyle~%
zJZuaMUs)L#?yxd2tYu|jC}(A0Fkxk2U<6%az{<dI5wv!Ig@K`%g@M79g@NHOGXujp
zW(J0SW(I~dW(Ec&W(J0PObiS&nHU(RFflO1FflNwFflN^VT6S91yEQM+o>m29VpF#
z&Nj&`Rsc-~6oZbtfSh3hPIk(q1BgBa)S<D|6!6F!xF7(v_dx|aav`9gpbXJRrhQ<|
zAk)F~U63XWY<31NtOGh0335Q824wuRBm=Bo6KVPga*PkSZplwmfV4Fg3=}jn^-}df
z1r$suKMi@@F0lwaf>;8n^58ZS)5Zk50JEW~3^pDzSO{wGfe+FF74^^&Mvxs~UsKo`
z1$8wcQy0+Li6qcLGpUflfAC~bZfZH`utk)qFlhS@>~=_KV{0&irLngfLCy!Y7{N*)
zr6r=-4Hks89l<G-=wty75@pQP2W>!r7gK=}AE-@FdZNQ9r{NVefrI5ho`6;FWUfy|
zHG-5jD7XsK1GRLBYJbB0gxo&I*EWaNS#W*BC@esM4sJecD3RI@$K3`;3QI)$n{eA3
zGQA0zn=3BK%*jyzPm>nsE5JK@(9{o_#RMgCP$q-SWrBF%BoCWDz&>-Jke^qcSd;>3
zdeN>K4hmH~IU4E<@F83XFMy^;KnGb7t|=h76r>Z@00Sp}+|7KDdr%wYnA4A-q8#cl
z@Jwu4YHCigLN4fd)8zc3qEyfnP`*NLDlF_^2a16e=R;bxNP`ndg>5NlS{yXBlaiX0
zSdyBUl#{9lDQsaq9k3aobH*}2M*x<TCgvdKdK5riLGUamD2_p%Rwyn_Pfsl_NlgK}
z7u0<z1dTp`d86GGXn%9icUM4(k|>;BlvrE<UXiZ@KI|_(4^*Fm4sHe?YY09g5$7%p
zx^;M<r`92NOOSdl$imd?t$>0LHD4pT)*v}ZCmX~9VeqyQJf~QL^AAWN2vhU401`T-
zkik&+Toh!Y0u<dP8L+YrHpl=vZn22OQ(HkkLoD8dm=50{LX$J5$sF>A)rF%+Q&aC~
QYRt0*k%vG)6Q7z40BHkbZ2$lO

diff --git a/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp b/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp
deleted file mode 100644
index d6e5ce7afad91b54068d7d5c3f91495151cf07b2..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 69632
zcmYc?2=nw+u+TGNU|?VnU|{fzj7VQERKuXSnt>rbzqlYjC9w!3g%9T@7H4PX<>6BT
zmaBsqsGpIan`)#F=jN2=rRWzFC01k>>lYN|XQd{W6zi83#TRGh7UZPHC+Fvt6y@jW
z7gUyH<mX|L$S)|#%uURJD@-oRPtH$CsmzH-lhG@v1iNIE7!83D76PRuX}T7?48}%=
z1|Xj+D=8`n3x$H1qj)p~MnhmU1V%$(Gz3ONU^E0qLtr!nMnhnPgg{9FKSMnO0|OJ(
zzXDL25sl`B^39;MAe4rw<A?G^ptK8=hRO3m`QlL89ZJLGS)qJIDD4fUVe&9{=|ROO
zL1~yg8<ZadrDs8Dm^?d_?**m5L1~yg2b2%%5jj95pcDfSRGt}1PlifBDFzm(xF3|B
z29<zP44hDLSU5d_N<b+FE~va5lztDDfKm+HP;oaX4T~_CJj`9Nc(H&+C`=xvZIm7j
zfzc2c4S~@R7!85Z5Eu=C(GVC7fzc2c4S~@R7?~lEn8Lu2&&$AY8q~oT051Um2gF-`
z28OHr3=Bv385q*}85qL(85jil85l0{F)-}lV_;ar$G|X+kAWeJkAWeWkAWeGkAZ=Q
zkAdMO)T~}!28IA$1_m8o1_lLQ28LZc3=FkA3=Ba$3=G0N3=9vs85j<6GcdGrGcb5?
zGcf$-VqiGQ#lSFyi-EzEi-Ca=)FI|#V7SW3z%YZ8fgzcbfkBs(fq{jSf#C=T14BCp
z14AGO0|P$?1H&bD28Jc<3=H+`3=Br>3=Ch`7#P;GF)(DaF)&21F);A6F)&<WWnfs!
z%D~XW%D_;@%D|w)%D`}yg@K`pg@M6>g@Hkpg@NG*GXukAW(J0}%nS_C%nS^A%nS^#
znHU(hF)=VSGBGgNF)=W(FflM3U}RwEW@KRSWMp9QfOW7H6ciLnQj2mk^AdBM^NUM-
z^HWlD^b%82KpYLFveX<mkd%^+LRwK`ZfaO+PJVJ`NhL^DN5L@FSd)Q(38g`)=}I~Z
zAc}ZR=|!0Z1*t^=`NgS9ItuCei8&!fiOJcSdFe1Sj35>(EBN_`xLPR`XXKaWq~xiW
zD3oNR7NsgAr79#Uq=8(nPzH6ff~|sq0!Y#x>||wy#Jm)R#FP|}4tNkKB<B~G=qQwD
zWF}`QWEO+vQZkE667!N%6-x5Ke2{a%MiiCi<w2c<7HlBjqIp^u8dSJ7RX{=tM8SMX
zSQ7~$RZt2Jo#IrmCyNy_^PnDrItd~S_8CT8E7&T)SEuNKTnF}WP-<~zN@-$_MsZ1E
zNh(?rfk{Fl3Y1DfxF9o6FDNxFwJ0?&In@o6r1Zl5gM3^tRr%(ZWaj5-<Q3>ur55EE
zYnW+j>VO>wG6sZ;@{{sQ^m4%oHa)cjY?x<?hEhp>evW|>nqr6x!TM1=2XiHg2VsI>
zPlC;brF#vq3CbYr!8j>3IkB`jRiPv!u|&OCAyJ_qv8Y5LKMkBvAa;V&S$V2LN<Jug
zX6NOXLlb6Jer8^YLP36UCdi}3kPuJ?$AW^1CR9;sDk1@A7AqJiXe1|s%t^^CN=+`w
zsZ>bIFUl>=Ni0cKfS3nM?vStorB&ptgW^s6@feVkn4FrMnpXk~4TuB5-d0dh0DDd^
zuMAZbWS?GgenF)MTKJO`S<2wx%S-{~z{EUILQPFiEdmE5G@#2f5=#^k3o;eT6Z1-n
z!I1)Un6g5Mzl*<>LUKlGa<)PyC@M0GK_-=Cq(TZYh4NH|#N?9F#GIT;umK7hsi_Jj
znFXkR0s9aVjNl}L5g;gm2@54ent`i_B^r&K{QPW%L{KgS5%~(msi_JX`Q@O3B{@I0
zATuXbM<F*cJ5>R)WK1C`GcP4GFTGeHwImthHgL!krGk=Lo<eFxB50u*D7mFpq=J<q
za;ZXQo<eedPHsW6LQ!f#J~)V!K{lo2D-`GJC{*T`D&(c6rhtUN>(xL}t&pft1kKan
zd<9Bf;Is`k4YoW@4{8`BoWTw*$;d2LNXyU3$u9?If|3lFB2a;<V63TtT#ABHiGr@3
zLa?ibv8F<5QBi&o*hpB>hZ3+jQ)vY($RR-k=c4F_sR2hzFsPV<6;>4nI%tU)n)i$}
z6)NDtprDaomRgjUlcSKBn_rp-&fB^9W#Ehfu@_X<gWQN>El2=tCCq-9t)(ylxMoNZ
zilQ5)25f>eWEmmM0C1{==R2sA4K)=?k(>-p+$H%%;Iaf_5k?)4Vj<kQ!KJwniy*q7
zH3M9lC{4-=X_-aEB?`t0iAAZ96a=ljp#cug$k5f13Pq{upyDetKM$M)ltG!ZJToT;
zRD~Cp=H_PRr7M^yq-3Un>x>exmx>v{Y9Zdt%+Et{mLn)<gGy9z=(&~VK&vN+kCed)
zKQ}WyqXd%mOY#*E$sbfiq~?`m7J&;0kjap2P?DdokeQOL04jFWixrAe!9_=2YI16E
zabi&=xLE;q5+Z*>voJJ|f-*;u0w|a0D5O^CDI_LC3Ph+TP*bF|AO%#4gPf3@UsRM@
zT#ygR(kZD$nPrJ3sl}kIY5>)dmJc#6RUtP&DKjTkAt|vqRY4a-WhR5&mQ`9@qEM6y
zN~++(v{C`Y1n2S0Jg^8z3)C=3MounD1vwxQDX<YC3<(E~{337|q?V-?!NL=&IX|yB
zGbOc10WJ7*@)Juk^U|FXi&H}ia=@t}Bee(~*~$62xggJH=7Cx#p1}$w`9+1LsS03+
zfs;B)+=4T(Z)r|RW>S7ir3NGn!A?VU4Y*9oEKw-UOGzz)cp^0gRE8l1mwK^6r7olz
zg(p6Ufhmb4i3+v~5DwUMJxJKNB$gy<Ky5;nO-)P7FDmiL%*`x8(}yghrC^|E3ig;X
zIhr*Ri_(kr5|fitb5e^EK?z(zAL>S5$B1}GXJ=O**C59bPk%qC6H_ZPOJL3|D9X$$
zK@KoIB&R{uL)+4zgbu4VK>=B;keFAgP@I}qoT{UcR9d2tlL~6<r00WUGqVKZB~UGt
zn^=*VTbiqo2&xBCQxs4f3snXQNmx85<(I<hZBV5Up95*_#}_Hsf}8zdO_^y>e}aQ8
zJ})shRl(L)L8-DdH&e-qAtg0U0b;5%sD|?N_IC~d2bK;*n}U@>5U8o>o0ylFo?4`%
z04n)GwkcRCfSRMhuEv^HkP;;`w;;c$L?JgbH@-MOr!2Lom?1L_R6gVvmE>e5>6Mmb
z=IEtm=B30J7o;X@s3R#<*R*1QYKKrT^)S(r%-mEM8(J#FLp%gZZwzTg`MC-hi^uiK
zGK)(Sb26(^5iF>C6bf?kOWca`b6ql%OK@pVN-WMyF4lukFpZFU+B-8ZH8-&&GuaE&
z4h<<v%nE=xFR>`SAhD=86_=^GkdQ2f8JM10;+vTVTFdTOlpX-mS)`)?t+#yg^9$f1
zj~ov$%i#SAm_R{hUVd_NMrOW3VlgO!VB&eDxdoLVvAhBXQ2&1#4+FzD&;+*t0|RXS
z_!K__!vuZ?hERS61`~b;23~#!hNFB84554s4Dx&o4D5Ui43~Ks7?$udFckALFvRjQ
zFnI7XFj(+1F!1q0%=-#56FizQN{oiUXb6mkz-S1JhQMeDjE2By2#kinXb6mkz(9on
ztg8&_O6nEoq^1^V80r~7dhZ~)l+5CSoWx2!C=Kcy27|k?8j#*BL}hsfXv8R_s5I3I
z+`WdX)=Mi&EmW{oFhmh70u2e-D(Iq!Dj?4W`KA_UBqx@*K<w5?t$_4uA$CEA9h4H2
zGcr@lQd2-<l%+W(O3<FUUV3UtNPKyGs)i<1IZ~HaqX;%lq@w^H`pApV&dV<;R?t>3
zgor@&z?JD0<QHhDr<ABe#Bug=k;evf6yVw+x<J7Q?~&?(y4cS7d1a|Z>7e;4jhy^+
zJ<x!5v5o?WSx}Uk4rQhwnCV4{DNxbklKcWVGZn;4%S`voOG&M;g-&2%+UM^aoS#z)
z8s7n1R{_;uTCAf0*KDW*n(-{R1w|sl-8uQ`3bqQ6s0IbCb7D?TQetv;v4&=}VJtXW
zGt(ep2~h+dMM=u9NJ~j6P=X`|Wq5c*s}~ll$AZSQVDSJIQB}||(#k8)E6CJT&{Y64
zz-EJoZu9d%b1L~o*$U;EB^jA{3c3ZE3c3mfnF^q>Ljzr-f=uv8YGPhxZhlcJ#PM)D
zb)c?SuvNf5mIE0c(ggSa)fgBUsu-dB|DjnD)c>Es&%ltv&%lt(&%j{8&%p4DkAYzi
z9|J=h9|J=y9|MCd9|OY!UIvC$ybKH(ybKK5ybKIlybKI~c^DY(@h~vV=3!t+<6&S(
z<zZlu;$dL;$IZZSf}4S%j+=qOkeh+wAr}L~F)jv%d0Y$(rCbaQQCtiRMqCUGd|V6+
zk2o0^c5*T>WO6bvsB<zfJmO$rSjoY_5XHg3z{A17aEP6OVGcV3gDX1&!#_3#hQDkK
z41d@d7%JEp7?Ri+7<||m7`)jS7=ExaFx+NkVA#dVz);W1z+l44!0?=ff#Deo14Azh
z1A`w61A{LM1A`9>0|P4y1H*G>28Nx?3=G-K3=D?M3=C4t3=IF67#L15F)++xVqgek
zVqlPAVqjopVqkd5$iQ%gk%3`9BLhP<BP0x`gTfIShIl9i1<1%OIMwFo6_*qxX6BU?
zL)HtxxEe|o@k#j=pkoc73Y2sdkhzdZ2blrG6{Sg71ap!gvu`j}AR0c)ql0h>NF0R0
z<HZOq$X241Sk7>lgQo~!vn`NmI*>uha0M7%E5-C$DUweyeNhA&dW6nWBMrGKD1e58
z4Kda08Y*Z(#x1dE0_#Ee5ad=6hEFOH4isd^k{u}cG(yMvAhSrYavm~>jyzcmnkdOE
zRsc^^6qn?KCR}oI!1EGFfe$WLuoPLz#f9+lN&&<{Oy`1!o{JSq!ILGRIfcwTg^a|a
z6a^$>6^ipgvrH+V<s6A643ME;P&i^tl0g!7afyaf4rp*6G!LevqX1_kt$+b(hl`_y
z4|G)xJe@((sX_&K!oV@P1T?#onV+Wv6@jVKL^upxDT)OM6Trziurx6RG(V8+lvq@h
znOdZglwX>c0$pAJawfzum@Ozq!34nufhSvF@dz3-2fG_S^$0QwVl;{o2-i4)?bWEr
zNzzfMC{5B)fVWk!L@8P=1WpMViFqkGsY!`N@I?~f1Oc1i2hB}@)PWbYzzUyY-ICIx
zywnukl6>9#B+!v8nPsVv*|*fZ6y4OcwA5tK%qXOFkeQc`6v(AX3br7ZMjKi|YD<t8
zD{{c1IZ1F)T+<QYc`YS{{32-f1?POkYz0`lD8CrDirma1yz<cb4v32(>z6<?ilC-I
zX-*1g!qiAXO97VGKtZEWo>&Q*bp)*;08PfGq^4!&rKW)AD`6A#1)$-7PzHh~Lj}lO
z9B5V!G#3e54**{*kdz9V>H$d=gC=5fQuESFG8FQY$};myi*qU=rIWG(cs?UP54>s$
zVg+=yQ9fve7R>TQ$Pz2qq7~S}t2EF$n#7!()EtG9qQtyn(5y^q3V6QBP@yz06Fd#C
z1ZoB=L8<{zagqw3xJ(8u04pg^P0dp%DbH5`TVJe@pQn(hkeis7nU<deo(x1xG-MVl
zK!XOF!a&}G`#b~W%6w4xMpx)m>Xc-pmL$e1z?S}GmVg$QKqpm867$ki^GXzo3lft-
z6S7d_GV{Q5XsKnXc_lgupd<wf-ip#B@D#bBLSAWZQfiR`Xqq`EKQCQDR~M=gW=VOf
zLOE!K8z@rqAO#SJS(2y&N^jtTM<KZcJh5I}0$B-^Qwa;?l6;5`&{TSIez6AF1DZO;
znR!sA0?1*Yr7|FE67xz@Qy?0OVUby!30nUK@o7<NK~ZXPDp*xwu>v?qu$mgH19D<f
zDkymJps@h;7ASa;reZ-V5)}$^67v#^AdZJQy*yPRBe5)1ArI;Wa4e$fhNuPQRd9}j
zR-3RS1ukS`5%nyxno{T@L+}DDutJE3K@npMYS|{|=OrhWq~;}-q-sQi-43z_qlzZR
zuAKaG@PbARyTIj3UV$E_UCPu0V7DtP=z<nJf+l8*K^eX@F$bkuEXzzptRDio2ikmt
zPWOScAE>rPGpr;ZvM56Vq(2E#=_sU?<|TvrFp#1b)WJ~DFe%9ZtAtBJVufU1LhXmF
z-a+w!f-*#oWFxU@fRwX%+617LKWM8wp^H3~!8wtl#iiih6sQ3MF5*E2X=a`RXq_Kq
zsg$w;L@pIv`xTeqT+Rs&kkN%z;B(dVP}(=p32Imi2eoxW<Aqg_`U+eXf~uZ^)Vvf>
z{RAr63KB~)z{v-;3JO{?p+r7;E36=;8#rab42LbTGJ{v16fdy?2P8_H5Zsc)yKDzk
z9YQLZa!`u_v<9dcTsOj-?WrjW<*DjLsS4@&`6=N3e?Dj(J!pfv00V;}w9~qkpMhZ}
zwC}IW&%p42kAYzV9|J=-9|J=d9|J=e9|Hp)9|OZlUIvB==={GaF9QP)F9X9z9tMV!
zJPZs=c^DWfc^DW1K<fZ_7#Q|(GcZ(gGcY)CGcbJQVqjRz#lR5B#lT?9#lUcflYwC>
zCj-M2P6md_oD2;7oD2*bI2ahdaWF7^<zQe~#KFMO%E7>p!ok3x$-%&IkDY;GB0B@a
z1a<}nb9M%XpKJ^ayVw{QcCs-rG_o-;Sh6uN2(mFSTx4Znn9s_<Fprgip@EfwA&8ZM
zfrXWU;S>u4!)6u+hUqK}49P4E48|-B4DXp47`8GqFwA3SU}$1yV8~@=V2EI5U=U$u
zU=U_zU=U(vU^u|Uz|g|Pz~Ilsz@W#(!0?fgfnhTv149EN14AJr149HO149xcq{IVN
zaUcvHbjwdJh)*ejBpi?+2%|0S1qp%UTnAFAfQo$`1@xtZ;BpA06oe5YY%Z{ceVK_l
zpsquH9=Pw}R+O6RnOEjgnU|QGnOuzMnnR5RD?&+Qo_Qsy>7dcd6eMF@OLJ0-AexEj
zb7X>-uqohil3RHS%t??|hcc*AEC=;3Q=m>&&;TvvEd{lqN{c})E=MOvO>k0%FEdO{
z0SAV%f?sL{XtO~oylo9x#-xy#2MS5V@<Y&KKd8sSB?GKZQUDK#>M4X|q$2fV;aO&M
zU<MHskfsJG?ZGf|LvVCp2Am>6K?ACpC>@vq8>XzFt7`{dZ3`X~hK{y?@(c3%I(6{)
zjUrSBWHV2udagoVeu+YHX;G>|c}6Oz!wBv_K}W4ii$QB)GeAwBl8k%>P%jaxAF>t|
z+%HYfhj#m63ys0uTBy3ryyBA7MDW^oSe65=8qQHj%CA6aP-<l6r51t~;FVM&i$hWq
zY#^efs8Ruv*Ae9!jxk0=2@A`^pfM$IX-3*u6SO!WQ#ZK72-1sEgn|UXPJuO(U`->)
zh!Ln<Edwt+9UZAc%$I=rxC#VEsz5o5hT}#wHhy$i3bG$?bXW>nevS@HjpSh|@FoCI
zS2eLHSG^dt=>#;!2i}#K4{GryDx{U><R}#7mw;;vP}N<YSg8ZrH=vN10~*RO0*zXN
z=6|8-31k?gOAGEufW`w$^T3r9XoDJP)2;$&S3)^x*B@vHph9kHVjgTC4y1De+X<Wu
z9wr438m8x$lz@hSGg1}6O@hq4w0y|8G}sosoFs5-8p0_}g0<*Dj)4zVfw#34K}Lr`
zeuK_<fQGuj3jrV%I>Z~;29d}UgP1p<!*viRkm`JhF6i!1<Q9_xb%*9a>;FM#=S>35
zTMIBSz|PP6#?Qd8ou7fBhM$4K4s>W8KLf)VJ_d%p(7peQ_!t-x`4|{fp?d*N@iH*<
z@G>y?@G>yi@-i@}@G>w+@iH*n;bCBy$HTx7%frCn$HTy2%frB6%)`K7#KXW~$iu)O
z$iu*}otuGSF*gIlB5nqTRBi?a3vLDmWo`zB7hDVsH@O%X4skIsv~V#nIB_vBsB<we
zuyHXkT;XJ3*v84g(8|fc5W&g7pbcFkaFBz6A&Y~7!H0u^;UhZ(!$WokhFR<k48`mW
z3});M45sW146^JD44>E-81Aq!Fsxx?U|7w@z%ZSSfuVqnfk6j)Uch@+28Q#j3=C6P
z85qi;X9kqAGBA{|GB6mjGB9YeGBB{SGB7-5VPKfS!oU#0!oa}B!ocv4nStRUGXukZ
zW(J00W(EdZW(EdsW(J1qObiUym>3vVGchpaF)=WJ&gbK2VqiGW$iM&!`-!0N2ammg
znjoM7QBWh2xK<^6)*8~9hI3JL!<wStcIm*5*g*=YLAk3DT!f*78}*0WKoP8ryrT{@
zJ^`9I0~P2=pbcUrpq+jSxrr6J&|Q2b`3eT0U3Z`o72Ms1Oin@yZ16aztwJ<-0a&bp
z7Gz-(bdeXh90ALK79iz=Hu@vhih(7Sl$5|Uq{|4h0c@aeXmAK<rVcbWsE`R-X$2ZM
z%q+<Ox6^|ieO(p6Ql-VIpczx>Bw<D>WGDq}VkUG#4|M87NoFEwB&Q@FvQnWWA2jJu
z3E9J)s*qe-R0NvsgYDsm)F6-^CFC%Gk__msXi(izP?VpPlbQ=&8Ksb@02&?2NmW3Z
zmWHo{f*7TXAAp?;u@1ak%GEU>u_VI{ybU`uFWuic0IUKORA`&zFjXS2ic`ScF$dZq
z2dV3dQb9B9h;?(YHCte-;O<wjRj39nAXBhXC@j`dP%kT1SFlnj12I!dKuo03#agh|
zvSQHedvUZC+M<y{=wy5$Lf#6Tn4lAVd4-V4`U;SIUV&a>QBh(gbit!u1$5m5c+5Mo
zC>7L5fp*m)+p3G<^SI@y3gCeXaMJ-cqX(G`ha51VtdO1x+EWYOh@70N0O=irCa6Kn
z4DyRAA?27dWRyY=Old$DaX{BWg2g~{&sIu0pomw{1*NB=5*-Dl;?(5)yp&=kh#{c)
z09$ZM)dMA0=q^sM_9CdspoM{;MS-RU2099Ei8-L%nTDylmJkDqOF#!vpc(-V=2B2F
zK@EZ?NWH?+#Ntd)iIJJC5nW*b%8Y0#LLjDs!x*Xp7Q`?^LO=^MA<AHFG0?zGDtPR(
zI6pTPw2GrRGbtw(Jh`8nT9R5+3|@%<IbpyPI_d+C9<WoDL4(MU6@g$rs04z#t2ng;
zvAQuDX&a^<OgJpHC@H@<RYOw;dAJEK?USFLo?4`#37r|nGCB;J>4Ywd#Az|ocn@@~
zC6cw6l7uZqUj_pi(1Pyc)(1_IC#Nc;<`k!vgB!*9&}sSd{G#k)1r6|um`u<z08n9C
z3fk=rjSxt%q1F)KmBF3``2{5!&^Sa_j2y0zg~Y*u0jP=*L8D-c63`I!&KMeq4QVWH
zb;_@Bb8$g)02cGm15qOr-doE|NljEJ$;gNL6x{w#$_GujfesA<ZI%V^o(~LA0B!!w
z%+D(ZEq2ND2@C=)kAMwKmKWvcrGt~CvI1B)<iM9==wgNR)V$Q9%wz@l*kNXBF?b*}
zIWZ4BZJU_~Dc(Wn$bd_L^wjdq;#7sg0;r)13cBENQ79Lb3KT%2jQNlwe30rCNW>OH
zx;04g1zpVx_8_Qogs21Wl?F4x^#$5SX&nV<Isk2v*0f@P4h195R)U^;kdg|UudFZt
z_XRT*AcxW<rGkSawJ0?OrLKS)3t6fI;~=jg56;NX2Q?l7AT^{0Xth~1bchq&@&gq~
z;2HzkKG1=m*aVh_MJMQR9neub;AKwW8XsyK()0*KEhycBuz{Wdq&WiOfG|;$TNs14
zI>>z;aIAsM0%1fcfM;F_>r@xa60i?ZvIC*HE|9&b-UNpo)#mbO;alX`Cdso{3IMdC
z8zmt1O5?%n{E(XvnA1P-C`8u_Uaf<%00`Lt@ETo6Qpb!3DkV3#^FTHXR0>0mF%m-+
zO)HUs3imZ+{XY`}1A`l=|0%%006QNawEn+?pMk-fpMl{I9|OZeJ_d$3J_ZIaJ_ZIE
zJ_d$wybKKIc^MdLco`U~c^Me|co`V<c^Meq@GvlJf}Ztf#lyg0$-}_#j+=qu5;p_G
z9Bu}NG;Rh4Np8qle@D0&7!GqWF!XaVFt~FuFfeg3Fl^*xV3@|qz>vquz~IWsz@Wv+
zz`(@Gz;KU)f#EI(1H(oR28Ip}1_lez+5mP2hK=kD3>EAQ44Ui=47b@B7$!o`{S#qh
zU^vUlz>v<$z!1pFz+lbFz+eSk`wyD?U&+G2kjKKnpv=O+aE+OP;VLr&Lk+aQZo|yL
z@RbSDXJ5(0z!1&Ez@W&)z`(`C!0?WdfngUT149=hG|b?Ac5n(BUGEMXXMijgKnoO<
z@(68aZwRh;2l)<!VPzX=$Oc@=kTwMg9-js2MGKzM<?f(G<fF^oi}Le|h*<89+EUFd
z9$n6^pa5!lk1l5icV0%|a&~w-2f2+38<7GpQ-_XPfC_PBHG_RUJGiq5>Wz+0972yU
z1V=TglWcg@L(ChUIK&l1kYNdBJS`tuo_Gx#7e$PyKt@REa#}Tb2pqC$AQQBWCKWU@
zL+*wGW$=l(uv3wVZv>)fgSELKLlvOGFi?R8I`R$A5z#2haGntjSA|?W;yff8t{Oa#
z1{n`R^&!luC{10MAh_*3q>qUna{EI-^Zzp$7#KJ~g8%{y46ys}j`1@vtmkK7n9R?>
zP{+@}kjc-$pvup{pu*3<pv=#}aFLIJVJaU3LjoTILp&b?g9jf2gBl+L!&_bkhAq%B
zfiPYM1`%EchBG`23|n~^7+QE381#7<7_M<MFzkVj1&DDoFr4FJU^vUgz_5agfgzoX
zfkBmvf#Dq|1H)}j28Mm0eE^&c47r>P40@al3_P3+45v957&<u^7+g6R7>qa=7(_W3
z7}z-&818_^0N5EAj<GW^EM;e4NQaIKd}U)`c*e%SaFvaL;WQfqLl+wZgC83Mg8>@@
z!#h?6hBeS}f)G{)27BoIz5y!(!wnV&hRG}p43k(G7>ZaJ7&KTI7(`eY7=&3E7|t;>
zFdSoMU|7V=z)%O>Cm;b5V`5;~$Hc%en~8y;iiv@tfQf;@mI)HRCm0zRYCz*G;4uSe
z2}<RO;CRq!v<kKg(Vz*u0?_cXMukDNl~JsYf`OiaCi?I%xS9ja_r`-t4A?qc@XQ_Z
z@wJHANYD!U)WqZrg~Xy<ux5}x@ceRoPHI|-jsk>T1U?rScJwRSj5BE31=3l8YsNJ>
zjcuu?BI3kSkbcmlEQlU@^Vg8M3;Z+Fko~KWscF#Jzm9n+m{ZQ+hyaBHVsaX%S|X;W
zL7@qqoQ972Lq=g?96}Qg$b*u_q)#w_tjE`|Ao`{TJdG1%GZeHHic5;X;i3nf714wo
zkAi&QKj?@8Bj_YF$b}#bsc%6n7{(Zu21#I<2>}U1>PQ91smkDE3osmvdwu|90Quw9
zAl)#_z?CjabxdehWOSkcbUboON@b27{idR@1UmZkEG$MLi-REb3n->h@}piNq*g!{
zL`}!2Jdgo6C%r(DM5k`3ji5FzIL*;AaYO9_+e1>?2Kfj}P@_$Rfm9(|LE@|p7VV_Y
z+<=UrS44rU7m$T8Oj*H$9B|Z#8$>pMJ5(8J2RyAI2IP@Tew?K|%%j8^1Fb+Zan~9!
z^FW8CBUdUY-iJFD)*XVZi_(GIwF18c1tyCb```)%<yaY*y>J@VaR%>&hxDF7i~LeR
zOAqr)OF%adfYxb%rC}!zfNo1j&PgoFEUAPCFw8(u(=j?TRtG%Lnh9D1R#K8$1UC|N
zbQyT{0C?FrXaSZ&aY<@HG3fAd(C`&_+a-pfM0GV`E&$PBk3)K!ATGFXj1gWaVGRpJ
zZ~+1Cal+LjCayqc;Z6bYSsOeD8DS|&5UCYpJP6~q0<;Vm?OX)dMOOGVg1Vugv%-Q>
z;rD#u(})r@@MMT~g$bzTp9vc3C;}bLg4$eywC1560f#AQaa?pPqz+P67~K$zk;n(h
zNo}D1KPN8(Ln8G4KhRQtQ1^5lKLbM%KLdj;KLdjrKLY~`KLf*QJ_d$Kd<+bcd<+aG
zd<+bqKzsdp85qv-GBB*>Wnk!ln&ZpMz#z@b!0?ZUf#C)Z1H%a(28JYV1_l>y1_mu|
z28PSfetsGk1A`;<o<C4OznznTp@frxA)J$e!IP7LfsK=a;W!5a!&2zJe<kR?|I_RY
z4C~k#7#6TIFqE=0FqE(}FvPPnFxas(Ffgz)FdSlIVCZ9GV5nzfV8~-*U`SzOU{Gdb
zV0g~Tz;Ke4fuWt1fx(rPfq{XQf#EF+q@NF36L6A+fgzKHf#EAN1H%_)28Jul3=Cb&
z3=Cz=3=E~r3=C1s3=HPX3=E>o3=H2u_W&|6Ff3$ZV5ntcU`SzNV9;S=VED<%!0?0-
zvaWv*BLl+%Mh1p_M4um&Z9$cQ9{4n-9LTmF@Ci9ZiFqmcxq1)^bej`s`HKlS*?=@C
z*n)@if?bXEg1{=FD_J4yVId`EA*4!xa-t1EXHFXEK@J>+2^wG#gqjVp+BrWjEi=6o
zbnsbz9`ZdW5W^w%fvz(IUtb74-UxmUDdfy7lqnX7K8XFfi52mMDGIg<#(IV(kWM9R
z%mKPI6TA=;blzopu>xo@CO81VniXtOZXW>`KoGNGC#5Rb!fx_{oSYgDx(`GzCqFSo
zL#edLG#*;@D?xOD8nqywCl(c_7U_Yn>4*o3!2%X64!S)hwItr(IRGLDRtCMX2f~J^
zK)<y^Lz5vhO(7n9Xjyzb_;}Fx_}s+Iy!dz}=oKjg+{1=-7U0D_qKO0=LW)O9pi0mN
z4k&bxddMJU$c-7uS+bxjQc{t1$Ah+aL4q8QovEqu1)xhRv0Igzip3(R7O45)gaXQ5
zAPlh%#Dd|`jyX6aU@-u$qe<+wpz9^F%L2*fAlp%61tUhVbR;1SJG?_ADDh5y#{|W5
zDD5FKMnh2Bc*^i4wxb<$&_)E%Rj=UmM^Z3@5)hW=^k~N%6fiJMe)<QQfZTqkM%*B>
z0X&mJtstS&0rjd$uW3NuLDz-iCAbS<JwaG!8>%a>7}7X{Nu!osAmhPh6V~1~s8bA{
zhXM7B;a&&n0by`s2D;@6c_I~Zj3Zc|63(I!ZW<0}qWBT!c5r3`cXC5OXMjRHhUn(v
zjuU7N3#<P@Q9(pa4Duuhqbfyl8P3p8O%2E|PK7y~lt2gB2*oOpyB(oC1qC#Iab9MD
zg0?OAh#I&WEUJ*?(2YQrg0NK}6mF7;nJkcIT<a~AAqO&ol_<b=JSG<9BDN30Him<h
z;4}u(BL}GgVcbzaI=WE|Z|dS4-2esQ5FgwC_5VR@>_Po+1_1^JIZ)@Hfq`KUKLf);
zeg=jdeg*~seg=lk(D{B>J_d%DybKJ-c^Md%@-i?q^D;1$Lf8I-=K4YX{azjhhFl&7
z27ev~hTq%_470fz7(%%j7_7J%82)iFFl^>xV3@?kz)-@)z@Wgzz;J+*fnh&%u3wXr
zfkB*;f#E3!1H%dExqp*57#Ns27#KdVGca6cXJ9xA-P1ploq?f+oq-{loq<7>oq^#w
z8w0~;HU@?zYzz#O*%%l)*%%l+*ccdikmmWbSQ!{pSs56FSQ!}pg3kP7VPJU2!obkS
z!oVQK!oVQO!ocv8nSo(FGXukHW(I~?%nS_m%nS@M%nS@x%nS_D%nS@qnHU(3F)=VK
zVq#$EWMW_lWMW{DgU<1PVPs%<$jHEOl#zj93ZmZ+4OQYOP_hE``9}u{Mh6KXd&FTg
z@8Iem#-YPBJgip)udhc32|$?>gu%@}l!H1k0*jUtoG{D4Z99~<6&d|iv`!R?*`tF5
zh?X)+z)%|Kh~5z>IgJhyfI<O;(OanK{5;V30Y-s@R~1|m&Hz~i8;ig&OGBwB9;53+
zfmR&_)UFV8pcHBaX#A2~!^zdB0qV+uJV|g^6&z4d0$l@24uFRutn&uzP=j>g@6>^e
z2N$PUd($9UaF04Q71W<b3@3slU|3nf*D*NQGt5;1w8h^FGGdsMQwcpK0I8P*n#KgJ
zR76?82r~vm<LaLm<tG)zXMyhch&RX9K?mst`vNf$4H824K1!g$!U|l9gL~+tPeWrS
zRUL(#ctR<z1hsFCt^viBIHMM%i;7VTZry;w3>niWW^>4hS#;l{gc(sWi@FdNcb^>=
z9?A*|DW!=yprngP(Zxh{{87Rdcefu}CXqSf19B&7!b1rYoau<rs1L|oaMr?EK!8+$
zF|`-CfDA=0eLx!uskP1pWHzqSrX)W<$AE-l1~mv#f*o&C1oi*zL1)#mK<@u@gLY9t
z=l{>+XJAO@XJGK;XJF9hXJEL&$H0)o$H0)y$H3sm$H2hK$G~tHdhTB(F9Smi^c(<g
z=$ijkJPZt(JPZu#JPZtvxfvKPa5FG$=4N16%FVz~1-%O(g`0uFnVW%um79U#I2QxM
zWawFc#as*wnOqDE8e9wvZ#Wqk&T%p@^l&mTcyTf?uyQgme1e__aGryK;T#78!)gu&
z26qky23rmWhO6uh47t!d0e-MCFx-Hy2dH9WV5nqcV2ER5V6bLmV31^EVEDz#z;J|>
zfng~t14AP#149NY0|RL7KWJ>=1q%biWflg811t;-OIR2fia}un4WChJGz3ONfUFPz
z6?>z@Tcc}W6ciLj;2IcE>W7UZz#Bw}VKq>90pCg(kUY_yHazx#8n5{50m&0(4{940
zkBw+eOSHvAsLe*q9x}AQLSPmRrIm}?!vTdm#*!0|AeOmSkTAHtIXb=vy<Qg{u&@XQ
z_gM8x<3|?(frAFi%9PRZJ*<cEf$|o#{|Qb*C|wmoW7i<fSb`dD{2HVR*$NVeu(4<-
zGL?gq0!TXuV-`Q)A`jcyfnepR!$u$>5XLNqpgx1w9VnK9w2UqSf>suwz<{okNzTs9
z18;>uNpA2kgmsZ&W0p`|BVZ8_sQ<6U&%h7_TKF%(z`zV`(qH3eU;y?1LE>io3=B*l
zd8phdH5vk=Aut*OqaiRF0;3@?8UmvsFd71*Aut*OqaiRF0z)qZK%)etUdM%3j{@k}
z;nKX6)FSxNPM~`j!DnocdA%2SR}eX;x}~OqbV1H!1Fd?(IR6dQ|950&U@!&EZ3{3k
zm_Z{DbpGFbeg=jl=>302(EI=T`4|{dp!fcL=4D{G!pp!gotJ?jikE?bmzROzHV*^C
zG9Cs7cOC`?VIBqsAsz;X=iCeotGF2$O1T*r?70~jKxh74<6>Z#$;H5s%*DW<$i=|$
z1bUy}L{0_<7wF!9&|QA>I2ahhI2ai0IT#o|LC^bdfUf!XWM^Rb%*Md5fQ^B{k&S`j
zH!B0f6IKR>&8!RzU91cY39JkZT&xTXhgldHs#zd@OlM(W&}U&_5MW_o_{Pk@u!fm|
zVJb5Ng9kGM13NPV!+9nKh7C*%4AYnx7;Kmr7#=Y)FwA0PU}$D!VDM&SU<hMmU<hGk
zU<gK8+Yd?)7#P>m4$##s_?8<%hWy~edf?McU<ZW42CCw7PzJJK!r&2A@H#S>2E^i3
z(3C6kj81$JiV--+!oc>!m!-jMiid832l*PqWK_qIYH|+d$SF8TurB@q*&Sb`mz-Zv
zsexrC2TvT}UCaks=98+BnF7kViFpc{c_pdosYT#(v_aRVsuwGOuTV`a$W$m#%qxN1
zYX%An7=|5dkerd4oDI722z;bYW-<6Q9B_~-l&2z=DS`}9&`3>HD9J3q;eEvTFjza5
z7(t0oSVSR`1Y8FqO~71BAbfLhgfDC|2p+?Tc}yZt{{=Y!k15a-8$fFem8f)B6G3CZ
zp$p4CAfKTGD@Xtwq_FUX4H=_ogGpjWWkF^ho-=tt!2!b{l{k;(g((8jxY9VzC1l{(
zMoUgG55ruJ;%S&5*yA{g7x>Y?@OuEE86^>POI-=%RH;<ZEqx`aI^c{1x?d_EbdO**
z=(ee}%)G=L1@QV>(EWCqC7?UQ5aFk+06IoO!9-I5qA)d80d~p=D32N_Xh3exO35rr
zO)kl)R7lG&$}I(*$_zCVrW)aIP|<;0Afk8~e>}sp5-em84h6*s2!jI|XOV>?Ln$bb
z9GOF~q(TX3SO_B07Cdkv<rIM_e^6K}D`@28=VwE16HP1u9avVJnyQeIUk+j>=jRq=
z=A`N<fN%9Hs4U6I&r?Xs%uC74OD|SPElGw%9ylP2Qq_wU67yi^$beF9YDFqosRDdn
z4Ri=ra(+&3L9#+oYC%4@rd9^ol#;JdoUa4Ep)M~Kbm4ZsLS}A3PAcgBK!rqwqSWF{
z(E45QAvvG~3c0T_GY?@j<VaXp<pFjuxZ+3yUqz|_%1b2~Fh$@TVXUc;k_ozDH8~aJ
z1W=+@(6v(tb_Jawlv-4jUj%j^=tMA3nnnqGkN`N`VUYkU%qn04kf4EcQFO!9fK32h
zdYKB>ZlD9n=<xIp&AvvO3Kj5RP|(OPOD#&w$x%qm%`eRZ7ZJJnW#G_)*joxczYN7%
zoQ{UydkohM32qeKD2^V;Ymm{WaZy|e_aoXFY|09Q`0iw7aKg_8pZpAN&y*l1f6#G%
zsd*)tMc`Hs=*nYAHYmx@hhIsr4!en7AulyKwYWI3s1kgyDL4fq@+UM4r&c6_GFLG)
zCuJ7vD5O^CDI_LC%224L#Jm)R(t?!4l2mZhD>=WYD7CmCACje0Qj0Rn5=&BxL0JcK
zYJf7-xKstmfeZ?uxnKod5S5t>zSA<Rv>0@IEhwpijjU7vF~NB}GY>2R(gL*}vK<9-
zezOAdk=ls9AtW3$@{7Q2%G9z{Xww>23MA*}fzD+tQgBQzDFvSx2F`JAIr)j8s~$lo
zJ%<$JfSsL@T9ls$wK+LIHy7mj%skLt1)jkQ(7U9;(%{+&CCPy^uy1KjNoG=hN+ssO
zUoh8z%cRT_<YUWIK^O5N2bX%WLZvRGCeDPp2NEfu6S?4LeS<x&2MHUO#F9kl$y>@0
zr4U)<3jtxriDSxu&Pp={dyFj2;IqW^5|fitb5bD(pDE}=Yhqu=h<Hb5XICHBAjc3-
ze?LgsqMTZhSpst|<VZ9ej)tmNfSgXAnS$sGfC3U!*MKhk&I8>!oK#w(kds=XUaXLw
z501^u5{Q>TwNP$iMP_bku0mo?PJVf6iUNvbp~@g335(~X{L;J>P(LLzIYR-F>_I8L
zAhjqtHLoNwJ(U5}|L+3by~hn%|1S>h|6k{4U|7q~z>vz%z~INvz#zuY!0;Bd-k*<w
zA(M}RfuE0o;Rr7S18Cj9E-wSaLmmc(={yV!zB~*JK0FKz@;nR-uecc)wsJEt6mc^!
zSa35iFmp37FoEoW+A&IvhQMeDjE2By2#kinXb6mkz-S1JhQI)X0B8gW&logl5CZKG
zc4O35J>(pH&`2g7Z=pjUKR^t<V{tI-@ORvrK;uI=PK;LuAEHo_ky@Mz9)v9hO^4)z
zJGxNwAi{{lDDZ?PY+fL~NC|qe0c_+MH0BXs1iEGi8oUJJ1#VIfR+DmwHwkH$0qiW$
z7#i}xFvL|xkVq%sDrB=j4#O}D>@Fm;pg{=oJ-Azsh(~0jK%s?U6gaR*Hi{@$q0DB0
z+(y!DAuLK^gKfx@g_tIjGFymYGM33g(8NhlYFcVhYF=_Gc(y_>+&{?21rdLsI0Rvk
zN{Xgf2%QN69%ezzIH33)5jYCSCwaky!HEIvdYGRf!{H!z!!XGG6#5j@|F7p}V0aDc
zmkTg3h(HVAZ~P1lYxo%$rt&i|c<?hYu=6u8oabX;*uclYFpZCa!H185!IY1I;RP=P
z!*X5*hJ0QIh5%j$1~Xm;hC4hA3{5-?3|2f049YxEv!UjWQllX-8UmvsFd71*Aut*O
zqaiRF0;3@?8Ul0)0a&jAMkCKyB40!>x;_=sZNqWzVmy{ECs+e~u?sAKK{Tk-KDs^?
zwCV|M1Oc<3i?;3y6nZdBAbfGHPsP2Qj!+s0FE$xs>wi(lQ3!@F<lr7q|38VHfx(Oe
zGXJj!Es9?8Gca7?XJEL@&%n^Y&%lt(&%odW9S7LM$H0)!$H0)s$G{-U$G~u%mw{mh
zbRR%GF9U-(F9XAS9tMW>JPZuAJPZsqJPZtRJPZs5JPZtvxEUB$a5FGWgzg7$<Yr(H
z<Yr(v3qAX<hKqqAfs28`n~Q<LjEjNc6DI@1M@|NY51b4P^Eeq8k~kR{bU7IqcsLmt
zKxh9=<X~Wk<6vO$=YaYF>i$t`Gz3ONU^E0qLtr!nMnizwApmQafoPnkT4C9a1#ZWo
z9IA&t{RLji2r?Fg;VbU-Tr$(Z6FMcCi8+qRpuHi``H|FAtf!1YjRh-0nO-4u!Wfp*
z#n2CO8@<dLC6Yi9HApVA1~~<UK`W+9iV`#PN{S)Vsd>c^u7*-+d{TZzJVF6zW*>=*
zIY)@xOGaLdtN>bbY>25|*HA$Vx^@`*I%Pw|d?3iJAPnERpo4k40VK=7L%bBpu_y`K
zIUnXpaH5Ab{}Gdh2tR}FW=CW#G>yKAd5P(%MX;?L$llK@LwFa|LCP#v0I!8FF3AV2
z>d(mmuL(f12kdGrISP7SA6QyhL08ufymKHwFQ-x=1+=gru>`#SIX5#mzBoUpEVZau
zT>+#=5vl{am|8toAuqo~p}4dtRiQj16}0$5Au+E~0is8tv>3FmAG~a#BqLuTCp8BW
zy0De(#fiD83hDW&kPQUM`FSNp`8f*U<?>K_GV_W{QWH}^OQxY~tHG=GbHG;zBKcE6
zBQr0xurx6zv!qf1NgNVfF#Af1DiunLGxO4sw)>z3J}lOVT1r986@Ach5Hy1iP8N{t
zP+X#+M2v2@8nl#FTmm)>=2}=<sqo3n%`5>G&t>3k3OWi95tuqngts6P2&E_%AcVoz
z1ePYIfL7%vJ0%tsWu_KsKvuRx0vD8hAcnzgK`{y@2sQ}3#R2YEj6F&qqaa437=dt&
z6L>|vMnz7NjzUFgl8%C*o&m%aSkgFptpVEql98B~l9QU0SOnkU2u)~^T_T_z0iY}d
zUS|W#&BeMUrA2wEDY_;3y7@_2smUdoWzZy>nwO%RnwFNDT#{d;0NEjynU{_f$fZdN
Zwjh^A8(PJJZ320*0(_WdMNSe}6aYuxNx%RA

-- 
GitLab