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

---
 python/examples/.clik.py.swp                  | Bin 12288 -> 12288 bytes
 python/examples/clik.py                       |   2 +-
 python/ur_simple_control/.managers.py.swp     | Bin 90112 -> 90112 bytes
 python/ur_simple_control/managers.py          |   2 +-
 .../ur_simple_control/util/.get_model.py.swp  | Bin 0 -> 32768 bytes
 python/ur_simple_control/util/get_model.py    |  58 ++++++++++++++++--
 6 files changed, 55 insertions(+), 7 deletions(-)
 create mode 100644 python/ur_simple_control/util/.get_model.py.swp

diff --git a/python/examples/.clik.py.swp b/python/examples/.clik.py.swp
index 1db23ff573d61056928c496f846f9fca07b40603..cfa969c37f3da44874ff263c98f9fbf4d98d91ac 100644
GIT binary patch
delta 111
zcmZojXh;xEG6?hZRj|-AVqjok5MW>^^N&bx-zfTCp7j<x1H+-ooC=Fg<+vCao^moU
zoZ)0(*uu%cFolzWp^B4%p^}q<A&irOL5GupL7kI<L6DPy;VlOP!(k2vhC`blDm-G}
Qd_*Iekx_ZGy4For0Ez4whX4Qo

delta 109
zcmZojXh;xEG6?hZRj|-AVqjok5MW@4ca2DI-YEKBp7l061H<9ToC=Fg<+&Ibo^diT
zoaJO-*viSkFqM;mp_-F{p^B4%A)J$eL6?((L4%WlL5P!q;T;D9!x0V!hQpg5Dm-G}
Od`KgOakIMCO;!L<Oc_l8

diff --git a/python/examples/clik.py b/python/examples/clik.py
index d450674..1c3a4d7 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -35,7 +35,7 @@ if __name__ == "__main__":
         moveL(args, robot, Mgoal)
     else:
         goal_transform = pin.SE3.Identity()
-        goal_transform.translation[1] = 0.1
+        #goal_transform.translation[1] = 0.1
         moveLDualArm(args, robot, Mgoal, goal_transform)
     robot.closeGripper()
     robot.openGripper()
diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp
index 1207a0999fb68434c4d8ed30f2d2bc6621e58b20..e12a3e0820c44510204d6fa502fa0c6198de9deb 100644
GIT binary patch
delta 67
zcmZoTz}j$tRVc|I%+puFLeGeSfq_AQfuYTBqtK^*)~mb>43d*MCm1tIZnm9phj((o
YQ@P1HGdU*L&E=k~bBup;<h<nv0h#F*f&c&j

delta 68
zcmZoTz}j$tRVc|I%+puFLeGeSfq_AQfnmG%Mxjsrtk-xM7^Eh1PB3Pa+H5=F4liTo
Z<bbDgY`KYfiRr166^==7j-0prAONZq7IOdq

diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 82b0c4c..3b5c8bf 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -281,7 +281,7 @@ class ControlLoopManager:
         if i % 20 == 0:
             # don't send what wasn't ready
             if self.args.visualize_manipulator:
-                if self.robot_manage.robot_name != "yumi":
+                if self.robot_manager.robot_name != "yumi":
                     self.robot_manager.visualizer_manager.sendCommand({"q" : self.robot_manager.q,
                                                           "T_w_e" : self.robot_manager.getT_w_e()})
                 else:
diff --git a/python/ur_simple_control/util/.get_model.py.swp b/python/ur_simple_control/util/.get_model.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..79d1d380f57f95e5f65f7229101507be08baf750
GIT binary patch
literal 32768
zcmYc?2=nw+u+TGNU|?VnU|=x!k4VoEs$uAx$H0)DUtEx%l2`<i!iRGci?cKH^6)7E
z%hkaQ)X&JzO*PVob8|}bQuGUo5-T!`^$Uvfvr>~wiuFs2;)^qL3vyE9lk@XRit=;x
z3o1)8^7F7rl$K=X=%=Ta#OLOxq~_=qRD!J=B}PMFGz4HFP+F3vYr)H4Y-DHv5>i%D
zR1g*l1u;kQXb6mkz-S1JhQMeDjE2By2#kinXb6mkzz7L}k^(k{dIkmtCa8b?pfn>I
z%?jlQKxuy{4O7Pg<vT&?`A`}r&kp6=LFxTa8Ya&M<-^Q<59Pz;nW21`{dQ0ZC<T=r
zrA9+wGz3ONU^E0qLtr!nMnhmU1V%$(Gz3ONU^E0qLtr!nu!TTk3Il^E0|P@Qs3R@F
zzyRz2|KVp~c*@Vfu%4fRVJbfZLk2$sLkd3wgAYFggC#!$gC0Kvg9<+b!#_R-hL?N{
z4Cnb67|!uAFjVs~FjVm|Fxc=hFkI$kU})lHV2I#lV36WvV36cxU=ZbHVED$vz_5*n
zfuWIyfx(T3fkA_Zf#EMV1H*o928LE{1_mc?1_o|!28P>Q3=G$~7#NOmF)$qDVqkFK
zVqjqAVqmz*$-uCjlYwCwCj&zcCj)~4Cj-M{4hDut91ILgIT#osIT#o?IT#o?I2aht
zu`@7Kurn~|u`@8}vNJH8V`E^*Wn*AaW@BJbVq;*~&&t41!pgv)#mc~-$;!ZRoP~j*
zkcELkorQrxjfH_hl!bxe7c&FHBW4DMdCUw9$;=E4KbRO84l*$?cr!6DoM&WUILFAq
zFr5(+h8du6#1_yB3JMC!3c0Dp8Oezy3dxCi>Lm(Asfj5H#mR{|nR)37nR#jXMY)M3
znfZBo3=q|6`9%ul8HpvSWvN9V#l`t~3MCn-3K@xcDY~f&X_+~x#R@6;Afqx8%To0j
z!1|K&b8<3^GxPHx%OMnO6$&!*^pZ+5b5h(>^K;#b@^eFrQqnZQizRdvN{dp`;tLW>
zGU5}Hit}?yOHy^f8bE4OONuH(DhpEeoc(=#JcB*`{d5%k^7B%4z#fTD$t;3v(gfRG
zmRVeym_tR2!#smS9eoJc3ib)IKYc;#K;Rz5^dZEt%3ysu3TOeQqkzj7U}KB%n2R%9
zloc}b6p|B*Q$azSpPZbLnXgcgnqQEUs*sYJoSBlUP?E2ZoROH9o(hWL%p!%vf=pO4
zD9OkyR!Gjz%}vZpQOGJS1|^@IoMMndh5WqKVuhsCocwZy%o06?r2LW$g~Xy%h3wRV
z5`~P^qEu|r0*XSgIGWEDY{9V$mI8+jOaLrWUX)o}5^o$|oN5dW>tI)74N%%DDyY;e
zDyR&}_f0G*%B;|UsVpj})YO4-N{SNmiXj0H6DlgGRIpXZE6__UDoU)>h&I$SH8(Oc
zG&MCeF*PwYH!#*wFxE3PF*LO_GBY(cwXig|L>4kOHnT7{Gd7GxH5F_Tin*4024+SE
zW`@QF29^c}W@aX-x&~%C3I=)xmX;>w<`(A0CYEMK28QOQU_k=|LlZ+okhqDlk+G$j
zp;4?RBx^w&ZbWnt7{wz6fl)lzWyp>N%cD5eP|wuN!qmdp)Y!nn$k@UZ<RTpfU1L2{
zLt_IIOH&g|3v&~56Z2GE19KFALrq69%v2BLD>E}o3v)AbV^d3zE>j%^U5LjmEDa4T
zEey>q%?wNop&mCfGB7l;G%+(UvoN(ZGe+~cA<-T;ME1BLrpLkZC~h^-Gcd3)H#Ide
zF*P$XF}1KTHHJiyfq{{QnSrU1v5B#vfuXsPp^1)yxt<XyQ5YJV8e5v1n3@`cA`~S!
zpmw5Ip=+RLY-wa<Y-(&_X<}k-Zf0l<2@C^6Lvuq@12ZFIGfN`_0}F6y7#bU!TUweM
zo0wUe8yT9L!qPxmeo<;-Q7#cd0aFQ1127I!8h{C*X9G~0H?T0ZFf%nWFf%tXH-csZ
z12a=&3u6mQ3nODgsF10Jp@pfTp{22rktNjsFjK)6q5I#&$i&jr*v!ztz{JqP+|)=%
z!9YjB0OqyQf`Zhdc;Y;VPz&}Rf(y16lE@K)=w1ZnbVD;!OG^uLb3;&EfE{9}XJ~3_
zY+__!X<%V&WNK=G6s-uez!so*43u^ZK$+Xn)X>zx(!kup6cmL>J}b`1FU?6wEg~Y;
z;7Y;%g7d((Lc9eRL-&;~C@^&tO!bTnP0bAr4UH|#Ow7%UEK_w2ETFLi*9$fe)feC}
zFwiqJGcq+aHZd_WH8nCeHZXz34yf&bQspYxD&!aIfvRr3#H3;ntpQU5aVU(ZU<+w0
zXsBZ>s#J%l#H=E*7>-a>l$w)Rl3A9Dt{End$3nfL{G|Mn_>|P*<f6<1P-~}H4`i^q
zre0QlW**oV>ZL`dsqy8RB^mKx)tQCype9dhyqjl)t4n-<V~9sER1>&nVn|6%Q-H7k
z)X=nIP*PH2@beFGwNe0Ao{&&iNCLH)DpFGvKn<%zg@U5|0&v4BCpED+l_4p$q&zhh
z(!xqiE-6jSQ2^Vn0IGID&91!sJY7&@EvYE6BsE0=Ttn(HWF(e>nr|f;i6sifiJ2)n
z3dN-b1^LCPDLIv(D!M3DA+uN^Q6VWaT_HEKxHvH<Gd(XiHLpZLBQ3RDAvsr*A*r-P
zA+tmwB|o)TAu%VlEHkkrRiQXPH&r32C=uLnC`e4sEU8o|&&UL~7?MiU(=+qZ^%y`-
z1GNwp%0cah%)D%nT_CrX=NILqD5Mo7=7O!y%LlhMb8;#b@=}vii;EMBDs>pZP60U{
zY(!`fD6sOfi?NsiwkT5p)XG%I%U3AKNz5w-HCa;f6^irq7!Wd<DXEDH8TsWP$<&IH
zqQqoyYb6sDW|{eU3Mq*ti3*^Y%`YucD9SHp$WJOs%*+GDNl|_-NCX_*i3J6ziABZW
z7K}n-u|jTQdS<dhUTJPpYEiKs11uImCW7J;<l}<GqLR$y(wxL1Xtb3l7AruV05cw>
zt}-`=AtygEr5N0_R>(_DO#yo)DOCX^oSLEsPCX3DlmG^0g-HKU1$WOdR|Q9>V1J*`
z5LX3Ix>X4AR|xTNRR|4oaZ?EK@YiEdRtRu(_I7l4wbIvD@C;T6_izkR4_0t;_4IRB
z2yzYZaddWdQ3&@8@c?UtDvo#Y3{r3mau4-&^$St(^aIO*3`H^!tj04KEEVqI?*p<m
zKG@SYz{fS-+21cD$lpi7&ELnxHAq1tIUm%XOe|3^R!B?+C6LUdoK%H0P@E*@RhENV
zk_wq1+04A+lEj>x)D%q$Lx@2c+6zigEdk{d1yCsf>gA+>Q)FgI21v9xRUxf3FBx1;
zfYMZPVp*y}Zlyv_W?HI_LS{-fBo@IPrAh{d%-n+fq7sGFibU|rad7^P2c<cfOh!RL
zT5=93Nu?#{Fo69B(UX&zq*s($oL^d$oLZ~^5rdSPFs=E;FjjFToS9deTTlryF0TM4
zTacLtZQ&;tgPQx`<OJ%_m1LwAr=liha9bUeITIDq^Yc>_a`N-DA<ceJ&MwJF%~L3V
z6b_&mfXKkY3ndeP%7y$iP>8`&e`ZMrERPlB7Z-!$Gc&iKD8DQfRQgycfZYY^|1V--
zVE6~>w+b*Yz~<Mx_!$^F`574E`574E_!$`f@-Z;{;bUO<!pFd{kB@<28Xp5gI3ELp
z93KP2b6y69Q@jieTX`86GI$vn(s>yew0Id9Sa}&39zy&2tvn12K0FKz!aNKNj64hs
z-?$kV&T=y_bZ|2;#Bnn)NOChU{NQ3>*w4kl(89&Q5XZ&9U<>W<U*cq7=;mZ#$mC>T
zu;XN45awiHc+SDVu#kg+p^Srp!JC7DL7szw;UhZ(!!mXThGKRGh9Y(bhCp@(1|4<=
zhTm)q3{Tk@80NAuFhsF2Fz~Z6Fg$04^!+_p85rDI85meu85j<+FfbfsVPKfU!oZ-+
z!oZ-!!oa}7!oX0?%)k)K%)k)A%)sEr%)lT49Rs+@#K16@iGd-XiGd-4iGjhAiGjg_
z2@;lnLE(vQ9AHq1fl_5rYDsBP9;7xy?fU8^=ND9JXrc=0fQxWQcL<~%gws>=bM-)t
zjo{?OoKyu{g<=MfFbo$fXxS=&+GvI-!pflPrWl;$K?QYUa!F35LUMjlQEGCDLQZaC
zK8k9%9m)AQ`9&xVCqq3@yGsw$<O30gdY}d^%yJN2tYE7E*AI3h+~~|S1tn11LJ8DC
z2dl|T%uTfd%Y$kbkPt`;c}PGHBnTQB@lOJ8Re`8hR!GaNP)IMz1T~_;`oRRK@KLaZ
z1g~ClQEFmIs!L)?q6Vbt1a3p11~9a54XOe2Qoth)kdd1B-29}>oYeTF#A49+3uuG`
z)Ku32`wTMTf~o}B$q<!>kdd8WS7SZTl+?VE%#unCO^8F3A#DZS%)FG;3I#-a$q*9L
zV0R)Lua}sT;s$D6fCm5|EOn66)OCFQojiS9<DDFXUE>3SJcE55Lp+`1JzaDRbU-b<
zlK6t0#AI+81WRC`F$Ay?;ISqze^0*<O$C^{z-^9_e1*i66mandY18B;=9MPq<WxeO
z2Z|8PAON`$Hi`v_Dh-fAP|P8F1!5648E~3*#**GZy=6plgEi2x83Bz=G-oI!<yXWP
zXCxM+D(NWT3@-(^DjkKS{FF+JfP=UkRSIe$Y8-=8BHT1k>pVF}&ndq`!$1!-z@TRg
z@{oz90$3d=#}L9`J2UfAi%K#Ru_h{rQCN~qK|yL>ic@|{Wk|kPer8^Y25#?zU8Ag!
zn^>8Y3L6}QryX!(pd>>fHK!Ofh6XYf)NaVnQ-C#AiWSm}5{nB`Q^4lHd}ILgELarT
zAJCBVg!oDiG|UGIB4cpjVyXl2k+MRFzl*<>LRw~C3aE9OpQoUan^~j@Yny^iRaPh{
zN-Zg=RLCt&&QK^y%uC77g)4xnfVu}1DJhw`sd>ej`FX|BhOwZiu82nx0*%3d6e|<q
z*2JRpVm*+NxrxQa7$zCS!aPw>1PXa2e<v?j=a6`ChKu)fQPM#Z&}2{s*^C*BscC8X
zMI}C&xtS%=MzIRE3PuJ7V1MCPYDm0N17xMJK+lA9KyormQj0*DAJi2AcR0ZPm!j0f
z;{3csP`ee{Q2@8DKn*8c{wPb$$p>|;kbQ!~AE-(pJ|Rje#3x{-Fn@rXba{~Eqh72~
zmY7qT3aK(dbz*8!D%jzmepY^QCaCGFkOK;8g``S_l+?7u(wq`-Ckfo;%!Kx1%2O4}
z6Z1+SO+;l-e-K)TXXfUnrhs~sIh6{<sU-?9k3h^UhG+mqGYI1-pcM*=GK)cT6Up%q
z6`)EIQu=^&fiS2H2WMz-l@$=Fp$ScFAW0BL&gVg{Vc;$is8)dFD`Z*ZN&{APA{W@8
zavf}y1~^nfhJ!G~W$*$5R=Fq?rIvxGHHdV2P!!zh$c{pCGJZD|Bo?LSmBfPv0>I8w
zMt2u@<Oei<lAj03JO+A3*i{zgmw@Mkpd}OfFuQ?{f^J@cUO}clXdDk(dZXC})rzG$
zP*w<W4GsZY40p1EtpX$;V~cMETLmSgHb^|08YQq%SOObeAIw*YIgl8EXarTBm{mB`
z`LOB}oVJjgPAJI&lqk_GgP4s`DnX4!HcnX~A3PeR02+(|l_!u%x6Jg65{10{a!6uV
zR)7S60;r_}>h*yJLK2G<z=5uyQ35J!)Qdp_OG$}IIhCMMpVX3))FMbL1>%gvqFjtA
zCrI-Qv%v;xJc8uF?N{jbW~ld-6%vbb6|`XuP6hB7SVn45eqMZHK|xV|MJ6b}=I6P%
zfX2g=6;e{ui&9e+^3xR3ic(WS6%xpU(4M_UR$_8~Qf6Wv$YtQo)(WX5$)MsN)D5l7
zFGZSV1XaAS))&0F4jNcVOi2M3R*5+Z44`rhR5B%2Du8sux~wUgX=$mT?2(z6qnlEc
zS(XY4$xO)516U=f4AWsyRshX(<`;mRTb_}boB<k=0u9(fOCd;rgN8aviYgVrQ=6d5
zALI(O2uf5yBsYjIkm=w$91<zW-5=!CgsWW*FIKQ8ShzGO>L50NSXhe;l->Y1_drqz
z#0%htKB%Yzw~UdS*(Le;IR>chY*-aeavK}e{|{zjU?>MIuoqxpfUVyL&H01+{V(_!
z7;^X+7_#{o7y|eh82tGd7$o@^7$o=@7{vJ)7#Kn00=x_i^LZH<l6V;y%y}6YSa=y2
zCh{;al<+Vx6!S1JB=ImXxbrYD=<_fzyyj+L*uc%e;L6Rwz{$<P(8tBVz{|zJz{ADB
z@P?Ct;WZ}%!#Yj|hE7ffhEz@l1}jbm22M@}hPxaL45v957!GhSFihuQU<l%1V9?-T
zVEDz(z;KM6fng>)149~go}ZDOf#Dh(1H)oA1_n<y1_nts28I``3=9id85qh~85q1-
z85k5;85q8@Ffg29VPIIu!oaYAg@K`-g@GZ8g@HkVg@NG)GbBu0m>C#;GBGfmW@2Dy
zU}9j1V`5-1g7)!uFfuUoF)}bjF)}bPGchncVq{?0%gDgc%*eoy$Ox*Eu*4_c_Al{z
zh*yc|jKLELq&Jb7mr9N)r05~6maqy)O^;j^WBLQq4N*{lbhDAOu^yx!joINCFpI$*
zV`|zCYI&j7@6e_IwYpA3bUfglB-HK|tielRZwWISQPcs!QPd)v32C(B?9{;8&~)oJ
zfLj)Wu2%qV_QZplU6AGxwoHn(=1WXTfn;lV%M&t)02Tzd$^vo{^Ad~TEpcUqq}1fZ
zQqb@@coa;bvNShSy%^Mb0MBnFf~TZlEirHeLz^JEiP@>e3dN~;pqYgHB8AMn%#zH+
z9EFn9;*!j~bf{8LGY8zAf_9q9^NX^<vxNDjMG6I_MWFdSXje%Y)b0bdG{9{@4e*K^
za6o|8;ec8p;CaB@RD}X)=S@=wYDsyjLNaI?8ZxAxr-0m`(S~<ZAgvUrmfY0DJdl}S
zw`G=qnn94+0_27#qzebjYv48@NFihn6Wj_#Zg(kw+g&g>fm%Y~Ar1Tl#5rJZB8@@~
zg`Nz=&7d@j)-?e6BQLRNg!Mr{jnVkTqFm6BIH=+Pk41yaSLhNav?XE4YQgPnP*)5z
zG)9*tgw(U0_;rLBc7n?zxR*h*l&H%kA!Xm_VnVc(16w-?Z|cAVz-0+&wg`PO3w&9k
zGB#Ny@Tp<g4-G5MPb(=;EK1cc$}a|CUC@zXpk)O`#SmTKLJ@h<iN3FA5M*%)WQYK(
zmH1cJz_ozJ(?Lsd2kG(}l$AB0dK2r?8IS~I`~@^#0O`3R_l-d#11Rf?QD?1?rv*V}
z40MErQ1Jp)M*ot9Di#x8s32Q6sLB&i|38<3f#D=*z8Y!%{|P?>!!~{fh7Rc3e@}h}
z25Ei<1{To106qqWEqn|NQ}`GdO8FQV-1!(74EPur1o;>k1o#*j-t#gr9Oh+Un8wS%
zkj=}$-~e6s|B{D+;T#VG!&x2%hUq*E4AXcR7;1PJ7y@}17y@`07{quO7;bShF!XXW
zF!XRUF!*ybFfem7Fl^>xVA#aPz)-=(z@Ws%z;J_;f#EtQ149KT1A{mx1H)Mk28J^n
z3=EkZ3=DD{3=CJ;85l0JGcXjgGcZWAGca6aV_>+z#=tO-je#MXje)_6je&uaje+3?
zD+9waRtAPvRt5%JRt5$eRt5%ZRt5$hRtAQfEDQ{DSr`~{SQr>WSr`}sSQr@2LDvBo
zF*7jyXJTOJWny6HVPasYW@2DSU}9kKU}9ikVS<F^2~c>_bFLp$%npZD<|Cr$N3Y8=
zaG!@suxbN0-oQmVv?fEePzHKACZyH{I}lW<C@UnUm82Gd#(CAtQWc6)bMwnmQxuXD
zb5n~FA>AJEyjF2$Ze~tm5omZ9)IG5RFA)K+^HK+IcS!{;$SBCn%hpjSPEA$F1TA|4
z?T|>!1Fi5ZEe1^iGAM(MN>s?r%*g>Q69o4Iiot!l@{H8foMHuCJIIQ*(mc2p@M6*Q
z%(B#Ch1^tyM1{1RN>J|$tQV{m)bE41SBC+mjs&kX(*t`|2eJZ753M}}6Gk58ht`^)
zyo+9GA{hv2U?K^CSIU+Iz?M-IN1K6WOCg&RAp0}GZ7h5`Oc6T3+ZhOIFhOVlZv!Bx
z!5E|gwivxQ)tI0LBajBfGH+tEfTv+0U3SRYZb+OUhXiEz)96kWJ^Voc-iZR+tOMOk
z0!ep}zy!;qt-2cBsWQ4##n5DQr^@I~6||iy;EG4VRsnMvwjOASy@pb0k*Sg<c*Z(C
zwZt8=WHKkUxHvS(G!@iRfYcQz3sW&_Q!Mo;SQ~mp3a*nOMH!e$WaWunE!JuiyHczb
zCU&J*t4i!jaa5An6=PP9pq7TRLTN!tB53d%+U9~UnJLzT)Sn<#APkz~2CsnxubHLI
zx?a#gB&t2&SOi&5?Alzkq=9{TF33O-Mzw3u`UyF8WA_lGfeUJo7MGM3WTwDZf@Bth
z+Q+$>IhnbsC8<TQg=NM0;6-8J722@%8#)Z2{(lk!1H&#*e^r2i0e1f04t@rPaDD~`
zeSQW8c78}-e=;8fLm(dm10Np)11}!~!wX&ph8$=g-<Owx;Rp`{!!jNQhJ`!~4E;O|
z42e7p430bu3`#r<49~b37<O<oFf8O|U})lIU})rKU~uPVU~q%31<>JUV0h2Pz;KF-
zfngaJ14A_z1A`YA1A`oNZ2%**-#;0;55Ss}fq|Wqf#DVh1H*C-28MhN1_mb%1_p5s
z28Ji>3=CcD3=C213=Ep=3=EIh7#I$)F));}F)(<sF)+xpF)(~#Wnfsx%D~Xf%D@oC
z%D|w{%D}+L%D}LVg@K`&g@K`og@GZFg@M6_g@J*Ig@IugGXp~;GXsMmGXn!BGXujk
zCI*J7ObiU5wG#qN3=I5C3=FRrA>p_a6qYpa`-2j*GNcCsUJwGGAp^}7sTV7h7N@2t
zAlCSRbCoh=0p&<pO^v)z0J`29v?!KtE2+VyGEu7$z>5Ol3vr>2B)q<Q*v#;Q*BOIS
zG-&ZM(&FdAwjLVfM9f%4UO51+ph;K`4OwrA-%OO%2(Wox@)tv6_XK5Y8gO|7)l`T_
zphXd6YzH(y58BrOIvOJpw4nr4Wq||~ia~o<6CssZL2`TnNEXZk)p?-#@4VC^6r+aT
zvTE?a(&z$;3`j$CbOD9JAY2NLNTVZt0R`xkf!xd@)b-Wi?kcz#DnjY6qBP(~H&TLk
zDT4YXDB%L?jDfb}L3ZaAk8Y%dto2q$S^-X_b>5&lf3)=vZCJwxJfN%fA)`a!Wv*~3
z=!`w;!dh@X2Q^W_dpAMrK9X_oqr_~};aZD4fUSQ}|9>kp1H(zs0saCE46yV0Z}2lP
zY~W{Lu;pi9u;FK5_{GP-u$_;AVH+O<!$dv?22nl+1`$36hHudRzacLJg8?rCgFY_<
zg9tAJ!+#zIhD|&S3>%^Q0Q`9v7_@m97#MgM7|wGuFf8U~V3^3wz|h9cz!1yLz#zfR
z!0?WXf#Cob149oN14BF)1A`711H%_i28QFD3=DIi{eFH<28R0_3=Hcy7#Pwy7#Lik
zdjQU}Gce3%XJE)@XJBw)XJC*7tpQ+TU|7M%z)%A1_bak7FnnWWU^v0bz);J|z~IHo
zz~ITsz@Wj(z`)E34Fj6@+ea-J4S~@Rpn3>^DmZWn4jnatmftvcbJJv_!pI%QfbL+$
ztn-k^M!@qCkdXq|C`LeN2>5&=XMexokRV4-zmQ-BCr4**MN|N;c<~z!8C!sE*?<{8
zxCTjZ1`_Hh3DtuM)p$rifkPA)OrwJ^qk}M`gD``5KmO<-j6%{t@1&=6qzBYO0aszD
zE9apN6-eI@yfz%~c~8Z-Hm0MR3f{${qd@BupGF5NU_)5Q(|ph<1#Oi=jBaV*JoITG
GrzrttFx^Z5

literal 0
HcmV?d00001

diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index 7d9cdc2..5dd857d 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -181,13 +181,61 @@ def get_yumi_model():
     # this command just calls the ones below it. both are kept here
     # in case pinocchio people decide to change their api.
     #model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path_absolute, mesh_dir_absolute)
-    model = pin.buildModelFromUrdf(urdf_path_absolute)
-    visual_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.VISUAL, None, mesh_dir_absolute)
-    collision_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.COLLISION, None, mesh_dir_absolute)
+    model_arms = pin.buildModelFromUrdf(urdf_path_absolute)
+    visual_model_arms = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.VISUAL, None, mesh_dir_absolute)
+    collision_model_arms = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.COLLISION, None, mesh_dir_absolute)
 
-    data = pin.Data(model)
+    data_arms = pin.Data(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()
+    joint_name = "mobile_base_planar_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
+    # TODO TODO TODO TODO TODO TODO TODO TODO
+    # TODO: heron is actually a differential drive,
+    # meaning that it is not a planar joint.
+    # we could put in a prismatic + revolute joint
+    # as the base (both joints being at same position),
+    # and that should work for our purposes.
+    # this makes sense for initial testing
+    # because mobile yumi's base is a planar joint
+    MOBILE_BASE_JOINT_ID = model_mobile_base.addJoint(parent_id, pin.JointModelPlanar(), 
+            joint_placement.copy(), joint_name)
+    # we should immediately set velocity limits.
+    # there are no position limit by default and that is what we want.
+    model_mobile_base.velocityLimit[0] = 2
+    model_mobile_base.velocityLimit[1] = 2
+    model_mobile_base.velocityLimit[2] = 2
+    model_mobile_base.effortLimit[0] = 200
+    model_mobile_base.effortLimit[1] = 2
+    model_mobile_base.effortLimit[2] = 200
+
+    # 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_JOINT_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_JOINT_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('mobile_base',MOBILE_BASE_JOINT_ID,0,joint_placement.copy(),pin.FrameType.JOINT) )
+
+    # frame-index should be 1
+    model, visual_model = pin.appendModel(model_mobile_base, model_arms, geom_model_mobile_base, visual_model_arms, 1, pin.SE3.Identity())
+    return model, visual_model.copy(), visual_model, data
 
-    return model, collision_model, visual_model, data
 
 def heron_approximation():
     # arm + gripper 
-- 
GitLab