From f683e23bf78a6ae3b501e965a396cac2103b34d2 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Wed, 27 Nov 2024 21:47:31 +0100
Subject: [PATCH] figured out how to create ultra clean differential drive
model: just combine a revolute and a prismatic joint with no limits. easy
money
---
python/examples/clik.py | 1 +
python/examples/path_following_mpc.py | 1 +
.../__pycache__/managers.cpython-312.pyc | Bin 49250 -> 49670 bytes
python/ur_simple_control/clik/clik.py | 4 +-
python/ur_simple_control/managers.py | 10 ++-
.../optimal_control/crocoddyl_mpc.py | 12 ++-
.../__pycache__/get_model.cpython-312.pyc | Bin 10863 -> 13613 bytes
python/ur_simple_control/util/get_model.py | 75 ++++++++++++++++++
8 files changed, 97 insertions(+), 6 deletions(-)
diff --git a/python/examples/clik.py b/python/examples/clik.py
index cd07a47..be65bc0 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -23,6 +23,7 @@ if __name__ == "__main__":
print(robot.getT_w_e())
Mgoal = robot.defineGoalPointCLI()
compliantMoveL(args, robot, Mgoal)
+ #moveL(args, robot, Mgoal)
robot.closeGripper()
robot.openGripper()
if not args.pinocchio_only:
diff --git a/python/examples/path_following_mpc.py b/python/examples/path_following_mpc.py
index 19dd803..8afaf57 100644
--- a/python/examples/path_following_mpc.py
+++ b/python/examples/path_following_mpc.py
@@ -30,6 +30,7 @@ if __name__ == "__main__":
if importlib.util.find_spec('mim_solvers'):
import mim_solvers
robot = RobotManager(args)
+ time.sleep(5)
# TODO: put this back for nicer demos
#Mgoal = robot.defineGoalPointCLI()
#Mgoal = pin.SE3.Random()
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index 8aac4dcf219d27bcd32fee4d77ef29b50de1926c..0fbef10f71ac938cc681cd484d7d87e0a2dd77e8 100644
GIT binary patch
delta 5686
zcmaFVz}(it%y*iXmy3acf#G+9d%EF?iF^`_4inX@>vKh;L>U=U7*aTL#B#->#KCON
z9En`XD9K!@D5+fOC~2@5SB^}sY?LgR&7C8cD<37Fs}Q9C<}>CfMk%H+q%h_vMJc5)
zr10b@=c+`hfaQ2|RCCp$)WB@M9Q9m{C=D>1KSwiHE6QDqk%5yTMWBTtN}GwnogqcA
zg&{>~H516C3=B~^U@_qqh7=LFm@ZgMw1pu>3@)ZOIfXHkk!A8ZMny5vjMSq1y!ga|
zf};G2%-qD1%=|nTm(A}OXR(X16iG2KFx+AVE4jrErczQSPvtCSlV@OHC{ma#z?Hx)
z2Vz3_o2$7FFfuA{w&CGphlneHB{zo(TQG5%FfcGUGB7X{n@yfBYQy9Fft7((;tK;4
zt1IK?d!o_&jG~jRRKpp0CQnjL*5m`(%wLd`Us94<6rY=zmzbVfq{(rMr8qSwt;il^
ztOJN}nk=Vg%xJthNX>we&6I(GA(?SftH|a#>O36wAj^teK&tFOrm>V{=B9$R-eQd}
zE=esY0-N9rl4CE(FUUzvD=BgVDfa{sPLrSO+lYpOWI57POG4ty<5P>gKr%j)Ees~}
z+AuIMXmS?$f<yu*-!RZ-^9L~lCUY9fG8%7IH@wBfXu6rnq=bzzd~&%}q=G0&+7?74
zfe4VpZZYK*++wXtEy^!0vY-6HN}SCJB$Ygw*V@=E9mLE45t$$Y>?p8{vq0Qz5CQVx
zE%xHXvQ(e^bg1J%;%Oiv21LY8ZnKs%2RZT<TXAN3UTN+v*0iF;+|;5#kk8o5Qgh;y
zb5n|085kH8K%mGNqz&Y`D!IvtYQme}Ss!NP1cj`hCVNr#<XyJvia{VHg&+c?ZY4tz
zNE)Ki93*N!naNHA92-UEn+@#hS($jaH*a*l$T(TfRg9B0TZ4h2NV!CMvaPE-qs(N-
z-NL*%C7=`pPRAt*ljYsyCObq(PIf31p8VdGTTmG+!N9;!!&t+V!d9X>IYC=yaznWI
zWPNSn$rsc)WHK33*jKYal+`fAgMu2&sbS300a>6fP@)eO02{KJ5yGxvh?jt{7#LDG
zY8c`rC(FCZGn!5=bQjj-L{cRUSH)Gs5HADgan~?pSxn}3m*kg)i}BPj#LG=y>7y_?
zNLOUCwg>0rE$(8RE({FCOkNC4jFUI|s4_Z$Y+&chmSSKi;-9?HT~Zn@pTb|m5U&U`
zt%fnnW%5Q>(aB66!XUF4T|hedd9&f_p(;cK;+0^UQW$C&;*}>K+$hiJ22#!_FnPU)
zDTK`_2o8P!63@wup2EC{kY&hnoBYvCcyfcM@Z^==VgiVAg^{5KD#_?O`C_lk<hh>W
zP{V}5hVhs9gAC(_8-^J{EHw=A8j~Hp)LHpz7_tIDu3{GfB_4!Q#-Pc9zOvj}Fkhg{
zFltZU?JOr%Jf$QOrX)oa7DtQ>CDD@?dTVm)!Q@Kf!F&TaKN-a5HG=VL7_w489%f9R
zywO#dnK8>@vS6hwGecJ9WX3{SUQmh#E6tOFI7BLi17=wbLp)r%2BdMLs|ZuP!sOFF
z%8WUn)W$p;VmmL~_8Nw)1h8%SlNVM<vohB(XBC11fu9#<C?f+*OdLt9L>24^1_p+#
zB9J>LpYs*q)qsiBFlK?X&t!pVER%(bg_*KSL52#m)i7r=A{oq(RX+J*ubhgem|qpA
zp^>?sfu4b$A-Hre0wwJtP>L=BCE=pb$yI(*rr`_>4C%&T@Z0`5%(@~qkg$B~>%bjy
zoDM~x=q^%WU|`6s_|Cvkq&oSepB#rx5wrw*?`OnVI$6oz*qj?wB!CMeF%XL%M0kUU
zVh~XRDu$Ty3X5VvJaCyO0AgiKZuhrfOqhJYKSLf=2p5%s6f&2^7v5q{j4vz#m!Zrc
zl@*iS1Ip?_1@|r1#G<0a%Az=sa1^L~W`UH?AUECOEzV3yjZZ7hO9oYPMIb|qK}80l
z)B}}kRiZ)pN%<wdkQzrXK0Y%qvm`#gn~{N`*pP*Rp@HFsh(rfV59bXzrR#Er7v&5)
zSgvqLPqqnc;GU6riCg`NpxR|Yjm>idConUbPSy*JlmOXO<j%mru#yq%oMKS%qflf#
zd48x2Bgf=Dq0(T5!E!}wL2hO$S_pDKNJG&&P<;tg?inU0%gw~VP%Oj%b_c(3e?@0S
ze{E-Ncl`wpxgyieEn%Td;$TDgio)QAfNSXD$sZzAb4o#ilR*S1+#z8qQd*FbSd!`n
zsucoq5|dMNQ}arSrh|h!-#1N@3GC=w?1k~^nPsVY#h~Vff&!$tk7r<DKnpQgUCB4O
zG4de9V~S9Z-C`>*O3h2o00#!D-?m4oF)B{J7bVANy7_NZ5lcNdek(wZ-~rW-ZY9B~
zdByofkop`{0u(I<NeZT?mN+JtlqTkcI0v}p7bT}cs_+DmA5%dDIF+pesbx<rODzJm
zBbI~2LE)>(4)GgzT1k9SYGO)eUV1UOVk~mb0qI~$D=EqcDJ}rT13Rpag!z6M$oCQ`
z=_NTaCo`!C6r3R{lMNHTf@?uYo5Ue8jjetQ$h55>0udG9^Z}0ZZ6FCy$QNw~u|Q4)
z$Kx%&`1I70aB$#x=DE7wVopgZELsFogPcM@d{DBl5<+nkxUu5_N;nHAt7a@^G~IkU
zgPGA0l$(pX85kJ+G+Cj!7G%H@5U~_Q%!Z^!aN~isEHS4v6(#PMPUg>2U@Vwykmb&3
zy162YmBkcP8od0^z`*c))=V(4hZHgpTX>3+LB@jOt!O2P1<IC1^CoY~Q}+U8-=Ya1
zL3n(Db0{?Rf!Qdw9|eUynm^N1O9HDWE9akKG~N6m|37Oz$UQ}8K&GAr5g=!1O59>j
z%*g?lAGi1%OG=6|lS)fcU5kqHi?F*7r1KnzI1eJg4h6>;ICeZhVhAUrMz3`dIIE$>
zDkx)WLJOuHpj5_El3H8>@gbr-n!K`1jd9-Q3uPNv>Ot=I)8s-p?-n;a*$1XT;%*+u
zB<}R0%z}c{qWH4Z97sMuI1QA*prLt-sW1g4Fpq%(Qy4YTfC4k+EGX4X{$D+f(RA~)
z8ha*Da10_01;^$7$*=09g+R%qXcs7-FoE+)k?~~ldOgPXlilj=8BI60)r&DYgNr6k
zh(FK`0R<#DV}i`M#a5J<my(}bG!-O_oW>{LZ_r@;JDH;~hS7BLhkVJ+b&VWM^_?JP
zU;<?MOK`ma;%f2{VOh~#kZ!O|S3oRq(FlsATO39CCEza6EyfT{W^h?ibOaR8?74{r
z1)$ObWRrpd2eddmKAE>gfsu8xUdtIq)5*VEB$;-wOx{={wb`KcIn(6A_63Zbn}s?g
z85y}Y>vYC5)`R+2enlXM7FmKa8zY1bif^=vSyKp+J8rQg=VTTX%>}s?93S9flC8KP
zH8sVnXckBqRQ%myFG?-WNGwRb#gda*4DH&03PMQzvW0=clHnF-W?o5ZI;g-Z0vAZ2
zlnwLg4UkXS3RA!x1c*nW{w$wt*Db*&!py)>yk~Pvw<sge4Q~E^`%Zgs4LF&%M|*N%
zPYI*xWWgnpoB#LBWi$XKPe?*d1i24fy7z)uAP?MP$uCIFL#+_b^l34=PQE|Ef3jP@
zAlxJlM$4i%pparO!fzsLa!!76Dyos2Cg?EwPrg4Pfzfoc!^Boj#?HydW*xN!l_W*$
zL2g0Ky^zd{NTz7n80>}Q$tPy(*FOYl24#g?0`Pnp;v5hFasjB6xW$^8S5R7Vi?z5Q
zC$j`xh23J#NzE%V1Zflo5#K-rxI6^6l)#N4P%}o81Cr`M@dB<2ia;uFv83lG=G<aT
zy~UZHpD2|R4~}3^kFJ;locTa)8c;1YdBPkE#@Nj#=lo$21Dgr%8KUH;+Y8+kXM$o9
z)H-Wm_#nZ+$$Onc{vwC`bq<4z90pf8jEcY!IXQaKKWlYR;B!D?5)^gda<mAP0E?<Y
zUI5q3;Ci<f#6_`b%3?Lf<&$?W4h2VNFB>TJ7s*auvcy0ET=FAKxrL$k%MzW*=F5~P
zJ1-Rh>u%R5;$&oC&}4yR3$RwOjhI^5ml-f_nXI@}X>!yu5k}L^`O6+N8es|QB6!M#
z7L!H1phh34KmiA3%L+BdU6WU@@T>>5{vk~oMmJ5FB5+r{3FPEv5YYl6Kut+VqS^@J
zg4#(%yFn~ao1q9)Nf&|ZSa9M4mpY&_xac-W$vqHp97L3Zh<703BZy#TWMC*_W%OlW
zC}IaOxj+ODBLl-NmXyrok|KT(zZIks>_Tu)z8%B`5&0m`fWo<j;Ukk6E5|3Zaz<9s
zPgRo-tn^^hW%XqIlr))dl?k}PQq(@#z)o^<;wp1S@69t;IdgD>!wb|cDC(K~d{d1S
zm=9`)-r`Np&&^HDONlQmO)X6=5}aJSS<VIA<OC(CBKTm%Etb5*+|*k<If=!f`Yt{t
zu_O@@<VB$NmL?k{v4ag?J^B7-OE$0{f+ovtkzstY*?LPnBcti&*<1fJMrnc@F3d%u
zAn&sl7H3wa-r_1KEs4*|FHg)YfkmM#$k(ta1O){sQ&mX?6y+zU78k=>`NgSuDb5gY
zu&_-2vfYN!bh6=&3s&IzimgZ-Y)5KtK}qE;uJlxx9YvC$@CIe@B5*SRC9zxYRACgH
zoUqfC(RA|PD9O$Hclt89fIS9JW<_Ek{SqJo<clH&kTskosYSV&d5I;dMVCPCMK%<~
zPn#UDM}<*ra_OGezF_BZ;!b}8AfrJZE>Z!pz<D1nd4M}&AmYU2wR`oP!086W)zrVm
zS&*5RpPZbLnO_7d7;mxWmF5;y7J&;ya5e{LI#9g;Zi*Ly+Ac*)Cd=-tVFQPW)#Um6
zOhk%6-M?Gh$@w`sso;)RaS_ktNBgw7c|pDb4f7T8PnOuPRS)X*-Qv$J%_+$&fW%H_
zUV4!rNEJApaVHj~C+FuD<fN9Q7J)_yZZQ{E78ij^&s(e|l?AEAMIxXIiz&aj2sHk4
zixb?{%g@g#E&``S(D-8!czg<!9g0A{EJB2a04Sy1;;_lhPbtkwwJS24d~v@qOW9?H
hvdJ6=Y~;c@8TCIhfXMGfLJ^FN&WxWJz!Z4w3jpknbBF){
delta 5631
zcmZo`VSd!W%y*iXmy3acfx*bmCH>y9iF^`_HWSsW>lqnR7*aTLM03TW#K3IM9PwO<
zD2ZIjD9K!@C@HWQSB`Y9Oq2|m&7C8gD;Fh~D<35f<}>CfL@A^&q%h_vMk%H+r10b@
z<tj%hgXMU0RB~0LRKaY%9JO5aD0MKKKSv{1GfI<@fzz2GMWBTtN{flXogqcAg&{>~
zH515{3=C1)U@_qqh7=LFm=0J>w1pu>3@)ZSIfF4%k@*&9dTL30ZhlH?&Mo1L)S~>n
z_{4&OqWp@?+{BX1{JhPaOtaW|nTsSD7#MD`f)z~O$yv%K%fP@;BsbZFD}h@E#DwrS
z&*wV8$f&qEfrpPBA}$A(+*~Pa!Ng_6z`)?hz`#&!I(fIK4Y$PyRt8pyFAPkqR-2i{
zqWKv`CdaFWGjdPfuA0oqJ6T#ymfIGj+YUrHOm<c?W;EPfu4cf<XuNrax+;ggJxIa{
zMA(2_$5N7+n_2|Ya*H*-xFofp2&@xi<1O}r{DPd+w2~qhkaBkr;V_xUz(zC}q?02(
zwIn3IJU+F^10>@$Io4n@uN4CWgC=K@H%P>P@?QgOHeV3aZ?c}DETiFOAH!QrjK-VQ
zO-k4pLnqI%inJ90Nn3-61P}po*e#~Kf?KRrsYUt4P=`T6nkldB7F%&@UP@p}kpoC^
z;$$OhW4BZgGYv$fg9xxQ!G6d9aWg>#$j`UfixbOIee%<x&IO65fQTp%5j}aewVWu(
z*|*q=Gt=`*b8oSx6(#1T7Wq%UXD!ZV2vTIV`M33LMowb}1_nP(_M*(m_ifb`13)tQ
zAOhs~l?+87=_0UtQ;?|XWOX|YHn5=S=1{wOR;CqPo3A-vWMpKV%-ABrSCmr%N(SJB
zSRyyM!CP+f{Z6sTepAF`R<nRb8ETnI6d^1Ih8o5irWDo^<;f3KWF~KL6Q3;UEj;;x
z4@X!gV+tEgLk&YbC|tpMY8bP$zzh_kNT5U)CQ!q;ni0aTVTc!puoxIp*lQT#B_=m`
z%TL~*BOI5)fuu+Zu86aSAzm8J<EmlEGK29DbQL!PLk&YbD9M8bQ@Cpw;^CgIVa&3b
z%or&;xqZ6u<PYwgj5d=4S%mp{vRxP$ikZ9^ni!!fL?-WYSKvwE&6Z+dDB=eT2C@iC
zBPmSbt6_*&0NcR;3aZHqJVaP(7_uBD-*cCqywF3KF9k_;4I@ZisD>dP9=a(YcPdRz
z*euWJ0&)dAf3`UTL(!Z`{bG|tJS8E5ngZDx3=Bp7C7`4Rwrn++1bb2m%4Dcv$a0zN
zs4dLsIhiq1cyhXrs6dJ!tRP{8OHbbGDLnau565JMEl}gtg}}!1mw<8(*fea$W2OL>
z8isiF$q7^CCeJHmXXj7htzpRW1A9Ukl+zI!7y~9dwh2$(@5RNf2{xaBfdQK=qt@gH
z&T>-4Q%b^N%2GsNseqB8Byw_`k0v)f8I{C>`T8)mC5a$DFDNH~g=!eGlE49y3J#E?
z?xK?~`0z5OgB>#=oL!)XA>IV6l7WFCPYU89z7%$tjUegCd)yT!8+h<ee(fyGTC}`|
zF>A77v@lE2Y>4H&aLa2Lvf{v&<-&|-t7WcX&dLV`2tN<ZOhyKnm^hMHi89O?Sp{IX
zhWiO{tHZ=<!1<P~NVbL{O9O02@#KxVB0@FHnedDRj^9$S)d9_%^_2{oqJCAJhDPRk
z26_g1hTw{!2$YM8K$)iqRGt=r>Y|ryppuofBtJjLAoB|&14B_L0|P_4F&O-|e-3j)
zkqSsyzV&tB4mnPTB2fA*0;Rmnith{zMIh(D1Q}N)?CkI7=Q=sfpP$1TR%X`v8!;A7
z-r#R+1TOhRL4_0_i0}jvg&?8`<O-&|!lGyp4_v_VgIH;kr2;G%<0c0OWXOZvTmn+a
zTozw=i#ajAun1g)GlNu?O}-US<_fAeZm}j76(v>{#ejq(K-B{aq<R1q;kS5;GgDII
z(@OJ_LG>m$Ac{eyo`Qmc0;CWPm^?L5o{@d>*1%btD}xG|8I31J2T!&OW0L^8z?Ff4
zVI?Ej#9~k>rch)!IVMbok#%xqm^4Ujh%6}kXfhSe2RQ(wyl4%mI)y1_3zw7SU}9h>
z7GhvvXkfU(FWg_zS<zqHS=(KIfkUpyc(Y@8D3dtY5Wb=ixFMj@tf+AE+DO%$VvyiO
z5CICfB5<IHloq5UmZZ9Y>X3k(#N^c6)Vz|Sso?O+_f6Ae0z3H@dtrQfW?5=pF(|rV
zUXEp8V5kxe%1_EK@lDK2OiwM+gVnX%lkK7og1w~xb`jGpw(_FXyyT1`OrMoRt1-$=
zo*ga6XuNq>bP<a(IC{!JPT&F6u5Km4sd>fuMMa=Ww+NKSixy7yPY}-nMO#rE$Un&-
z0-PjPfV8nEmZcVf+_MxU4zf{`9pVq}w37Iu)Wnp`y!7HCe~@ezNCQ(^Nl`9{mj?=R
zb_NCpP{hLHcFE)m39^ieliwwLWi;OWB(a{YeiKObW)Oi0Rd7Uuqi+jH0u+cvTR|+4
z3&2rwi!VMswIm!I5}tXkuD6&|QVNR}fYcyIHi!>Of>lB&feUW;xPsEa!pV;^mNFV|
z?#N_jbOhzGqE2vS&kD^vAOjYGh{YgcCMY#SBa*c&F{d;YB^nn`K9i-um^=AxmOG>I
zX47m|7E@3jefgh(f#LbAnP6ZKDGDIA@DwG2j9myKmV*dT7AcxDIX_?B3mhkXAVGL^
zfO88pA%WQ_wjTy1A~b)dr<Md(On#7mhS7NQ+JgVA^&s~YodTJ98bpAcp($~TIWZ>(
zoR@F$IhK?ZWhRxDq`DRr<riUhA4umJ5OEenfE@~sF>vg-fy59_MvY$UB5<xji&an_
z(}Wfh+d!#^r6jeu1mZ(P*)Tb+T#a$|=HBuREJ+}D`)P6^oOg>mJ+;I!xui5P2i91e
z12Ty_y(qJwAhjsIEHwwxszo>rl)#{&d5ftq1tl<#OrBpO!FX!&j+$AF#+!p{?U|gw
zQH9VAj<mfXJB0o6OFVN6a=<x0H3b}SP;-lRfU*G-I2#lhPQFsF$M|M4bAvsj@n+`+
zF-B)_mga<b1B=l`;A{sn;}%;{VqQvqZqZ~=@FFMi$#WVt*nWZ1a`5C2Eg>wu-bKcf
z*A_@_wr=8J%4r8F2NNJiyaX53Ag(4K5tbI+2I&UdcnQP;7j~e?y2VkHUjpv<++qyT
zWCoWAMTaJ@ZxMyGn~zRD)uO=2H2HPQ85vMRI2mL)tSST5cp!It7GjyaAxvuX+t%ky
zo2xoS7#TS?_jkoJ)`L1xenlWx7MX)G93zAcigQF2swq?i&m6Z{l5;W(ie`h{2#zIi
zk;hhCkeZs}RWt)63@Y4iu@|KlXCxM+-eSqgEQWS+K*b!SrU0eXTb!ABK_#i_MTsS;
zMd0!Xl(1nwyatMGw!)P7<lK~EkQd<PS}7>+38R!Tkba9GGXq0$>*gapqKuOT`brp$
zCqG;)xmjQ$2jk>r{ZkD<X%te@#DiQ3F0Z>mERZ*DvE&z|=Al+z*%P!F?I+hw@}K;4
zf*{;9PDYDYpparO!fzmJa!!76DyoTrlXMt8C)Z6%U^L!*e^M(aWBcUPIY(_lrAg6R
zkW)b6QiN7OBho5bRtEbZadP@x{rdYL&7fRzO8}lXL!1KwKrR546t`G2^9o8!Zm|{@
z<YbnB>!e%EIjMO?pf+6*sB$j)0x|+zCW2co;Km84S)s`RNphgb0apiLmA6>Z^AmG!
zF{a+)OwUi0%83WZGN^7U<^bnFNL5s1I9Y$51!MT;jCp@p#K2~PyM-v3s%o(t<K)Q)
z7t1pmZ+^H~oY7hp6uumgxB^8ExVS6=CA6YSkSoEpDY*8m260ggGhV93xNvgR(ojf*
zO%_<5$Fq{zPm>8!7f4TbUS^;GPR$7OZeW-)d6^F5ipd9-nZZrTS7&ErV9;cN<OHx@
zu;rL~mn}D#ynmVU<e$q$7>zgct$566ge6#u;E51cd~q{^x@6$MD>9s{yh@F6%Ve)r
zp7o$iT?8(?7~M2wiokvD29S#zK|~XX0C$tY31~ft3u@vN?F6xwfCx~fTm-ID!3hmq
zyc_{Z+yoJKK*UiH0m@iKZ$PZ~Ac7IpLS|<4Wnd^`1u;251Q#O%!!4GS%;b_HUJ$<-
zq!R2xP*<d=6~qM*`5@0sU|?WqVfe@-#>(-@tb~zO^i#>?_|+b4TCARopJFDjU2OvC
zy0=b#zB+)>bF;-7XAW*~Sb-YmMO~BIw$w<0`GTO*fHyfmH#adaCBCpUwKTOzV6x0s
zITvtC5!}p%k4W5N$xF;ly~UH0SPZJ$;!_e!5)r{&1Zq2JvOy9x*zlE;>$X}#3e$ke
zd$-Ck-rIa@YdmO7$7=h3#wbwV#7~pCNEqaG*23b<s?=Lt1*IkNdHLmunI*93lLq-1
zRQAFenxM>6B^6MVpPX7;3~OqFN-AfFH~ukCp1jirn!sM}+-?PqW40nuusx}{1tpcY
zxIjZ;5POQmLE#O`%thd8A0^e_+NHwCJ^BAGS4QK_uDjEiT)-ZKC$S<CkX|tm0rEwW
z9LNgJlGLKy%sf!(djS+}$OeM=DU;vtRbdpEEVA#lFW70ExYHgV$Y_v<i<Ceta2`iX
z8sH8Ph&VLaXTP2kIL&~#ntHc53o`Talan(t^NT<Q;4Rj?(%gc|B5=V6&g<ap2C4*#
zKxJGJsGU-@X!4%@HEiJUu$XLjz=Z!6cXEDCPAa&sQ(VM7x$S^9HxI}oydZ*a@}2`)
z^`MT{E&klnoRZ7}NW^63r56c+6o8W$cVba`az3biQIcAu0ZMet#g)b20`eAXNo7H5
zagi`clqtWs2s8+Eixb?t%FoX!E&?Y)(12nQcxVcg3yMG<EJ6f?04S5(;;_lhPbtkw
twJS22Tzb%$CH^8q{NyzUZR7$u8TCIhfXMG@LJ^FN&WxWJz!Z4c3jk!!Pk;ab
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index d402bf8..3f39f81 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -160,7 +160,7 @@ def controlLoopClik(robot : RobotManager, clik_controller, i, past_data):
robot.sendQd(qd)
log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['dqs'] = robot.getQd().reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
# we're not saving here, but need to respect the API,
# hence the empty dict
return breakFlag, {}, log_item
@@ -237,7 +237,7 @@ def moveL(args, robot : RobotManager, goal_point):
# we're not using any past data or logging, hence the empty arguments
log_item = {
'qs' : np.zeros(robot.model.nq),
- 'dqs' : np.zeros(robot.model.nq),
+ 'dqs' : np.zeros(robot.model.nv),
}
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
loop_manager.run()
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index df84f3b..b6be29c 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -13,7 +13,7 @@ from ur_simple_control.util.grippers.robotiq.robotiq_gripper import RobotiqGripp
from ur_simple_control.util.grippers.on_robot.twofg import TWOFG
import copy
import signal
-from ur_simple_control.util.get_model import get_model, heron_approximation, getGripperlessUR5e
+from ur_simple_control.util.get_model import get_model, heron_approximation, heron_approximationDD, getGripperlessUR5e
from collections import deque
from ur_simple_control.visualize.visualize import plotFromDict, realTimePlotter, manipulatorVisualizer
from ur_simple_control.util.logging_utils import LogManager
@@ -79,7 +79,7 @@ def getMinimalArgParser():
#################################################
parser.add_argument('--robot', type=str, \
help="which robot you're running or simulating", default="ur5e", \
- choices=['ur5e', 'heron', 'gripperlessur5e'])
+ choices=['ur5e', 'heron', 'herondd', 'gripperlessur5e'])
parser.add_argument('--simulation', action=argparse.BooleanOptionalAction, \
help="whether you are running the UR simulator", default=False)
parser.add_argument('--robot-ip', type=str,
@@ -406,6 +406,9 @@ class RobotManager:
if self.robot_name == "heron":
self.model, self.collision_model, self.visual_model, self.data = \
heron_approximation()
+ if self.robot_name == "herondd":
+ self.model, self.collision_model, self.visual_model, self.data = \
+ heron_approximationDD()
if self.robot_name == "gripperlessur5e":
self.model, self.collision_model, self.visual_model, self.data = \
getGripperlessUR5e()
@@ -817,6 +820,9 @@ class RobotManager:
if self.robot_name == "heron":
self.v_q = qd
self.q = pin.integrate(self.model, self.q, qd * self.dt)
+ if self.robot_name == "herondd":
+ self.v_q = qd
+ self.q = pin.integrate(self.model, self.q, qd * self.dt)
if self.robot_name == "gripperlessur5e":
qd_cmd = qd[:6]
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index 2d36c72..940acd6 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -111,15 +111,23 @@ def CrocoPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocod
# TODO: implement
#path = robot.getPath()
+ # 1) make 6D path out of [[x0,y0],...]
+ # that represents the center of the cart
+ # 2) do T_mobile_base_ee_pos to get
+ # end-effector reference frame(s)
+
# generate bullshit just to see it works
T_w_e = robot.getT_w_e()
- translation = np.array([0.01, 0.0, 0.0])
path = []
+ t = i * robot.dt
for i in range(args.n_knots):
+ t += 0.01
new = T_w_e.copy()
- new.translation = new.translation + i * translation
+ translation = 2 * np.array([np.cos(t/20), np.sin(t/20), 0.3])
+ new.translation = translation
path.append(new)
+
x0 = np.concatenate([robot.getQ(), robot.getQd()])
solver.problem.x0 = x0
# warmstart solver with previous solution
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc
index 05d82360395cf608e6ad774a2bfb5c43c0ebd23f..361d94f551ed5213c11d66ae7a950361e4d7f57f 100644
GIT binary patch
delta 1564
zcmaDKvNntFG%qg~0|Ns?aEp8TW%G%A5{xMm)#JHRI9eE@1S**{IX8BlQ+9j~Qo_LS
z!2Y>07+ki0K5Hfj+_b;NnUr4<U!0Lxkh+ous_p|!dZ&F+0s{j>QT*naDqk5HV<-2k
zl`^(%W>ufaB%Ta06GCz_FfcGPFfe@HH@QWwlP!g%mZOAevZIX5WNmp_X3iRx$qR(V
zCMU|PGjdM;s4UCNwweW^v6c%gC(6uK!aUhdTxN2;JSS8QJ6sKS3hQKE1@XxV3am^J
zCMTN>0|P?}dkxEE2X>Lk@d|7#Fb+owSgwR=aumBPqwHis1z}N+6wVZ`6mGbIJT)v?
z@{<i%L|AHg;O>-}{8xdSTM^<e28JvpkXAv6Mv&ha7#M1K;i_eM`AVRo3=CN+Ahql~
zHQY74HGGplD$DS|?cy&{pRC6zGkKOMpHU5eyeQ248rCdb7#l&?@ULcs@E93tShDmd
zODoAvu2K@_Na3sDPGPA51)t31d?jv1qsfH|!i*-9`<3(=%_i?xl4fRuJ5EMqH5<f~
z7$ydWT7g=@8i6d!$-45=lN*%9d0ZG`T^Sf^g;V%zgeM0GiA+An&MqmO1qxD#E;v~$
zf^fY~jc~j?Or}O8MW9NEfuUBUMx;hKUJfSS<IciRBTyp<isgF28lg0nU<OS=zoIY(
z28LTQx%o+%IjQkUiN&e$MfoL(C7Jnoi8=9E`I&hox1^C}3yLy}a}!H4lOd8tOrT^4
z3cDf^5Gw*iM1qJY5D^U`Vn9SJh=>Cbi464&3`Ig9rZ58ogQh}}H;AnOB9cIa7Ki|)
z;v!I-7o~u>sUT<ac!AvGo1c=J6BJdHjwTckS(E`%nh7GZKtwi($N>?#AR-S$<TEfZ
z)E9w*r3e&eMFk+iLJ&~|B8ovo35X~K5oI8v97Je>2yGBi0dllxQ6-371tO|JL=A|j
z1rc>1cL^7zmgVP^mZU<0DLyYTH}#eXH1O(Sp$8T&Y5?hK1nCm@^>_00agBF!40eqV
zat-tM2@P?L_jI`>fh-Xa<QeSi7~<(1@99$14AKk^+!he46-2mz2saSn2_oD<ga-oy
zLoq0x6ciLDKbH{HP$=>OSt**4T9ls`pIA^(lwXkvirM@;7nh<53=9lkKw9Qa=2N%h
zma@3OB7Q^K;sT4r<N$SvR5k{AgX?k@7v(H=NZ#S$zs@6bkw@k-kK7F&>FYcS7kLye
z^C(?VG5^fUByV|vMfw99gM#68dCQCPmN$5maci*n%+9P}b%90Z10REy_T<&-@~j`&
z7&Npe-&0p$6qx*9T}#U0hK9+O<n8Gj)32MkT{LsMZ07NWnUU3t@yTR+4Floe3!!lr
z67sGm6kkjzzMN2cS-Na;lZKVD#|gm`_Se1RFM7vc_D;N#lzKfW|6)@9^`z2^Nu`(N
z%dW6gOunzNgHdqu49y^CaB}en1sgbN27y>1AfgRKfWl@aLy-@N4Gu#No80`A(wtPg
iq8SVf44`77*ln_gmW|LS9T!GMtIrWk&Wwy!U=0A?)rgh=
delta 152
zcmZ3R^*)5}G%qg~0|NuY{l7NpSF|SbNifDuRF7w=WYXl=IN_YKa5_kmf#HFDx-l4B
zwojj2s3I(_$?_Z`^TGc4teGIN)4nK%fq|hYdh>jhuZ)aQlc%bcGB$1IR-eekq$x1j
v%Or>~bMjmh3C5<$yG^9Hx)>N37#SECitQ)gH?iUVtmDGSXjSCHz`y_i0ZA<K
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index afe4dcc..0a2f075 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -234,3 +234,78 @@ def heron_approximation():
geom.meshScale = s
return model, visual_model.copy(), visual_model, data
+
+# TODO:
+# try building the mobile base as a rotational joint,
+# on top of which is a prismatic joint.
+# this way you get the differential-drive basis that you want,
+# and you don't have to manually correct the additional
+# degree of freedom everywhere (jacobian, control etc)
+def heron_approximationDD():
+ # arm + gripper
+ model_arm, collision_model_arm, visual_model_arm, data_arm = get_model()
+
+ # mobile base as planar joint (there's probably a better
+ # option but whatever right now)
+ model_mobile_base = pin.Model()
+ model_mobile_base.name = "mobile_base"
+ geom_model_mobile_base = pin.GeometryModel()
+ revolute_joint_name = "mobile_base_rotational_joint"
+ prismatic_joint_name = "mobile_base_prismatic_joint"
+ parent_id = 0
+ # TEST
+ joint_placement = pin.SE3.Identity()
+ #joint_placement.rotation = pin.rpy.rpyToMatrix(0, -np.pi/2, 0)
+ #joint_placement.translation[2] = 0.2
+ MOBILE_BASE_REVOLUTE_ID = model_mobile_base.addJoint(parent_id,
+ pin.JointModelRZ(),
+ joint_placement.copy(), revolute_joint_name)
+ MOBILE_BASE_PRISMATIC_ID = model_mobile_base.addJoint(
+ MOBILE_BASE_REVOLUTE_ID,
+ pin.JointModelPY(),
+ joint_placement.copy(), prismatic_joint_name)
+ # we should immediately set velocity limits.
+ # there are no position limit by default and that is what we want.
+ # TODO: put in heron's values
+ model_mobile_base.velocityLimit[0] = 2
+ model_mobile_base.velocityLimit[1] = 2
+# model_mobile_base.velocityLimit[2] = 2
+ # TODO: i have literally no idea what reasonable numbers are here
+ model_mobile_base.effortLimit[0] = 200
+ model_mobile_base.effortLimit[1] = 200
+# model_mobile_base.effortLimit[2] = 200
+ #print("OBJECT_JOINT_ID",OBJECT_JOINT_ID)
+ #body_inertia = pin.Inertia.FromBox(args.box_mass, box_dimensions[0],
+ # box_dimensions[1], box_dimensions[2])
+
+ # pretty much random numbers
+ # TODO: find heron (mir) numbers
+ body_inertia = pin.Inertia.FromBox(30, 0.5,
+ 0.3, 0.4)
+ # maybe change placement to sth else depending on where its grasped
+ model_mobile_base.appendBodyToJoint(MOBILE_BASE_PRISMATIC_ID,
+ body_inertia, pin.SE3.Identity())
+ box_shape = fcl.Box(0.5, 0.3, 0.4)
+ body_placement = pin.SE3.Identity()
+ geometry_mobile_base = pin.GeometryObject("box_shape", MOBILE_BASE_PRISMATIC_ID, box_shape, body_placement.copy())
+
+ geometry_mobile_base.meshColor = np.array([1., 0.1, 0.1, 1.])
+ geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
+
+ # have to add the frame manually
+ model_mobile_base.addFrame(pin.Frame('base',MOBILE_BASE_PRISMATIC_ID,0,joint_placement.copy(),pin.FrameType.JOINT) )
+
+ # frame-index should be 1
+ model, visual_model = pin.appendModel(model_mobile_base, model_arm, geom_model_mobile_base, visual_model_arm, 1, pin.SE3.Identity())
+ data = model.createData()
+
+ # fix gripper
+ for geom in visual_model.geometryObjects:
+ if "hand" in geom.name:
+ s = geom.meshScale
+ geom.meshcolor = np.array([1.0, 0.1, 0.1, 1.0])
+ # this looks exactly correct lmao
+ s *= 0.001
+ geom.meshScale = s
+
+ return model, visual_model.copy(), visual_model, data
--
GitLab