From 025e579790521235f99d57056c278c1eedcddd71 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Tue, 10 Dec 2024 15:53:33 +0100
Subject: [PATCH] cm

---
 python/ur_simple_control/.managers.py.swp     | Bin 0 -> 16384 bytes
 .../optimal_control/.crocoddyl_mpc.py.swp     | Bin 32768 -> 32768 bytes
 .../.crocoddyl_optimal_control.py.swp         | Bin 49152 -> 53248 bytes
 .../crocoddyl_optimal_control.py              |  10 ++++++----
 4 files changed, 6 insertions(+), 4 deletions(-)
 create mode 100644 python/ur_simple_control/.managers.py.swp

diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..f08ae83e3367ae7983ea04cac5959c3c261c03e9
GIT binary patch
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

literal 0
HcmV?d00001

diff --git a/python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp b/python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp
index f6c362e0bf70d628ee224cc8abc08fbee21221bd..7afae0d2bef6f7304be8900bbdbf553c2ca61646 100644
GIT binary patch
delta 33
ocmZo@U}|V!5=$}&^Ym4)&@*CSU|<knU^pBSk$!@Aqu8r@0FCttJOBUy

delta 33
ocmZo@U}|V!5=$}&^Ym4)&@*CSU|<knVAvB9kv>ysqu8r@0F15)3jhEB

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
index b254c2e3d14548bf200e7191d10442f742c779e0..0ee405b879f06e30223bd6cc93ebbd3579b14418 100644
GIT binary patch
delta 374
zcmZo@U~X8zER<vr=IN_op=ZRvz`!8D!0;$^qtFL_M()YX0=<l(lP3y@3-W-Z7#J9s
zq4ea<GX>oFHz}|r@XlmlVBlne=nUN~*x=46HJgcnp^Axtp@4~j!IlXkeS(pJp@wnt
z%*N^U3JMAe$_gR=F8)>u8Hr`73W*9SnQ3XMMX7lu3Pq`4Mslh`T7Hp2YGQJRLSj)a
z14s`Dhs2l1r^e@`rj_U@K-fi@=@}&owhBf0N%<vu>8T}PVGT{Nmeic$R4XtGu32I7
z>tylChLIwZYv=NBKGbAqtCg6-z@Wv?z!1a_@dnJ7*Z3J2=0U~H_!$_O_%}NW?&q7t
zC^>m?qlpa2NM(i6yp+_U#N?9F#FEVXJcZ0+g_Qif)X9q*XNZB_8LObBkXV#ntd|#`
KotIxy%m4tp<6q1G

delta 211
zcmZozz}(QlER<vr=IN_op=ZRvz`!8Dz>pKNQRo9dBiCeRfnLV2$rA;{H=hu&;Ge{(
zut|X>fmfe_fnhNtL}}P&!3K9m$%RY|47p4U3_eT@4C+h_4DT5sN)}CaR8*Uoz%!ZY
zyvgJZjggb@pXZ)@Az3s)Ss_s&B{MB8wJ0^OL?J&fRUs|ENTDPnRUtK%K|w)5L0KWh
w-^Cv!Qk1HYkyxxylCO}Is*tEqkXVudSDchsoT`wRm!dHFLGqo=HO(zH07-W|_y7O^

diff --git a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index 4bac405..4f1ce3c 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -397,10 +397,12 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
     it is instantiated to just to stay at the current position.
     NOTE: the path MUST be time indexed with the SAME time used between the knots
     """
-    T_w_e = robot.getT_w_e()
-    # TODO there has to be a path for the base and 
-    # a different one for the ee
-    path_ee = [T_w_e] * args.n_knots
+    if robot.robot_name != "yumi":
+        T_w_e = robot.getT_w_e()
+    else:
+        T_w_e_left, T_w_e_right = robot.getT_w_e()
+    # TODO: have a different reference for each arm
+    path_ee = [T_w_e_left] * args.n_knots
     path_base = [np.append(x0[:2], 0.0)] * args.n_knots
     # underactuation is done by setting max-torque to 0 in the robot model,
     # and since we torques are constrained we're good
-- 
GitLab