From e7ea42fc95241f8b1a729cac5a8f99a6a259ccab Mon Sep 17 00:00:00 2001
From: Nicogene <19152494+Nicogene@users.noreply.github.com>
Date: Tue, 30 Jun 2026 10:56:26 +0200
Subject: [PATCH] Automatic update of urdf triggered by GitHub Action

ergocub-software = c4fbfdd12bf22a6ace0f4a9719dcd183f753bbcb
cad-libraries    = https://github.com/mesh-iit/cad-libraries/commit/39d958532d47ec17332b42330f3711c516e857bc
cad-mechanics    = https://github.com/mesh-iit/cad-mechanics/commit/f5367584f40cdfd19934d7a8fedfc16edcca747b
creo2urdf        = https://github.com/mesh-iit/creo2urdf/releases/tag/v0.5.18
---
 .../simmechanics/sim_ecub_1-1_realsense.stl   | Bin 6884 -> 9284 bytes
 .../robots/ergoCubGazeboSN000/model.urdf      |  53 ++++++++++-------
 .../ergoCubGazeboSN000_minContacts/model.urdf |  53 ++++++++++-------
 .../robots/ergoCubGazeboSN001/model.urdf      |  51 +++++++++-------
 .../ergoCubGazeboSN001_minContacts/model.urdf |  51 +++++++++-------
 .../robots/ergoCubGazeboSN003/model.urdf      |  45 ++++++++------
 .../ergoCubGazeboSN003_minContacts/model.urdf |  45 ++++++++------
 urdf/ergoCub/robots/ergoCubSN000/model.urdf   |  55 ++++++++++--------
 urdf/ergoCub/robots/ergoCubSN001/model.urdf   |  53 ++++++++++-------
 urdf/ergoCub/robots/ergoCubSN003/model.urdf   |  47 +++++++++------
 10 files changed, 267 insertions(+), 186 deletions(-)

