From e256910d97ab7e25a1925fa230d2899e16a897e9 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Wed, 4 Dec 2024 01:18:34 +0100
Subject: [PATCH] fixed path planning. i was an idiot when i did the
multiprocessing by assuming planner will run slower. it's faster. now we
don't pile on the data_queue. this should be shared memory too and i don't
know why i'm avoiding that like the plague
---
python/examples/cart_pulling.py | 8 ++-
.../__pycache__/managers.cpython-312.pyc | Bin 51600 -> 53810 bytes
python/ur_simple_control/managers.py | 44 ++++++++++++--
.../optimal_control/crocoddyl_mpc.py | 50 +++++++++++++---
.../path_generation/planner.py | 56 +++++++++++-------
.../path_generation/tunnel_mpc_params.yaml | 3 +-
.../__pycache__/get_model.cpython-312.pyc | Bin 13603 -> 13603 bytes
.../__pycache__/visualize.cpython-312.pyc | Bin 16871 -> 17114 bytes
.../ur_simple_control/visualize/visualize.py | 3 +
9 files changed, 129 insertions(+), 35 deletions(-)
diff --git a/python/examples/cart_pulling.py b/python/examples/cart_pulling.py
index 9df36a6..103355a 100644
--- a/python/examples/cart_pulling.py
+++ b/python/examples/cart_pulling.py
@@ -17,6 +17,9 @@ import importlib.util
from ur_simple_control.path_generation.planner import starPlanner, getPlanningArgs, createMap
import yaml
+# TODO:
+# - make reference step size in path_generator an argument here
+# because we use that for path interpolation later on as well
def get_args():
parser = getMinimalArgParser()
@@ -24,6 +27,8 @@ def get_args():
parser = getClikArgs(parser) # literally just for goal error
parser = getPlanningArgs(parser)
parser.add_argument('--handlebar-height', type=float, default=0.5)
+ parser.add_argument('--base-to-handlebar-preferred-distance', type=float, default=0.7, \
+ help="prefered path arclength from mobile base position to handlebar")
argcomplete.autocomplete(parser)
args = parser.parse_args()
# TODO TODO TODO: REMOVE PRESET HACKS
@@ -73,7 +78,8 @@ if __name__ == "__main__":
planning_function = partial(starPlanner, goal)
# TODO: ensure alignment in orientation between planner and actual robot
- path_planner = ProcessManager(args, planning_function, None, 2, None)
+ path_planner = ProcessManager(args, planning_function, robot.q[:2], 3, None)
+ #time.sleep(5)
# clik version
#cartesianPathFollowingWithPlanner(args, robot, path_planner)
# end-effector tracking version
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index 8b2e1e5ebef5450829084854b558d13de09d65bb..c2815be629cbe704bc81afe0a7c810441fb93951 100644
GIT binary patch
delta 7570
zcmbO*nR(L^X1>$Byj%<n3=B!#{^@70P2`hc^q8o=yFOPjN-<X{N{Nvng&~D8M>$Fv
zOshnxq%frL<f!JVMX7=1cyrWqHKH`YY`z@LT&*ZAFq=O|J69)4hmnDkAw{5tAxf8t
z!JQ#Ru!SK-Xf+eaLI#E?J+PQ?3qy(sTudJ<#xL5!kRk>bG}xTM$jYe3a*I8nC_g#1
zxcC-pU}<V;>Ma(Z{N(Igyu}%bMX4$Axv9DNMU|WDnAF)BWhO7<>|j%5U|=Xxnry<A
zz^wpcLin5Ka~)u0RNb7w!^aL0R{~3Jt`xRlVl>;lNpuZAqxj@_)tlUoAPFZB;WF7-
z&6v@2bGe!Uql`HN14A++#2a9eje&uInSp`fa~}KT!|L|z(-;_P8B5qFJ4}|DTu{lm
zSx+N}k@4T;gUwn@McXGQCW%e1n<_F{V2j9PT^;esGFoAiQ<8)yx0#D@q_AXbFfbH(
zffc<}5t%IEEy9t)D#^gWP|FS$gzDr^VXI-!WLyn(2IJ%d%@UJWdkM1@N!KuBX-#(2
z7M}dyRCw}HEo*L$8jc#KDlP_wT28PLHs+%2oY_na3`N$H4>pTW{-!O$uUujX_6h?7
zLkfEhOr3}jNE<9B7#Suo_1J>7oHrL?lmN4y=!gh1O<?NDE@6imkYzTRF;aB0fsV*z
zTXPW+POup;hl0$Q$%1gM6xg|fx@wa*9Oj*TK}TeAw1x2G?>dr`XHOSq%CZL=Wu>dd
z=m7T9LJ#4|-MY~rH|y{rxtXJeF@<wAC=wVL7#JBUc`A93WVvc4U+fbVNa04PsA0%*
zogBy_EXb3>gYaM_k0$Ttd_87H!6I1(28Nd)SKnd_iBC!_o?NIeI=Nfl!BM%$8I&a4
zK!gJW1H&zrlFZ!HB2Z$v#Ts8+l3Gv%PBX3`Irf75f}GT}k|Ga~a&HjfGMUG~MkgF3
z%aNX15)xk?pL&a{C_gE`Bt9=OH}w`vN@7XkE#`dR%pxC<2EWO%29tU17#J8dxr+Qj
zBEggY8fa?;f|x-dA_`;}b8247EtZtbw6tPSrdEK1BGbt}hN?^q%$ti1)ffXFh)Z7=
z*SsjMd0pJ(qPWQp_DkaSHxyN`D;i%^G`_BAaZ%CYx}wcRMVlRg7ZvSiu-%Z5xh|n~
zQ9|pwgzZHM+v^gZ7bQGTSX>B;x!@IhMIvtUOrt&Q?o13qIv@Nd#~7btG~cXlQo_a<
zIeCs%q^&r}efA(C14MvA?G{sB!7bLR)S~=iXsG4pr=;fGV#+JK#a5h}mlBv#<N{Kh
zIoZhC*ew^t%mWemAOakQ-~cWFaSK5NC_-+r7bljb`sAlW0}Uje10v!<M8f3N)^eht
zz`Mm(oSB|intO{ittc@!wJ3P<J!^3`Q;;IN$-k|Y8C^Ha+iYj#1m$u+P4=R~$@gv5
z6+=Ms<sbqS4=WjpK+;8EU6vqG%gO3?vVu{JteT%Im~>eAA@Y`+L+$1Xq{x6&fYMWu
z9Ec?kB0%w91S$((f=Z8DtR?yRIR=?u7#SFfA{iJM(v893xBYWit}4<13Cp*>4%{Kf
z=}@E!GDMAmfg!WvI|D<J`s8qb1vY4?)%q*&6eWWcaDa&F$s7EY+>25`Y)%jXN*+br
zAQm`mBtR^F5aA0VDnUdQC`B>l6&59cc;F}y0I~8WO9fakrcDkG$dCtnp$4Roxh%f$
z7IR{JVG%fzLD{gVZt|^wvIJ1Fzr~tZRFqg*ln62)7L<%xAjufy+*`cGnJKC9X{C9|
zC7JnoMIb|qSr`}?K*<*pz@V_N5)A@n5Z}bS#Prl6z4-Xdyv&mL`2Nis0<BmW%_l_%
zPqqtVlK|;1@?>COSjh;sq8OBc6^cwJ$Arl+@=dM`lLn~`k=+P#1yj*dkc&Xdi#9PZ
zFu)bFh0DnbGBGd|3o$S-G%(!Y7w)g<tmv=ptnIG9z#&&;zS%K6lt~<H2wzbI+z@ar
zQ#pBUq-stzNDv(2ppYyAhmuHXK}uptsv9WT1>__qr{<>Sl@!ee2VlN$nkEz2$+y@G
z<I^+CQuB&IsY5{l5(Y^O3=C*t04qC0Cfh|F1ba&X>>{RHY~@9%dC3{zfI#(GNwgZH
z`sCTsa*XDa_a2hmd^tLjMHw7tbs%T(fHI|9NpNajaeh$|D6ba9fIPB%vVVej7AP)@
z(m?!d5CKjn>p<Gr6U$PIK=HB~Bn}EsO?HT1xYJ7Fi&7I)GV{`li-JM2MIa4KX(dIa
zAYK_L*g+KsD7F<~o?SKhLV_%#*5r2yUm49eKS``-tKR`qy%R(rLKYm^;3(V$k^lu{
z(QXh6<N|QC+~SK*Pb~=thlXdKtLrW1l$64vWgs=k5f0*m(q)wpY9a%b<$j=auyFFD
zjHQg`lke=5oV+mel_MxC7ENSeVDQsqg%(vHGgg3zl^|k1Bz=L)8`iSKoYGX3*jzdJ
zOqK#;+2prb?u_P}O|w~9OhH-s<$neShUc?pf`NSzxY&T$!c&w9GIlwLSPLRRS*K{x
z<otYfFL1Pgb2~gnz_|#Tp1|y4<g|1Wl$OxEnx0w`*f{w?{uxH|&1(z(v(|&$Q*;4j
z)kP2ia)zeFE#}0W9B>}L#phU3Qk0ogT9WEoRFq$Y-F+aPmq5g25CL{5IMToo<OLE#
zI2kpDts%t~TEv2~nkKaH*aJ#PEG4PMB@iDX%8bcr<!X#eHusiqU`YbG+fS1V;k;Yi
z>8T}-$t9(UIk0MG5y&L&^rFmyg4Ck;veX<%WsPteD2+iw^A=NK3QAy}nmoTog7Na?
z9W}EU%{K?v+A}$UqY9xL9BGF^b_o0Bmw4tD<bX4NY6>{upyn3s1LXuJaJg4xI{8Yy
z9^==^%nkO8=9`@x#2B5yIhzyW4J<|%fpZ?nj9Y9)iFqmcxkWQU!Hb;AC(mirU}I)x
zU?>ir{Gla;rPsU2eDc}?$<5YH985X=Amv~J<cOExA{@lk<Rik;qK6>eU>k3MSl}`b
z6j`@8it<YmK?VCQ#t=<raG_9iV)FVHQAmw@dh)3j1xD`4uUkL~Z%3;cTRkY%`xSw#
zF0uyYB1Q-s6p`RI8KMr=6oOQ8pzyfGlAM!SP_z(aJ2?Eo<r!OXL27D>SJ6C>FsS6Z
z#a@(JoRL_NdW$6|vl!Yc1C?Qrs$d5LLlDC)&dj`$)bygnlGGw_VFOCCFt^_U#Uoo`
zN_=u|N--#q;DuH#C|3!i6d;hcjMV1+Jx3WC%_l!xEV)@=A_wE-W&KkPK#2}gbfkkE
z2QGsrgIFNf-(txxNX<hny0a%}F?vs~o8&+F=>$QzX`GA}A3&kRUWDI3*5sW0;#5=<
z11IS)hEA@Vl)z}d`TnF<PR9PpsdJ9Hf{KTt%^;_sW=Tkn1O;{xIK<HMCfElc#}}nC
zFfdHcpQ~Q~6l4e}`QH+NXSxvQfB=w7Kt;eU*37(u(vn-O#RWN;CE)tz7IRK&UXd|K
zqbP{@1tP$O9k{6iZjFH25t<y3v;>M9a8X|bQhAFdJwGw$7Gvry&h-35shoIlB!lXm
zVo*y6TFHRg1N!qU7?U?=%=^P41~wDiQbfruRg2vi=TAPkSf0^*^TWmBjMkc<aOQx-
z8Q7m-AA{0aQ6tEe;F=O#Gd6>`D25p?RbyN`IcjMrB;qCuEYIUv$?T`e1S#a@Cp#}Q
zPym<V2=ne?m@;{p4&#=|2bP(^P03g1VPs&?WPxM^uwJm`n0l8jH(=Z~`M@%z$v>Bi
zFq&`XTk)9D2ur{g!ILAbWD;OxV1O5Sla*JgF&>!gwaT*|+yVxdON?%sGDYBqcn8SM
zogktMM1WdukfgK~#052CiuQw8t3U*($}Iv{m*6A^E;&wtB<_QVM<C)fhydlRqE8^!
zR}jGlY9n(nnlUgGae<h;AcCI})DcU`OfD%B0`a>+D#0!Ux6yk+To6$_*<<xgW3U8>
z3kq^r1DgR<@PRm=LA{IV45<uJj42FJOeu^}%qd)xMb>EXq;R${L~&IzX>u3&ZgyGo
znPu`GB~HC276yjt3^j~344^a$GJ%1ihNXrf9@O{*b8A>q7{MKTy%N^Rdk?Cy)iBjC
z)-X?AkS#WuV-vq4D_9i+NLvasOd(7RRJ4QGH4O2ft^$b3P{IkOKsq2M6_|rf5=Ax%
zGM3;0wo+CXE<==gAeZ^-in*n+tYLz6k82p>;fB<(q_Bb=da?vAhRxs<wpz9l{>ghL
z%d)biu-7n8{-`V>v4#WI53gZ}2X#iko+%NZJh54pk$Ey>izqX5mJGD#FFaXVOGI`J
zymMc}5D)5!fVI>xr!b^2NKNV#Hv=)G8A_DF;tUK7NR9!u;=#h`p;p5-c|oz{<N#e^
zhcs5~{wz@k>jVchY_O#S)cgYrgF`^Ah6y_0Vrwooxqp+eKs>0^3s#UPSHqO00~%*w
z;;Nc#?<$ETSHoJv5HB$~VXFM(4LS;w_Z}9R{6t5DSGfd~I>EYAxF>IH5}W*0TV%4#
z5usR39zSr8QIqKwTS;P3dTI%zFaq@#izb0m_!Cg=#Gag=o12)I5&}|}T$Gwvl6s4!
zII}8Mlj#;~N=aow>MgdU(zLYHqFWsC1&|@ND*4F^w+JgI<|-7IBo>uq=A|oW%79J3
z#RoDjJ|(j#72H9aEVR|6UIb(hPcV4k%@;iGc8e`9sj?)s_!e_<Ms5+flGqMX#0su$
zi=;r=lR2q0?G_(&xDI4$VqQv-Fi4d!s$hIhesXpZxHkpvo5DN#w^+afB=tNwiN&Cn
zUp#1}qzKfdKq|H%T@SG1i`Iio=K=Q>VHV1QoWcbb0yoTWaTI6df(KrTF*+ik@~cW5
zI_!w(drrQ;l}+jus3rA^5!^u(k?3IQ;k+TI)4_6uLwfS}txM}aurcsR%?P~2Eq6yw
zd3Jh-OOO8pafJ?#JK`$W#SJft8*b1zU~(eox?kc&zr-uzNgXad9(R;fmTN84S`l+y
z*Z!ie{ehb6?vWSWBQGSST~Eron3Q!rDeq!Z-W8?%4woxD@;6isH!3cO?{K-!BY%-c
zej(!p9;G|#+KbCTvT7<%6x3JL-q1JR5OcudM96jD)Qi5U7t*t@r<Yw!FT0>u@r9XD
zv1;<PZGMx*w=1iO7;kX7ZsK~;#Pzy~*F_Vr3&!3TgnX{@_<mq!;8B8%W{5LRzPoek
z=BL{e7&)~;ou8G=MRJq<c1nta`Z-17$n^v$H6~83+bPc|HhJDoTSoKEcXr-nlK>@E
zzal+Q32lh1eDc`?HjK)XxenTzfl_}mtSAO$15m>r)YAGa#t15BOAw_heE743X>w1k
z3>PRn7BhJ<G%-%T;3)<kT=kqR%n28sd~u4z<oteF_8P_%#uP@W$q7l~tRR;3WJg!=
z$#V|N)mJiVG9|uXWMEL>g0&VvtvF{$mI}*EL{9W@C7F4}C8>!i3i)XY8Hr_}+>@wK
z2pXeSNXsu$D9K1wfTq4;9gu=lg+v8NidTS)S}T+#7G)+T<)qf@!Obkm$ShXKP0UU$
zRwz!*D^69&OjD@LFI6Z{%qvkS$p;ChfQ`;c1Z4%74GQ^rIh8taQ;=L%T#}fa4RTv)
zfkJ*x3QR+>LS~)<)VX@~aE%~GgR&t=Lt<WvLTMh1m!Af<7S#h8i3J6zdBqADsYR)X
zpaW?Hxj7|2PrU@<^OAgpl8nr}Y=y+6{L&JI%o33GAj^{T^Gb^9^K%q(^79K6^74z|
zPD#lJdps{SH3j7Hg8ZDE(vnn#jKrKYP>6#^PIDmxLkjst3gsDzC8=epMGC1o#i?*Z
zO7ay_QWJ9&$}>wcKrSdogtVU~2c)p6WT<CgC<2vRMJk{qss<v|LFEN|Vsc?=W>G4r
z|B#cKSe#mfl6*m>j1=y4Ql*Jjgy2+KU+Koez)%b-@ERDth%pF=OmOe6UBa}SYa!Pq
zeys}}S{L}WZt#nN#-bLmTvsr@s9=0u!SbSl<t2X03mldg_$_a6@LcCmxX7V!okQay
zhsG5Sts9COD+L!wb+BCLkiN(vJ=5_5ha8%!s~lS3JU;o!VzJ4I2e)&8+jVS33X>HM
ziA!sO!U5D^F9P@3QChY^hg2BDCzl^`g^VaM<uFa&5GKU|ZSR;&>KC89f4eYvaKCh-
z$mFUO5H@JSLz6{>uSmIs7nI3CIE88Q$8fR9A528}XEUTQ&t-;HxQvq%-Nc!)L_zf^
z4+~tBfphYQaPi57$uf+LpsYL@!$4M;fs<v9i1LBz5|BL%Dhx%oa2+f~DkZ#=8@%Nw
zADt|$F`FTUZ7vhSib#e^1`sZ1s9=s{C}-4U_bUR`vXc)ViHqee0#znO%Anx+0!jmH
zrFl7-dD)P<2i}k_k_Rc!01<K^!T^+_I7?ECax?Qloy=>X!V)=9LHwM_=|@#eV?e3l
zGh`H!oBuku+(mA=1&Wurm2dC}^m}xA^!s)CUEz^|@%%geukgrDo_6$1{UJ$5A!l8N
z!@7=)F2>A9j6h@&I99oFH`Rqf&I5I?ii|-la}Z$-BEa1vv^F((uoy&~1J#6}V!MUm
zBa;Lx$7eHDRu(_T4-u0e9Fq#<XVw0s#LQ~&nTwaznGu}Iia@$F4R3K4Wai~3Cud~l
z7lFE(w^;K^a|<dVtp{*#3f!RqmHkDaUStudCMa4l+3k1@H#jxgfWlm1a=>wk$xn~3
zst30viok6X?!=<><b2RjOG#>x4oDeuab<B4s7rZ^71RVOE)oNYGUXQ+ff{SKIKczZ
z`T04;MWAF-WCn@@@W?)>n*erj5vu=haoFVMr<CTT+7<aRg65n+QPn(|<D?CX{bdIG
q$*w0Av_qH~`9Cs%$nQC9k(`YB9~nU8_cEbqCPrt*PYhrR93}vuaIY=^
delta 5767
zcmdnAgn7bbX1>$Byj%<n3=GSz`KIqZKao#@(P5(c?s`Us6owSW9K|R_Fs&4&l){k0
zlcSug5~Tu`<IPdcRf|#sv-xt=b2Xwgz-;~;&0MW0Ek*`Th7^GohA3?&26u)O!4`%T
zq18+vix?Q9biiW5Eet6la4}u5m}m<_ir8u<26nK@D80=EjI4};Ot;trit>|Fi;Hiu
z29~Clrf#0iq|VMLHTf`S2b(+t14EI*<Up<jZaEMW!r#1y>i{F8^5$9|K6Z$>0$6hM
zG+_%SMw89wMc42%icYRky~%A4l5hYKPLmVVj2VqLPf;^qlrd#sU`S>Jy8}Y7F)%PN
zGcYiGUcol`fx11T@n!{$>5PoCCoAZPPk!Gb%v2OIS@E>^<oQv;llin7Cx>c_Onzb^
z%v2;jd47r5WIip?$t$(3xY=viYnZCI7#M0fz{-EQi?DNKGchm}Sx*ko)#g_&(T3Q^
zz>vaH15+a`1kwTvEk=e3j6Ju&7SFN}o_s(@a`K;T!a_w%6PS89N+5y9z`&4YFxhab
z=wySb!jpfxi;8f7jevTIkzoQ;j~UD4Ky3+-D<|7qvQA!9B0RZ&n<CidH+4iN-`162
zG@E?ULU{6IUE#@6db*Q$Sc**kFhh89o?i6iOl=WuJ|xGo)i9>8uLgw>0|NsiLnT)w
z7m_T;<OzD>{G15+8ip+U$&FsZf?O$F2!B;_X>xB4(r0FrtP;x2EG|w>QAkNmOv%a2
zOI1j!d<k;=<Z1&4M};CsP>i{N2s;J_hFdHpnYpP&pt!un8ed$JT2KUzQfH7HdqI9d
zPHI|7ksC<4Cx~#G{KvpXG!!Juk)B!-5?>ylTI2<i@tK@rIGNXmfq_Ajv&a`D5;&Q~
zNL$4p#0&swU{1|Txy6!_nU+?}$-uy%00TwFlOv5(nKm$OZZJ|~oczsr4|_c$gOJV#
zx5+zAPO*U#6!YZ9QzDz^nntrRhEM)!9ce2HGS(JEB!LK!hi)<D72IO2N-fGShI%MB
zKP5Hi7E@l?Ew<v+yp+I{A}5gI<jMUu#%}2#W(J7J1QB5Gf&HEZ;%0*gP*~k!FHS5=
z^~q0%dJ`m`1|niWMC@b^TR9_;cW<#3XQt<s=H6mWD@x2wEeZsAnY}DECq6khrI;1w
z;UZ&@Hk-*2w#tl-n`>;hGjf7F=%>kEls(zTUfn+kBwq+3Kw-I(p$H@m(Pa)21rc)@
z7#NDB85kH^7(Oy-uyT9~VU%SJWBlaA7{<t|`MH2ehm{|q&wR4YTFK3)?OO#5q(Hhs
znu=sVELji%ipU~R@$wRsOK-83<mcxYWPV{}U?>Wo+z=opr^dj*kS^c)I&g;^r$do4
zNRJ8w14CxTcLs(c)yZ1|6xg6adMiLd_!d`Deo}r(d>$wOI6#U^C$j}AdE8=2Ni0b$
z0>yd}sMIXt2FZZ~Pz=Q42NB*Nq8LP!fP#)GudpZ<!~@5Z0Em?_IX}>XF=6tGzzlh?
z%gaCtnakn}Z!ssv7Z!n20w^ODRZP|oDsu%T+gq%OMMa5~MR6blqClyW1(G^J&c4N4
zoSBjupH`ZeT#}ieR|GP&m<1YDkT4IL{3=L_v3oOXuoVlV>12oSNC}XtB6kJ`hLwz9
zql!VfOQFbk^2Tr(My|={!ll6qgXN0Wg6v}|S_rZmq@idXDB@tsqa)-Pc_&vzNHdyl
zo)!_xBo0>2R}=<U4lbCBCo@H>)|Y|=CxZx3=oEo{E>c>Ml30@J21;)MIf==sxv6<2
zMbp87lJA?Q$pm)ZE%w6r^vtr<ykby9D=0wx70<xHP$e1!iZ0*8yu|d>B0X4s7GPpv
zDBi)qz|g>OgIlP>{)Vvl^vTPj*Fk)x2=&!1w(_FXyyOgUNT7P`WQ-c4^5hROa*U>%
z`D2S%>cJ6H0df})D0{k<1gGW|=NA=$a&1v0$Wx0!l7i`}C637@rHMHq&H--uMaij<
zk|zPAA{9h{lgBEMTK2@U)FP0xmxIJXfvd?5@h*2-NqkXiVoGLSdNDW`7CGmDbTFlr
z6y<{y7l0xGRM<lcAV`2L0|kf#N`NFM=42)nfkHS$ZL)LHS4Pv#mdW*O##=z9Yy}aB
zSOF&ra71qdNq{1uXgi1na#~T?<ky*!dW%4O<Rk#%gVJP`5Q@Fva@cFKQPwI()6I9Y
zm>C^G*|4aafq}tKlNDNqf%Gl`5lcbDY)DE27euUOi8-aID6zeCvSN+`W6@-%9Ct?3
z&0RUHET*7r{PI5o1H<!KGr_>V2wXxyY~d+N1{n*AmZFs)7AU(E&6|9rK-~zGt%@dq
z1mV#E&N<NZ24<t!esr>Ip*UmhWYfYkkkXt9T$)Q4J!7p0Iiu(d$gr~@0%W_U#4YB;
zoE&g=zQyNQQc{$eR9ceiT2z!@gxy&no##Nrc@P11A2>e1VebJFL%0+*@~n%%c?vDg
zKsitoT26p6!Y!7P)Z!9|#}LKC<lPl&j0-kDs@T9%4|2DkCKtkax47Yn8CLDg1EoUl
z^rFmyg4Ck;veX<%>P9#XlzgCpc#Ekp1tkoRfs&^%YBEVrEeT9H56Z2R1?#3Unr>cE
zXU`-GjyQy&;3(Wb`A>ti5GZvN?E<BKCU6;7WIS23QIGNC<cLOlM$^qR8pRl$!DSF9
z#2@H}fC3Vn-9TpCVk=6_OUchInhFv|PSuk?Hfb>ZXPPYC9E}{^6Ph`g>N`P-!34+&
zFTsU1h^xs*gq1~iLAt^AT>-JcB_SxTZgCXlmn4FU@LP-_n#|yGpy&uFxY=_P3kpCb
z0mvo=1rBKGcYLyZn*t;IWQVphjHa7;+s)YOL5bh52;`O`OHgKFgs?#oTZCF`Y6>Az
z|1Fl}oXmnEP=yTf0JxN6D=tV)P4Oz41#%^*^t#1flv<pTSde;)B`32O+MoiJW01OG
z3j>2Z!!6Ftypq&(P<c}XE>}QF80MK9lOOhq$b;Js<(Me}(zp@ZEYf$Bk<oOs&m=|0
z$-gH|H2|eJNO6$}atXK??gg<xY2+45enDy;YRP?Iq86k3<ku4uz@{iMTD$=T1$z;G
zgIJSu@{3bZO<FcthcR&S>xuqCau<2zuJR~6;Sszb<bIXMqsVl#$&^-3#?HyR<{ouD
z3UbbR5P_O8A^8%K0?~3P*ehUPFz5SbPChbEo$VpWkj0Zf&J(K#<@;N#nRx}JCAV0M
z3vx0`z*WsH=A6{LB14cmVG!{RM1Tu6a8m@_+5oi^G&vwC2owR3+8Ctr7E5}5V$Lnb
z)LWb&Mm#toL9M)EP)i3|mw?mP<hJ={jPaZI&i}(A1~wGjUPQ?v=a;xK&Ymo`RG!gv
zvd<*R$^J|KS*wG>ngbGFAg_T7z#>qBDyjy#6kJh)tHD|j7sb4eWonEoC$C=?3Nmj+
zJI_jHKTRe`fi63F#&QD%aCwa|>lTIyFP7^tZkVjIQfadJ3K6g=9qOEn3=Enqkkk&=
z3$_?j@9z}`jN2ybtTbaZ-JHDgF{2TdKrMnNL1+<I#LLLQ059VvSFBcJ+%tLpYR`Ib
zgBM&5F}i8W6oFgfO(6F+gNPOo0d6FNQ_V&Y7u1+3+6`iXYVjgam0AR@7Qv|tTvmX}
z&!XEPCHFwYaS#E@Nk#8KtdAgq8Px1$Wi(@8C}IaOxj+ODBdC*<l9^mm#1G=Pf>eTC
z2+oY{ATEf=pFDTXOmJ~m)DCKMi-211(-~43q8L*cqL@+`qnJ}TCugqJVyk4*<SO#s
zJb&$H7EMqq1XMD8HehC8n9fkcSi=DF3#21k!;-?7t--)hq*uZ`d19%sB&f&)OCh?d
zHB2>(HO!MI=!s3{-pnrws+_<oYFJX3k@ZflR^}8$*H$z6B8w<UA0x};#?8WvU|YBm
zwt)E{odW1O!4}F_i%sU<!p{x2w1ju^!L7oAd|($aFff1}pjN{)`QjOo$)8k2CJX3_
z@x_BW0ALk)ay3ko1B*qOSgR(>Ye@<qWNKJz7~;7nCwj?)1_=0ybW1=bFj!Lx8@TsB
zVX838Y=*f^V27;J;9z8&tf(b1xjtTcvVbP*<PF<IKsxJ-c#w3muLiZeA)Rc7N(N01
zKXBJalj#;)Nn%lYY6&Fkg@dBB2UK}*#1}w@PpS+9g8ZFbgM;IJ9sL~LU4yI?G8K}G
zQWHy3Qxr-vQWcW(b8{2(QWOe7qbv$(`9%s<<^e(elN-0o$X6Gq=A`L?I~q{q^g#Wi
zT1|eieUrCrHxU3g^ub*rNau5M><)=&c!ntgwO=*aAcYb*Ypn)3k0&!Pvm_pB^({US
zBR(awC>7k7xy1#Rg){|0ojXVY1!?OBO}?;0M(7zc14D5bBe<O}BGJLp!#SC4=hDex
zyF83<aP#+Dc3NKNR=mWmc!x*uf{5#Zl<RKc7u~|IyTx2|i@D$$dqF7fLPE;*grbWH
zMOS!=C(qlZEFt85mB;4;GXswjxchmF9ptFQyp+igvcxw3-*uPKQWMmOUCCS|3d&ck
zg~gdwskgWaN=xGN^2-x5OI9*Ld?|}uHh>}|ZnEfJc}BL$rh9D}O();YlAQd1SM6rb
zea>tWpa}CTk_Y8ZC1h=rQ;yg$3Qt~m1f&C0$SR!N&H*l0*owp_-#;lXEeX;Csw|4Y
zjYO0ZRq&JwquFG$Q?87rllfyM8J|s_*epFcd#MP2k#Y&BI}b^Yj5U)NCW}tKJ4u9N
zHbV;2T;|n`lMAQHPo8~LcydN84<qN~iS6Q(bDKma8!Y0Qymp)T<ea6#s7A2BjhM{8
zT}VKgp#;?11skuzP-Hu~(OVLviKR%Tgmv<|lX4)-SY|V%uugt?MwW@qZ!+)MI5$pk
zVnP(MMPeYoNq`7YP!}nJ!ke=swJ0|;FR>)G=n^PUkv$CJr%gU_R)yP|g@K{?B)I=K
zS@GPN`h#MQLQYx?hqN3Soeh`|>w`#0Cy)zw;Vb~M8C01Tseo7-AOc(sq7|UvUO$L9
z1xk%Z&@l}OR*uhRQmnp=9~>Ab7oL~&VP-Y>%*Dm(%m_|bMIa5D`nNa>GV}72lQS~&
zi$JZ1TdaAdxdoL);8p{;wggvNpaQmNG01<QlB#IQ<lE<K*ub%FH97Xe@%mfbiACwj
z`MCu-sU@jJTA-}OTwGaP1ZpGPVlAmGNG&cB0f{o@7Z-uXoo;c0M-1}wbBc?=nbrhk
zIA~y|2vik-T~~za&s!Wex%nxjIjMF<R*axwJ_d&31_lO(56p~{*IqPZ)SCSKqJnk+
kGb8^;1`zo@g)NekQU4<Yi2PnARL#if%=n1`Oo4*{0KPAzzyJUM
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 79b63f5..c64cd26 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -17,7 +17,7 @@ from ur_simple_control.util.get_model import get_model, heron_approximation, her
from collections import deque
from ur_simple_control.visualize.visualize import plotFromDict, realTimePlotter, manipulatorVisualizer
from ur_simple_control.util.logging_utils import LogManager
-from multiprocessing import Process, Queue
+from multiprocessing import Process, Queue, Lock, shared_memory
# argcomplete is an external package which creates tab completion in shell
# argparse is argument parsing from the standard library
import argcomplete, argparse
@@ -277,6 +277,10 @@ class ControlLoopManager:
if self.args.visualize_manipulator:
self.robot_manager.visualizer_manager.sendCommand({"q" : self.robot_manager.q,
"T_w_e" : self.robot_manager.getT_w_e()})
+ if self.robot_manager.robot_name == "heron":
+ T_base = self.robot_manager.data.oMi[1]
+ self.robot_manager.visualizer_manager.sendCommand({"T_base" : T_base})
+
#if self.robot_manager.manipulator_visualizer_queue.qsize() < 5:
# self.robot_manager.updateViz({"q" : self.robot_manager.q,
# "T_w_e" : self.robot_manager.getT_w_e()})
@@ -997,6 +1001,7 @@ class ProcessManager:
# need to create new ones, but i won't optimize in advance
def __init__(self, args, side_function, init_command, comm_direction, init_value=None):
self.args = args
+ self.comm_direction = comm_direction
if comm_direction == 0:
self.command_queue = Queue()
@@ -1011,17 +1016,28 @@ class ProcessManager:
self.data_queue = Queue()
self.side_process = Process(target=side_function,
args=(args, init_command, self.command_queue, self.data_queue,))
+ # both ways, with shared memory for commands
+ if comm_direction == 3:
+ self.data_queue = Queue()
+ # "sending" the command via shared memory
+ shm_name = "command"
+ self.shm = shared_memory.SharedMemory(shm_name, create=True, size=init_command.nbytes)
+ self.shared_command = np.ndarray(init_command.shape, dtype=init_command.dtype, buffer=self.shm.buf)
+ self.shared_command[:] = init_command[:]
+ self.shared_command_lock = Lock()
+ # the process has to create its shared memory
+ self.side_process = Process(target=side_function,
+ args=(args, init_command, shm_name, self.shared_command_lock, self.data_queue,))
if type(side_function) == partial:
self.side_process.name = side_function.func.__name__
else:
self.side_process.name = side_function.__name__ + "_process"
self.lastest_data = init_value
- if self.args.debug_prints:
- print(f"PROCESS_MANAGER: i created the command queue for {self.side_process.name}", self.command_queue)
self.side_process.start()
if self.args.debug_prints:
- print("PROCESS_MANAGER: i am starting {self.side_process.name}")
+ print(f"PROCESS_MANAGER: i am starting {self.side_process.name}")
+
# TODO: enforce that
# the key should be a string containing the command,
@@ -1074,6 +1090,23 @@ class ProcessManager:
#self.command_queue.get_nowait()
self.command_queue.put_nowait(command)
+ # TODO: implement
+ def sendCommandViaSharedMemory(self, command):
+ """
+ sendCommandViaSharedMemory
+ instead of having a queue for the commands, have a shared memory variable.
+ this makes sense if you want to send the latest command only,
+ instead of stacking up old commands in a queue.
+ the locking and unlocking of the shared memory happens here
+ and you don't have to think about it in the control loop nor
+ do you need to pollute half of robotmanager or whatever else
+ to deal with this.
+ """
+ assert type(command) == np.ndarray
+ assert command.shape == self.shared_command.shape
+ self.shared_command_lock.acquire()
+ self.shared_command[:] = command[:]
+ self.shared_command_lock.release()
def getData(self):
if not self.data_queue.empty():
@@ -1081,6 +1114,9 @@ class ProcessManager:
return copy.deepcopy(self.lastest_data)
def terminateProcess(self):
+ if self.comm_direction == 3:
+ self.shm.close()
+ self.shm.unlink()
if self.args.debug_prints:
print(f"i am putting befree in {self.side_process.name}'s command queue to stop it")
self.command_queue.put_nowait("befree")
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index 143ca68..078883c 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -221,13 +221,19 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
T_w_e = robot.getT_w_e()
#p = T_w_e.translation[:2]
p = q[:2]
+ print("CTRL:", p)
+ # NOTE: this is the actual position, not what the path suggested
+ # whether this or path reference should be put in is debateable.
+ # this feels more correct to me.
+ save_past_dict['path2D_untimed'] = p
if not (type(path_planner) == types.FunctionType):
- path_planner.sendFreshestCommand({'p' : p})
+ #path_planner.sendFreshestCommand({'p' : p})
+ path_planner.sendCommandViaSharedMemory(p)
# NOTE: it's pointless to recalculate the path every time
# if it's the same 2D path
-
+ # get the path from the base from the current base position onward
if type(path_planner) == types.FunctionType:
pathSE3 = path_planner(T_w_e, i)
else:
@@ -242,19 +248,40 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
if data == "done":
breakFlag = True
- path_pol, path2D_untimed = data
- path2D_untimed = np.array(path2D_untimed).reshape((-1,2))
+ path_pol_base, path2D_untimed_base = data
+ #path2D_untimed_base.insert(0, p[1])
+ #path2D_untimed_base.insert(0, p[0])
+ path2D_untimed_base = np.array(path2D_untimed_base).reshape((-1,2))
# should be precomputed somewhere but this is nowhere near the biggest problem
max_base_v = np.linalg.norm(robot.model.velocityLimit[:2])
# 1) make 6D path out of [[x0,y0],...]
# that represents the center of the cart
- pathSE3 = path2D_to_timed_SE3_base_and_ee(args, path_pol, path2D_untimed, max_base_v)
+ # TODO: USE THIS ONCE IMPLEMENTED
+ #pathSE3 = path2D_to_timed_SE3_base_and_ee(args, path_pol_base, path2D_untimed_base, max_base_v)
+ # TODO: separate out timing construction so that it can be used for the handlebar path as well
+ pathSE3_base = path2D_to_timed_SE3(args, path_pol_base, path2D_untimed_base, max_base_v, 0.0)
# TODO: EVIL AND HAS TO BE REMOVED FROM HERE
if args.visualize_manipulator:
if i % 20 == 0:
- robot.visualizer_manager.sendCommand({"path": pathSE3})
+ robot.visualizer_manager.sendCommand({"path": pathSE3_base})
+ # now find the previous path point of arclength base_to_handlebar_preferred_distance
+ # NOTE: this can be O(1) instead of O(n) but i can't be bothered
+ path_arclength = np.linalg.norm(p - past_data['path2D_untimed'])
+ handlebar_path_index = -1
+ for i in range(-2, -1 * len(past_data['path2D_untimed']), -1):
+ if path_arclength > args.base_to_handlebar_preferred_distance:
+ handlebar_path_index = i
+ break
+ path_arclength += np.linalg.norm(past_data['path2D_untimed'][i - 1] - past_data['path2D_untimed'][i])
+ # i shouldn't need to copy-paste everything but what can you do
+ arra = np.array(past_data['path2D_untimed'])
+ path2D_handlebar = np.vstack((arra[i:], path2D_untimed_base))
+ # now we need to construct timing for this
+
+
+
x0 = np.concatenate([robot.getQ(), robot.getQd()])
solver.problem.x0 = x0
# warmstart solver with previous solution
@@ -263,10 +290,10 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
us_init = list(solver.us[1:]) + [solver.us[-1]]
for i, runningModel in enumerate(solver.problem.runningModels):
- runningModel.differential.costs.costs['gripperPose' + str(i)].cost.residual.reference = pathSE3[i]
+ runningModel.differential.costs.costs['gripperPose' + str(i)].cost.residual.reference = pathSE3_base[i]
# idk if that's necessary
- solver.problem.terminalModel.differential.costs.costs['gripperPose'+str(args.n_knots)].cost.residual.reference = pathSE3[-1]
+ solver.problem.terminalModel.differential.costs.costs['gripperPose'+str(args.n_knots)].cost.residual.reference = pathSE3_base[-1]
solver.solve(xs_init, us_init, args.max_solver_iter)
xs = np.array(solver.xs)
@@ -308,7 +335,12 @@ def BaseAndEEPathFollowingMPC(args, robot, x0, path_planner):
'qs' : np.zeros(robot.model.nq),
'dqs' : np.zeros(robot.model.nv)
}
- save_past_dict = {}
+ # TODO: put ensurance that save_past is not too small for this
+ # or make a specific argument for THIS past-moving-window size
+ # this is the end-effector's reference, so we should initialize that
+ # TODO: verify this initialization actually makes sense
+ T_w_e = robot.getT_w_e()
+ save_past_dict = {'path2D_untimed' : T_w_e.translation[:2]}
loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
loop_manager.run()
diff --git a/python/ur_simple_control/path_generation/planner.py b/python/ur_simple_control/path_generation/planner.py
index 49658b0..38eeaf2 100644
--- a/python/ur_simple_control/path_generation/planner.py
+++ b/python/ur_simple_control/path_generation/planner.py
@@ -13,7 +13,7 @@ import pinocchio as pin
import matplotlib.pyplot as plt
import matplotlib.collections as plt_col
-from multiprocessing import Queue
+from multiprocessing import Queue, Lock, shared_memory
def getPlanningArgs(parser):
parser.add_argument('--planning-robot-params-file', type=str,
@@ -462,7 +462,7 @@ def pathVisualizer(x0, goal, map_as_list, positions):
def pathPointFromPathParam(n_pol, path_dim, path_pol, s):
return [np.polyval(path_pol[j*(n_pol+1):(j+1)*(n_pol+1)], s) for j in range(path_dim)]
-def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v):
+def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v, path_height):
"""
path2D_to_timed_SE3
---------------------
@@ -514,18 +514,26 @@ def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v):
total_time = path_length / base_v
# 2) we find the correspondence between s and time
# TODO: read from where it should be, but seba checked that it's 5 for some reason
+ # TODO THIS IS N IN PATH PLANNING, MAKE THIS A SHARED ARGUMENT
max_s = 5
t_to_s = max_s / total_time
# 3) construct the ocp-timed 2D path
+ # TODO MAKE REFERENCE_STEP_SIZE A SHARED ARGUMENT
+ # TODO: we should know max s a priori
+ reference_step_size = 0.5
+ s_vec = np.arange(0, len(path2D_untimed)) / reference_step_size
+
path2D = []
# time is i * args.ocp_dt
for i in range(args.n_knots + 1):
- s = (i * args.ocp_dt) * t_to_s
- path2D.append(pathPointFromPathParam(args.n_pol, args.np, path_pol, s))
+ # what it should be but gets stuck if we're not yet on path
+ #s = (i * args.ocp_dt) * t_to_s
+ # full path
+ s = i * (max_s / args.n_knots)
+ #path2D.append(pathPointFromPathParam(args.n_pol, args.np, path_pol, s))
+ path2D.append(np.array([np.interp(s, s_vec, path2D_untimed[:,0]),
+ np.interp(s, s_vec, path2D_untimed[:,1])]))
path2D = np.array(path2D)
- #print("=" * 15)
- #print(path2D_untimed)
- #print('----------------------------------------')
####################################################
# constructing the SE3 reference from [x,y] path #
@@ -571,10 +579,8 @@ def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v):
rotation = pin.rpy.rpyToMatrix(0.0, 0.0, thetas[i])
rotation = pin.rpy.rpyToMatrix(np.pi/2, np.pi/2,0.0) @ rotation
#rotation = np.eye(3)
- translation = np.array([path2D[i][0], path2D[i][1],
- args.handlebar_height])
+ translation = np.array([path2D[i][0], path2D[i][1], path_height])
path.append(pin.SE3(rotation, translation))
- #print(path[-1].translation)
return path
@@ -633,7 +639,6 @@ def path2D_to_timed_SE3_base_and_ee(args, path_pol, path2D_untimed, max_base_v):
y_diff = y_i_plus_1 - y_i
y_diff = y_diff.reshape((-1,1))
path_length = np.sum(np.linalg.norm(np.hstack((x_diff, y_diff)), axis=1))
- print(path_length)
total_time = path_length / base_v
# 2) we find the correspondence between s (path parameter) and time
# TODO: read from where max_s should be 5, but seba checked that it's 5 for some reason
@@ -644,14 +649,12 @@ def path2D_to_timed_SE3_base_and_ee(args, path_pol, path2D_untimed, max_base_v):
# TODO: tune this value!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1
# TODO: tune this value!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1
# TODO make this an argument
- base_to_handlebar_prefered_distance = 0.7
# TODO: we need a function that takes time as input and spits
- !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!11
- need to have set interpolation to implement that
+# need to have set interpolation to implement that
# out a path point of distance base_to_handlebar_prefered_distance from time 0.
# (it's the classic integral for curve length)
# cart_to_base_prefered_distance as percentage of path length * size of s (again why the fuck isn't s=1????)
- handlebar_to_base_as_s = (cart_to_base_prefered_distance / path_length) * max_s
+ handlebar_to_base_as_s = (args.base_to_handlebar_preferred_distance / path_length) * max_s
path2D_mobile_base = []
path2D_cart = []
# time is i * args.ocp_dt
@@ -711,13 +714,14 @@ def path2D_to_timed_SE3_base_and_ee(args, path_pol, path2D_untimed, max_base_v):
translation = np.array([path2D_cart[i][0], path2D_cart[i][1],
args.handlebar_height])
path.append(pin.SE3(rotation, translation))
- #print(path[-1].translation)
return path
# function to be put into ProcessManager,
# spitting out path points
-def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
+#def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
+# let's try shared memory
+def starPlanner(goal, args, init_cmd, shm_name, lock : Lock, data_queue : Queue):
"""
starPlanner
------------
@@ -726,6 +730,11 @@ def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
out path points and just the path points.
goal and path are [x,y]
"""
+ # shm stuff
+ shm = shared_memory.SharedMemory(name=shm_name)
+ # dtype is default, but i have to pass it
+ p_shared = np.ndarray((2,), dtype=np.float64, buffer=shm.buf)
+ p = np.zeros(2)
# environment
obstacles, _ = createMap()
@@ -752,8 +761,11 @@ def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
try:
while True:
# has to be a blocking call
- cmd = cmd_queue.get()
- p = cmd['p']
+ #cmd = cmd_queue.get()
+ #p = cmd['p']
+ lock.acquire()
+ p[:] = p_shared[:]
+ lock.release()
if np.linalg.norm(p - goal) < convergence_threshold:
data_queue.put("done")
@@ -765,9 +777,13 @@ def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
r0, rg, rho, obstacles_star = scene_updater.update(p, goal, obstacles)
# compute the path
path_pol, epsilon = path_gen.update(p, r0, rg, rho, obstacles_star)
- data_queue.put((path_pol, path_gen.target_path))
+ # TODO: this is stupid, just used shared memory bro
+ if data_queue.qsize() < 1:
+ data_queue.put((path_pol, path_gen.target_path))
except KeyboardInterrupt:
+ shm.close()
+ shm.unlink()
if args.debug_prints:
print("PLANNER: caught KeyboardInterrupt, i'm out")
diff --git a/python/ur_simple_control/path_generation/tunnel_mpc_params.yaml b/python/ur_simple_control/path_generation/tunnel_mpc_params.yaml
index 1f1ca86..5568e99 100644
--- a/python/ur_simple_control/path_generation/tunnel_mpc_params.yaml
+++ b/python/ur_simple_control/path_generation/tunnel_mpc_params.yaml
@@ -13,7 +13,7 @@ tunnel_mpc:
max_compute_time: 10
crep: 1.
reactivity: 1.
- buffer: 1
+ buffer: 0
# MPC
ce: 100.
@@ -29,6 +29,7 @@ tunnel_mpc:
integration_method: 'euler'
np: 2
n_pol: 10
+ # TODO: test if this is the length of the path
N: 5
dt: 0.2
solver_tol: 1.e-5
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc
index 5f9c8d3e8db06097c501d6e568f9c82e342ef74f..0b9be5219556e6bda86aa8b5060d344b697ac177 100644
GIT binary patch
delta 71
zcmZ3SwK$9KG%qg~0|Ntto}6F0n8ilE4s}MJ$<anilXHv&H=j~J!O8Z8fq|iT-ef*?
cyUo`OZJ7mJ7#XcTM=&`vGFlb+FfcFx0L%;%00000
delta 71
zcmZ3SwK$9KG%qg~0|Ntt-W}ic)8-rbI@B4tCPy17P0leA+<Z#?1Si{P1_p-Wd6W6n
Z?KWRCv}G1>W@NPb9L5BqihLLt7yuiF6l(wg
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc
index 5e23ff210ab7394dfd4a3855298def2a26a4fc03..2095b13bf2b1b332271a3dd4614c22b5bfcd9ccb 100644
GIT binary patch
delta 730
zcmaFf%y_Gnk?%AwFBby?1H-#U|MW)JjeHBGnUYv1Z<M|`*-FM_a)A-+<YE~~M)t|Q
zGN%0X3=FmGHS8%I*%}NCMP-vO$|y5(PM#}&h>ek90%IP_<lVA|8QCW9mz4!;d?+i<
z&L+vgz>p<1d7+x@<aKg9lRM<ZCSQ<~n7mC+N|qZWx0(@TI|BnFLk)Wh57=P28n!IO
z$qQLU%u;wjN>X@1WDN_H#|PrAW<t`&57Ji5G=Z@vyM`@GdGbLu5t$m66n<2N6Bv7%
zSQsWS_Dr9gE^o>xIC-x8c}AhhxeDIwx7b4BlM;(JZ&x_S$lbxfz_5~mfuUHFY4QZ6
z;K`?y_(VM}O6qPXzRF^KhlTS3m;MzNgAYvdliw<(G2arAoLr}@Gr37wLq%hf{S6WE
z>0T4PW>j1h(YP*Rc2UG^L-7uajkVX!11_2eToDPJyiB=(N0&iD`Xeg?zwnPQCX<y^
zVkHlWyV`i@Gn^2W0+ITTHePznr*s*;^fu2>ImW8h&A`A=gdnmQ7#Kn{8E>)V7o_HC
zvLMNHPwvnPW$fO3Ps^K;v3#<Yjyhx2<RBeQ#>UAtI<f-QAaiO!L@kJ@o4is-p0RcE
zNgV~{HU<WUTP%6`DXF(u$}+2rZZQ=a-eM{=Dk=g61s?+ggTmyG?81}fbro3`F)%P(
z-R!D6g_kjZ@-3@4#)+FPtwk9bS5Ee~5o4S+InBn8yA@=?Y!ESL@_rkC#?_PgZ9`eN
zGcYiiZO*iP#cV$lBv%0<L_vf(h{ymDpztpOSy2R1y^^6w3?wQ7avz6HZhlH>PO4qe
e!O4>x?Zwm?bw5QghB7khes*F`Vq_Ess|5g|k;Yp9
delta 637
zcmccB%J{sQk?%AwFBby?14H{o-}Lh?8~GMWGwooRyixk%<Wn+SjBJyy%9slKF)-A!
z*RZFsXKOGp6qVL6XYoyLFcqC_qrk<;G1*r32v6RX8uk<>u(bB%Kn1bMs&b1NIVWq%
z%L{W#GB7Z#W(3*6z`)2*!=A!5Ia*$RvWL9H<Vo^UrraR;6dn*+!;%8w@q&1(nUM7G
zfy^#un!wm&Rl}C0Jb5FFuq=NKO9~&V$_b1;fh-IY7<<wt3o4jS_B7|7>@K@wvVk@)
zqu^w5Meog7if0+Q+Zh-bRx&U!6u)4c?4%sb#d(2C{|bx22PWCcJCxHHZ%&q137b4u
zC5K0oK|=Z?D+9mqk1qz3^$o-(Ypc3T9yE64c9&u}&L9OMr5tTMC7Dl3FnUUE?p8g<
zs@287z)*xBvKSZ`LNpm~vE&z|=4rAZ$#hMw)(&Ot+I&&ln~||>vaGH;W94L5T}{S@
z$@#jn0#zV$szF2zh^U=BM^~P)W%3?f1;*CNcXcJW3PC>LV_;xVnEa7lc(Rb5BI`m1
z28K(UE%c`FGUiM^YaPcpVY7~nD5Jy*kO`p-3=Frpa|?3flk@ZPQj<%HW=@W@^%HFY
zNzMWhvq3~Mh)9{-FD5hjhOH*!s>w`tp{&~&7#NH<$Jo7MR+#~kF9#8#Ai^9(WPk`z
ts1|`-P_%7wmXj3Yfyu2-_F^iGx}Sm=Lm3%$KRYoeF)|7li7_xR0041`pPv8#
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index 3c039a0..2332454 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -172,6 +172,7 @@ def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue
# set shapes we know we'll use
meshcat_shapes.frame(viz.viewer["Mgoal"], opacity=0.5)
meshcat_shapes.frame(viz.viewer["T_w_e"], opacity=0.5)
+ meshcat_shapes.frame(viz.viewer["T_base"], opacity=0.5)
print("MANIPULATORVISUALIZER: FULLY ONLINE")
try:
while True:
@@ -187,6 +188,8 @@ def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue
viz.viewer["Mgoal"].set_transform(cmd["Mgoal"].homogeneous)
if key == "T_w_e":
viz.viewer["T_w_e"].set_transform(cmd["T_w_e"].homogeneous)
+ if key == "T_base":
+ viz.viewer["T_base"].set_transform(cmd["T_base"].homogeneous)
if key == "q":
viz.display(cmd["q"])
if key == "point":
--
GitLab