From 6c761b0a4f0955c3651bc47faacc00484fbdcc52 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Tue, 10 Dec 2024 16:06:51 +0100
Subject: [PATCH] cm
---
python/examples/.cart_pulling.py.swp | Bin 0 -> 28672 bytes
.../.crocoddyl_optimal_control.py.swp | Bin 53248 -> 69632 bytes
.../crocoddyl_optimal_control.py | 75 +++++++++++++-----
3 files changed, 56 insertions(+), 19 deletions(-)
create mode 100644 python/examples/.cart_pulling.py.swp
diff --git a/python/examples/.cart_pulling.py.swp b/python/examples/.cart_pulling.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..e874c004798a168923f4377f425b22573deb4629
GIT binary patch
literal 28672
zcmYc?2=nw+u+TGNU|?VnU|{eJk4Rr5RKuXSnt>rbzqlYjC9w!3g%9T@7H4PX<>6BT
zmaBsqsGpIan`)#F=jN2=rRWzFC01k>>lYN|XQd{W6zi83#TRGh7UZPHC+Fvt6y@jW
z7gUyH<mc(9RwROii}jNei%Q}PN^^2D^V0PSD#3P)5~Cq78Uk2CptK}S*MgV9*vQZT
zB&)2Xs30s93Sy4p(GVC7fzc2c4S~@R7!85Z5Eu=C(GVC7fe{h{B?YVu^$ZLQOi=#@
zLup1dnhnbLh0-BV8m5jF%2$EX3!yYjo(0M`fzn5zG)$fu%2$HYKcO^C9?Bi1MnhmU
z1V%$(Gz3ONU^E0qLtr!nMnhmU1V%$(Gz3ONU^E2C2!X^D1_pHo28Mo6M_Yh_0oMP2
z!Oy^Om7js(BtHYg8h!?bK7IxUMScc`n|urm{d^1zfqV=M+<XiS=Xn_zCh#&aeBfbV
zxWU7~aGZyMVI>a(!(<)?22&me22CCY1`Qqt1_d4lh6mgX3=6m!80K>`F!XaXFywGE
zFnDn@FmQ4+Fg)R6V3@<jz!1R2!0?%qfnfnB14A_@149fa1A`GK1A`bR1H(fO28Mkc
z3=A_k7#Pwy7#LhQ7#QR@7#RMuGcep@XJ9zR&cJYzoq=H?I|IW6b_Rwxb_NDxb_ND<
zb_Rx{Yzz#YYzz#sYzz#ZYzz!0Yzz#>Yzz!WYzz!+Yzz!vSs55sure^TvobJPure@w
zU}0d`&%(gq$-=-O%)-EMkC}mC4Ko8nBQpa-Dl-FvFf#+gbtVRe)l3Ww)0r3;s+brU
zDw!A<<d_&3&NDJF9A{);*v<$E*FI3#5*IfL3JMAanhKd|3i(OJC5g#7sl^HfMXAN9
zc_lguDfxNoB?`IuWvL7xb)e-B0g%-XV7`))5|{?bVZjB7B^mJrIf;3BsYMD_3IRp=
z$*IM~zKMB>>8V9J3YiL43Yj2V3KEM;;!_e!5;d(DQc}|tkgRcrEX451&o9tOEJ`od
zQ7Fn!$}dr{QV0UkaI@0$6LWMFit}^IQj6l#ixP_q;5_JZ4IKusQ;SkdN{jLoGK>Aa
zbriq~;|ubOQx(9HAU}X)6l@hjib^4V&P-FtE6~fy%uCEk*UQT<%GF3MDvB>lO)kkV
z(p0cfFw!zi)ityN>sKbE80>gGkVo`DE{;zvD#|Z{>Q%@rQ7BGL%`H}_%r8~QNGwZL
zD9KkSElyR)DlINiD9K1wNG!?)iKVBOfT*<0yv*VZh4Rdj426=6OmMU%XBKDX=Rtg<
ztPtYw;%}vpmYH5!lnPOjUs?ioDZ&p5wh9HAd3rhd>1G<iu3!gg>Os5*F$OBAU<(O)
zz4X+QkofZWR1HnN#N-msyfO_$FlvGgNAN)b8DAcslvtb!Qmj!@l$ckXlUM@b73G&C
zmZWMzG%4fO1yP4;08|&;K&Ur@U5z#J3iMJdQ#Fh=bwDwfSX7i)sSynhbC5gYOY-A0
z67y1WQj-#k;tPsW(^89yQd8nnGK)(R^O93_6b$qXbQHkq5$ZBhGt)ClVxhJwE5Pgp
z1vVt)(^894QxssPD5MqT=PH2g1BVF22S{!N1$j|HrCw1%Wk|knVo6bE1<2h6nNaHu
z^b8=*uSzY-FV--ITMRY{WNTV}5jahx7iAU{q!uY;7K5#WB!?=6v>cFd3Micv<Y(rU
zfO1+sR6~AgUJBS*pm+qi5gxaoOsJRdn;C5w3s#`4P@bw#o|p$tZ%L^NnR#F%k`j}%
z74p-d?BY}fjS5}RQYuY_#Jm)R{5%DCz$rii4r~}SXSpTj6sLmod1kRY#PLC?If*5i
zWvL<gPKm{-{@%!0UDJvICFkblmw<CGByo=BSgPb$tWlF#T!0!C$OR@ST0nshEl)w&
z1susBhF)HLL4J;ctwL@=a(qEzQDSa!v^rQ!9g?gdiVDzG6hNgB+CVu5qF<?`G%qhT
zCqB0zSqWkY$aaX~3bqQBiMcs?#ffRD@j3a4DH>^zG6h^T<`<;qffJM-!U(9V;?pv7
zQZ*G4ixtu!g)q2$hDsvrhKQC_7NnAEGq@7W%uA1l7zJ@Q4%<QYDA+0}h2~`@S0?AA
zLaJCuVGb_Q!Gx7UkgKnMn5#lSkZZ7Oh=PZsvv)8&1|V)PPA$?aNGvK&jRys|1|&=p
zi_(+xL8}=-g>Y#}K8&LQR#^m51PWUa&PdHEuvLN-lBp>QpbA|fu_!qwH7~s+15(E2
z<|k$5q(aJ`g8bsl5>Vk(l8-0?lpsbzjMqy{NddX0G&eP`L_=Ly7gYS{mgMWgOAK9z
zZAGamy0F4jT}J^FD7I-i`H7%LNorbRX-<i)fu6aJLJZi?Ah)7ph}*&C52&nyTL9vK
zDpOD^15`XFr7D1$2&pOf9SYa43pTO@r=v`DAZ~(qSix2yJ+&kN)CkJVOLr_vFGdMy
zWJS(7nc2t+6qFTmGD}j65_57YA#DIqnGb3=D1b9P*yqRw#QQr3fC3k{6~38ynYoEM
zjz#GKpmb5Bp$RUC(^E?z`aro_SwTS|DK$9})bc1#1vfxSG7?J^K$e3OKxSS^YEeNx
zsL0OGQvlWGMW8Z2u~?xzH7AEbSwUAJH!(X^p$Js9rsgH5DioKb7AO>FR)H%JkQwpm
zsd=eIi6!|(3W<53x)z)s6f#naQW+pA-HL%BGq)hWs05s~8Nk&k#`;`6WV15!^Yp;^
z3{)9Hv=)~n7J-8v)Z9di1|5auqSVBaRNuq`m`xB1b25|kN=q_xAa<se<|UWp=jRkF
zKx7pP5{pVQ6LVmCl8f?_^HWkP;Q|GjdHKo78JYQ@Vx=H64`K|a)01;Dv-Lm(%;5CY
z5?Bh*0XKnDi!&4R0uoCy-12jB^2;;x(!(=LGT{D!u2BXxjY0Z!6f*P5yfgE10|Rsv
zQWA3uQd0toQ%h6wGxN$)i;7cq6wov{7UlZq<y7KwUs+~xX<|-hRjMAG1#@XZPJW47
zQGTvVW^xHG?Vu0@H7e6Vr5=a_4?v&%bZE03m)@kr;>_e?JqQJ}Gz}aG-kEu+xrrs2
z$zGsFO-NB<Rsb$j@(W5pX&ttfTMr&npgII*sut<G(aa^;Bv59{Pc8swIGART7%cPR
za#1d%-vkfG^wbjc?5v}J)L()IvJP5L3AGmnv5Zh6Pb^9YmowmIF|5LX<$#jR+*BAl
zuQa!y5|r@s3K*0X0wO~^{Qcq`gWR3{eFJ=4LtNwiy%|9L|6DEx20?Df`hVE``!Rk7
zhNJup43qd77+m=o7`}k|`g{xwYxo!#rtvW_H1jbqIPx(t$nr5T+~8$kn9R$-;K<9s
zz{bnKaFd6DVFeEZLpKitLp~1!gEtQYgBK42g9#4<0}BrW12Yc;!((m+hLhY33@5l5
z7?yG~FjR9hFqCmKFeGp@FxYT2FbHu&+#~>U7jb>(QB|WMFd71*Aut*OqaiRF0;3@?
z8UmvsKt>3FMlO_d^79Mgp>2G<;*$IV5AYy$k%nJ>UaF1)h=dHJfK-ErkwD!mgpS0r
zRG<8G=pavK8mLoUtXG^^1{x7dhm3B42MiF}vomvYd=v9B3rcf9J@GJDhYvI=08#}r
z1m6DxSq4*W1sRkDse?EM<Ye%i48jSZp<2+$lODWJ9G{<;Q;DH9CqErzG-y^x57f7G
z%*jFO#}p$B1RI9%6WBlCApmH<U#}#!C^s`Nu_P7R%YkYwD9X$$(SW!UF?|6EPKcaR
z8fbz?0XBeT1sw|ljS)d7yud?<(CH@~1<)vr4tP8aHo*lkO<5taBo{K(l35HHv_T#q
zD=A7$&d$tBhYVgTgU3`rV^PRz@=HrVLsW^-krmK*1Bee&2J$Q1p1c%=#GK-M$XE+V
zHZi%RG%*M4D7e8O4}b^dOEOaPKqCOfB}Jvlkl7^o_zXB4l_5iTa2J9G4M0PA8jwjO
z@V;o!pq)Y@(!dUAOehsJPn8dKmNRHL#5FI)H7yN1LxwVZ;TzzL8j2ML*uyY2FGV*M
zY657g1rlaZ7eQPL@)+I$5>%T&A%`AQ;K3JULqUld)SJ~S&Ph!z&@hGgi9j|0X;M}w
zPt1gjkmaTpXCx<r=Swp4GC?CnnN^U9L}Vu@*rLsbLR||^$Dk1taF0AQKM$JV^a`V`
zjAC^ZjFAfh=s=l5YF;sDelIa6Gd&L+Lz#IB`9+zjc_oS9!PunKlJeBlJO!}fsh|<J
z6a{En0cQztfPy8FZH5dXgFK*tI*gW|n4^iv{2=dxO+gG36~QxeacW+QbAE1aVqS_y
zGKAIy_x~B$85rKOL;C-)_4)hw85rjBGcd&SGcY*vGcd65Gca7@V_<0EV_<OQV_<Lq
zjR){DFr4FMVA#XUz_6T`fuW0+fgysIfk6^F1~89@fx(4`fkBstfkB6dfkB;zfkBOj
zfq{#Mf#C}`1H%z+28NZ~3=Cb|3=E~vzJEM71A_)P1A`bh1H(5i28OR(3=CJf7#Oy4
zF)&Q!VqhrYVqoy+Vqj3@Vqo~k$-r=vlYwCgCj&!1Cj)~bCj-L=4hDvm91IMJ91ILz
z91INd91y=9XJ=s8$Iieo9pq0kYK2iPqaiRF0;3@?8UmvsK!XqfRopO~oDX9`XlPxn
zV5?BA1X~rRq+q2GT>z0sCy7&!TU9=2dMYI|w>Y*I+!%qG4-!(aRfvYnH|r>rXQq^7
z=s?$$Am-v>$_?}kU@LTBLJA7-Wo^+0vC)RH3fc-9FfT<LgGF@FlxQmGD;ViOn&>#p
zF+erPFgDsCRsq!<gII_;C>{bYHG?d;!nsljMFV86S;1DpK+i-$Ss^7qwHUl2FR`Q~
zwFo{<3u?TfaPmNXjLc#MBNr6Yz~QH0ixei<LjXk+B=8k%5ut?LyKqgQP5~^4K`VxG
z6AR)Ki{o=Li%TH=P@-DYNvR6Cl?v&lsl_GW4h*6Ll3$XMTBHD)hRe?@2HOf+rlek!
zs*qS*T$&5=dQoa}Nn&0)Xe}dnP8k%9@j41fE>W;W%>P1riORSE)D{I;R~Tdm%!|be
z3d#_9Jo>>3K%GVfTcky4270DC3Z{Cdv5=kyq#Yd(nk|PqslouFJ~=-xIk6-a)U(xy
zhA2x<EeX`ng!K<Vyc7-1SZK=}qNEU%-fb03^bEk(!vqat6>JqOkp&fOQ6>q&oe_wW
zKmiAx_|DDDjfX6-gamqK8g%v&Hes%pmYJ6lUtExytf7viSX~nm>!8j%WHkhM>Qh6L
zAu~-OK0YroH#I(9!PZtmDLy_oF*7edUdc)U(!qd`C?g|9rFqb>K^9i9#WM2=>F}e>
zdxIx1i**#hV+i17r<o<GxjG7<5eN_q;c1Y^5m>#TC^Nq(v!pV<C^bE^xFoem9aJ2u
z8>s838-g*k<U%zVRDi03`h7+&@uhj7u4M{HA86TKsvgo3LU88}97iyRM60J17OR6I
z3KDD3_|+>)EzU?RNY#M&O)ob;B{fGcuS^HJJ`kZDs=E+}jzX9Yuv_5k{}6*CVAm8P
zO9aBhJv|;2Rv^#SLShJMWdTyR5!6M84xS(l=fI)>>@V0r3M5_OShx!w(Ln9IgI$NL
zNe|4?s3?vH^}2NwN?{C8s?*C&tbit6kjtTKoS;e+Y*CgC>lGB`C*`E(>J^qI7H0;7
zy3olQ(G>=<3R>9Jg+S5_)Mij^1yQU4b`f+jGThV<1#JaGhz?NdLRJG>4Ts2Ldcoke
z%+A4q0UA&zL4pUO8CHZs3;^ez<l@2tB}i7oVl;g91Jq!r{0cW07h=s$%CAUENr4%U
z;vRTG3Lg6L^!9fS0GA)ol1WDaG{}Qk+Y9OcKjUCvn8XR`|HJn0KZnl$>+&-&9OGkP
zILgPsu$_;AVJ05~LjfNHgDxKf13MoB!*gB+hON8|3}w6w43WGH3|hPl44S+Q3>v%)
z49|HO7>@BUFf8R^U})iCVDRN(U|{26V0g;Sz_6H`fuWU~fx(5FfdMqfzmJQ7p_Yq*
zp@xfr!HbK5L4u2c;Tb0b!xm16+j=<}81y+A7(Q|^FkI$fV3@|iz>vbhz#zuKz;K<N
zf#DK614AM^149Bk1A`ws1A`?y1A`1Z1H&se28Pva3=G+93=9Tr3=G#;85s7nGBE68
zWnkFJ%D}LLm4RUnD+5CtD+5CqD+7ZyD+7ZJD+2>ND+9wP76yioEDQ|0SQr>qurM%m
zurM&RvoJ8EvoJ8YvoJ8IvoJ8Qu`n<^WM*L4%*?<rfti6Jo0);ZjG2Lf5hTXMz|h3R
zz+lb9!0?%of#D7#1H(o}28MdXegII~fZ@>=8EC{e6;x(r=IM>L$Pi<ApdmcOP(LWW
zC@YM%$P^S5Mq6YG3JS<gFTK$g*=UPQ0n%HBv_>H9l+hNM(%@*3feLSM`wPy1?05t3
z=K_m?3DBN7B#HbIq>WzS<~Hu$EfyJw4(Lt=@Lo1Ze;u^oz8JhMqXe{L4K!2$o`!>U
zp%oO0Q%gz<6u^6yK>Gr~48&#v$d)}&_Zy}Ii{&_lAiWi6Dg`b22OZ-K=2F&kfEfjC
z7~|<XpsOU*d%#eSwf_KBMYsn6RY+nV0;&t#(ub~g$I**`s>EqN_I?CZ7ubBzl6=r6
zFvq+US67tTV=NsDh&^Of$)FUEGnFVSL&tMqi4oM1)rC1Sw;-8_0Uu@Xz6y{=Xb%{<
zPlRLlDrlep-pxhZ^_EnWnwagDlbDWs>;jzSAgdWLMk^ozg%};6)8K+K!bXsjK!YdX
zF%3OEJ+NybGuR5CH6IEf1)vcY1=t9S0&K4;>X=G2ma!PnI0;NY$P(}fjb3toK_zs|
z0>g;Jk_@CV8L)n+i}F%aQ&LmF3n)O*2i_Z$nU@Zh1?^Y^&%{Ft5^yTQo>+YYoRP*n
zkRu;sUoy(@hK@oeXh{TYKQ!1@kTpcazk;m-wcP@)Ho%nvE_Xp=5WL?9T%zaar7D!?
z7iEK8o|y)Z)_Bk!R2u~&EAae3sOujI>XQmEFu>0Do5jz-;K|RxV9C$G@SBf;;WHlt
z!zVrlhP!+W3<vla7{d7&7<l*?7*6ssFf8U}U`R#k^Uva8V5s0>U@+%lV0gvNz;J_`
zfnf_b1H*c528IG|28JkZ1_pC(1_mi^28K^u3=DU;7#NOlF)(c6Vqn<F#lWzbi-Dn&
zi-94Ei-94Mi-Ex#I^WO7#lUczlYwCl^lX3(P6h@^P6meK91IMx91IK(*%=u2urn}t
zu`@9IXJcUa$i~31g^hus1iH`v6)OY7OI8Mk+pG)>$5<H{N>~{fY*`r?Y*-l>m_Ymg
zSr{0CSQr@OSQr@CSr`}|F*7hsWoBTA107Dt%)qdLiGiUL+Mj>R$iVQ1k%8d=BLhPe
zBLf2{{31YM2%j7RB@_?_4{U<A@<yW%KE~>$R+OaXrD%Z48Y36PM(F6wSa6ch1P$01
zCFZ54g43)X*r@W%yp;U%c+dte=r|)GTat596G6+!z^MSF7=-f*^dMWuG>UZ;K!c(2
zAk%deic5+TGt)ETb29T%<3SFKw$d?-)kKV3>Kf`8YHA``O{!@In5H4k?|`gC4hzVT
zdj{w*glr8BxEG2c;yIal#RZAUsTzoF;(77edHE&93R;jwcc9HZDJ76`PRsy=n+Fd8
zoaTapCmt#A6||sR@QE;8863i(GX+3P!-^AgLHoEX5j(a)o(An12e~2z(s~DlDn?wO
zIj$f+HC0EUARbgS67(TRG4uogjNxv0FhI;wuvLH*tRP2#urhRd29`QX6hO%XTvkH%
zor0I0f!C~ojRF-luwx{!RuR#<hTz#gTU!NnLv`?!5=NB)o-eUgP&ZJwf&_^&d?_C&
z6hUDH(U_B%3|jT3P>>H^JPA5nAyoksT}TcD>3}sFN}$X1ic|B7A*;cZ70NSG!4p~K
zsi3JV@N58h^DL-w3*PPsY8Zn!CHe452<%m51xE!RPd`@$w;+FCg>VnoAXkMD4_5{E
zAkTmR*B}MYU<E(_aD@<mu&9$`uq#wsxQDBsLU4#<P>83WyADW!r=Mqtr=yQ&lq+Z-
zae#k7sE=a^#Iyj%5D$fL&kzp<M?VEmzYy1;0Dm9H5Kn(U1%EeK==eGY2YZIOLfiRJ
zYmyU-Qx%M$Gf|+p1Yx8p0?=+nN66+w$TWtI0?4b-`Z5Q!R}*_B2r@?*%^=cEK(z}r
z_6OO~seyIVq+zPAF{JebvK@qRxd3VVC3feh<`kzQ#^*ssL5IwVXe@whVqBFl$S7sv
zS_o9M5j=sT=L?<G1UnGiQ~;*}kdr_d+_^%Z1c9#7&G7}#UZrHFfzHmz11&YwOU^GY
zDTa{I>d>Pt0`iMf)nmbepgJKlr8F@|559O6G^GgdyQ0_)vcIS_4>T7}W1B$T*i0P-
zWXnMHXlh<*E@+o?D)zY`WaY)sNm7uTK^W4mvVtt`%`8y>Ev3%Lgsj_z??hL~FH(R^
zh8Kh6poeo5K#tjg2SsKYXb*R8K{6uk5sW$%>%lRMv`t$_p(qt}B#;6m?!jdh*hH|E
znQ00d@PZZ6=re(w3xX<aXRBZY^>k@*DyUaws{lC~1e^bntXIfSD$C3-EzYR~+o!Am
zE^iEx${Vm_^gxSG15-e!szA<Afr!FpOrfm_P_+S@+66h*&Q`$$5>8Na4WQ=2dNPn%
zPmmK)`xFRqct%t}_7F;t*n&=DK?x2WP=N_D6@*dLYTJU_&|trU(iun+l(;}NsJ{g7
z??9Xl7Y6ypRsniIlQPJ{)S`l-)DlpI3aV8Sq36XIDdd&rCV>jwBIwab;KO3THLHRl
zWaR(@=u7~R%fL=Su@zipf@(~N8qlHu1v>>p0|NtydtfOEbY>B_jDXCzfD=s__^=`!
zcpVFxy3I_?QOE!tFH@2aIt>YY_7rGQGuZ!d&%yeL5ZjAOiZnnA{6U!nY=^(Mrlx{2
zXebGEt`$f)AJQRF23OxmXWHbXmVizvQAo~*tMKy=akYZDDLXZ_pcr(1LkTFvK!;+L
zC?uwVR^Gsz3bsxm0~DB`CDyRL(J7GrEm#)qpeb7g;*PxmdsrFM$Kcf)FdrutrTQm<
z8mc*|#Uc6m&N=zTsT#1BG9>6hi!^dlK|u)d6X?JV@EJWhiN&c!3dPBZ;1Uv4mXv2C
zmZX-Y7J;qCuo*P9jlAFitO{ZWG`K*)06zJnEL8!ttRqDsHLs*7Gqo7pwMfoS0k@fw
zQWc6)6H^kCa$qeXP)n&OHL*B953CT}kIXDq0G~Pm-nUSm3O$JoJd&7_S)5*4433h#
z)Z&uV6ot$*$dVM$Iv`MCr;wDHmskYeh+bS$1c^K7nP!kB@fw*>1IwXj8>OTs=0J{4
zf?WBgUJNs%C>2?!CM04>TQva=DcD(QCYlQN3W+)8iIv3)B}JvF3L5!E3Mu)Z5<Uau
z(#&FHSsevfq(B-W#+sl5C_r0C?7^lQflqGB%ZD${D%Jrfad660$j=64F3{5d(wrQH
zlKk?-q7?Af6^K_sjd<wr6etsb4K&1JU{Wch0s<SFnxYS;L7j*Ev^3q4#G>@n66n!D
b;Qj@oegY5lCMpyd6@W^e#FEtXN<9VuPLnkQ
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 b627567f5f7baa20c97ef87c749e9283073497e2..d737650ae8977398706a53b075ca34bd36a1186a 100644
GIT binary patch
delta 2417
zcmZozz}&EaMKsAE%+puFLeGeSfq_AQf#G^YMEd-VqHp>6`5EdN7#Ns9;vlKXf&${1
zyilGgloo{2j8K{%N{c{g7bp#r=Y#UaptL)bhRL%+`HGVb1;hmfpgcV&J!!M0fII&r
z1s2|XUIvEKAjb$WFbGT*RIsmq2^Hz(Wnc*4Wnj?ZWnfU?WnkFF!@y9>!@v;4!@wZS
z!@%&6n}OjFHv>Z}Hv@wQHv_|OE(V5^Tnr3TxEL5rxfmE2xfmE2xEL6&axyT?;ACJ(
z=44>d<z!%B;bdSq!ok4M&cVPC$icwC&%wZOiJgIA2|EKrJv&1^gAqFe!xuINhV^U=
z4B2c93=wP$4E$^i43}6L7?!d!F!Zo8FqE+}FsQIHFkEF}V5nkYV6b3eU{GaYVEDnz
zz;KzFfnhB(14A@31A`tj1H)@328L}+3=EA-3=DQm3=Awx3=9Vt85p`585len85lfZ
zVWyy<pir_oN3oVi7Q_VM^!&t}_#7PtFkPfz3ua8#iIiolEX~bSvf9k8_fwHKALI>A
zP~0*wC{7j(Xs_qzWMJ69!NBm1gMr~I2Lr<*4hDu+4hDu44h9BI4hDvM><kPO*%=rn
zurn~2vokRKWMg31#m2y}lZ}C)k&S`Dl8u2ukd1-iA}a&Kd{zdAd8`Z!4Xg|dL97f6
zEUXL+r&t&mHnT7=OlM(WNM>PRFlJ$3c+bqhu$7sCVIDJsJwp>S14Aw|149He1A_=N
z1A{O#1A`DV1H%C(28I?U1_pm71_nJQ28NG}3=EqY85kNE85jx~85klM85oinHzx*0
zu}{{GjGwF<nX>tg8yCyw+Sulz`ot6lhHhpChH4gwXJM)L7(WBUe0~OoBz^`4JAMWR
zBYp;kGkgpT{d^1zDSQkJs(cI#pLrP=uJAH2Oy^}_h~i~n;N@jtxXr`Bu#AU+!5x~W
zgm@Sjo^vxWtm0;1DCK5gu;*rA5aVWGxW>i6Fq4a+o*|iwfkBarf#C@!1H(2>28M~8
z3=A%u3=C|X3=9W37#QYpFffF1FfiD2Ffe>#XJFXK&cM*X&cKk&&cNWw&cN`Qje%hS
z8v}zQ8w0~{RtAPAtPBjBSs56*SQ!`+SQ!|&SQ!`&vp_t!f`x%0orQrxpM`-zfQ5nK
z8?!qD!y0A=hN;X93?9r34D8Gd4Ck2`7&b65Fic}&V6b6gV0gsHz%YxEfuWg^fx(-R
zfgy~MfgyyEfgu={$Ur%8^2Al5lk3+@PG(uFFnPgVw#f$0oMLH3iMgp^sX6({nI)A$
zsl}NorHMK5lM7ZWOzu<R6N9D3kfOxo?99A$=ltT5_@c=hHPj~8sEWzO7iltplz=cO
zv7#u;fhl8*pIo>~c=Ce1tds3$i}HXJ#uw=&=ND9JOcq=#Jh@@L#AL123X}h++D@L(
z!!p@Y&2#dcy<8yoO}^42#uQ&Pxp1}F<T-me5&Cn`^s}s0U{O{m$(ZctqCEMpqUL0&
zZmr4tTVz>4PMh3V8#MX6GRNeGRV<UQE2~ZZsjN9!VJ*w#hV_Q9keD2$;xu_-UHs%f
zb-|Ofx@#tHQ<a3P!R|7b$%c_4lWXVlZ@#Ez&7KEJrVqFn7(%!q$y5v!=nM=D*ZCP3
z*77qjr1CQ``0+C^i19NpyyatHILybukjcluz|Y6PaD<nEp^}$@L6?_-;UNzL!vh`$
zhRr+-40SvV44ym;3~W3M3}?6*7^ZVGFobf0?AXl6@|<gOx?=TYHKpg1gKG6A2f4^k
z{;S9>8(*XZDLPPMqew@=Fx5zNvagE@NR@be4mMRelli+%#BrDalC#924`MnzF`y(X
zq(rl$R#YB2(O{~Bq?#SIqR>=>8eNct0!_T)D7xW^7nEA$L8*80X=S6yKb2+EKrvI#
z&A{*)8Z)r!?HfM>!y0}DhN=7v3?BRp4D9?24Cnb67&h=RFihiPVDRB%U@+xlV0gjH
zz_6T`fdN!)fhs36UIvCcJPZs?JPZt0JPZuVJRq|-GqSXDPfnVxF<dhkt_;cuDlhcw
zQ*(+_tr$RIoy5+-V8#K-1+dEh6+Z*R6@CVW%lr%s4g3rY$@~lqPS7yl!^gmo&&R-!
z$H%}R%E!QPotJ@O2DH$Q=Vf3J=Vf4c&%?m59@-eF;bCBi<6&Sh;9+2R#Ld94f}4S1
zA}CvcN^eJQ1_nWH28Od-3=EUG7#M1}7#I?`7#O^{7#PgB7#KcrGBAAPWMKHf$-pp=
qlYt=#+Ct#rWMDYY!N4$)gMlHAgMq=H1LTR#j4T=Ko09`iu>$~+?I2qK
delta 621
zcmZozz|ydQSv1KY%+puFLeGeSfq_AQfniTrMEdfLqHp>6xf$vi7#Ns9;tUK7UXuj{
z#5q}^JpaiP1;jU>5U}8%#Mm%NfrVFzfq@~75n_bbWI+Y{`b)eF46At=81i@-7`%8H
z80>f%7;f+|FwEv*U?|{WU@+uiVEE6?!0>>ZfnhN>149}&1A`<t1H*GJ28K;s3=F|s
z3=HyI3=DEy3=E$+85p*4GB7l9GB9{^GB7A{GB7;iU|?9v!N8Eu!N8!)!NBl=oq^#H
zI|IX9b_Rw3c7}QeA$A6avuq3uoooyYWo!%#x@-&#A6OX}4zn^atYT$gn8M1y;Ka(n
z@R@~yVFn8WLl_GKgEI>QgBS|~!$)QYhSSUp43*3b3_i>Z44lji3|E*K80IrEFvKu1
zFeouGFx+EgU|7k>z>v-e@p>xA`_S-VV3-`|C^I?Yb;aa<Md`@_Z@DLbxFWZiL+LR0
z<h%EkH}mNIP~_!iU|>iFX%b*y@SiLg&|a_0$-p4a$-uzK$-uzC$-uCdgMncJ)Q=_{
z3=A*X85riUGce3%XJBw*XJBAqXJ9zW#=vldje((uje)_1je$Xije+3~)Qc-w85p`*
z85m+&85sCk85pjzFfi<6VPIIq!oZNn!oXn1!ocv0nStQ|GXuj)W(IqPUS<Y{3T6g|
zWM&2id1eL%Ic5e1Sx{IoF);KqF)&0iF)&y%F);jLWMJ6G$iUFe$iPs;2=PV|<K%n4
dB`4?XmE2q#Xv{u&{!P}&+-?^)PmIhj0|2p6YJLC!
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 5b61558..3be6cc0 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -12,6 +12,8 @@ if importlib.util.find_spec('mim_solvers'):
import mim_solvers
def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):
+ if robot.robot_name == "yumi":
+ goal_l, goal_r = goal
# create torque bounds which correspond to percentage
# of maximum allowed acceleration
# NOTE: idk if this makes any sense, but let's go for it
@@ -46,29 +48,64 @@ def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):
# to do so, you need to implement a residualmodel for that in crocoddyl.
# there's an example of exending crocoddyl in colmpc repo
# (look at that to see how to compile, make python bindings etc)
- framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
- state,
- # TODO: check if this is the frame we actually want (ee tip)
- # the id is an integer and that's what api wants
- robot.model.getFrameId("tool0"),
- goal.copy(),
- state.nv)
- goalTrackingCost = crocoddyl.CostModelResidual(state, framePlacementResidual)
- # cost 4) final ee velocity is 0 (can't directly formulate joint velocity,
- # because that's a part of the state, and we don't know final joint positions)
- frameVelocityResidual = crocoddyl.ResidualModelFrameVelocity(
- state,
- robot.model.getFrameId("tool0"),
- pin.Motion(np.zeros(6)),
- pin.ReferenceFrame.WORLD)
- frameVelocityCost = crocoddyl.CostModelResidual(state, frameVelocityResidual)
+ if robot.robot_name != "yumi":
+ framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
+ state,
+ # TODO: check if this is the frame we actually want (ee tip)
+ # the id is an integer and that's what api wants
+ robot.model.getFrameId("tool0"),
+ goal.copy(),
+ state.nv)
+ goalTrackingCost = crocoddyl.CostModelResidual(state, framePlacementResidual)
+ # cost 4) final ee velocity is 0 (can't directly formulate joint velocity,
+ # because that's a part of the state, and we don't know final joint positions)
+ frameVelocityResidual = crocoddyl.ResidualModelFrameVelocity(
+ state,
+ robot.model.getFrameId("tool0"),
+ pin.Motion(np.zeros(6)),
+ pin.ReferenceFrame.WORLD)
+ frameVelocityCost = crocoddyl.CostModelResidual(state, frameVelocityResidual)
+ runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
+ terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
+ else:
+ framePlacementResidual_l = crocoddyl.ResidualModelFramePlacement(
+ state,
+ # TODO: check if this is the frame we actually want (ee tip)
+ # the id is an integer and that's what api wants
+ robot.model.getFrameId("robl_joint_7"),
+ goal_l.copy(),
+ state.nv)
+ framePlacementResidual_r = crocoddyl.ResidualModelFramePlacement(
+ state,
+ # TODO: check if this is the frame we actually want (ee tip)
+ # the id is an integer and that's what api wants
+ robot.model.getFrameId("robr_joint_7"),
+ goal_r.copy(),
+ state.nv)
+ goalTrackingCost_l = crocoddyl.CostModelResidual(state, framePlacementResidual_l)
+ goalTrackingCost_r = crocoddyl.CostModelResidual(state, framePlacementResidual_r)
+ frameVelocityResidual_l = crocoddyl.ResidualModelFrameVelocity(
+ state,
+ robot.model.getFrameId("robl_joint_7"),
+ pin.Motion(np.zeros(6)),
+ pin.ReferenceFrame.WORLD)
+ frameVelocityResidual_r = crocoddyl.ResidualModelFrameVelocity(
+ state,
+ robot.model.getFrameId("robr_joint_7"),
+ pin.Motion(np.zeros(6)),
+ pin.ReferenceFrame.WORLD)
+ frameVelocityCost_l = crocoddyl.CostModelResidual(state, frameVelocityResidual_l)
+ frameVelocityCost_r = crocoddyl.CostModelResidual(state, frameVelocityResidual_r)
+ runningCostModel.addCost("gripperPose_l", goalTrackingCost_l, 1e2)
+ runningCostModel.addCost("gripperPose_r", goalTrackingCost_r, 1e2)
+ terminalCostModel.addCost("gripperPose_l", goalTrackingCost_l, 1e2)
+ terminalCostModel.addCost("gripperPose_r", goalTrackingCost_r, 1e2)
+
# put these costs into the running costs
- runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
runningCostModel.addCost("xReg", xRegCost, 1e-3)
runningCostModel.addCost("uReg", uRegCost, 1e-3)
# and add the terminal cost, which is the distance to the goal
# NOTE: shouldn't there be a final velocity = 0 here?
- terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
terminalCostModel.addCost("uReg", uRegCost, 1e3)
terminalCostModel.addCost("velFinal", frameVelocityCost, 1e3)
@@ -104,7 +141,7 @@ def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):
# and this should be 2 * model.nv the way things are defined here.
- if robot.robot_name == "heron" or robot.robot_name == "heronros" or robot.robot_name == "mirros":
+ if robot.robot_name == "heron" or robot.robot_name == "heronros" or robot.robot_name == "mirros" or robot.robot_name == "yumi":
xlb = xlb[1:]
xub = xub[1:]
--
GitLab