diff --git a/urdf/ergoCub/meshes/simmechanics/sim_ecub_1-1_realsense.stl b/urdf/ergoCub/meshes/simmechanics/sim_ecub_1-1_realsense.stl
index f21a84f8b8e52f6eb5e8888667e61f85cc60fa36..1f133b39f1bbddbf793ece39d8b1e0df46cc6d52 100644
GIT binary patch
literal 9284
zcmb7}cU%-#*T;{FVvX3aU_qoPiX|jAn3>%XjG|xxj3NjqDvGiKf(DUAQPji|@&;?3
zL=+bLYrzKW46GW_sK|p@W5X6pBtl~HYJ7a=?r`06+5GeJ*L>i7@9(sGXYL_hH(j^T
zF>thhxbKKyF?^86pzw*l+OdJY0fD~Y`2YVo0RYKmhxz7D17wr2MsgMhYVWR#H<g{)
zSG?N0Mwm4#-DF%4UVML8du`0a*`}RF&k}%$s@;5OT7aB1?~OEa(Fl!w$5>MrF`xun
zVH#lfHdmqQ&rWi)e^*O|rvkL^BO^>xPc|*VdYA^7e11Ql@vMn_ec@+v$m?kuo4Zp?
z4u7XH8dzHi<d(Y-d-t|9xw(T}HO5Wb;YE<COXJ`YtjB6xSa^}2mj76CbRElRV45Xf
zzA5LoMNE}_Du0lc9B88fSY=x2_(s6d;+OzN|C(Xkx;#kkCbg2=ySNvVjOt6hi?Q#R
z1~~O{y=1sGS6(qFQ@ZuP)6E1t{*}?d+5pcS$GQQe$s-d7<Y5Bqu^L1A#R%`*&r4oi
zLW}yJ9ivUTHpBE(m#)FS<GcYJiD@egtr;)NEAou)wga{EZYG*$9Ph8eo?sflWrt2k
z8FNG$U)0=)H89N*o+m=MCaasvL#|Jfy_U8scHt+R{<6&|!S%;^12~f#$9?s4s9d@M
z%*4ubv8JRi152<K&H}*RSL-<6Y+Lz@sL6~5CWtm&3E*k=bRl&ySG1z(OX=ZXqqXD5
z4Ky{_VbAmx?ihf5BlJSnr7Ui0Vz&2~0&g>svU0HoXMxSG=S;nDAo(|MV|Xfu32cS6
z0V;z_`4&HaB;CCoEidkqtf?3~(Bv?3Z3*@S=MCWI&^W<q)&b-97jvaeqXwA08sZmK
zg4e|~z(*C8{5`vBW7+;$j0UDzV%7HZT<)B|jpIsF%-JQm`tAJ&rh>S#0I~8W&N;57
zbmdM8qk(-V1YHTBHlUX8>?X*4{a^B>hGCik1xcoj(e7H@iMYp<dOY;wLS6>R&DORO
z$XdxW7Mb9o2SebM09JdfliF;ZCd1is9us(XVLgBa|A`d1H|3J&cXRnKF8FHu7fdom
z>>8}ap5XceOlx|CPwE*Vca-+<Bir@RZk`-#TIknOi#@?Kz|#9cf?fVANv_OdG%(E)
zhduN{yEQJx3(MDWS5y7WH6LC&R*OAky=vo;D$Hz^$#v75Hev!>;SN>Kc0VJ3>_`v!
z^2mWgBiA*WM&C{|*)~aH;=mpPcvbD?)_mqDUsyQOT#wwo5vIH`DGY&E0(hM~iL-m=
zEQ^yzFcrkPB6XrG0eBCu<#lHp$$uRg%y@-8!C6pVA2Buj{L(n-xYraRewbEs=eKdD
z2W6|Z*b|mGv#GQ2vxBWP1pI_aEnAzjTk!S^Cc8L?0H^Qm;A_t~%1_Ha6~gAW)kgUE
zn~po_v{;YTI21cfn0aWBQRgy{(ZDoI#B|)w9rP)czAv83WEaOla;19`Xe#+`wH9DZ
zP9~?FRVF=}Jco%KTVWdD=xx3H`j;!-eWiaD5vzk0k*2emtxCu<JvrHP_djKNIkmVe
z_tC~U10`a+EY{FxLbF7I$6|T#$$tF%nS+`4ASDRR6677Id<U3+h!uT*kZ0-@rHLLf
zOq0EvHOvdo>1Otd_D_A9#-|d)bas6rYPMU&foYcbB!0SK?;dwEK~6F|_*zRe=&6SF
zRE;^u&N<ut5pO1Fr6@sZ8lOt^AMjn?3VT1RI55o;_6u?iI<G<(vTj<NJH@mf_p;uV
zkW-uLk!OIj9ky^C8}7*4HA>H%)YytVQ(E<^-^T;~F><U&v!D|83tWg-x%ItrT(|Xu
zR~C)_`5U?I(}SD?1N8DyO|s^fuiurF1+6vvPOZpFlvcW3Ax>3xE*M{rz;99OIEYs>
z{%pR1dPTdP#6fdq(a4|pEw`*}nDdiBy<GEovc`S&){^uYs#nyCuB2v{bmGuU!*U~P
z9L*KP`616#kJ13;ibk)?oeLh;)2N>X3-RFf=kmS8X7bvPALYH=T3hVtc|_Q?6U@27
zaZo*gDX%iQ+|psrGp6X}{<>t%!FRWdhsLXMu*3n48pp@aBjxp3Z4C`B{U%v!s8(#`
zNs30&`OXCq^)%||jp_j$n*wsJK1}X-^hI8yO0GnSLxxYii|M*FSA-xv4Uj$RsZ`P|
zP_j7`CeJqd5JJ$*J6vq-p_*xc{FA>*Hjg6ZoQvDIIzlD{*1*~Tt*8DVDIJ_vH7bG8
zzyw~2c6+YjQG+(F#A^IFcaiSxv^EDcbU#|+_-03Y+2&QdUGrkc3s{|MSO}F?H4fwl
z8G;NM=DuQ2AFM}nMLLmm`=bV#b%e#raou{YwMN*DIdY%VTk@QOFZ0$JwZuVbfHm4Q
zX(+@ChN@txE)Ky`L6yKO0g%W^G$?X2VVwmWd90nzKz9_*F7bqRiN&kl8xv%Qw14pj
zzisG-)k$`#6@4bOrGfx4=97U~(bIrbkUmpA<r~f}N)8mi<lER3TI~;NrB;*fC{hoH
z3zrN@PFZG+`f*rmINVspO}v@swnwLz?}JV=t;GS2e2(r~)+=?_UcJ7G+ufm8VNyN9
zde398NS9dNYL+e`9%8RljYaP=ILB&d=ae|Tyz%-xji={3ZAJw>6D@h8v{LKG8Qk`Z
z^H1*kK&&(q^<$!02Ot`me>d)3UqjVXX<9+mE0W!wDUJr+^F`)v9~crKE<F8}`R#02
zHc4#jw5skssKx|vB{_?`{PRy-W%E=yeN?hW^mP}Dr>JpI--!mL)h-Dh`^!J(3hPlC
zVE(f#ZpqDbFT*%JqftL^7J|fa=Zevg)pLpYeSWZFsz^KBHz8I8n_zPs7O%bt+s|#d
z*HG}Q36YamPSZ^LpCGaA?G=m_iG$K=99|P^-7MceQ$>0jU_FZl@k-or$1Nv!n>o8(
zL#K+%UM@Fhmk^X@iN?OUxjp<7tm=Vj3(>S-7B}SA6WoQbQkgjF*TzB+ua1R&ZOGZu
z&6PxmefRWSY?U{FoXC@0tI}{G%>Q>OuK7ewd3lT|PWs%OE1Dm&5~TsQ?P$t5`Tov#
zsfds<Q9sj^R)~O9xmn@%oT^^Wjdk}-k<5MYy4YI)A^az=wz0v&*^pL@1}3&I*{A_n
zEJm#$=hfNIs=VHW#tTJPbLcm<Sp!>PTA6Q6NR^jQd1lPbXhCO7AH2eOqt#Tqq||o0
z%gUoWt!keAN1_<(@(ELafRKwz<;pds#!-t7arGmo>k_Xht=u_x=W}*}4nhOYPq6R0
zP7|IMBDyV0F|SLl2thjrAUHCEdvdO$@FdY+2wN~s<K&nq$}I;l1YSv*0i28BY^oxK
z!9A;aOgzdKG|CiDOq2SUZ!c62!1Ij1(d%fmu(_gwuOA1s!Zbj*hmmvpeJAf37H1U)
z^_|2)PYD3|Mr-chzw5Y~S$bgzIB3WTFmdmCa~#x)5ajpdTSA#YUKqu_smS1NIq8KL
zEvIT~Ljy$T^fe5DR{}WRcnDY6www=|J;%x`s)1Jm=+fDav$<Q{OLxuME9!~rm3sG4
zrh@^o-p<o2jE%OXTHQJEHwwMI05n<In)9A;+_>s;YF*?5O*C>!;Cr;{)d{~s*MIg(
z^+b(aT?ydL;iYoF$4&Y3Yd06xRnSEDVEx-2IlBPH>yqSQ_dk}pKL}(dMI>5E;FT1O
z&hf$ztp~bIjW(F)aO9R!e-4KyYyeTq^g{RGN|*8_shm2ka~H`363=1q6i?OIP#W<;
zgVeyHq0-hw?ECZ&#J#LnT0*FvU|OAisv2SQMtva2_YNf}ZLP8N^hreADW(LatqEyi
z*AE1_%UB4Nwk87dzWG4V?2<Sr%@UsDukjU`_64IyL^C%?TmyVJ1ZZWv#=m)eC9j3M
zHG%a=R}yal9x6S%^QpV*R_U}+t(v<?Cc=0+gsZ96{B1991F1PCu$3BzT1|lUHC+V%
zU3+u=OUDT6y6z&G_2J1B)>FND;L^%Kyut*w!rIC^YSlizYl{?P|MUgS9>n_&Po|Vv
z->sdzPm>+SsDp6~fvvEI0FL1~{HgtI+^XZ_>n2|ABAKe;oyrm<@_v(=xRJ=!zo-PZ
z!ttpZ%OC7?B^vnbv(`{o0_fN_URaX9yr_D*K}u>kPMcU5DjvAkrfzz!)&TDnfbO<>
z!6!J`*tB6PSKWKG*7jMLXnQzagEPujFyXRZIQOW$=+Ldl-k88vSQ}u|{cHS%_b1%e
z^^9h|6R{`cneImbn@d;u<`;kD{9DduzLqe7znlOfT_OdW?;<$9b&a$zy|30Qew=u<
zZAc0B1k(T>H_!4zHe_+_YUeT<m}ZIlw=4M-@>91TH%Hgq4b*RS>^pg;rvv~Y;x^tw
zgnFC61pew*{+qF_k`J3dy0A3QI#)D5`1@M<Pn_;5zt!m0d%&V-<{pJTQJ>Y52%u0Q
zoKHs$cUK6kjtP8w0hqh-Iv*4ErR)4ZXEPHPye@gBXBWU{3Q=G)!mF1;;I%M;?`;63
zeXjF`jeGPiwu@$_QFvYQOwTTWVui>|voDYp0<VP$JfBhC&dQlMc#VBQGv!Rgf5GeG
z$rQkgi>X3cP}ibyMe6C(bjt+My&tE=I~DI0W#Sd3e8Fr;Fm_(o%6wi?0$bsI1MuWh
z7a=j*gNxoWM!1pRK^xxHU-U_vUw0;|dmHNkbh_4C=+tL3my;qe1hyj2R2yK)=p24T
zXd|v+U_A4Eh0hgyDgj&^XXIbMUCG7z%x5(4Hx*0#-T22`<<=tHo7*54&#&<81WzoK
ziC1=td0z6(H!eK3!V@SwJ7Ec|@ehP~@&a%p{ThFBb$6k9e;@hbEKltQO`NzUZhtX8
z8StK0=83ISgr7nO@t^dSz5j?CuAQ(eL2R<eS%a&FX+^_jzHsK-BK{|@Y$GPH71mb%
zujY4|w-FxleS3^$ZbjG=d<z43o?zm?Zhw`ZAOteRme{V^W6x%Z`{D+bU>e}BnbU>r
z=*|46olZzt9a~`<;BccT;Z*qzevhj&AMm!jcJ=KTac1XE=5D9GgtGt;_WeAe%ZMc2
zwqz4SU@M#vfOx|z{_XKZzD2(&%y&EPE4FtFT)*UdOFlwPdV8i5u@$D3J@{uW|0J-r
z@PDg3n0pkyJ7F5YHuC_#=*k`5cS5j$Bgc0tmhc^%&o3S`TsVEio~Z}ErLx(5xu}YF
zDDe?=yIwH$z*d-6ra1eK^G@An3(|{t3E#<ZHCf_CSqIMlWrA>c=vL<LjA@p5KKeCZ
zIw)Ll+*ri)F4n*_KxW88e(k-n!lnKfnL8)0GfOyiIL$|Sg$ldJ?qj+H6FAcV{{?pi
BS)l*`

literal 6884
zcmb7}X;@T87KYEn;EIX?j0++HcH<sKT<N~meFen?h;aoo2r4cKDk?-opg~+>45FBc
zNsNjH0oNEu5rpo(bfZy3V>B8c4XB_4>Zmc2#U(h?)n&;kFh7Q0_kr`C?`(Ce>asX?
zMr^#tgs~x!fuknLk-h_bBgY5ogC_)rP6z~#|NmzV0PJvdCXbTFD38O_%$-Mt>JJ=<
zwJzuzQmFrrO0PbaV0CQSoBU{aQ(y4++16kD8cG3v*zyW#dG2jRw>Zn(aQ#P}``3%C
z2`(xNw!$=k<&J@<JdIj7)w<-u+#vmp+(@gd<q5%hm<EV5wIuqi26NX3Zz!Xkd+MVL
zgRSik&lNPVHh}MqR1#8t+&I2ILqq*`t$fqk{xPv&JyB!EmR{uN>L1MWZmR_iOpC<*
z@x96T&i$0yRwk)-@<m<hp#@fX@(?}F7Uu+T{d+}<|2$f$TbF7cK2Wk%??dZ^mFFcK
zJEj5lg!h$#lH!yVx>b^YigW4qmGM^Bjv7G&YXkW7+9wrEo~Q)Qx+xG?Pt-7cn?%k$
zc};TKP;KJbIzJ|Q96PQXK<Mo!QoDlj%B|lvO0*`cmPK3ZpSINFNH7i1vR^70RFiHB
zuME;+4NQwfx4c%QbYp{gvGxt+%;fF5hw0O-t41!i;Qr&f0p!U&ByX2~%ALt06<Vtx
zmnGJSNIwgvMdIVY?$R0W8OpouZwURsv`AdbP9;H_&RXc@rG2Ydqi@~6oi%O!ID+fL
z^P}?yu>D*rNf_khZ#do6^zG9SI}w*W#$H`Im9)~B2B<!5AOoH~&=ltx1p-@PZGc3p
znw-ffF(w4*m9S(_yGGK%`4$`rt{Xr>msB!3am~oH|K6)Tk``(w_BRAtuwP6Ac*pi5
zm6iWAe&r+y8kiP|wk?iGS2HrqM~+4-or2A}d6@y$xvr~)4&vSb_!rhotxlI1lR8cp
zG;r+L6F}v)HpFw<&*m|4gNfl_7roD)!Pb>`V}wb>GY0Thw`tO=**}@h$x(#%xm*=$
zJvJ&sAh0KZTbWO_x7}MPv0K9J1Yd)A+Oa2q&Jpe;IPA1}n|^>06^?}0iuNC1@*Pi7
z<$K(CE;5kpJln<IkCJxrdK`&J+??r6E=L_QcIq1-5ZDUO8$e9=7|Bt0)ws+lQOF!?
z&^+i&JHZB9RL45Zk$$-UyK(fGL_q^vVH#k{7mm`>em=_A<y!k1EV?q&di{@hVGUB6
zuT6lU^SiX07w$Hn&s%6GW?dO=9aopxjNqOC@_V>SW2#r0O|#<#0#AlGR{&{>=HLPK
ztPD>ds_rPiZGAN`)LsSdSBxrubE@*^a$oKJ><%XU=4?tg=3&zyu|Xg1zIS!R9%{w&
zpwC!?YHvvV$o6KSQMKE+R9$H8+bk+>g=v5=(pn1X>1xb6l48%CcIv@@*IRJrSR0_C
zyiU9A;%=iU*q~IT9@0@_Qu&%@1os56BBe(9W$hs2g^jZW0>4+V9?PRv^@h(8UmtrO
zd`00%=rhlW5fKkN?Rr_}m==i{uJ!5@i`?uQd`78X9y``!8Vyya)wR*y&7#6I?-L`|
zx2aISb9ml!jdXo+`bQR~;ULpWV?BU%Kh|kIcMQujP8F+*tuW0r_Vs?CzHoW&Zu%Dh
zokFL9Sz$faSw`e%o>$-fdCM+lm1*IGNMoYjZRd-*YTZB6!l#`+bDGzUX{eR5zo;&z
zu|`cqom$o2!(OY-IYDw&c&464IH5+CBXW4HSls}EX{}C3{vW)km4i$tk>%7_T~4#8
zoc8~YqH>Vwdx;WQuXTT}L08l}GjSfcqK$MTK|6SH-thd!4zj55TEP*rsNCFh^BZS(
ztjw$HD36tSb%i`guTw|&e(t5~Pkr6|EMDGT>~GJUThZThnrT#p?#=6))w@}BsTJ4b
zG{9{`rS?6S*M~c;Fevj3hjcqzNb<OoYb-2}#!5F5(>+|JoDValZ`#D!-z7ykQF35Z
zvIWyLsz!oVcUVlvJqfLb^re5{1h%5jJU@WkjRxgJ{UuY!T^)Qr`EOEb`TP~~CgNs)
z@A7OZ!E*xet4dPb&+OE8DDyHmCAeQs&`Psf{W*QFy69t7vs&#dUnK8*CCEN8jT(2e
zQWRa;1=FO-McSqs+=^;&8sOTnnbO3$Hzmj4BS`LzE_#>FW8|2Lv39@QiV}P(0UAzz
zrw#16OIi@KkZ6xz(WP~pCr{Am1Oj^kSXAFzx;11G+0$i{gbDBaKKfDn66CFNAA8-n
z9>Cq63#II&+2p695~*n(+zQhGOM1_dRL-L`)=0jeJIM5ufn%r7bbYWBz_XkM(u!N_
zG}CV{ZI(IDmYr9aC)SVA%MPoTed}fKN9+FSdOol8nXlkRja_}_NJl+$^2SCtkBVEd
zs2I&OB7Do#l_`8T!0R0A@jU#NB`NRkAX3|?u0Gl62TSRj6K1z8#eVrSjTV4LwPVHQ
zzlf?a59*24YDr15@=oPFwfpN{Qd13X#Wfglc=JSM;N}h_*YBiy-O>Qd;K7H8^OadI
zqQZ5fJCW@S=agm+i@$IBhtm>*iKcqI>Hx<N>y*OyAxBlKeUz9ujm?KnDb-fI(BL~9
z{XJVz)8|Jk>ipHxo@*s$tifZa1op%<E?wTHewhaM%V@Uy*k*U;yNcm%3uUcaU!UwP
zG%6@Pp7p`=sPG=eb{~L)8|Eql6TdKj;r*#SD(bhfR*b-&00woMqij7@VUD`0Hsg44
zBs4$niOp5Q$})BFL_SySBuoiv)i}F6sy3?=m6Uu>b32D#&3caaT}}h6FMHR#`{6U~
z?Pm%04svxK7413hCcufaQ<RV|&zRFk6iPT=oGqr=>~1g|c<Jo&NEpFS`gGbe;(pD2
z`5I(a_+DcBigPSgIoxfawkTkDanloW(=!}fQ8#uNlq<ugnvz@!v{aqnqv$gyuqObs
z?Uou7eIobbi(9IL-1L@;@1Fo3w+*DyZ*^WyiP6OFvJNu6pW~YlzT+|t_rl|Up}}a8
zn7O1Mu}rH~?d`&E!VWUMZR1GrX&HbL|C+Tt@8tx`3DW?V(rP64b3^j|Ht|z9+rzP6
zd`4l!=@#XxmlG^POanZf?@f%C>T=zN20X8igG|3n;6CHJvFh$E7;LXD`wJtm6|Oqd
zI8(hpk80q3qPYh1#QHHbdqLjIdQNv|+D%a-{i1<9*%6yJQYtfjqYBkOGzQAu{*d+9
ziav81;I~@_Qv8}r$x7RlU;^iaX@KvYQ;EuZNO8v_`?YJPgzE2oHd^lUNN@jM!u_%i
zvTg#b{wReEsJLXBdZ^I;#mn|3Ri<B3aU|H2?W@5l19|JsamLUQ$4&H0L0HWUxqU@@
zJ&q950FQzUBx~0llX>?CMoeHU`aOVa158-dkL;~ms_Eb?DY?Tu^nR}d$<+bxKc8I|
zJDU<VF#v%%KBT@~fTsPYI)T8ki<x&{RwHHZNz_bVKT9C6UwkeF@Os~y6fcX=tnv*I
zP8oP&aBlzxw(=$k;gd9%SBeC-!gaH)kFdv5#~xjVclaz)I3eRIh*51zjxf(T_ta<m
zgG6Dk72d(<ECJwFoupjQ6sVmWh6+0;-hD6)FrsZw;+=Y3GRZ2X>Z>a{YmZp@%P?OH
zt}c!c;FHS^M0PkSjrdhB5YN9Y$@iYN6lj3jDXHX}v?%SU4I2CRT|Nz11Jkyz&R(me
zT;q4P?*LJP295;Z6Kv}|+>O+|S0P1&4HC|~_>3#|W9TDCGU%%!DXvIo|BnKnc6{~~
zi4(^{$WOInr249kioZ{`uEuqmT<~$61=mfCYG-JvcH{C`X`(SfxZ_}2BwTC%Modnj
z<l54$X6zTAqs3Y!uX7<On*vC;Yis#`Ozi6%TVWbtYWHNR`fMyQ?>7i{9BhTP0owZa
zAr}|)CTsl5gu4M=+n5H}7}`w=Pgj$RhuL>Hes{utaTNfzW~rpAoeN0D+FT(jfw1k=
v+>`BgXL%xdTpC9V%eUJ93&1;wqr%;^ee2WT(#CZ5AYOTaVn2{yF{=Lo)41pn

diff --git a/urdf/ergoCub/robots/ergoCubGazeboSN000/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboSN000/model.urdf
index 2eab1b15..f593cd55 100644
--- a/urdf/ergoCub/robots/ergoCubGazeboSN000/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubGazeboSN000/model.urdf
@@ -1472,8 +1472,8 @@
     </collision>
   </link>
   <joint name="l_index_prox" type="revolute">
-    <origin xyz="0.009192533317427712 -2.7755575615628914e-17 -0.030713451316238316" rpy="6.383782391594655e-16 -1.942890293094024e-16 1.110223024625157e-16"/>
-    <axis xyz="-0.9999999999999996 -2.0816681711721439e-16 3.99680288865056e-15"/>
+    <origin xyz="0.00919253331742774 0 -0.030713451316238427" rpy="6.106226635438364e-16 -2.636779683484747e-16 5.551115123125784e-17"/>
+    <axis xyz="-0.9999999999999998 -2.0816681711721453e-16 3.996802888650561e-15"/>
     <parent link="l_hand_index_1"/>
     <child link="l_hand_index_2"/>
     <limit lower="0" upper="1.5707963267948966" effort="1e+9" velocity="1e+9"/>
@@ -1505,8 +1505,8 @@
     </collision>
   </link>
   <joint name="l_index_dist" type="revolute">
-    <origin xyz="4.163336342344337e-17 -0.0015000000000000013 -0.030000000000000082" rpy="-1.6320278461989814e-14 -1.1102230246251565e-16 1.110223024625157e-16"/>
-    <axis xyz="-0.9999999999999997 1.2698175844150033e-15 1.3045120539345794e-15"/>
+    <origin xyz="2.7755575615628914e-17 -0.0015000000000000568 -0.02999999999999997" rpy="-1.6320278461989807e-14 -5.551115123125783e-17 5.551115123125784e-17"/>
+    <axis xyz="-0.9999999999999999 1.3253287356462613e-15 1.2351231148955076e-15"/>
     <parent link="l_hand_index_2"/>
     <child link="l_hand_index_3"/>
     <limit lower="0" upper="1.7313666179783749" effort="1e+9" velocity="1e+9"/>
@@ -1571,8 +1571,8 @@
     </collision>
   </link>
   <joint name="neck_yaw" type="revolute">
-    <origin xyz="-1.3010426069826053e-17 3.765597982987134e-17 0.0691500000000001" rpy="1.9500353504997633e-16 -1.4141248935670295e-15 3.767470613092508e-16"/>
-    <axis xyz="1.4082090529075477e-15 5.709519748275242e-16 0.9999999999999998"/>
+    <origin xyz="-4.0766001685454967e-17 3.151498988337666e-17 0.0691500000000001" rpy="2.2275926002387476e-16 -1.6650786597892902e-15 3.2676263100504264e-16"/>
+    <axis xyz="1.6591628191298083e-15 5.987076998014226e-16 0.9999999999999998"/>
     <parent link="neck_3"/>
     <child link="head"/>
     <limit lower="-0.7853981633974483" upper="0.7853981633974483" effort="1e+9" velocity="1e+9"/>
@@ -1604,8 +1604,8 @@
     </collision>
   </link>
   <joint name="camera_tilt" type="revolute">
-    <origin xyz="0.06251 -3.0000000001127547e-5 0.0763925000000007" rpy="9.055928654249867e-15 2.5958199361703294e-4 1.7122849027886369e-15"/>
-    <axis xyz="1.9074075506529947e-16 -1 -1.4146048262463281e-15"/>
+    <origin xyz="0.06250999999999989 -3.0000000001138484e-5 0.07639250000000042" rpy="9.528820759122059e-15 2.595819932592144e-4 2.6559516676797224e-15"/>
+    <axis xyz="-7.529259780358045e-16 -1 -5.660091301660127e-16"/>
     <parent link="head"/>
     <child link="realsense"/>
     <limit lower="-0.5235987755982988" upper="0.5235987755982988" effort="1e+9" velocity="1e+9"/>
@@ -1613,8 +1613,8 @@
   </joint>
   <link name="realsense">
     <inertial>
-      <mass value="0.0973042045264468"/>
-      <origin xyz="-0.01566548310600667 0.004519995218644514 0.002745610154123612" rpy="0 -0 0"/>
+      <mass value="0.08233251527028056"/>
+      <origin xyz="0.002757312110436068 -4.942831990157318e-4 3.199624651573085e-5" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
@@ -2142,7 +2142,7 @@
   </joint>
   <link name="realsense_imu_0"/>
   <joint name="realsense_imu_0_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 0.010000000000000002 1.7352099479506222e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 0.047499999999999994 1.735209986808428e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_imu_0"/>
   </joint>
@@ -2196,7 +2196,7 @@
   </joint>
   <link name="realsense_depth_frame"/>
   <joint name="realsense_depth_frame_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 0.010000000000000002 1.7352099479506222e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 0.047499999999999994 1.735209986808428e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_depth_frame"/>
   </joint>
@@ -2208,7 +2208,7 @@
   </joint>
   <link name="realsense_rgb_frame"/>
   <joint name="realsense_rgb_frame_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 -0.011499999999999996 1.7352099701550827e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 -0.011499999999999993 1.735209981257313e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_rgb_frame"/>
   </joint>
@@ -3144,7 +3144,7 @@
     <sensor name="realsense_head_depth" type="depth">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 -0.0115 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <camera name="intel_realsense_depth_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
         <horizontal_fov>1.2217</horizontal_fov>
@@ -3174,13 +3174,13 @@
   </gazebo>
   <sensor name="realsense_head_depth" type="depth">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_head_rgb" type="camera">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 -0.0115 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <camera name="intel_realsense_rgb_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
         <horizontal_fov>1.2217</horizontal_fov>
@@ -3210,13 +3210,13 @@
   </gazebo>
   <sensor name="realsense_head_rgb" type="camera">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_imu_0" type="imu">
       <always_on>1</always_on>
       <update_rate>100.000000</update_rate>
-      <pose>0.00751548 0.01 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 0.0475 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
         <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini</yarpConfigurationFile>
       </plugin>
@@ -3224,7 +3224,7 @@
   </gazebo>
   <sensor name="realsense_imu_0" type="accelerometer">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 0.01 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 0.0475 1.73521e-08"/>
   </sensor>
   <gazebo reference="head">
     <sensor name="lasersensor_head" type="gpu_lidar">
@@ -3237,15 +3237,15 @@
       <ray>
         <scan>
           <horizontal>
-            <samples>720</samples>
-            <resolution>0.5</resolution>
+            <samples>360</samples>
+            <resolution>1</resolution>
             <min_angle>0</min_angle>
             <max_angle>6.28</max_angle>
           </horizontal>
         </scan>
         <range>
           <min>0.2</min>
-          <max>5</max>
+          <max>25</max>
           <resolution>0.01</resolution>
         </range>
         <noise>
@@ -3516,6 +3516,15 @@
           <p2>0</p2>
           <center>319.5 239.5</center>
         </distortion>
+        <lens>
+          <intrinsics>
+            <fx>457.022184</fx>
+            <fy>457.022184</fy>
+            <cx>319.5</cx>
+            <cy>239.5</cy>
+            <s>0</s>
+          </intrinsics>
+        </lens>
         <image>
           <width>640</width>
           <height>480</height>
diff --git a/urdf/ergoCub/robots/ergoCubGazeboSN000_minContacts/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboSN000_minContacts/model.urdf
index 91e562f4..4d4f1ade 100644
--- a/urdf/ergoCub/robots/ergoCubGazeboSN000_minContacts/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubGazeboSN000_minContacts/model.urdf
@@ -1067,8 +1067,8 @@
     </visual>
   </link>
   <joint name="l_index_prox" type="revolute">
-    <origin xyz="0.009192533317427712 -2.7755575615628914e-17 -0.030713451316238316" rpy="6.383782391594655e-16 -1.942890293094024e-16 1.110223024625157e-16"/>
-    <axis xyz="-0.9999999999999996 -2.0816681711721439e-16 3.99680288865056e-15"/>
+    <origin xyz="0.00919253331742774 0 -0.030713451316238427" rpy="6.106226635438364e-16 -2.636779683484747e-16 5.551115123125784e-17"/>
+    <axis xyz="-0.9999999999999998 -2.0816681711721453e-16 3.996802888650561e-15"/>
     <parent link="l_hand_index_1"/>
     <child link="l_hand_index_2"/>
     <limit lower="0" upper="1.5707963267948966" effort="1e+9" velocity="1e+9"/>
@@ -1091,8 +1091,8 @@
     </visual>
   </link>
   <joint name="l_index_dist" type="revolute">
-    <origin xyz="4.163336342344337e-17 -0.0015000000000000013 -0.030000000000000082" rpy="-1.6320278461989814e-14 -1.1102230246251565e-16 1.110223024625157e-16"/>
-    <axis xyz="-0.9999999999999997 1.2698175844150033e-15 1.3045120539345794e-15"/>
+    <origin xyz="2.7755575615628914e-17 -0.0015000000000000568 -0.02999999999999997" rpy="-1.6320278461989807e-14 -5.551115123125783e-17 5.551115123125784e-17"/>
+    <axis xyz="-0.9999999999999999 1.3253287356462613e-15 1.2351231148955076e-15"/>
     <parent link="l_hand_index_2"/>
     <child link="l_hand_index_3"/>
     <limit lower="0" upper="1.7313666179783749" effort="1e+9" velocity="1e+9"/>
@@ -1139,8 +1139,8 @@
     </visual>
   </link>
   <joint name="neck_yaw" type="revolute">
-    <origin xyz="-1.3010426069826053e-17 3.765597982987134e-17 0.0691500000000001" rpy="1.9500353504997633e-16 -1.4141248935670295e-15 3.767470613092508e-16"/>
-    <axis xyz="1.4082090529075477e-15 5.709519748275242e-16 0.9999999999999998"/>
+    <origin xyz="-4.0766001685454967e-17 3.151498988337666e-17 0.0691500000000001" rpy="2.2275926002387476e-16 -1.6650786597892902e-15 3.2676263100504264e-16"/>
+    <axis xyz="1.6591628191298083e-15 5.987076998014226e-16 0.9999999999999998"/>
     <parent link="neck_3"/>
     <child link="head"/>
     <limit lower="-0.7853981633974483" upper="0.7853981633974483" effort="1e+9" velocity="1e+9"/>
@@ -1163,8 +1163,8 @@
     </visual>
   </link>
   <joint name="camera_tilt" type="revolute">
-    <origin xyz="0.06251 -3.0000000001127547e-5 0.0763925000000007" rpy="9.055928654249867e-15 2.5958199361703294e-4 1.7122849027886369e-15"/>
-    <axis xyz="1.9074075506529947e-16 -1 -1.4146048262463281e-15"/>
+    <origin xyz="0.06250999999999989 -3.0000000001138484e-5 0.07639250000000042" rpy="9.528820759122059e-15 2.595819932592144e-4 2.6559516676797224e-15"/>
+    <axis xyz="-7.529259780358045e-16 -1 -5.660091301660127e-16"/>
     <parent link="head"/>
     <child link="realsense"/>
     <limit lower="-0.5235987755982988" upper="0.5235987755982988" effort="1e+9" velocity="1e+9"/>
@@ -1172,8 +1172,8 @@
   </joint>
   <link name="realsense">
     <inertial>
-      <mass value="0.0973042045264468"/>
-      <origin xyz="-0.01566548310600667 0.004519995218644514 0.002745610154123612" rpy="0 -0 0"/>
+      <mass value="0.08233251527028056"/>
+      <origin xyz="0.002757312110436068 -4.942831990157318e-4 3.199624651573085e-5" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
@@ -1584,7 +1584,7 @@
   </joint>
   <link name="realsense_imu_0"/>
   <joint name="realsense_imu_0_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 0.010000000000000002 1.7352099479506222e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 0.047499999999999994 1.735209986808428e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_imu_0"/>
   </joint>
@@ -1638,7 +1638,7 @@
   </joint>
   <link name="realsense_depth_frame"/>
   <joint name="realsense_depth_frame_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 0.010000000000000002 1.7352099479506222e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 0.047499999999999994 1.735209986808428e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_depth_frame"/>
   </joint>
@@ -1650,7 +1650,7 @@
   </joint>
   <link name="realsense_rgb_frame"/>
   <joint name="realsense_rgb_frame_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 -0.011499999999999996 1.7352099701550827e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 -0.011499999999999993 1.735209981257313e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_rgb_frame"/>
   </joint>
@@ -2586,7 +2586,7 @@
     <sensor name="realsense_head_depth" type="depth">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 -0.0115 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <camera name="intel_realsense_depth_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
         <horizontal_fov>1.2217</horizontal_fov>
@@ -2616,13 +2616,13 @@
   </gazebo>
   <sensor name="realsense_head_depth" type="depth">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_head_rgb" type="camera">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 -0.0115 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <camera name="intel_realsense_rgb_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
         <horizontal_fov>1.2217</horizontal_fov>
@@ -2652,13 +2652,13 @@
   </gazebo>
   <sensor name="realsense_head_rgb" type="camera">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_imu_0" type="imu">
       <always_on>1</always_on>
       <update_rate>100.000000</update_rate>
-      <pose>0.00751548 0.01 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 0.0475 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
         <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini</yarpConfigurationFile>
       </plugin>
@@ -2666,7 +2666,7 @@
   </gazebo>
   <sensor name="realsense_imu_0" type="accelerometer">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 0.01 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 0.0475 1.73521e-08"/>
   </sensor>
   <gazebo reference="head">
     <sensor name="lasersensor_head" type="gpu_lidar">
@@ -2679,15 +2679,15 @@
       <ray>
         <scan>
           <horizontal>
-            <samples>720</samples>
-            <resolution>0.5</resolution>
+            <samples>360</samples>
+            <resolution>1</resolution>
             <min_angle>0</min_angle>
             <max_angle>6.28</max_angle>
           </horizontal>
         </scan>
         <range>
           <min>0.2</min>
-          <max>5</max>
+          <max>25</max>
           <resolution>0.01</resolution>
         </range>
         <noise>
@@ -2958,6 +2958,15 @@
           <p2>0</p2>
           <center>319.5 239.5</center>
         </distortion>
+        <lens>
+          <intrinsics>
+            <fx>457.022184</fx>
+            <fy>457.022184</fy>
+            <cx>319.5</cx>
+            <cy>239.5</cy>
+            <s>0</s>
+          </intrinsics>
+        </lens>
         <image>
           <width>640</width>
           <height>480</height>
diff --git a/urdf/ergoCub/robots/ergoCubGazeboSN001/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboSN001/model.urdf
index 4d1dfa57..d1799673 100644
--- a/urdf/ergoCub/robots/ergoCubGazeboSN001/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubGazeboSN001/model.urdf
@@ -321,7 +321,7 @@
   </link>
   <joint name="neck_roll" type="revolute">
     <origin xyz="-3.5561831257524545e-17 7.375606977361926e-16 0.010999999999997845" rpy="8.452269018635315e-14 7.050925500045285e-14 1.2263451870709675e-14"/>
-    <axis xyz="1 2.30576840860686e-16 -7.196923107658096e-17"/>
+    <axis xyz="1 2.3057684086068286e-16 -7.196923107658096e-17"/>
     <parent link="neck_2"/>
     <child link="neck_3"/>
     <limit lower="-0.5235987755982988" upper="0.5235987755982988" effort="1e+9" velocity="1e+9"/>
@@ -353,8 +353,8 @@
     </collision>
   </link>
   <joint name="neck_yaw" type="revolute">
-    <origin xyz="-1.734723475976807e-18 1.0057301276633031e-23 0.06914999999999993" rpy="2.1582026845669264e-16 -1.386477883657293e-15 -1.9981249037222812e-13"/>
-    <axis xyz="1.386477883657293e-15 2.1582026845669262e-16 0.9999999999999999"/>
+    <origin xyz="-5.0306980803327406e-17 2.0696535769897827e-17 0.06914999999999971" rpy="1.6030911723564583e-16 -1.3309667326328268e-15 -1.9976738756079363e-13"/>
+    <axis xyz="1.3309667326328268e-15 1.603091172356458e-16 0.9999999999999999"/>
     <parent link="neck_3"/>
     <child link="head"/>
     <limit lower="-0.7853981633974483" upper="0.7853981633974483" effort="1e+9" velocity="1e+9"/>
@@ -386,8 +386,8 @@
     </collision>
   </link>
   <joint name="camera_tilt" type="revolute">
-    <origin xyz="0.06250999999999991 -2.999999997621366e-5 0.07639249999999997" rpy="-7.545682259870277e-14 2.5958199361694967e-4 2.017046159162252e-13"/>
-    <axis xyz="-1.998015835203486e-13 -1 -8.597927048660853e-14"/>
+    <origin xyz="0.06250999999999987 -3.000000000113848e-5 0.07639250000000047" rpy="9.528820759122057e-15 2.5958199325913113e-4 2.6559516676797335e-15"/>
+    <axis xyz="-7.529259780358163e-16 -1 -5.660091301660221e-16"/>
     <parent link="head"/>
     <child link="realsense"/>
     <limit lower="-0.5235987755982988" upper="0.5235987755982988" effort="1e+9" velocity="1e+9"/>
@@ -395,8 +395,8 @@
   </joint>
   <link name="realsense">
     <inertial>
-      <mass value="0.0973042045264468"/>
-      <origin xyz="-0.01566548310600667 0.004519995218644514 0.002745610154123612" rpy="0 -0 0"/>
+      <mass value="0.08233251527028056"/>
+      <origin xyz="0.002757312110436068 -4.942831990157318e-4 3.199624651573085e-5" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
@@ -1634,8 +1634,8 @@
     </collision>
   </link>
   <joint name="l_index_dist" type="revolute">
-    <origin xyz="-2.7755575615628914e-17 -0.0015000000000000013 -0.03999999999999987" rpy="-7.482326172536535e-8 1.8041124150158794e-16 -5.551115123125783e-16"/>
-    <axis xyz="-1 -2.9143356888525625e-16 9.714452192336961e-17"/>
+    <origin xyz="-2.7755575615628914e-17 -0.0014999999999999736 -0.039999999999999925" rpy="-7.482326169760975e-8 1.734723475976807e-16 -5.551115123125782e-16"/>
+    <axis xyz="-1.0000000000000002 -2.359224176539984e-16 1.457167792507203e-16"/>
     <parent link="l_hand_index_2"/>
     <child link="l_hand_index_3"/>
     <limit lower="0" upper="1.7165313193364231" effort="1e+9" velocity="1e+9"/>
@@ -2172,7 +2172,7 @@
   </joint>
   <link name="realsense_imu_0"/>
   <joint name="realsense_imu_0_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 0.010000000000000002 1.7352099479506222e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 0.047499999999999994 1.735209986808428e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_imu_0"/>
   </joint>
@@ -2226,7 +2226,7 @@
   </joint>
   <link name="realsense_depth_frame"/>
   <joint name="realsense_depth_frame_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 0.010000000000000002 1.7352099479506222e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 0.047499999999999994 1.735209986808428e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_depth_frame"/>
   </joint>
@@ -2238,7 +2238,7 @@
   </joint>
   <link name="realsense_rgb_frame"/>
   <joint name="realsense_rgb_frame_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 -0.011499999999999996 1.7352099701550827e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 -0.011499999999999993 1.735209981257313e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_rgb_frame"/>
   </joint>
@@ -3174,7 +3174,7 @@
     <sensor name="realsense_head_depth" type="depth">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 -0.0115 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <camera name="intel_realsense_depth_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
         <horizontal_fov>1.2217</horizontal_fov>
@@ -3204,13 +3204,13 @@
   </gazebo>
   <sensor name="realsense_head_depth" type="depth">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_head_rgb" type="camera">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 -0.0115 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <camera name="intel_realsense_rgb_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
         <horizontal_fov>1.2217</horizontal_fov>
@@ -3240,13 +3240,13 @@
   </gazebo>
   <sensor name="realsense_head_rgb" type="camera">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_imu_0" type="imu">
       <always_on>1</always_on>
       <update_rate>100.000000</update_rate>
-      <pose>0.00751548 0.01 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 0.0475 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
         <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini</yarpConfigurationFile>
       </plugin>
@@ -3254,7 +3254,7 @@
   </gazebo>
   <sensor name="realsense_imu_0" type="accelerometer">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 0.01 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 0.0475 1.73521e-08"/>
   </sensor>
   <gazebo reference="head">
     <sensor name="lasersensor_head" type="gpu_lidar">
@@ -3267,15 +3267,15 @@
       <ray>
         <scan>
           <horizontal>
-            <samples>720</samples>
-            <resolution>0.5</resolution>
+            <samples>360</samples>
+            <resolution>1</resolution>
             <min_angle>0</min_angle>
             <max_angle>6.28</max_angle>
           </horizontal>
         </scan>
         <range>
           <min>0.2</min>
-          <max>5</max>
+          <max>25</max>
           <resolution>0.01</resolution>
         </range>
         <noise>
@@ -3546,6 +3546,15 @@
           <p2>0</p2>
           <center>319.5 239.5</center>
         </distortion>
+        <lens>
+          <intrinsics>
+            <fx>457.022184</fx>
+            <fy>457.022184</fy>
+            <cx>319.5</cx>
+            <cy>239.5</cy>
+            <s>0</s>
+          </intrinsics>
+        </lens>
         <image>
           <width>640</width>
           <height>480</height>
diff --git a/urdf/ergoCub/robots/ergoCubGazeboSN001_minContacts/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboSN001_minContacts/model.urdf
index 35a724aa..51116c60 100644
--- a/urdf/ergoCub/robots/ergoCubGazeboSN001_minContacts/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubGazeboSN001_minContacts/model.urdf
@@ -231,7 +231,7 @@
   </link>
   <joint name="neck_roll" type="revolute">
     <origin xyz="-3.5561831257524545e-17 7.375606977361926e-16 0.010999999999997845" rpy="8.452269018635315e-14 7.050925500045285e-14 1.2263451870709675e-14"/>
-    <axis xyz="1 2.30576840860686e-16 -7.196923107658096e-17"/>
+    <axis xyz="1 2.3057684086068286e-16 -7.196923107658096e-17"/>
     <parent link="neck_2"/>
     <child link="neck_3"/>
     <limit lower="-0.5235987755982988" upper="0.5235987755982988" effort="1e+9" velocity="1e+9"/>
@@ -254,8 +254,8 @@
     </visual>
   </link>
   <joint name="neck_yaw" type="revolute">
-    <origin xyz="-1.734723475976807e-18 1.0057301276633031e-23 0.06914999999999993" rpy="2.1582026845669264e-16 -1.386477883657293e-15 -1.9981249037222812e-13"/>
-    <axis xyz="1.386477883657293e-15 2.1582026845669262e-16 0.9999999999999999"/>
+    <origin xyz="-5.0306980803327406e-17 2.0696535769897827e-17 0.06914999999999971" rpy="1.6030911723564583e-16 -1.3309667326328268e-15 -1.9976738756079363e-13"/>
+    <axis xyz="1.3309667326328268e-15 1.603091172356458e-16 0.9999999999999999"/>
     <parent link="neck_3"/>
     <child link="head"/>
     <limit lower="-0.7853981633974483" upper="0.7853981633974483" effort="1e+9" velocity="1e+9"/>
@@ -278,8 +278,8 @@
     </visual>
   </link>
   <joint name="camera_tilt" type="revolute">
-    <origin xyz="0.06250999999999991 -2.999999997621366e-5 0.07639249999999997" rpy="-7.545682259870277e-14 2.5958199361694967e-4 2.017046159162252e-13"/>
-    <axis xyz="-1.998015835203486e-13 -1 -8.597927048660853e-14"/>
+    <origin xyz="0.06250999999999987 -3.000000000113848e-5 0.07639250000000047" rpy="9.528820759122057e-15 2.5958199325913113e-4 2.6559516676797335e-15"/>
+    <axis xyz="-7.529259780358163e-16 -1 -5.660091301660221e-16"/>
     <parent link="head"/>
     <child link="realsense"/>
     <limit lower="-0.5235987755982988" upper="0.5235987755982988" effort="1e+9" velocity="1e+9"/>
@@ -287,8 +287,8 @@
   </joint>
   <link name="realsense">
     <inertial>
-      <mass value="0.0973042045264468"/>
-      <origin xyz="-0.01566548310600667 0.004519995218644514 0.002745610154123612" rpy="0 -0 0"/>
+      <mass value="0.08233251527028056"/>
+      <origin xyz="0.002757312110436068 -4.942831990157318e-4 3.199624651573085e-5" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
@@ -1184,8 +1184,8 @@
     </visual>
   </link>
   <joint name="l_index_dist" type="revolute">
-    <origin xyz="-2.7755575615628914e-17 -0.0015000000000000013 -0.03999999999999987" rpy="-7.482326172536535e-8 1.8041124150158794e-16 -5.551115123125783e-16"/>
-    <axis xyz="-1 -2.9143356888525625e-16 9.714452192336961e-17"/>
+    <origin xyz="-2.7755575615628914e-17 -0.0014999999999999736 -0.039999999999999925" rpy="-7.482326169760975e-8 1.734723475976807e-16 -5.551115123125782e-16"/>
+    <axis xyz="-1.0000000000000002 -2.359224176539984e-16 1.457167792507203e-16"/>
     <parent link="l_hand_index_2"/>
     <child link="l_hand_index_3"/>
     <limit lower="0" upper="1.7165313193364231" effort="1e+9" velocity="1e+9"/>
@@ -1605,7 +1605,7 @@
   </joint>
   <link name="realsense_imu_0"/>
   <joint name="realsense_imu_0_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 0.010000000000000002 1.7352099479506222e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 0.047499999999999994 1.735209986808428e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_imu_0"/>
   </joint>
@@ -1659,7 +1659,7 @@
   </joint>
   <link name="realsense_depth_frame"/>
   <joint name="realsense_depth_frame_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 0.010000000000000002 1.7352099479506222e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 0.047499999999999994 1.735209986808428e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_depth_frame"/>
   </joint>
@@ -1671,7 +1671,7 @@
   </joint>
   <link name="realsense_rgb_frame"/>
   <joint name="realsense_rgb_frame_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 -0.011499999999999996 1.7352099701550827e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 -0.011499999999999993 1.735209981257313e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_rgb_frame"/>
   </joint>
@@ -2607,7 +2607,7 @@
     <sensor name="realsense_head_depth" type="depth">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 -0.0115 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <camera name="intel_realsense_depth_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
         <horizontal_fov>1.2217</horizontal_fov>
@@ -2637,13 +2637,13 @@
   </gazebo>
   <sensor name="realsense_head_depth" type="depth">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_head_rgb" type="camera">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 -0.0115 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <camera name="intel_realsense_rgb_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
         <horizontal_fov>1.2217</horizontal_fov>
@@ -2673,13 +2673,13 @@
   </gazebo>
   <sensor name="realsense_head_rgb" type="camera">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_imu_0" type="imu">
       <always_on>1</always_on>
       <update_rate>100.000000</update_rate>
-      <pose>0.00751548 0.01 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 0.0475 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
         <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini</yarpConfigurationFile>
       </plugin>
@@ -2687,7 +2687,7 @@
   </gazebo>
   <sensor name="realsense_imu_0" type="accelerometer">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 0.01 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 0.0475 1.73521e-08"/>
   </sensor>
   <gazebo reference="head">
     <sensor name="lasersensor_head" type="gpu_lidar">
@@ -2700,15 +2700,15 @@
       <ray>
         <scan>
           <horizontal>
-            <samples>720</samples>
-            <resolution>0.5</resolution>
+            <samples>360</samples>
+            <resolution>1</resolution>
             <min_angle>0</min_angle>
             <max_angle>6.28</max_angle>
           </horizontal>
         </scan>
         <range>
           <min>0.2</min>
-          <max>5</max>
+          <max>25</max>
           <resolution>0.01</resolution>
         </range>
         <noise>
@@ -2979,6 +2979,15 @@
           <p2>0</p2>
           <center>319.5 239.5</center>
         </distortion>
+        <lens>
+          <intrinsics>
+            <fx>457.022184</fx>
+            <fy>457.022184</fy>
+            <cx>319.5</cx>
+            <cy>239.5</cy>
+            <s>0</s>
+          </intrinsics>
+        </lens>
         <image>
           <width>640</width>
           <height>480</height>
diff --git a/urdf/ergoCub/robots/ergoCubGazeboSN003/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboSN003/model.urdf
index 965bb046..2bbe4aa6 100644
--- a/urdf/ergoCub/robots/ergoCubGazeboSN003/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubGazeboSN003/model.urdf
@@ -353,8 +353,8 @@
     </collision>
   </link>
   <joint name="neck_yaw" type="revolute">
-    <origin xyz="-1.2588645634161594e-7 -3.511736027695927e-16 0.06914999999990112" rpy="5.476218470552679e-15 -1.5706357540219146e-6 5.308786623366525e-15"/>
-    <axis xyz="1.5706357540212687e-6 5.476218470545924e-15 0.9999999999987664"/>
+    <origin xyz="-1.2588645629564577e-7 2.1530156161317587e-17 0.06914999999990118" rpy="1.6029881711049145e-16 -1.5706357538557005e-6 1.1347346732224471e-14"/>
+    <axis xyz="1.5706357538550546e-6 1.6029881711029372e-16 0.9999999999987664"/>
     <parent link="neck_3"/>
     <child link="head"/>
     <limit lower="-0.7853981633974483" upper="0.7853981633974483" effort="1e+9" velocity="1e+9"/>
@@ -386,8 +386,8 @@
     </collision>
   </link>
   <joint name="camera_tilt" type="revolute">
-    <origin xyz="0.06251000000000004 -3.0000000001050474e-5 0.07639250000000042" rpy="9.111439807333146e-15 2.5958199361708845e-4 1.6567881612289887e-15"/>
-    <axis xyz="2.46237494755185e-16 -1 -1.3590792672083997e-15"/>
+    <origin xyz="0.06250999999999986 -3.0000000001138477e-5 0.07639250000000058" rpy="9.528820759122063e-15 2.595819932592699e-4 2.655951667679724e-15"/>
+    <axis xyz="-7.529259780358055e-16 -1 -5.660091301660095e-16"/>
     <parent link="head"/>
     <child link="realsense"/>
     <limit lower="-0.5235987755982988" upper="0.5235987755982988" effort="1e+9" velocity="1e+9"/>
@@ -395,8 +395,8 @@
   </joint>
   <link name="realsense">
     <inertial>
-      <mass value="0.0973042045264468"/>
-      <origin xyz="-0.01566548310600667 0.004519995218644514 0.002745610154123612" rpy="0 -0 0"/>
+      <mass value="0.08233251527028056"/>
+      <origin xyz="0.002757312110436068 -4.942831990157318e-4 3.199624651573085e-5" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
@@ -2172,7 +2172,7 @@
   </joint>
   <link name="realsense_imu_0"/>
   <joint name="realsense_imu_0_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 0.010000000000000002 1.7352099479506222e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 0.047499999999999994 1.735209986808428e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_imu_0"/>
   </joint>
@@ -2226,7 +2226,7 @@
   </joint>
   <link name="realsense_depth_frame"/>
   <joint name="realsense_depth_frame_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 0.010000000000000002 1.7352099479506222e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 0.047499999999999994 1.735209986808428e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_depth_frame"/>
   </joint>
@@ -2238,7 +2238,7 @@
   </joint>
   <link name="realsense_rgb_frame"/>
   <joint name="realsense_rgb_frame_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 -0.011499999999999996 1.7352099701550827e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 -0.011499999999999993 1.735209981257313e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_rgb_frame"/>
   </joint>
@@ -3174,7 +3174,7 @@
     <sensor name="realsense_head_depth" type="depth">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 -0.0115 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <camera name="intel_realsense_depth_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
         <horizontal_fov>1.2217</horizontal_fov>
@@ -3204,13 +3204,13 @@
   </gazebo>
   <sensor name="realsense_head_depth" type="depth">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_head_rgb" type="camera">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 -0.0115 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <camera name="intel_realsense_rgb_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
         <horizontal_fov>1.2217</horizontal_fov>
@@ -3240,13 +3240,13 @@
   </gazebo>
   <sensor name="realsense_head_rgb" type="camera">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_imu_0" type="imu">
       <always_on>1</always_on>
       <update_rate>100.000000</update_rate>
-      <pose>0.00751548 0.01 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 0.0475 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
         <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini</yarpConfigurationFile>
       </plugin>
@@ -3254,7 +3254,7 @@
   </gazebo>
   <sensor name="realsense_imu_0" type="accelerometer">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 0.01 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 0.0475 1.73521e-08"/>
   </sensor>
   <gazebo reference="head">
     <sensor name="lasersensor_head" type="gpu_lidar">
@@ -3267,15 +3267,15 @@
       <ray>
         <scan>
           <horizontal>
-            <samples>720</samples>
-            <resolution>0.5</resolution>
+            <samples>360</samples>
+            <resolution>1</resolution>
             <min_angle>0</min_angle>
             <max_angle>6.28</max_angle>
           </horizontal>
         </scan>
         <range>
           <min>0.2</min>
-          <max>5</max>
+          <max>25</max>
           <resolution>0.01</resolution>
         </range>
         <noise>
@@ -3550,6 +3550,15 @@
           <p2>0</p2>
           <center>319.5 239.5</center>
         </distortion>
+        <lens>
+          <intrinsics>
+            <fx>457.022184</fx>
+            <fy>457.022184</fy>
+            <cx>319.5</cx>
+            <cy>239.5</cy>
+            <s>0</s>
+          </intrinsics>
+        </lens>
         <image>
           <width>640</width>
           <height>480</height>
diff --git a/urdf/ergoCub/robots/ergoCubGazeboSN003_minContacts/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboSN003_minContacts/model.urdf
index 466db4b7..afac61b7 100644
--- a/urdf/ergoCub/robots/ergoCubGazeboSN003_minContacts/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubGazeboSN003_minContacts/model.urdf
@@ -254,8 +254,8 @@
     </visual>
   </link>
   <joint name="neck_yaw" type="revolute">
-    <origin xyz="-1.2588645634161594e-7 -3.511736027695927e-16 0.06914999999990112" rpy="5.476218470552679e-15 -1.5706357540219146e-6 5.308786623366525e-15"/>
-    <axis xyz="1.5706357540212687e-6 5.476218470545924e-15 0.9999999999987664"/>
+    <origin xyz="-1.2588645629564577e-7 2.1530156161317587e-17 0.06914999999990118" rpy="1.6029881711049145e-16 -1.5706357538557005e-6 1.1347346732224471e-14"/>
+    <axis xyz="1.5706357538550546e-6 1.6029881711029372e-16 0.9999999999987664"/>
     <parent link="neck_3"/>
     <child link="head"/>
     <limit lower="-0.7853981633974483" upper="0.7853981633974483" effort="1e+9" velocity="1e+9"/>
@@ -278,8 +278,8 @@
     </visual>
   </link>
   <joint name="camera_tilt" type="revolute">
-    <origin xyz="0.06251000000000004 -3.0000000001050474e-5 0.07639250000000042" rpy="9.111439807333146e-15 2.5958199361708845e-4 1.6567881612289887e-15"/>
-    <axis xyz="2.46237494755185e-16 -1 -1.3590792672083997e-15"/>
+    <origin xyz="0.06250999999999986 -3.0000000001138477e-5 0.07639250000000058" rpy="9.528820759122063e-15 2.595819932592699e-4 2.655951667679724e-15"/>
+    <axis xyz="-7.529259780358055e-16 -1 -5.660091301660095e-16"/>
     <parent link="head"/>
     <child link="realsense"/>
     <limit lower="-0.5235987755982988" upper="0.5235987755982988" effort="1e+9" velocity="1e+9"/>
@@ -287,8 +287,8 @@
   </joint>
   <link name="realsense">
     <inertial>
-      <mass value="0.0973042045264468"/>
-      <origin xyz="-0.01566548310600667 0.004519995218644514 0.002745610154123612" rpy="0 -0 0"/>
+      <mass value="0.08233251527028056"/>
+      <origin xyz="0.002757312110436068 -4.942831990157318e-4 3.199624651573085e-5" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
     </inertial>
     <visual>
@@ -1605,7 +1605,7 @@
   </joint>
   <link name="realsense_imu_0"/>
   <joint name="realsense_imu_0_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 0.010000000000000002 1.7352099479506222e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 0.047499999999999994 1.735209986808428e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_imu_0"/>
   </joint>
@@ -1659,7 +1659,7 @@
   </joint>
   <link name="realsense_depth_frame"/>
   <joint name="realsense_depth_frame_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 0.010000000000000002 1.7352099479506222e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 0.047499999999999994 1.735209986808428e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_depth_frame"/>
   </joint>
@@ -1671,7 +1671,7 @@
   </joint>
   <link name="realsense_rgb_frame"/>
   <joint name="realsense_rgb_frame_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 -0.011499999999999996 1.7352099701550827e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 -0.011499999999999993 1.735209981257313e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_rgb_frame"/>
   </joint>
@@ -2607,7 +2607,7 @@
     <sensor name="realsense_head_depth" type="depth">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 -0.0115 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <camera name="intel_realsense_depth_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
         <horizontal_fov>1.2217</horizontal_fov>
@@ -2637,13 +2637,13 @@
   </gazebo>
   <sensor name="realsense_head_depth" type="depth">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_head_rgb" type="camera">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 -0.0115 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <camera name="intel_realsense_rgb_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
         <horizontal_fov>1.2217</horizontal_fov>
@@ -2673,13 +2673,13 @@
   </gazebo>
   <sensor name="realsense_head_rgb" type="camera">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_imu_0" type="imu">
       <always_on>1</always_on>
       <update_rate>100.000000</update_rate>
-      <pose>0.00751548 0.01 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 0.0475 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
         <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini</yarpConfigurationFile>
       </plugin>
@@ -2687,7 +2687,7 @@
   </gazebo>
   <sensor name="realsense_imu_0" type="accelerometer">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 0.01 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 0.0475 1.73521e-08"/>
   </sensor>
   <gazebo reference="head">
     <sensor name="lasersensor_head" type="gpu_lidar">
@@ -2700,15 +2700,15 @@
       <ray>
         <scan>
           <horizontal>
-            <samples>720</samples>
-            <resolution>0.5</resolution>
+            <samples>360</samples>
+            <resolution>1</resolution>
             <min_angle>0</min_angle>
             <max_angle>6.28</max_angle>
           </horizontal>
         </scan>
         <range>
           <min>0.2</min>
-          <max>5</max>
+          <max>25</max>
           <resolution>0.01</resolution>
         </range>
         <noise>
@@ -2983,6 +2983,15 @@
           <p2>0</p2>
           <center>319.5 239.5</center>
         </distortion>
+        <lens>
+          <intrinsics>
+            <fx>457.022184</fx>
+            <fy>457.022184</fy>
+            <cx>319.5</cx>
+            <cy>239.5</cy>
+            <s>0</s>
+          </intrinsics>
+        </lens>
         <image>
           <width>640</width>
           <height>480</height>
diff --git a/urdf/ergoCub/robots/ergoCubSN000/model.urdf b/urdf/ergoCub/robots/ergoCubSN000/model.urdf
index 4a9b250a..3335b63e 100644
--- a/urdf/ergoCub/robots/ergoCubSN000/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubSN000/model.urdf
@@ -1472,8 +1472,8 @@
     </collision>
   </link>
   <joint name="l_index_prox" type="revolute">
-    <origin xyz="0.009192533317427712 -2.7755575615628914e-17 -0.030713451316238316" rpy="6.383782391594655e-16 -1.942890293094024e-16 1.110223024625157e-16"/>
-    <axis xyz="-0.9999999999999996 -2.0816681711721439e-16 3.99680288865056e-15"/>
+    <origin xyz="0.00919253331742774 0 -0.030713451316238427" rpy="6.106226635438364e-16 -2.636779683484747e-16 5.551115123125784e-17"/>
+    <axis xyz="-0.9999999999999998 -2.0816681711721453e-16 3.996802888650561e-15"/>
     <parent link="l_hand_index_1"/>
     <child link="l_hand_index_2"/>
     <limit lower="0" upper="1.5707963267948966" effort="1e+9" velocity="1e+9"/>
@@ -1505,8 +1505,8 @@
     </collision>
   </link>
   <joint name="l_index_dist" type="revolute">
-    <origin xyz="4.163336342344337e-17 -0.0015000000000000013 -0.030000000000000082" rpy="-1.6320278461989814e-14 -1.1102230246251565e-16 1.110223024625157e-16"/>
-    <axis xyz="-0.9999999999999997 1.2698175844150033e-15 1.3045120539345794e-15"/>
+    <origin xyz="2.7755575615628914e-17 -0.0015000000000000568 -0.02999999999999997" rpy="-1.6320278461989807e-14 -5.551115123125783e-17 5.551115123125784e-17"/>
+    <axis xyz="-0.9999999999999999 1.3253287356462613e-15 1.2351231148955076e-15"/>
     <parent link="l_hand_index_2"/>
     <child link="l_hand_index_3"/>
     <limit lower="0" upper="1.7313666179783749" effort="1e+9" velocity="1e+9"/>
@@ -1571,8 +1571,8 @@
     </collision>
   </link>
   <joint name="neck_yaw" type="revolute">
-    <origin xyz="-1.3010426069826053e-17 3.765597982987134e-17 0.0691500000000001" rpy="1.9500353504997633e-16 -1.4141248935670295e-15 3.767470613092508e-16"/>
-    <axis xyz="1.4082090529075477e-15 5.709519748275242e-16 0.9999999999999998"/>
+    <origin xyz="-4.0766001685454967e-17 3.151498988337666e-17 0.0691500000000001" rpy="2.2275926002387476e-16 -1.6650786597892902e-15 3.2676263100504264e-16"/>
+    <axis xyz="1.6591628191298083e-15 5.987076998014226e-16 0.9999999999999998"/>
     <parent link="neck_3"/>
     <child link="head"/>
     <limit lower="-0.7853981633974483" upper="0.7853981633974483" effort="1e+9" velocity="1e+9"/>
@@ -1604,8 +1604,8 @@
     </collision>
   </link>
   <joint name="camera_tilt" type="revolute">
-    <origin xyz="0.06251 -3.0000000001127547e-5 0.0763925000000007" rpy="9.055928654249867e-15 2.5958199361703294e-4 1.7122849027886369e-15"/>
-    <axis xyz="1.9074075506529947e-16 -1 -1.4146048262463281e-15"/>
+    <origin xyz="0.06250999999999989 -3.0000000001138484e-5 0.07639250000000042" rpy="9.528820759122059e-15 2.595819932592144e-4 2.6559516676797224e-15"/>
+    <axis xyz="-7.529259780358045e-16 -1 -5.660091301660127e-16"/>
     <parent link="head"/>
     <child link="realsense"/>
     <limit lower="-0.5235987755982988" upper="0.5235987755982988" effort="1e+9" velocity="1e+9"/>
@@ -1613,9 +1613,9 @@
   </joint>
   <link name="realsense">
     <inertial>
-      <mass value="0.0973042045264468"/>
-      <origin xyz="-0.01566548310600667 0.004519995218644514 0.002745610154123612" rpy="0 -0 0"/>
-      <inertia ixx="1.3363140922191058e-4" ixy="4.7877399688365374e-5" ixz="2.5482221369268495e-5" iyy="1.8708641575714753e-4" iyz="-7.171072206331662e-6" izz="2.963329027014652e-4"/>
+      <mass value="0.08233251527028056"/>
+      <origin xyz="0.002757312110436068 -4.942831990157318e-4 3.199624651573085e-5" rpy="0 -0 0"/>
+      <inertia ixx="1.1574711197088633e-4" ixy="-1.7492230739037293e-7" ixz="-2.5944440100815574e-8" iyy="8.421296719556895e-6" iyz="-1.0106462328987549e-7" izz="1.1038824885095133e-4"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -2142,7 +2142,7 @@
   </joint>
   <link name="realsense_imu_0"/>
   <joint name="realsense_imu_0_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 0.010000000000000002 1.7352099479506222e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 0.047499999999999994 1.735209986808428e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_imu_0"/>
   </joint>
@@ -2196,7 +2196,7 @@
   </joint>
   <link name="realsense_depth_frame"/>
   <joint name="realsense_depth_frame_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 0.010000000000000002 1.7352099479506222e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 0.047499999999999994 1.735209986808428e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_depth_frame"/>
   </joint>
@@ -2208,7 +2208,7 @@
   </joint>
   <link name="realsense_rgb_frame"/>
   <joint name="realsense_rgb_frame_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 -0.011499999999999996 1.7352099701550827e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 -0.011499999999999993 1.735209981257313e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_rgb_frame"/>
   </joint>
@@ -3144,7 +3144,7 @@
     <sensor name="realsense_head_depth" type="depth">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 -0.0115 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <camera name="intel_realsense_depth_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
         <horizontal_fov>1.2217</horizontal_fov>
@@ -3174,13 +3174,13 @@
   </gazebo>
   <sensor name="realsense_head_depth" type="depth">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_head_rgb" type="camera">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 -0.0115 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <camera name="intel_realsense_rgb_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
         <horizontal_fov>1.2217</horizontal_fov>
@@ -3210,13 +3210,13 @@
   </gazebo>
   <sensor name="realsense_head_rgb" type="camera">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_imu_0" type="imu">
       <always_on>1</always_on>
       <update_rate>100.000000</update_rate>
-      <pose>0.00751548 0.01 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 0.0475 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
         <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini</yarpConfigurationFile>
       </plugin>
@@ -3224,7 +3224,7 @@
   </gazebo>
   <sensor name="realsense_imu_0" type="accelerometer">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 0.01 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 0.0475 1.73521e-08"/>
   </sensor>
   <gazebo reference="head">
     <sensor name="lasersensor_head" type="gpu_lidar">
@@ -3237,15 +3237,15 @@
       <ray>
         <scan>
           <horizontal>
-            <samples>720</samples>
-            <resolution>0.5</resolution>
+            <samples>360</samples>
+            <resolution>1</resolution>
             <min_angle>0</min_angle>
             <max_angle>6.28</max_angle>
           </horizontal>
         </scan>
         <range>
           <min>0.2</min>
-          <max>5</max>
+          <max>25</max>
           <resolution>0.01</resolution>
         </range>
         <noise>
@@ -3516,6 +3516,15 @@
           <p2>0</p2>
           <center>319.5 239.5</center>
         </distortion>
+        <lens>
+          <intrinsics>
+            <fx>457.022184</fx>
+            <fy>457.022184</fy>
+            <cx>319.5</cx>
+            <cy>239.5</cy>
+            <s>0</s>
+          </intrinsics>
+        </lens>
         <image>
           <width>640</width>
           <height>480</height>
diff --git a/urdf/ergoCub/robots/ergoCubSN001/model.urdf b/urdf/ergoCub/robots/ergoCubSN001/model.urdf
index 378633f8..23d9c81c 100644
--- a/urdf/ergoCub/robots/ergoCubSN001/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubSN001/model.urdf
@@ -321,7 +321,7 @@
   </link>
   <joint name="neck_roll" type="revolute">
     <origin xyz="-3.5561831257524545e-17 7.375606977361926e-16 0.010999999999997845" rpy="8.452269018635315e-14 7.050925500045285e-14 1.2263451870709675e-14"/>
-    <axis xyz="1 2.30576840860686e-16 -7.196923107658096e-17"/>
+    <axis xyz="1 2.3057684086068286e-16 -7.196923107658096e-17"/>
     <parent link="neck_2"/>
     <child link="neck_3"/>
     <limit lower="-0.5235987755982988" upper="0.5235987755982988" effort="1e+9" velocity="1e+9"/>
@@ -353,8 +353,8 @@
     </collision>
   </link>
   <joint name="neck_yaw" type="revolute">
-    <origin xyz="-1.734723475976807e-18 1.0057301276633031e-23 0.06914999999999993" rpy="2.1582026845669264e-16 -1.386477883657293e-15 -1.9981249037222812e-13"/>
-    <axis xyz="1.386477883657293e-15 2.1582026845669262e-16 0.9999999999999999"/>
+    <origin xyz="-5.0306980803327406e-17 2.0696535769897827e-17 0.06914999999999971" rpy="1.6030911723564583e-16 -1.3309667326328268e-15 -1.9976738756079363e-13"/>
+    <axis xyz="1.3309667326328268e-15 1.603091172356458e-16 0.9999999999999999"/>
     <parent link="neck_3"/>
     <child link="head"/>
     <limit lower="-0.7853981633974483" upper="0.7853981633974483" effort="1e+9" velocity="1e+9"/>
@@ -386,8 +386,8 @@
     </collision>
   </link>
   <joint name="camera_tilt" type="revolute">
-    <origin xyz="0.06250999999999991 -2.999999997621366e-5 0.07639249999999997" rpy="-7.545682259870277e-14 2.5958199361694967e-4 2.017046159162252e-13"/>
-    <axis xyz="-1.998015835203486e-13 -1 -8.597927048660853e-14"/>
+    <origin xyz="0.06250999999999987 -3.000000000113848e-5 0.07639250000000047" rpy="9.528820759122057e-15 2.5958199325913113e-4 2.6559516676797335e-15"/>
+    <axis xyz="-7.529259780358163e-16 -1 -5.660091301660221e-16"/>
     <parent link="head"/>
     <child link="realsense"/>
     <limit lower="-0.5235987755982988" upper="0.5235987755982988" effort="1e+9" velocity="1e+9"/>
@@ -395,9 +395,9 @@
   </joint>
   <link name="realsense">
     <inertial>
-      <mass value="0.0973042045264468"/>
-      <origin xyz="-0.01566548310600667 0.004519995218644514 0.002745610154123612" rpy="0 -0 0"/>
-      <inertia ixx="1.3363140922191058e-4" ixy="4.7877399688365374e-5" ixz="2.5482221369268495e-5" iyy="1.8708641575714753e-4" iyz="-7.171072206331662e-6" izz="2.963329027014652e-4"/>
+      <mass value="0.08233251527028056"/>
+      <origin xyz="0.002757312110436068 -4.942831990157318e-4 3.199624651573085e-5" rpy="0 -0 0"/>
+      <inertia ixx="1.1574711197088633e-4" ixy="-1.7492230739037293e-7" ixz="-2.5944440100815574e-8" iyy="8.421296719556895e-6" iyz="-1.0106462328987549e-7" izz="1.1038824885095133e-4"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1634,8 +1634,8 @@
     </collision>
   </link>
   <joint name="l_index_dist" type="revolute">
-    <origin xyz="-2.7755575615628914e-17 -0.0015000000000000013 -0.03999999999999987" rpy="-7.482326172536535e-8 1.8041124150158794e-16 -5.551115123125783e-16"/>
-    <axis xyz="-1 -2.9143356888525625e-16 9.714452192336961e-17"/>
+    <origin xyz="-2.7755575615628914e-17 -0.0014999999999999736 -0.039999999999999925" rpy="-7.482326169760975e-8 1.734723475976807e-16 -5.551115123125782e-16"/>
+    <axis xyz="-1.0000000000000002 -2.359224176539984e-16 1.457167792507203e-16"/>
     <parent link="l_hand_index_2"/>
     <child link="l_hand_index_3"/>
     <limit lower="0" upper="1.7165313193364231" effort="1e+9" velocity="1e+9"/>
@@ -2172,7 +2172,7 @@
   </joint>
   <link name="realsense_imu_0"/>
   <joint name="realsense_imu_0_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 0.010000000000000002 1.7352099479506222e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 0.047499999999999994 1.735209986808428e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_imu_0"/>
   </joint>
@@ -2226,7 +2226,7 @@
   </joint>
   <link name="realsense_depth_frame"/>
   <joint name="realsense_depth_frame_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 0.010000000000000002 1.7352099479506222e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 0.047499999999999994 1.735209986808428e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_depth_frame"/>
   </joint>
@@ -2238,7 +2238,7 @@
   </joint>
   <link name="realsense_rgb_frame"/>
   <joint name="realsense_rgb_frame_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 -0.011499999999999996 1.7352099701550827e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 -0.011499999999999993 1.735209981257313e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_rgb_frame"/>
   </joint>
@@ -3174,7 +3174,7 @@
     <sensor name="realsense_head_depth" type="depth">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 -0.0115 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <camera name="intel_realsense_depth_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
         <horizontal_fov>1.2217</horizontal_fov>
@@ -3204,13 +3204,13 @@
   </gazebo>
   <sensor name="realsense_head_depth" type="depth">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_head_rgb" type="camera">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 -0.0115 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <camera name="intel_realsense_rgb_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
         <horizontal_fov>1.2217</horizontal_fov>
@@ -3240,13 +3240,13 @@
   </gazebo>
   <sensor name="realsense_head_rgb" type="camera">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_imu_0" type="imu">
       <always_on>1</always_on>
       <update_rate>100.000000</update_rate>
-      <pose>0.00751548 0.01 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 0.0475 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
         <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini</yarpConfigurationFile>
       </plugin>
@@ -3254,7 +3254,7 @@
   </gazebo>
   <sensor name="realsense_imu_0" type="accelerometer">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 0.01 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 0.0475 1.73521e-08"/>
   </sensor>
   <gazebo reference="head">
     <sensor name="lasersensor_head" type="gpu_lidar">
@@ -3267,15 +3267,15 @@
       <ray>
         <scan>
           <horizontal>
-            <samples>720</samples>
-            <resolution>0.5</resolution>
+            <samples>360</samples>
+            <resolution>1</resolution>
             <min_angle>0</min_angle>
             <max_angle>6.28</max_angle>
           </horizontal>
         </scan>
         <range>
           <min>0.2</min>
-          <max>5</max>
+          <max>25</max>
           <resolution>0.01</resolution>
         </range>
         <noise>
@@ -3546,6 +3546,15 @@
           <p2>0</p2>
           <center>319.5 239.5</center>
         </distortion>
+        <lens>
+          <intrinsics>
+            <fx>457.022184</fx>
+            <fy>457.022184</fy>
+            <cx>319.5</cx>
+            <cy>239.5</cy>
+            <s>0</s>
+          </intrinsics>
+        </lens>
         <image>
           <width>640</width>
           <height>480</height>
diff --git a/urdf/ergoCub/robots/ergoCubSN003/model.urdf b/urdf/ergoCub/robots/ergoCubSN003/model.urdf
index 52cdc0b2..56ef4fae 100644
--- a/urdf/ergoCub/robots/ergoCubSN003/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubSN003/model.urdf
@@ -353,8 +353,8 @@
     </collision>
   </link>
   <joint name="neck_yaw" type="revolute">
-    <origin xyz="-1.2588645634161594e-7 -3.511736027695927e-16 0.06914999999990112" rpy="5.476218470552679e-15 -1.5706357540219146e-6 5.308786623366525e-15"/>
-    <axis xyz="1.5706357540212687e-6 5.476218470545924e-15 0.9999999999987664"/>
+    <origin xyz="-1.2588645629564577e-7 2.1530156161317587e-17 0.06914999999990118" rpy="1.6029881711049145e-16 -1.5706357538557005e-6 1.1347346732224471e-14"/>
+    <axis xyz="1.5706357538550546e-6 1.6029881711029372e-16 0.9999999999987664"/>
     <parent link="neck_3"/>
     <child link="head"/>
     <limit lower="-0.7853981633974483" upper="0.7853981633974483" effort="1e+9" velocity="1e+9"/>
@@ -386,8 +386,8 @@
     </collision>
   </link>
   <joint name="camera_tilt" type="revolute">
-    <origin xyz="0.06251000000000004 -3.0000000001050474e-5 0.07639250000000042" rpy="9.111439807333146e-15 2.5958199361708845e-4 1.6567881612289887e-15"/>
-    <axis xyz="2.46237494755185e-16 -1 -1.3590792672083997e-15"/>
+    <origin xyz="0.06250999999999986 -3.0000000001138477e-5 0.07639250000000058" rpy="9.528820759122063e-15 2.595819932592699e-4 2.655951667679724e-15"/>
+    <axis xyz="-7.529259780358055e-16 -1 -5.660091301660095e-16"/>
     <parent link="head"/>
     <child link="realsense"/>
     <limit lower="-0.5235987755982988" upper="0.5235987755982988" effort="1e+9" velocity="1e+9"/>
@@ -395,9 +395,9 @@
   </joint>
   <link name="realsense">
     <inertial>
-      <mass value="0.0973042045264468"/>
-      <origin xyz="-0.01566548310600667 0.004519995218644514 0.002745610154123612" rpy="0 -0 0"/>
-      <inertia ixx="1.3363140922191058e-4" ixy="4.7877399688365374e-5" ixz="2.5482221369268495e-5" iyy="1.8708641575714753e-4" iyz="-7.171072206331662e-6" izz="2.963329027014652e-4"/>
+      <mass value="0.08233251527028056"/>
+      <origin xyz="0.002757312110436068 -4.942831990157318e-4 3.199624651573085e-5" rpy="0 -0 0"/>
+      <inertia ixx="1.1574711197088633e-4" ixy="-1.7492230739037293e-7" ixz="-2.5944440100815574e-8" iyy="8.421296719556895e-6" iyz="-1.0106462328987549e-7" izz="1.1038824885095133e-4"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -2172,7 +2172,7 @@
   </joint>
   <link name="realsense_imu_0"/>
   <joint name="realsense_imu_0_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 0.010000000000000002 1.7352099479506222e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 0.047499999999999994 1.735209986808428e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_imu_0"/>
   </joint>
@@ -2226,7 +2226,7 @@
   </joint>
   <link name="realsense_depth_frame"/>
   <joint name="realsense_depth_frame_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 0.010000000000000002 1.7352099479506222e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 0.047499999999999994 1.735209986808428e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_depth_frame"/>
   </joint>
@@ -2238,7 +2238,7 @@
   </joint>
   <link name="realsense_rgb_frame"/>
   <joint name="realsense_rgb_frame_fixed_joint" type="fixed">
-    <origin xyz="0.007515481871156526 -0.011499999999999996 1.7352099701550827e-8" rpy="-1.5707963267948966 3.7596758519972983e-16 -1.5707963267948966"/>
+    <origin xyz="0.007415481871156544 -0.011499999999999993 1.735209981257313e-8" rpy="-1.5707963267948966 1.439727114483606e-19 -1.570796326794897"/>
     <parent link="realsense"/>
     <child link="realsense_rgb_frame"/>
   </joint>
@@ -3174,7 +3174,7 @@
     <sensor name="realsense_head_depth" type="depth">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 -0.0115 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <camera name="intel_realsense_depth_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
         <horizontal_fov>1.2217</horizontal_fov>
@@ -3204,13 +3204,13 @@
   </gazebo>
   <sensor name="realsense_head_depth" type="depth">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_head_rgb" type="camera">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 -0.0115 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <camera name="intel_realsense_rgb_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
         <horizontal_fov>1.2217</horizontal_fov>
@@ -3240,13 +3240,13 @@
   </gazebo>
   <sensor name="realsense_head_rgb" type="camera">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_imu_0" type="imu">
       <always_on>1</always_on>
       <update_rate>100.000000</update_rate>
-      <pose>0.00751548 0.01 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00741548 0.0475 1.73521e-08 -1.5708 1.43973e-19 -1.5708 </pose>
       <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
         <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini</yarpConfigurationFile>
       </plugin>
@@ -3254,7 +3254,7 @@
   </gazebo>
   <sensor name="realsense_imu_0" type="accelerometer">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 0.01 1.73521e-08"/>
+    <origin rpy="-1.5708 1.43973e-19 -1.5708 " xyz="0.00741548 0.0475 1.73521e-08"/>
   </sensor>
   <gazebo reference="head">
     <sensor name="lasersensor_head" type="gpu_lidar">
@@ -3267,15 +3267,15 @@
       <ray>
         <scan>
           <horizontal>
-            <samples>720</samples>
-            <resolution>0.5</resolution>
+            <samples>360</samples>
+            <resolution>1</resolution>
             <min_angle>0</min_angle>
             <max_angle>6.28</max_angle>
           </horizontal>
         </scan>
         <range>
           <min>0.2</min>
-          <max>5</max>
+          <max>25</max>
           <resolution>0.01</resolution>
         </range>
         <noise>
@@ -3550,6 +3550,15 @@
           <p2>0</p2>
           <center>319.5 239.5</center>
         </distortion>
+        <lens>
+          <intrinsics>
+            <fx>457.022184</fx>
+            <fy>457.022184</fy>
+            <cx>319.5</cx>
+            <cy>239.5</cy>
+            <s>0</s>
+          </intrinsics>
+        </lens>
         <image>
           <width>640</width>
           <height>480</height>