From 898c6425379cb33606b537a9984ed464950e7d9b Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Tue, 10 Dec 2024 15:48:42 +0100
Subject: [PATCH] cm
---
.../optimal_control/.crocoddyl_mpc.py.swp | Bin 0 -> 32768 bytes
.../.crocoddyl_optimal_control.py.swp | Bin 40960 -> 49152 bytes
.../optimal_control/crocoddyl_mpc.py | 7 ++-
.../crocoddyl_optimal_control.py | 43 ++++++++++++++----
4 files changed, 41 insertions(+), 9 deletions(-)
create mode 100644 python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp
diff --git a/python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp b/python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..f6c362e0bf70d628ee224cc8abc08fbee21221bd
GIT binary patch
literal 32768
zcmYc?2=nw+u+TGNU|?VnU|`r25|KVrsD?ptH3LI>esMv5N@5X63Lnl*EY8l%%fqJx
zELR6JP(LF-H`PcV&dn*!OVKYVO038%)-Nc^&q_@$Db_D7iZ9O0EyziYPtMOPDay~$
zFQ_cZ$j`$fkzY`fnVXmcSD0LspPZkPQkfH<Tac_*Pzknulo$<x5gY=gC26`AybQ)h
zh6bQeP*ze@5EcprF-P%e2#kinXb6mkz-S1JhQMeDjE2By2#kin2nm6b0yc(v1_lNu
zsDI<3G$R_#2IVK9(J*!FQ1NgmeE>?s<XNG7Gbk+%m4H$VEKqStDD4N8fKm+1P;q%E
zod}hHQc%%RYBU5!Ltr!nMnhmU1V%$(Gz3ONU^E0qLtr!nMnhmU1V%#uTL>hkFfjNr
zFfcp=b)*Ft7-0SX$@~lqzxWs!9`P|ST;yY5*vH4fuz-(&VKN^BLoFWzLm(dmgB2eG
zgC!pWgDf8d!&hDghIU>ChBjUXhE`q%hD=@thCp5h1|41ohMznP410JO7^-*}7}R+f
z7}R(e7;bYjFwEv=U?}8fU@+!pV36i!V7Sl4z;KU?fnfp{149)T149NE149581A{*o
z1A`S80|Pr31H&Co28M;43=FB93=BS;3=H0!3=C?V3=Ho%7#OZ_Ffc6RU|=ZXU|@*j
zU|`VVU|`VXU|{&b&cJY)oq=H+I|IW6b_Rx0b_RwLb_ND-b_NDTb_RxjYzzz!*ccdY
zvoSENVPjxe&Bnk`$Hu@=#>T*q!^Xgn#Kyp&%f`U)pOt~(JSzjkTvi5#sjLhPajXms
z8mtTq?5qq7S6LVswzDuWw6QQSw6ZWTc(O1sh_Wy+ykurz*vrhoP|eK1;Ka<p;K<Ct
z@SBN&;T01D!(Ju^hJGdn1`8$z1`Z|$hJTEZcz6nm3v7`^wwQu~f<kd(S!#SiVsS})
zN@jA2f~`VzEdxj#gb^$dPrWq0B(YRo!Ac>oK(8vbD8E>vC_gE`L@zf#B{fGcuS`=1
zt`n?2rLZ_YIX4Bb4h3a}@>KPrRE5Oi;?msAymW=M(wrQH#N?9F#FEVXJcW$ZqErlH
zaT{5P?uf!-!isb9)8jKsQgcDR2ipZ*TH=$RU!Y*CP>@(ul9`yJ;S5?o;_2-h;Oq=p
zdIFNyQAjLGFV;~22ZfG8aehu&YLO-b*cb>$4@_xP6vt=gWtQkDl)@Mwje5C>74cvN
zMIa{>X@WIEl_=Ok&C@F=%1_Ek&DAR`O)Sm~E=erOOxB35Fo;#qLRS$2F#&3uf~^8n
znFhoXxJe-j+6smcwaN-fr6mgGsR}9idFmw!8Hr`73MHAjsS0WNMG7gYC5f3i#R^4v
zV3#Q?WR|EGLo7{EFaoP9R!B-s%P&e*D9K1wNXslLE>TF%&&^HDOMxT^WrdQ|<cz$`
z<iwnuN`>N#{L-8hg(R?bsS25S3eeEb0flx_YI0&}ajHUIemNuzK`9C1bOl?5+{|2P
zU=`~HgG1UmI50p18afc)WTt^*1QO~+3bwWiO3B5A1xi+6_oEmGUl0T}%qhRZ&BcYV
zc}e*dX(=f%BcZk{*eWC!r6!i7LX(NVbASdYhT%!0!azqMJwGuA;>-#I1zS)?O3u$q
zPAo~yODsv%hz2Jjz4X+QKn+c(J}@suLo*hlO-V@!?2klHmH}lFP>`lploTZ<m!zgB
zq!s1of}^3hB(Wq_p+cijC#4XgGf^R>GA}VVGr3qHC$%g!M@JzsFGZm&H77qgvm`UM
z7?c2@(FV35FTVs7qlqP8^S~K0M*-&7On4H=hop<5(maLSf@E-#NmMAv&&(^)Ey>pf
zGZZrO%2JDpQx&o^^HOs`X}4GpY=tfefLY*N1!jRfn39^N01ZXo0B6)t1cjiELQ!d+
zZAejRs-_hrl@z6xlosVFBo(D5X1nDirt6@UYdQ+BJPx)5Ou$5<p#@v4f~`VnakN1!
znk=js1Id@A=75T{VoW9KAn6JNXcWSVL@RW|3X5S1tss#NRaqDd^$55`El$l#2~5#|
z*#?O~7*D}gp`tk25SBh*20*PUErz74#G<0aN?4{ZEru9TflaakDhbJDkbFaQX$1}l
zL~)gqSzLm!G}_QAR#QP+AsT9QX>qi!VJsv_paoR40VLeO=0L?1Y!#5r0eKeHoC+j!
zloiSoi*k!g5{pV8rBabXd1gt5LP1e#S!RAIIGvV)Y7(#wP=`R1u^u=RRv3V#!0fyN
zJrXmdvO;lci2|r(&rHlwfMh#xE-X&XE6y)cfJ7^(*nuZb1zQDB+GS7%i7F@*6lLa>
zXec@7gK|lFYF=`xLSk}8W@=e#ijG22YGQImYKlTiYB9(Y;5t!J2@;!`X`limCo?ZG
zCtWWuzbIEDwWuh*EH$|#zerQT22v&Ifyy8~P_BtjEh@?{f)s-Yg`oN^GfyukKiy0t
z*cGfqQx9T>0ytctf(o`!^U_mGLgLHgQ#Ca85|c|j^U9#D0AG+D5Qix%Bxj^1XM+m^
zkSHV!aJ4jG`4Ck5LRuWiA?22sQ=H0xS|T7-zsQAyf|WuLDEs&(<|U@57QrfG9feFC
z1xO1gu_RH`N`ZkvSs_zB7nDRwN-7nKOTkrFBB)-=$S+p_*9r<Lsb$3qYTz2DAT_@r
z2UJ~wN;n2(aJdOeL5WE@l?q9zB_*jvdI}7XS~Mje)P~6f6`F}93gwBF3Yo<U$*Dyp
ziJ5shl?u6~$r%c%iN%?zMG7VP3gCsTxv6<244`I^LSAZW3P`v(HB|>x>K3J{7c1oD
zgN-T4SIExGFV}<GpQuoro0yZMkXM?Ulv<>apQeyml3D~#9>pNNi6siTnduoNpn5h9
zWMNWHszOPAz79w~wBFD`)Z2+AItnF4mEar%@&KqN2NmauB?_hn1|C(AJe3Hlyo*W{
zGV+TutMc<e;iv)fYGQV3F}NiFc08zF2Du(qS?GY97a$X%`anTal98IHkdvR64ho2j
z#5{$PjLf`rO_29MDhm=zGQh!63^Fna(jL*x%u7kFP=GZNkQ*ZTc?$VCDGDixC5h08
z$OJV)@=7xEO7lyLb1ESzvRI)!Bef(WwMYRZotUSPo(hVoRE50Mas`l`49Z}aLu)%|
z!$bkxGRZ7fC@D$=Eke#Os#HkJ&&kOz&&*2)1!!JA)I4=?cqhVIG!Ts-uY%3ZEQa_z
zu|y%QG%p#HpBa?FL7u9RpIiWH3KeIjq=LdNIkmW$0g`RNg$c&0XFZT><I_{~ppmXu
zkdv5~2l7~ML4HvQ*fT~h@t|g03MloLWCVcfWVfRH+<?TAjDW<V#9W9tNL7A(u&Xgc
zW*R6#<`<RZWG3mAmSpDWrDf)|K&CTpk{XCxM+=2WU{S}`aqg!sGoTPYNjmMG+d
zLbDj$N(Ut`h?6rRZa`|S;WryeA;^5Fb_fM?3j>6!P*PctTFd|r6mTHs=jRkdoegWK
z!<6Qg<`z^cBo-^=6~KhRc^k9@941hZnU|lOoROIiQdf|f2Qe8lxY5><>%k3*N0WiM
z1-&h(gWNiFbp=I&8!Qd^I|sm|of3;v9rIE!72tAME~K!7he3L3iEm~eXx+PGQF=gP
zQ8B0>L@F@hg(GTF8BmlDN^?+I22lTBjf;U{9~Wf(KWu*eG(Q8wTz&?ITz&=yD}DwB
zBYp-3L+F^mZaxNvS$qr(8GH;3UVID;l6(vd5_}8{Z+ICPR`D`06!J1K6!0=I@bWS+
z{NZ6>IM2htu!x6&VFnKaLnsdegAfk`!%1!ihF)$4h5~K|1_N#ehHG354Ev$(*b8zC
z*?rwnjiVtj8UmvsFd71*Aut*OqaiRF0>d~2Km#kFPFp;*b*xuZnx_F7n?M#;uvNfp
z8AFCMbYOir<S~BK5e&qbBE)1=S<q;PI;fLo<Pu++2kwV~CL+K?Cwe7CiFw62;NE()
zl~F8Y)B`MqWtas#3IpqNm8BMCrd5J^%f+xkr_3s_L6A8U&~R`rxIYdWe@cbwEy;lO
zJyR7@^HOwE)6&4BMC!!~MX712MW8_^&~OQ83>Z2-4;|6Utbz;zLk-F=QUIB*kf;Fa
z6=tSoCW8jsOF=^z;L)BC56@umSc`6MepzN-x^8)9UP^wsLUCpl%q@t%V`^S;X;C6*
zysIQ5u>>(<3+g53<(Gi^lh9EP&_o2t4`5e-A{P>3AQlL#r{J6ADT720NFfNT<5LTp
zK0+Ro0$U0l3IL@B(1;myMhJUvjfe>%NY^_a+G!s;(?#I00EHrCAa$rr$zYEsEHgBa
zq(a6}FgRd9a~Gh%A!%|3G8Hn&W@kYC|3h313{KpT{y%K}eK$V?gC#!$gEl_{!xlaU
z23tM`1{*#G201<khH1PE4E($d41Bx{3|Dy=7<TY5FwEv*U?}BbU?|~XV2I>lU<l!1
zU{K;=V0g^Uz_5jzfnhT@14AWrzTbx%YTq!fm`1%e8UmvsFd71*Aut*OqaiRF0;3@?
zTtfiV4*)H1(gpVnz^j>D^HN-)9dDHBKi>dnup&_R0kqHodv66}B@}3I0p5coZygk@
zn;Q-8L4#J}Ko%u|R-mP5XzCTEf~S2oAY&r%g`PT^kbV+EJ9Jr7Ar2jdFdbljLhQg;
z;{=u)yo;P5Jzn_w$&%EfT+qrJU-07Bl*}~nFbZg$s9th@aY-?Rj8;!gjW5VAPF2?~
zE-3;pR?*9g&(6y)Db|byD*=UAW=d&djvjm@20Xw}k`e4`9G{Vxmy(m3lvo5>I0X(M
zGM$l>SezP<H0(r%d*VT6z}=y&keQOLkeQ|c8oL24Q%MEQA14-7LY4r4l0OI+mFDGT
z=A{z~rJ{IPI4Ni=fI=#h#Nf$<tSSRp3Bp8Ln?prwQ*(+_A&aX(4p3I`bqo&n40DC9
zm4K`$$*F{_u7xKT#PC}&c>F9UHLV1^zNiSicqg$a7aZvz6G50rZ&4nWp#J{?W(I~S
zpbhu}3=FXS`OEnk7?SuI7)<yX7})t47%uQJFwEp*V3@(jz!1#Gz`)1H!0>^WfnhB#
z149}w1A`+k1A{&<1H(Zc28LQ51_m=81_o&!28L_g3=G@385l~r85sPy85nrE85nMH
zF)+;JVql2nVqg&GVqo~m$-uCMlYt?MlYv2;lY!wA2Lr==4h9Av4h9Bq4hDu_><kP)
z*%=tFu`@92WM^O~W@liKXJ=rLV`pG^&c?uShK+$?9vcHg78?UY7#jnF5*q`9A{zt4
zT~-E$jjRj|x~vQgyICN9+{nVf;K9PcpvA(#u!xy~L7JI?L5i7y;UN<P!x1J1hUH8Q
z42?_-3=K>S45>^E3<*pO40=oq4DT5k7!EKpFzjb!U}$AzV5nteV2EI3V1S)d0IA(T
zQ8!u_z!s{G)&(Q5E}%lm1<FMroS6n)i3)B<#OEdErYb1fDkxQ!=4K+T@+Z<pP^E@a
zF%n$?LFz-Kibol`k{2{np-`S!3|>kIT89iO<(0sT)`_VL$#4;*IR*+Gs2kK1Q&K?d
zI6-EqLnj+R?C4DBJ_68;N+zn4K<f`v^Gb6;D+*I#i?~7STtOR8kVCT=w(5SQuKPx<
zlR-<4^~y4fOA~W4t5S>LbB>?}t}|pGj7GImT2W#yXron0hLRO{btX#lt`-r4pjZT_
zY{F)OdH_(<z=;vb6wq4ElGLL3JOxz+BLmPb7z0S-1hzc}?qHC6GYd*{5=%gf!@*ty
znTz5)Xp0oSR8#|`!pH^DJdHPm?1R#SH(28{QbA1^=+qX-Q+Wk?nRz9tMFkq570Q|5
z&?qi}Bu-SbU=ya%Ryu~UNWBnULmfj+O=#zXY$FXYjnsiGb;tA-WG_WVaY<rwwuS~c
zaG+Z#ax(LZ3lfu4H4O9&pniuo>=m>?bE3t1`N;+GDJ7_KkZ1<Y27~w9L5#;@6nMEh
zWbQN`G<T|?1ugU72BNEgn5?Ye=O5x~1(_d)EC2`1q?YFwWrK=?a?q;lqWt2_w9M2L
zh2nh3s%UT%9<p%@>_cV9HX@`cPuQ|hP?*DJqTwxC(7ZQj^*3lSG-&R(SOK&jqPQeA
zF$KIX9khZQ$t=j;8D#~~Qv0;T;u3|*{89zbj9F$LTu(}Aa&}2#0m#XTMfs(9p#2Xy
ziIvc8Ly(pBIjNup`^AZ{$#BpvG|*Cf`1<h5{8G@y!nFLNWXQ65(E53(2SMw;GfN=;
z#9RWPfaH`+*dDgbG_XsHQo*~}AWQW#b3snb%uCBJ0_|(c&(i^`fli^rLI`Rn!~l?!
zKv9yLnpy(VoRXTDl9QR23ilhh_Q*|5%qs?)1+ge!At%2ayi`6FwlF~<tthpyG&L`|
z5|rG)Q}9rW^3y<D8z8%1KuH?19Sd#&BH$o9FO&0g3rfLm$<G7DV=8247_?spT8$4%
zuL_{lo|%^pwF8pOoIKqX9D{>HeFH)~{r$jFAPceQW=LX(>`MZtUeId&X!JD_(E3SP
zArrLOpfo21v?UQ12+8>cmAW9ksS2rOsYR9GO#|TV1?8X>1j&hc;4n+c2Rj9nT0zc<
zPb^B#NzF?y$xzS+@3}+XG6OO#9=l7TGZl0d3}Y2^L8-a~ry{6}LAHS~c=HO}evk;F
zOa!|IvWvl1Arr1BGYwoLAi2{H6sN^{pej4QBtIU>1n?+9QEEzjN@j5hXz>K3SO?eV
zpc=d=F)uw;L)S=0LDx`0OCcvUPXmt^H9^WWAx0osl30=v4|ArjA=sT@ry~LqGaT^;
z6PDr$6kAED3jP{~n#dW>Un5Tw9Hp5G$%&xIPfArt0&g=&O@WLpC@Yv~DuDO<!4hYR
zf{_b22o*B(O7ayF^Av(zjghu%fPJ8>V63T-m7fXPYXaJv1KK49G9WRhC^a#q5}fo?
z6pB-m^Yc=`TL~aD^U4ZFnxH&aTvAk;Tmn)B%C?}a0^jWl+8_*54Aq}mtdLb&4B1?g
zTv}9=npdI#icR%m1<;}om}<ywS&)8EQ3-MlD4anHV8A;ZK$|N-+jbO?jZi4aFU|yQ
z9)XoihMEeXjT-RgCV1N$*c{0A%Y4v|SXd4KIf77<(gUym*Jog0I1So!F2KN$0O`mw
zEaPWjn8nY)P|wf6P|DB15XsNL5W&yDu!j$_9-yC(fgzWVfx(%Nfx(H7f#DM`1H(pM
z28K>v28Kpn28L{228KxJxPSvM1A{hny}$<^28Q=M3=9W&7#Q~RFfcUmFfb(YFfb(W
zFfi!zFfg$3FfjZ9?FZmyVAu;iBOr#GfkBg-f#E$D1H%d~28Kc|28IGI28K{B1_lc*
z1_pC31_pjE28Oeo3=H!*85kls85kTm85sU@Ffi=pU|`t8!NAbN!NA}P9aE6zU|{&c
z&cN`1oq=H*I|IW+b_Rwhb_ND}b_NDPb_RyKYzz!1*%%n6vN14BVPjwjU}Io-%F4jd
z#LB?X$jZQw%*w#<h=qZnh=qZnkcEN4f`x%WnuUSkA2S2P4Q2+0>&y%cdzcv*wlgy@
zv@$a=xG^&@h%+-Vh%qxTTx4QkILXAou!M<$VHR|Z!JLVKft`tg;R+)I!x=_MT$}>M
z2Q)5-r9edxXlNNkgX&pWwWL63+b|@N4ziUgAYYI<mO^L{9y~k?-Bzok0NSn%%0#0>
z>~z~ltW4(Mc6w1}K|yL!0BD?@h*5R;7&aBGBWoxJ?f@bN(Up;V-g@wX<IKbyJ(PI{
zQo{gg+=2`nh#n*fb~TQMS_N*Rj1D;{C@7Gzz7SOGfm<;k1_%>5M1XDB0d1Io#1R6J
z<*+`WYnZ2xf}@{{f`?<ULWsYDldD3ItFM2UtBZnLkiV~jhii~4q|FA>1u9KJ?K04b
zDkT}{g9*@%GNKg<8$bYQ1z~0A4oZbY1!EV`kQ!v&vO+#+8!BiXDiz!WM77Vz1!?Re
zGdDE_TF8Ukf>@cYqX0UF1>7`3S}lwagLGYxyA6=whYV>$&(Ns!$;{0x0j=kTG<G3w
zPRvUI_pjB96*5cUF$g+F1=_y>?a<E7%u7jBfXw+6=Y!RQmSTguJvphx#gOg6@D1Kb
z#*}B|D<mftr4~b`;!<G;dVu$xgAIhV5<!kZ^)tjzu<jT*KA`K=QAb>ikj7g;Cd3!y
z=fDSTpgI(6AzQEE4g*;S?g)Vn<A9xa01^b@%rvlS(4dr3N`79d5_Iw!BnQGI9_9hk
z0>X&dXwY(Sk|(o4Mu0HF7-A>2LApQ~k|sdIet{_(xXwsIqz#Z35QdH#JBI}MSSh6E
zmw<Nsg8~{fJ_8yl$W$l?pUab*UzQ3z`2nO2gkghuDXB@N>G5ELiXojjs4S>s3kqky
z{5+(|Nr)&ox4>FApkroS5=#=HyChI2GeNFFWL%gQ4e*8q9feGYmq1g&l?AEbp+T4u
zO_1H-9ou?t&>im~l?AEL_*Mp`S@14?P{*ejG>#AL#(~ojY%B|`q5!me9h?<P@)e3w
zlM{22K|_|H%?Xff4;c^ub<a|vXKsL84ReSdC|00@v0<5s!5N7~sVTmxx%owv8U@fX
zO3+bHU@xI>&j7myEQ4!zMj^69pa#@H(7+gYm=En}Em#TzZ4rTNWI)a;kOMD3@ds<P
zCsitdj!pt~J5jf3qeWVN5vb=28oL6G3xGP5sLB<}GcuDi6p9NnONzmzCMblUgF|3H
zLio^w1VG*M)J#wh6jVYb!jCM#FeMRuS_{ZJh<;E^6+=p4<h>>u#hTDl6F?o+%p!2|
z(<s)2^fJIrdRzq9ap2ZF`nnmUu{*FLP>_L^(0~==S#N`+4_YCBGcS%4L=>zbhk}3(
z4nRFB3Q`clQkjC40&0k(A3XvI6;Rwl%Q)oSNMH}3W;|qRq&-Za8L-kkNJd9iLcRS=
zp!xqu1_p+Up#G)+0|V^*{=@tX42$>~7z+3q7=rj27!>!>#!7@qJkFl^vsU})iE
zVDRN*U{K^^V7SlAz_5&$fng~x1A{d$1A`VX1H)e)28Ihf3=GG37#P+;_XF7SFfiEg
zFfds2K=uS=b2Bg`b2Bj5aWgQ;a5FG`;$mR<$i=|0kc)vK0ou1W;9_8q;$mRn;9_9-
z06OQ7lY!wDCj-M?P6mecoD2+EoD2*qoD2;AIT#qOaWF6}hn@l0#=*dl#=*eg#=*eA
z&cVR&f}Me30XqXj1v>*n7&`-lBRd0wGCKpq3pNIZoooyY&1?(|4r~kzyle~%JZuaM
zUs)L#?yxd2tYu|jC}(A0Fkxk2U<6%az{<dI5wv!Ig@K`%g@M79g@NHOGXujpW(J0S
zW(I~dW(Ec&W(J0PObiS&nHU(RFflO1FflNwFflN^VT6S91yEQM+o>m29VpF#&Nj&`
zRsc-~6oZbtfSh3hPIk(q1BgBa)S<D|6!6F!xF7(v_dx|aav`9gpbXJRrhQ<|Ak)F~
zU63XWY<31NtOGh0335Q824wuRBm=Bo6KVPga*PkSZplwmfV4Fg3=}jn^-}df1r$su
zKMi@@F0lwaf>;8n^58ZS)5Zk50JEW~3^pDzSO{wGfe+FF74^^&Mvxs~UsKo`1$8wc
zQy0+Li6qcLGpUflfAC~bZfZH`utk)qFlhS@>~=_KV{0&irLngfLCy!Y7{N*)r6r=-
z4Hks89l<G-=wty75@pQP2W>!r7gK=}AE-@FdZNQ9r{NVefrI5ho`6;FWG<>iHG-5j
zD7Xp(w{(bVf5QEQ+&;(GHiy<(aDBrlEI@${Za!-$k=hQ&-3CVrOGNvdaN8R)y$P9{
zD=x{*$x#4LlNRSIz&m@;)DN1)1SN7%CWFjnf_UI051T%~K69XupI4q(lmcmb(XJT|
z3ROHg8tM!1AzTPAfTl-42U!uWDImENq!ZQv11Emm&3ur1P#fi#(~qE{9O^LeOl(?e
zYEH32F6emE<ou$dRL~SqzCvy)EbL$hih&j9Lt3>+gA+)FZ7FD495l6)lA4rQlA4&5
zld1<PY+*eeuo<9p#xg)h0G5;{<{;*J6hK`;@GK`NjzOMQC@xJ;Pc1G<O#!<X)O{%g
zjXr>Rqumu~e{;}xS3rr9D4brDSX=;JOs)ey>@PkKRG)$lZU!G~2tFeb=hgwbb$Fns
z)**LGka{l2!qn@nfPxP-Un9EKAUQ}U8^i)(@LmTzr&xpY4@e;hQ}eU{5;~=j!BF^I
z6l9_T6x}5mu(A#|$N)NSv53S|TR}cUEZ&2d4&T{8lQX8t9P)<Mg`-DPQ}1YM%(DfN
Lhd@9RpPCE+b@T2|
literal 0
HcmV?d00001
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 a9bc0612533751805f146d6ed5daa83f8a7d0b2c..b254c2e3d14548bf200e7191d10442f742c779e0 100644
GIT binary patch
delta 1634
zcmZoTz|_#dESh8x=IN_op=ZRvz`!8Dz>pIXkv?Oi=v#h9uF1>-XBqh?Ulb5G;sVJs
zFfhnL>GzBf29#pphHx32pfm&1=34^K_%<o96!3ELGB70aLQLnMEI2`5?ISM(!#Q3C
zhSj_b3>{EeUtR_VX<i0~e>@BfH+UErPVj6_n~=j3p`f5*z@UH((D=o9nFR{kwhBr4
z6?j#lNls>*Dm+<WDhmj+C@3hXAPay5CmTeH@+;aZC{>o`W-3{M-7xu~m)v9nZ&qcH
zVaf^$#hH2OIjIVXMY#$^scET2sd>q%3JS__wTuj#KhEW6s!vQ|U@+rkU^vAI2?kg&
z?%`)(Sjf-7ki*ZwAi&SSu$hm6VJaU3gDW2c!%JQUhU2^p3`=<#7@Bz*7)p5=81#7=
z7({p>0o2RGz>v$sz~Ilr!0?}&fngyx14A@71A_xM0|N^;1H(=(28J123=CCV4D}3J
zTnr3nI2jmDLmi>X$-p4a$-wZGgMr}$2Lr<*4hDuv91IN191IK}*cliuvokOpWoKZR
z#m>MmlbwN~g`I&RnVo?_m7Rg%IU57RW;O<fC2R}~li3&;I@uT)JlGf*c-R;i-mx+;
ztYKwf$YNz+P-SIc5MpIu_{$Q{!0?`hf#Dqs14ADR1A`O`1A`<B1H(^d28Q*_3=FfG
z85m|UGceRMGcd$3GcZ^&GcZUqGcY`5VqiGN#K5qKiGiV$iGd-IiGe|miGhKWiGkq@
zBLl-jMh1qXj0_A@CKomuO-x|joImj&n;-)?^g$5>!bPPFlRwT?WdxBjTyQxMS6!qi
zJ~cJIAip?ONkLnoxTHuUQxhz~R1`m1c8)Pvle#RrDv%*?Jo)2Xb%=61R*-$Elkd$k
zp4>l2VY0wf&dK((ML|i4w;-`3BR(}XIx|*BfdQ1zEEJR#d>w;>J;PiTLi}C)trQZI
zOG*=Saw-)HN=p<n^N`Y0T7Hp2PHI{SD4BsZPxcMroxGrjho>k%sVF`xKQpf+-hA@L
z9+AlcQ@JJ^ICJuVTmo`-Kz?!RWWy4r$$QmACQI}xGlKku$Duq(4h369HFxIVbZ1U{
zYAUij`9O|O%_&ZWXE|9=mQw-=D=R3ZlqTkYLIM#$#Yj00taS3Xxk{5|=iu=tC{n=q
zU3VauR+<|Dru{;ACNJnwg#{!R$Wj#N34rr*01E>{2`eNo!-~AE{0t29_!$_|`572I
z`574W`572)@G&st@G&rC^D!{E@i8#4@-Z+R=4D{$;bmZ`<Yi!p;bma(<7Hsr=4D_w
z!^6O^iid$AlZSypori(pF*gIl1#Sk0&D;zOOSu^ss<;^#D!Ccz8B(|z7@WBo7+AR(
z7>;u>FihrRU})iDU?}EdV94ZRV9?-VV0gpHz;KR}fuV<!fx(NDfq|8if#DMe1H)Yo
z28Qz-3=HQu7#Q+67#M!BGcf#QXJEL=&cM*m&cIO3&cIN`&cG1Q&cI;9&cGnW&cN`S
zje+4P8w0~KHU@?!HU$QTOg07vLpBD6f2<4)FIgEFuCOvN9Asr+Sjq|tmCcMSVJw^T
IC-!gw0Dbar8~^|S
delta 155
zcmZo@U~V|TB${Lp=IN_op=ZRvz`!8D!0;?6BE5K{=v#h9j>*gdXBk5#Ulb7EtRZ-V
zZxW-!CIyxPUS<XchI~ed>X6BT6ZF+K@iH*X=4D{W;$>hE;$>ji$-}@fpND}Vn1_Mk
zH#Y;r25tt1ncSPxCgku;zOXJ}@|smVlMUAUOm0|jIC;Tdw#f$0oRd$ix7aMw>&-aX
HcK%@i=5Q<X
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index 65d3858..47aa20a 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -329,7 +329,12 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
#print('adding base', path_base[i])
#print("this was the prev ref", runningModel.differential.costs.costs['base_translation' + str(i)].cost.residual.reference)
runningModel.differential.costs.costs['base_translation' + str(i)].cost.residual.reference = path_base[i]
- runningModel.differential.costs.costs['ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
+ if robot.robot_name != "yumi":
+ runningModel.differential.costs.costs['ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
+ # MASSIVE TODO: actually have different references for left and right arm
+ else:
+ runningModel.differential.costs.costs['l_ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
+ runningModel.differential.costs.costs['r_ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
# idk if that's necessary
solver.problem.terminalModel.differential.costs.costs['base_translation'+str(args.n_knots)].cost.residual.reference = path_base[-1]
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 0e4588e..4bac405 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -491,14 +491,41 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
runningCostModel.addCost("uReg", uRegCost, 1e-3)
if args.solver == "boxfddp":
runningCostModel.addCost("limitCost", limitCost, 1e3)
- eePoseResidual = crocoddyl.ResidualModelFramePlacement(
- state,
- robot.model.getFrameId("tool0"),
- path_ee[i], # this better be done with the same time steps as the knots
- # NOTE: this should be done outside of this function to clarity
- state.nv)
- eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
- runningCostModel.addCost("ee_pose" + str(i), eeTrackingCost, args.ee_pose_cost)
+
+ ##########################
+ # single arm reference #
+ ##########################
+ if robot.robot_name != "yumi":
+ eePoseResidual = crocoddyl.ResidualModelFramePlacement(
+ state,
+ robot.model.getFrameId("tool0"),
+ path_ee[i], # this better be done with the same time steps as the knots
+ # NOTE: this should be done outside of this function to clarity
+ state.nv)
+ eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
+ runningCostModel.addCost("ee_pose" + str(i), eeTrackingCost, args.ee_pose_cost)
+ #########################
+ # dual arm references #
+ #########################
+ else:
+ l_eePoseResidual = crocoddyl.ResidualModelFramePlacement(
+ state,
+ robot.model.getFrameId("robl_joint_7"),
+ # MASSIVE TODO: actually put in reference for left arm
+ path_ee[i],
+ state.nv)
+ eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
+ runningCostModel.addCost("l_ee_pose" + str(i), eeTrackingCost, args.ee_pose_cost)
+ r_eePoseResidual = crocoddyl.ResidualModelFramePlacement(
+ state,
+ robot.model.getFrameId("robr_joint_7"),
+ # MASSIVE TODO: actually put in reference for left arm
+ path_ee[i],
+ state.nv)
+ eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
+ runningCostModel.addCost("r_ee_pose" + str(i), eeTrackingCost, args.ee_pose_cost)
+
+
baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
state,
robot.model.getFrameId("mobile_base"),
--
GitLab