From caad67a2bb3d73050c35838f4c19afc365de4ca3 Mon Sep 17 00:00:00 2001
From: sladro <sladro@163.com>
Date: Thu, 11 Sep 2025 19:58:51 +0800
Subject: [PATCH] =?UTF-8?q?feat:=20=E6=B7=BB=E5=8A=A0A/B=E4=BB=BB=E5=8A=A1?=
 =?UTF-8?q?=E7=82=B9=E5=8F=AF=E8=A7=86=E5=8C=96=E6=A0=87=E8=AE=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- 在Environment类添加task_marker_ids属性管理标记ID
- 实现create_task_markers()方法创建A点(绿色)和B点(红色)球体标记
- 实现_clear_task_markers()方法清理旧标记
- 在setup_environment()中自动创建标记
- 在cleanup()中清理标记避免重复创建问题
- 标记使用极小碰撞体积(0.005)避免干扰仿真

🤖 Generated with Claude Code

Co-Authored-By: Claude <noreply@anthropic.com>
---
 Snipaste_2025-09-11_18-23-45.jpg | Bin 0 -> 17950 bytes
 src/gui/main_window.py           |   9 +++-
 src/robot/robot_loader.py        |   8 ++++
 src/simulation/environment.py    |  75 ++++++++++++++++++++++++++++++-
 4 files changed, 89 insertions(+), 3 deletions(-)
 create mode 100644 Snipaste_2025-09-11_18-23-45.jpg

diff --git a/Snipaste_2025-09-11_18-23-45.jpg b/Snipaste_2025-09-11_18-23-45.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..cce2e44fe01d999379d5ead21e12ecd395b75b18
GIT binary patch
literal 17950
zcmeIZ2UHZ@wl!L0L`jMYh%`}>sN^Ixpn{-)AVHGktYnZNG>U-aEP_BINX|$`Z51Tv
z97QB&rJ)JkUHuBf=XdVC=e+md|J?E38{_XDQdZhkYwf+~nrp7zgi*p5fKF9OMF}7x
z0stcD3lKoyw6c|>m8+S%l_i_GH`{GTJ7+UDcPqjK@C2ZuprE86r=p~!JW54%l!lRx
zhMJn@?8#H~jGQoTE)Ez6`#AxLi|6=6`PexwDF}&5$;ipcabLKqa^>=M30XNABC4ZD
zX{c#f=;&BvcsY1w{+B<5_W(U5(IcW}5+Ztln4XA)o`}#ylnK2&8PSgq@ascFOhQUV
zPC<Eu>L?vROhiILOiDsVMoJ1@=?9$yNa@LrvkNJZpU^a;;Ba9OeiZwXl2ft#!%3~a
zZ(Jhgu8)sUojT3P#LRt;hnJ6ER7_k#QcC*D)oV)1*Hu(+YwPIh-O)F&u(Yzav9+^z
zbNBFk;N|TT_#`Me<Y{PFTzo>}v!v(ADVbTXvU76t@(U_nS5{Tm)YjF1Y;5|}+|t_C
z-ajxnG(0joHa_!pc5Z%QacOzw`xbh82eZ4ke?SC~{4@vp`_rJm*#j9)L`+IbLP~LH
z4-xSL=pP9^DH*#E`EdnJ3Nx1z9Kw$%85Cn*mVY?HDWdi5q`7M!)hRB~8Sd|gCjGGJ
z|CvFL|63OQWzb*t5XOOHBt%e$k<bHhV5KlGCWAPD3g89)&*uLlE;trnl4xFRuRuY1
zdig;2?m(;O39K)ehT7sG00z-^bEjz6HxE1sKr0e&FPj^42VR3(o*)1mh@Cu4x`HYA
znE>QE;X(<(aMrjtS>XS<_y73n|Hos)d_8FIogRKn@rk94to`f-Q$ZcdA7CX_tO&mV
zT6MclpyUw6Zf*Jry7~W8GgzYX4rVNgpXdP>Omkp=p(dyDgeZR#18|E-p3!I0*D+2l
zgW{&mVDy#%bGT-f_-jV!1R3z-{J(W`R_rn58oOpH19cAyK3s267S8bVEtpbPNSqi_
z=S^&J$`DGt&M)f=y%X_20_sG4FP~{gx+Ixq$!!bDuL}GQAjI!p!jjE|7a5JGli>Y1
zs!KK^MzjAjfu%0{)53I1Vynh;^uSrTk<JI>M}4|;K|dW%M3hhnC@`|^X3(hgWqB$w
zT1j$#u2Vh!KN94`+_}j0UoG;#RGuXOE5&x9FU0a5|9+BGu?nfrlXj8E6Lq^xU{w$N
zX=c^+f9rVo)!3N>VTQ6vmk`&+eT3@g^p92t<$pcKd^PV{la@=cTGlr>-lsWdR>|1<
z(*HoLC&_yq-TpdAJdvG<?I2xa)mTVZ@mvnu?<XBwcND~gvl%^~h))k706X6`HEl)B
zbV>gG1B9Xx_Ci=W)ycGOyDcYi^sKRa+@Fa^=DU6T&dpp&O2cSJL^j_&k>nDyU*qO4
zkNvs&zdMP^y3WBxZe6LoB!6eI1~wa3d(n&O&j9`LAPOE1#%!+%CMABCb)!<n;NJi4
zv;OIQ4z`+B^Unj{Q_)&X5&(9C0pqJ(VS@^P!zH3_``CS-CmDQ1Pt7)Cdsb5xQ!2yw
zwFCa$;B}TfnzaKM>&6Ifuj2*01W7xF%>Ul+$BaEMt1%>F+x1{q2*5~DfuK@EeXH5u
z-t-V?|F`?cL%e)nw_Hh^PaAl&`ng`Kg6rAyzc)%oZFO|ik@2IX1rAkwO)WTr>ixg@
z9gfRCHhemJZ)TFOr7ow+)I+EL`cK1%US99~0oDTcLPxfjXGe_^xOi8t&-vHp`@Agf
z<}VZbyTASoe7*l0PVHPl1Z9(br`vJ_!l2giX0>$t=+Bq<^DL5}+VIxeXJ;qg)Ci<n
zNJ^GG$P(v#^>65lKXZ(HH<-P{=;p0CpkV6lbx`LOWo)nRt6%+cv|$j|9&;o}_C1a-
z08mz>os~ZtKc%sJom6rc7IaDKtS3xBCy9!<_E_y^3lEySEPWw%v?rhU-x~N<wkex@
zR4-YwT8NzOnwDJN)9p22>TtU;zsZl>)lNahat6bLxSjTB_<&kB0&xC2h<>CL$cPw^
z<HosZ%&}23g5vOvV&>o{!7`6<{L|lHU|F_TRHvD>2?~Jaf?XPY;d2sQf$!!Cz#|A6
zm&X;{d4+1GYT?oF{%3Ck)n}VrW49`5a3lnvSar$M2LAQM1KpammlLTkooQs{L66k+
z-rC8<RpGxP&z~Or?=vEruYCpr*Uh&ED>(9Hf{&1_2xJZ$ygu`cxZ9UMx&(o-mz`b(
z8GRl_qM}UN5O{Z!ilfKm6nCuQkPTn$u+t+r$x2!=im3=>K4g@p&EN%IZ^Mm(N8m02
zu(}pw&og!|dgC}^{V7=KmWx_oDW+oHk#3$t)Mth(guLN<Yt|GCYrX|fyrAAG)eyQc
zuCn_vx~S7`+`*~!;wjNaYXabU=fi;zSb5jM{ZwdL=@_CZd;SDDy>K<@ogT^6y+L(7
z!;>eiT@yny{uO#W`Ep`e{){KcTw7Zm<~OgaO(S>9i&L&D^NGP^2+wGOmpwiBT8k2~
z8igok?b@dC7&O^aT8A%PfPZ5l0FkWieu3$14@HOwfL?xxT)F~mINIjI%kFw=0`Np@
z4&Qc^);I%U@Gy3zF&RU<cXbo~2C*3nin?Ce4@@x*p{2TxT6MgM!1Ta}lQ@<dlXnyd
z0Oe4|Y9WAFg<w09wc9u_o!fVc=BS~?d4rJD#=VD`Z=)X$wjYlwvR7HN7)<yxmHzX*
z)UY~@^gU*H>`^Ll#w_P$ykt(5x$oKTi_?2Xv~~=`;8&5|AS_Ea+^ZY@h2M7ZhUvVY
z(hCPK)Ud?j5inu@6$baovgSm0g+~2t_(oaQvwIimQBG&+F5+=!xMi%|OgHbd74_Tj
zCm$0tYjBUZERGvKStaZ6Ga>*%;+Y38jWJwUyQSekkemR_-iUt<lZfS4A=&CP$Q^o=
zZ-1kZ*`bwH#&cP=!jAzFd8EzV%xW@1KY#urYV#3DUYNG?wC-VheNbwJe%J9g=x2(=
z36eZDt5@ubWAt<DKIsXS{i}YB+|}q0khw1<lw%;=y>)`>VZipj^2y%x7ffO$^GI>C
z7L4bnD++J#Y0@e1?!%%Q0g$gE0Po-jgRZ3uM^W>%!b&qmPp&|twgNP5SYy>z+b!+1
z8SprbXg_7#*cJ#rzE1%Bppm__khB$)ga)F>7T2-(a7}Lnul|G6T`o<Yp}adsQ*@1u
zALC<@#VP{J-K?F8Qu_hTUYE7ixGEp;3r<_a6h;0kG^Z8>=nKwg@CgZs4=;JO>=Zt6
zY>TguMoE9w+!}jH0NjTs%Z+btOe=Cycd*`HFxkCZm?HbhW;F49hacJ<S$qTOKz%iS
zt@l%hu`2wt30S2e;z!Y*r1{lBhY@>K89$fZaiOt(LujKHmRM}~jUy!xeOh?y4ENL%
zl+Ox;Wq=u;Ur%_P^I>T|b4!j|T%^l68k*5b^!{zkYntn^9n|#%ps=#RspuX&+o0f}
z!bpaLj{8btc|1sVBBjLZUFLj}xyRdg5f7`2vcOs~<&9~rw>!PphA+?@rOVhl5}7v_
zaw9xNnMwNmgX2n167|?lOp@G@?b1BU9`sMMCqPMwm!xzc*K**#T9KC2Rub3MCL0pj
zLa%l~-_EvNzoRBjgQ&#_)b0%GV_H7NCdv9i67G8vh?CgUv{%4>px#+Hk}h>hsO{V-
zMm#Tmd{h>E^-)%8P_#5yHk2cLa7o}w-aGX~0w7C)-?l^R9F@PECII>%W}B15ZdDRx
zN3nyU!s(4X@a}Mv*|P*7p=a2tUfL4na7K^mu5>zbI5RVBPc<;<c1%(DzrtZ@+GAH7
zDi(Y{v6Bxev(3I!QXaDZGCrEjwJUv6+R|n|CjX$y7GKCYc{O-zg7HQLDj1P{LB-?B
zf^mEQ-A>w#P0e@t(aQ$B7s}b&wk>b?`tqnSL{2EJXYowP+Ro?H9PJ7luIXoaLxlkN
z@;}`B1od=Wx4fJ$;2#~ms?y~iGiYKS;U?-3$Z*`5=<}1sZ{7T9QnI57{YwVSZ$}xA
z0tf5_fH~T1L9c3yD_U#N>VEmB6ENEeoL63=WQ(WP3%{e~2s`No<(hEWHx(LSkiN|6
zeA0U3&K6-4aw&@E(VCOgqp%>Y#QG!6c%=(ZCn|OoLLMpy;#M`BIQ&wi5cFe5&)&9g
zHBw>pR}vbu*e=SxVB^|fQJ$#vuksU;{9X^K``fmh+PcjfhEmw(dUh3632Utv>dn!Z
zK>~p8h7T8xD%Z*Nh5X({mOb@V5?50)tW77^zO|yq#wxPc@2+)4=L}DvdhiBmTJm6h
z@O#b(@FeS4=GzoY<Y*b9M+GhfGWdEQ9m@be1%f^*`_bRBCR$}v=$7dU&Q_?z$tttr
z`J_t>gz~s79g>cBWvj*oQL_@Yl*}N}o0?oj$ttfC^8O_eS=#n45p@4nuG-3FKCha;
znCr4c=Ke9xr9miR-1D4kzxaY%YMf_5MEv&2%-s9QG!Ld|BTWgwvq%Y(8Ggx@fpq_b
zR|W1C>av=i0<v;O<{MT#BsAKtzBY_J%XjJq4O4hOtm!kKXPYxpmkUCC%pLEk-RDp_
z^<vJVy`ff~ocM7A;=K9GhD5X1hW7ID)R>uclFSHy88xb@D_U<m%jlmL(Le1z`YA?g
zd-q_toyM?2DsGJ7?m17MwE`*0CV5jIXMt<#lwIvNqu06!fE)qXn>RUsImnx0>;(t`
z<M(z!TDCVqQKPC8x}smce3Q(77@jdA<Ug%a{a%8Ww*DooyFC6~pQ!58&C-+OhK<xk
z&&r!m-r%f=jUQmz@L_JtG4AWq`d3N{(b%4&40*3$M4O}KeudEm|L;2mnogDiS-0E!
z3BY1nE`F=h^+uW7g_-$f1iE_w+&7To9+gURiMy7=z;031bB}-ER^pDLt8u4Jl?B5+
z!L&eLsXJ{wtRHK_b23XT2VeBcwL&9m`67Pb>055sjROU{kTDqO$4vlGf*^yN78qk-
zhJgeZqzYoQ0};eBh<bG@nLPJ&Qg-*vi%jfFOjL^t2fo86r3`n~*!^>*^q>8!Xze+C
zd&=Mgi=YEnI5XJ-In(?1i!A<CuQjV9y4*aJLMGOAByKR2yG5r%;gqW9ZqO1BCLM`K
z69E4gIKHi(vQj#5V4&waV(uJtUdm?l;_=GYc3QO0^rAD)aGFYDK54lbAAz78DNx{=
zM1EI>laFUfd8asG`cBN(dZB_Xp^u?=c4iEUy9HdmE!|OefuJvBn;+Hee3C_0GNohj
zd#)Z<3anz3-spv=Iq!H~PQIx?RGua&q1|6rQ~c^${^D{CdQQ~!KK$bek{sHZoh~Ov
z1;@_RR|hA%-gdA&x8lJM7@(EwPFbi`y=U6u#?hs)e60@=SkoLTp2L#JH1a=Tn|n!8
zA`|V?{A!kWsf(TIoaw&~^F+ONbnWAo*Vq!11Rs~j$$j4pTglbp=M%S(6yFF>zY6u<
zCLDj@^sNw6wxP@w*op-G7NOo#gXZ>~2Q(@)*K?GQ-CqDa<ZdH^cywv=2D$9??LA}l
zubgT9kO**gIB8_NZndpNmOy%X)f!^45HVeS=w0s7haI&DhfxC1qYJfnA7uDB((t7g
zP~<QK@ilvzZ5KXtHarKp-UKhIAq2Ow082WR)>)*PN=^;wfE3l0e;yr<O52}LNvo(Y
z<VneHbmym0M;seasj)n_=0l5#%*A0~v4t47wF_RmBzgL=fx^i5eQS%OM&J1~Sd%%x
z4V|Z$aT@r==E|^q)+g_jY&w{m{q=PfRx+@j#R*7l>;9#-<IM}#CSy0w3NtdxzVZ#>
z3Ie+u2tYay`lnl{M6rGS=U`8>*V!kfxYWpFMZUCOwUCvL*Ke;eACg;$ICToMC%|X}
zYo0`cRc3rrj`o?)KBdG$QJYA2^;X!ZssHzb`xLo@m=#%kK^BEzsOt?k*sZteTTJG<
z<0aYhOH!n6`y+~(al#&Ak}LBP|J)VC0Wz>7EA-T7+S6?<F15G6d`_(Vxrb*k1(?A4
zA#wN*1OPz>z?@z~v#=Jv`6Yw=?M||y+z5rn{t{pDhtw~z;ym4N*f!UVV-mD=b1!FZ
zmU)NbcW^6pvy<t1qnDe&1JK>ArPlS`hXtP95p-6%Y-1vZ-+t|knieJe-C4Qdx<b+U
zmsO(umsU^6|CY1<Gie+AhJT|c9jb3le=;J0E8^4_i|PD=@c9RdarNajvPCiCG>j(#
zv=^5_zts}46O)TBwl?t>acBf~ZU}v0ZY<>2PG4t3&~tBLXjt(;Y*dY<qgFpU!szNu
zAIqg#8FiP`+2z~ZM;x_W4Vmfe^^P`u6N%O_VnxO-E*ovVsu$6bTY8gp3;sPwE=n9Z
zQfy^y+}FJyhwwv5Z=a7cEJMSv>-&9{r_C1NyA$Zl)(b67uRA9!KDYG3(94>9N-_!Y
z*4ZV`+ewUC^-=XjF8d>*1vTXbLE)<mwl8(X0B7K=fXwR~N6X>$aQ+J)TT)kg&)hdy
z;dlfjBx**GU5-j`(jrd#r_7B={tPn==c7UZj5W4Sj$E0!lwX%+$P`4(-cWx^x57x`
z8;!_-EXAtPUcu+)hyw1KojIqF>~;HQ+WfP~=zHsIQ38eL^bEYVNEe=^4@z)m^wvQ|
zi;nv&tD(RO{`88+Y<>%cnzrMvXP*xNh|O}Eou=(#vb!)o85mi?&V|L*om}m?P<q<R
zW@%*Aq9a%?B5`V@4NQXI{K<49V*7*!7DE7LG<S|EJAFbVBaB~8e?!b!dL^~!#xD1z
z(caRlLkF00!rds~-Yz32`+`46cGdUQ@2qaw$Z?*^hE2Bj4B-0IcD%((kBv`hTugT6
z=Hr`)pu6+vJ;_GmvQcfjeVT4&Q9a^ruI{O<G_JXggXF4K_kT`x$*aSjD4$O?dkRY;
z044Q#f@g#Zc9foTu|x^DaST<z6nn=YehYDAIZ7Ie#e7;C=0i#~V#Pa*Ho%GUw?}3#
zKR8I$6U~^dtBA8Iczh@hD88%@@UJ9_$A|FX@E7q11YoTY;lv5Iewx$YL?V1Qb1`Ep
zN@I0XP3Ohfk%2JYm@Cd7LSAZ#w3LHNRh8pIqut+c*0AQg9AA$V9Q2{=Apj@!>jif%
zj(gLBqi?nY!96%mhqbX&wloHD@2-ym0eA=l`5_L;I?@WSz9`EEjcHowCgn0Tt;3v~
z^lW8){LQ^7{>PpXG?HLq#7yg&9g~|t@(|fC0qDQjP-xEFmzF7(wz09|aFrwg9vVBv
zW}2V$>3R8NUYLxQK~qN$&;PbzO`}2qA5M$gLG(vEVkurLDTleY7UQHm_#5IHBqhOv
z5mxEKXRJ2{(8{_rNA$vXaM3J%+u^Htg|F3ton9gf)GI<0bvva4Xcm8_84EiE{atBU
z-RSBU#cSSTwmr{OCu9&NE+{BnuZ9vwImG@+c=J1piGd9lU#he~kp8{{_>cf>@gpKx
zKO>znCuOJ+$5weG7FuJncxYaM@^b^2ZEQ$c{^-*A<JT_>ke>KhrdmV2^}Jn%M%_?W
zgU!<O{rX)cv0UBdZLrA4OLRy^^qP_KoqwM5dP<)PXGpm!+=^D(nt#f8cJFeuJpZiJ
zHT~mX(-%T8EHLzEL>J;CvI^<qdKF&w1;TqY=l}&m{au>tVWM}mA)-h-g!mZkAUD%S
zPXJ77AD^wRsfcjbCypJ+n0=&~s}*))a>aAKT4l<Q=F<l}lyiMdt+sa5x@s~mF^%4y
z-u(zA{w8Iihuk}I7l)U?e?!eEAF^+w`k`*miO5>G7aECffhts)qK1?C+H2NtA{Ng#
z#gFh5yYuF)nNRpe7SR2$ke4)uLd!*^vW9hDgU$|p#fGdifEP`5zvps9-~p_^cBLnI
z$%%6G{onFu=FQhE)&Zh|qSI9duRzbPu~|o#ZdnH}tp&Zh)B^p{7uXYEI{bSN_*SnB
z%`?7FV|?-oH9GF?dn4wPWJ4iPmIF4|fH>51D884P(fcaFMu!Q&bP^i&(*_cy8lE%r
zxRyQ5HFMU!6y3WgUZ+o4J=1RnwFNG!PobFSJ>Lbl<8Q#e1usvd#}~l^6YyM2BS<$T
z1NuUe$n*^Y(9}y_jWi>FVO!lxQhEG_xNy_1<m&Ps74RC;w_W|EhS%B4{J6xi$(T4I
zYyjOPHq{;HC!+IB$9{%KvSe7d!KZLypib1qJ>T5oh}^C(ch>YET987=k4GC-8zmO+
z{({nC!i5Kx=%>#ABs=5--??J7Qe}G|c*_;!3FB*OpK`tZRzFXCljZ2%5WrsF$lz*W
zYi<2-_9<?)HNnWLy>o_}7tP@l_c%acO26g7>rU%4B9XZx1Yk(NJLTDYvpUxwr)aKm
z=vA8GBb^|^!wq{1c}@Mvm2nZRk&pHFyXxDy>qvLbj08^ljW5_Yw@tqIM?ds4B0iqU
zMf5lzvCD|P6aYR$J%C!Q>cMZLaB9FmxG;cr?~428{yL;b_+2CbC*+yxks-zBUnwxe
z(<#e5M+5<;Hw)sWFF3!7QUip47vHFW?KHXT)UTZ1H$BzVquvc5iGakK{MyR3rWC7!
zpWKVoU7w*a&+0OrvH<`gd80Vi^XN+Cz&BY`kLtLX!DP4gz-UJtlbK%}yY4o8@pHX}
zm|%pFfj9xED2CS}HnWW0ja!4=?D#}Gd?WOB%(9h@V@>d1CzY$7`$~pjbaHWM_-<d;
zc&BXYHL#-+nszqefvB&3$m>vp6>7Pm>q5`snU2(g?~q`9UiYi?C$*m(ZHikGh2qGL
z_AoKE42Yq)K~6b`dpg4<Hg5)!tz<BCvm`H<(`7@87S;I$Gqc1$=ZYES%=|8q97}kJ
zj08N=>B?*B`>niVULR>~b(I`{7n~ArC?uNPfRBYZ>g5+8D6u!NnB+Wf9Ejy;0lg*g
zIH&Jf>BfzTo~Qrg0ot{2*#6w~?srskl=lM~ozdKUef<e}Vx_B5&Ty|5lDbt;<)r39
zd{tXwT-}xKRQKBFhzW=oGcRLqty%Q2p&`<ZgedgpDg=k^X9Lp}7NOt)KLO1OXpS+<
zCdg1+4(vgK{t)Xy6@#L_yq+}P$5OUz+2X8gYXz8}NhZDy5LwlsqEATBnoQPl36EcL
zRr_je_dF5x+;WcBk)q1CL$J|c5*2<~;<@=nf2R*D_^!EVkyb0=n~TD#-fi1V>zkh|
z6ZYMscjn$i3`$ic?JVFr8}!b7U5ZcZiQiAzb|asnl2||BtS&H$-Imx^GhbA-p?~s>
z2e$G`Fu-f^vD7+v%`3i9v8HK#$>!MAB^*3-Vow*N=83UPZ>M)+@U{t4CY9o}o$tAI
zW~&qq3&{<M@aCHEM|`8fo#M#$ma{G7%m-V`p#cD?jfgWihxWxJxBmaa{r}0^@QU+@
z#dySYGvq6&`AI1~^GJMtg@=PTca)Um$JS{tf7zC;!dyGJSLl==Rfn&80fWXlibCsp
zzf>6}w~ktrBP7(NZ|?W4&5lZCPioW{BYGa-mM|`B7wjI;{@G*c-+=w&rBNHo&!!BN
z&Oh+rSv9IUu4Q+Y?m=jK!)Z^qp(u3$;lJvZWb3CUCs$9EQ=?W%&Q4LXAbW37_}}}&
zIUm7J!In!3`&gL#_EUR3Q&Q}-LELBeNLF=5p)bA%h@C<&Ra$o|ZtmCoekF^bZ`<KC
znuwE)*he~JjeBsr@z7jxRW5Uro8?MTNxE0k&nwl)5+=ntDur|v48?zf7{xV~*fo}(
zT5r35x>q->M>O~&^=3*+$uo%?4UBPxm>v#|LmMi_KP&3KeZzup&YQ+K9Ojm?qQY%y
zI@LTxdG9G#WR_GTF%6%Xhgq(*+sVbVe-#9&W2~qF_s^srv=4)VztRQua8ZxJ^KeTf
zr9V>y667EcdSfy$@pid?VCgEdRZoX;%;;m*l1@qB&!TwycaaJ&|M`S3k;nta;&1EC
z1-6ZJ@vjCX$?K96^8IP=3JiK!50!YUrrv)0w1Aqb8u|UZ3%I)@whGPOM^^^6m3I6Y
zlTMD&Xn%c^eXd$+T*D2uXY9;utjdE(dnT?K)#!<MUdkzw@2z;z4{>FnW5!BKuoADK
zSbBc@N1=#VAdjr8p-KG+3+&1$=sr-E+*QUFqeLmm*}Hpk8lqvTUHYS{nTi1da@l5Q
zDr9%+D^gDi=dm4l6dC)djJ5eKS~^SSbMyAcYe2mgE2`CU+5EbtK7G9~OW@vcY+T$M
z<GvoP)aAC1cDZ9qEhcd(-d0~8#qKlDAe*|fDx?mh%&)vp;te|xuWP4&g#PMvN9Q7E
z__GyY-4<Athq_BErr+?GIK>9~(VmZPZ^3MqNW#VPxc1hy2uglug_3M8%q6?xreCzd
z5pmDpKxJPKRvz28PKUT>2nlzZYnH5iR4K&`wbxXR3cV+?lUB2D&Pi`{eEF_cQuliA
zi2%keS%@_v?)6bnu6$J0Fh^o!dv>#MJ$t&J$Itn9g<!;f?rN@QvUWWkm!1ChJbLIU
zn2_etN1W3Hw!9$#Iq=;vC<5M{ZuRfzoEN)cCn&}@laTK`CZn!=8R9#OjvEX4`giiL
z{~AHW^fA`}+r7K!HG>!L-p{|}C%XUk!#1WM!YD1~)mn)Mav~Zcm4y&aAh66Mqt~Nt
z1F<fUs5OMx+#~=N%4IRWS7Cp@M)pbIIGgkvbTq$!hdyjx7cKJGch<=f6?8Mdwy-=u
z_vLAf?4rCW;7pEz-m}V}eVBUybqjj;oj?!^$Em@c<}O+-z(#k|%MjBx1RykPVxOV{
zO}pmXa|gZ&W!Fu8>$t!%?+~b&BG{p^Uu`j<!rGfo*s1Xse&-FQG#ISsW>kYMg4og<
zC(tGK`|%%Z+lAgWm}EB`j4ovb+)3oEGwgu^0gVfMObFKC^U5~|UTgzabQ}`px#@3I
zPz`MsiV6tYJ$8I73<q936t_mQ@K%V!_yMU}>({{2(tYe6;)6(K2SW;GpZdK5%X8A`
zw;jG%y+IrAhDOtG9<pisvwoO<xx3a36;)3Y^3fCVYBjirk+3z0qbz<$Y$Cz84^ap0
z&`u8sA@TSNsEDx&`h&1<>e27L`>SM<V&jazeIa#3UMl8}%6+aEmXk8CfslYnj6VHR
zV4Bx4F1j)NS!Mt12xc77a|%MB!mLrbg}XzZwCjiAof^V0%{7=a=A8n*8j1dZ+ANR_
z-1w0K4S-%S>`JsL7NVQ9>rmd1P5~Z3H_xm;472G#z1v5)EgYcHM0k0;>`K+EhH#Z<
zi1IU_Z!{KiK<SNNU7`ftHw*3)fYqOUlFoFwpR2QAlw|y!C%qahZ=_gDjQQraM)1MI
zk#Vu1#wjzozH*DL7E)fW#c9rHC!ZbX+Zbnum*10DF;!nbW2P4ffS^CxL2uu^?fNVu
zUL9U+>$Q0CH=iH_oV;8Y93*YxI6^v2DXn;Nzsfq?xxEO{>fH9Vdj#N5s#l3&wZxjR
z{of{okH2CZGA<7Wa%PdTMGqH<rWy~NNbw^c9&)4^uD>(!S&mk6HT^AIZaY`1iIcHn
z5pxEbppd9LT?)2CGCayU5DR&EaT&1*aZ@YhniyYE*r*dUb|8-JkHYfHz6l)560VPO
z^>vVPoGh{cJS=*3-SV<tzA_!FyKh6CCue|bDP+y5<L2S<bd0;Y!}$w|Buh`f=Nx*b
zqexm#Y?vx?EPB2=;F9fMRdkAF%X9gsu9I+C;;HR!%wE-)k+$EEI>fR5zV%&IblF#U
zqH-M*&lV?Itj$2rQzECbuKW)3uN0rfsp*cT?RlP~*h(9vd)r*Z@a+@(ddbf}ji29k
z7QB0X#-UaAQpT~aiy*ud>fF*Dd#ocQPQ-eefVB8(iloP&n3Ks+ipO`JJbPQ3wukfT
zw9g=nH{=6lw(**a)+QhCqOzpfJ=j>j;IH{{_mDR|UPpvd9<cN|m$Rjxlca&O@=a1a
zrfg-4wg(K%PpXj8IV-fvg_P$fcqfLbcQ2|qNY>S;$TurPApn-<EM}2^yq_exVb9{F
z@l=Pa&6}s~^VL_E^6J*eyUE<}cPUEo<xeZ>HLgwP`aKXAHK85fvr$R!Z`YqXo1)cU
zcK*mM86te2!AB~p)|sZSHcye4@uEUi7>*C}TqJB1(}B8fEbD)%MMS7v<@wQOssO_U
zx&7q>^9DNs1(tyavG!GLKbLc;8CfUh6s&yU*&mD%=`GzO00&@VkF>1019KWvF-yLv
z`Sl6c^x(S4kXz)k&z|+0-w4p5oRNaY2}Chv1u{MAuQrQfQ<lP+Nv~05m=cZGbp`#=
z^Q2JB3ibWtvkxX?awa_{MUc1$I~g>JzatbCaPNHi-R3ptLN;?ha{J76t|p9h{Wgt!
z1{n|sFmZjMy*;YkwpEyrI29E)OmzQVEJ-SjM*apu`itY*%GJt%pEdyb`F5TuRrM2}
zl`JETB#EAW_;B`J&g*>DlP7b+gH&!m2p8u0rWDO}Gxlnq(9+Ck8LkbM?bbndt&rQo
zBfKDOL(>T`AZe<h-u8e1*a5zFU_8CEb#Zk_=Hlvt3-h^XmXwW#dpB#V>v=CJ`J<z}
zFs0?)W!kHz&Mxu(Yl!QqqUCPcQf=H&U=gvwQjLH<H=x2+>2t8{<rD~;b4S<?t)XCO
zp2`8U*9mQBq_{nCO^A>xce8KW-?kRQ{UF2&7Ag%-v)yXrnDR`01VMzxeg2kOp=O`k
zih>lnlDZU2b^(=5w#%o_@LVeYdRH0y9&rn<;WJl!M&`wU&z-R%YQ+GycLX3>0tbJd
zK_k&*vIPY+VlaUy{14u^f6s^#i?E>rm`bk+2(Uy>g$f9>TeX~Zlczgc-P4(Ym&|r9
zoAIpOPl(PHH9CLU=&B3aLM}7yeT;O?l&ODkz^Uh<>l+I-rkdU4{<m9kg;E1r7ld8J
z*#?W}qury*=_5Ap9mBHXSEFvZOiroD24W35K%bh!LP1uXaY$!FD8AI{r;ZO*jeqF)
zA(*#GxE&|#_VkQ0R|AFc25fxT1U-b>9DwRRachuJi*V3K{3r%M#f4wRfZ|_`{4NHB
zV5)vJg89=3=0_tFzZ$7aD>XKpwncv;t}Lm2xUaTlkvbP8N$M|JG(6+qa^g38Ad;tR
zzXY)gV9hMdFi1I!N#Dp(cdM1uW5uCnys@sUjl*dTi`xG(h<_y$!qsqGzd6Q)#PuUw
zAQh%135$hiTWQuUAOBMKKnztW2w$uRtCC(LE!4esPhb@XK;P0;<=05l#b{@+?=YS<
zZi)IzVQ9P{QRhePA#1FstnMppTpWoW^;e3vlKap@2loHIlJCcriO9n$Mb?-`nOl>_
zx&VBC4Zaz#TmiNEw5{rfM^L90c?{nur6doFwsx+LLaji9-HMHFmiFB8s(_5o_T|kF
zea3gaqMQ1;WZD`!g4j%x;s@~#%WhJGQM456r$<MIdnY;&@8>fM)w^{K99DBa8-f%@
z?i0<74vPLosM)(b)uBUcqsTEm6ZHH+?q1(|)~Iai6KUBFmKP#>($TBR)lYq;j!AR3
zT{fsakcL*;T)a<*aXo>hSq1&(ppZU*v&Hn$+OW4=tAlSXH7>0}NvUnf>zL-=O++v`
zP9M=6eV-mHE`p`igi>1s&hPL59p+xFsNuHr$5W&%J0>81jt(ccS)S;IzC_p?Q7sBc
z!+$KjTa+jz<{s{L!};`#x~`PM4kMJ*`M3K8_0Uh>bBK=FPjy4nM=z@0O3)b{j2~U}
zz>wqR*<u+fCgN{tCjXuzCk)u*R>W=xrh-O8SH3DfYq_3s_mu_B3;pY@?2o$Kv9`~#
z;=BV@=h3XnFJaK{L@KY0RfuGMUc}#m-R#Gd(^@xI$9f{Y6&FXsWQ(qhm7~TUz}EcT
zeeZ<4Hp3^mtRT}^huPOgXL5~s-#b&hhLjDc0R<Z$+UYB6wwPAg7hE@dvw39T<Qz!I
zk>bKN!c_7MK*EoOnwPj9Q`9D#N4QS)$jc&ns%)gTGG|$>os=zz4aBOaT32+oyzfP@
zq<p<!<LS<A(PhjmVd=r&?yMa_XLn5X_fL_+%vwc>mJL|)#X~Ins=u)7@SJ`B0k4p8
zH12x&)qbgu8C*zvV8JprJN6agO{dDDWRFv0?u_x^Y{wav{s$Hgio}1g8v@^B$Y9jg
zC9gv89i*jSdG_|+E`H=F2<tzW@W3WRuOp4<w`YGy&g2(Lo#u^>3_h&*hWJc!5It|q
zU{xGZa3ROfgYkqR0XRKlG<0xAYsD?4uqY+<le#>cKO2BZbUVYa^ouKyTv3kp=y=zw
zSZbn(Xe(a2=(C{18gj$z;c#?$MQ`~W;=v>$WM4unZRhGo&%PH0?JG>ztvK1Dg6q^i
zK~agZPibs2Ol8GIp1vxTCqrOcl<3TzV`&w1GQ_;Z=PI7lh}V=shYUM&$;ENxwj}&-
zIicnL(9+B!kR5Wo>2|u3BW@B-D}DtwVkcd*PL1{Ik8{5=RPJ2dlz_N}L2WifeB86G
z%za|hy-Pk|{2bRM96Mj2Bm6dBz={Ez-e3M8F+Q58!(@Gr3Ro_3+9(pVs1BCrj4&n6
zD%C<<?j`NYZ{y{#cBSQVZ0+d}IB6ABFAQGRffR>2Ju|Fw#!oegI;&T+=fX^T19{&P
z?@z(WcAEjZ$hG7{r4Q8)LBn)hmW*X<gQc+SKI`M35|eneg7W2t5dO6@zZ(*IBlGI+
z(s)U^J36OhAJ^KX?n=H0mmtA54!^zE0@FCD7$nK#B_MMkShdAL=W-t+_IAjhnnKF=
zE|PT-h(vlr$&5V3IF#X}Zy~0w186IIAfP{-@m&Dyud4yM0r;WzfE5M&auEBQJa~*P
zfV~NolRs-<*LsOC$q@T|gWMg<Ab(SF9f(tMrjfTuIZ;Nx&ic48-S|^`1MQW-sOI?l
z8%MZYAE;LtcP_aWbJF=K*&Pe}JuJmVEs2kCEuJ66g%E(dht-`+1v{b+ZXD}DScB@8
zw$Nx!?7LYUywr$);9cJMiKVVjRvyP*3CG4Ld<mNuKlQsEDqy~_Px9lBwai*XnoN@%
zeAX5L6t`8iH1#Z?s<G3k{!z&&XG{M((Q~qe!CI~lq|J+p4DxwZf!T)^L~5#hAIbF1
zn`fevA1%Tk832oUzlJbT>-w3H+rek4fg6~K=>25^5ETZ<O+aa=Ec|<)JjH|~d@&c4
z<|XRLo64Pf9{v6n2~;F6MJ=L0Nz{xy1*U2jw@Uz8PUCn1a2SgD>ChUHuTY5qfQ^q#
zqo)Y~i^;bP6089h^o14(hJy@1i1u4kg;Z!I!XrE{@x^LLrs_mNP9|ATGC`v+{^$g&
z5ge`q%CJ%ZxiN?Y!J%5|VK@H2bZzjTURZM978-4$kz$&iMy?tU4rN5yHMBhwfN<!-
z^6tV|wcO3CTQd1Ouo=7$S|@ktjG`Y?ZBfa~gkUXU%FFad%^!cyIf{#SW1G1Fh5)_*
zS07>xQ|Xjv{dyLv8_hdGkH^pLHr{NEUtSXkb91%1X<=#fW04V!TeI$Gt0xkGkdR8{
zY{oHZWD{+7faJ<=F#W|o{}(q5#7)2vo&X9TU_#|psG_Nv62O`LpjcGu!MU(Gnfsh}
z2Rj9|q03MCh!E<7a@`M;qc=jiBz9GHFrZ0CO0ittXK2vYFaBP<F5DeF`t=LR1`_YH
z?1ANkSxR7OO!f~nu-k~{+=*8IE(#0_3cZcoT{>L&`dMMEx6R${n6=79dL17m@8Pj=
zg|$A_&)$OBXgZ}%xsC11{>sI+GknGR{Hiig9{#rq<L$M}E$a^hvF7N0S}auR`mXIu
zdkOPS@xLfbI{k;Tr1p^0hLdR9Onf_)9*;fCH@DSwmA;C%%>~BeEj&gGFaIG>{A1Yo
zwMY8+$Wv0qIaY;n?}GUxGiU+Q9HcW`MuPjIt{u>Nq^~~;{1Oo6sIgQWQpdj){QvC#
z$MgRi`-M(2)kDEbJY`yTqQf)pE%?W1(2oa_+O1<6*G?nR%W(W5(*1QbfVS)|qURL>
z*geF$qdoiEND!X_l$+&Ds`wr6A}!BZdo4^S3273=YBDm9`Xc2fkCEN;aTN0(zUq24
zR`C7^y#Dr1&vx#*HA^hdog!4&R7hW27PJD*S2C`+BxQ8pS)-6%xeDn#7kUw66<?xw
zTvn%NTQp5YF6&kp-?V;*ZMxN)0dM|G+N#q$s{{Lf(sc80U-7xQ^X@~EP>(n2u#n)6
z<@FR^jPX7^(louhgKCbpqWSnA4}9ohzYIwwi(j3v2h+346(OdJu|s<F{S3gc)wT1T
zC$%l(d#Ql;sJ9Ys59{bJPEkM?BUc;>5@qZ=d1RKt66<1aLx31V25<vcG2d(Mqg2n!
z#8eeV*I)5ZC_2q<WhH5<JRH*m1#Pa!oT43!D{5Eg+rz>dB!(2(bQq~F2}CUY4Xb}o
zK@v_KC<%HAegUicl=Bl66CEx8)7mhS%-GCWV|7ZY!~8ttw6!~ZYejR(TSn{JsMx{(
z@SB*77*`s~4PN4Fb;(K+akBdv$0k}k4YQ*z-Okr+o(8MjZx`NYN|^D})O5^}b(PDk
zceF6S7JFu+Ol~ZL;x_$=UWzBBEeSb@n~Et<Rg{9Q`y!X%rRpi#;DAu-0Y{?y^H6y*
zHY!sFKnutqg^Z;OP@-(z&9KzCf5v@Zqv{m#>FQV?vydtQvW|LfvMcS46dMs>Rhup(
zf;_Geb>Ni$lWS7}i98d#S&<?2VL;ji!55oX3lLMJx5$F7vsmehY!e5ZZVLy!q2+tL
zB(!(Jg#5`rwIsZKJ6><=Ayn9#J>P==ge%Nwy<AHGYEg**s}W+-q&<LE5dLG&0O5k*
z2aLn<efzhF4{HLrJR)bI2r2;L6Y@tNEz3W7znwwW%uAx7TsOXcuO)atesvOI(p@K@
z)V+93%J|Mmk=h}3bWn=kjbWV(1A>Y&of;Akn4$l$_>EBSd<|fC@V;GS^KBu3^S4dM
z!m$_m-n5SF+T?AE^{M+YCCm;n6uPm0m^?;qDjNwr*m>{4nDQ)Zae-1}0~H$b9jZm5
zkOQUH_#X@<t9$k_4Go$?hz}8HEg%O%5w@Q{SIhxOgs1|~t26k-5A5V6)akT`HI(;%
zy3eA!OnIw4kR=KVss>+dG|!ASPdg{#uK{|#D)D;{Xfd|LG$G`FLK8IT$VdZYoUZ|@
zNp3vf9bxNkP!%<MylTl>ON2Q}YQrHj<TigA)hY*k%G9s$Rg;I%4;uUr=(aSK6>Chx
zq^<zPbgIyn6mk9w-HSJlbzJ7-cQ;FA%^Jmy5Dm)xdFY`76JU86lm_kRmjCh}#p)+O
zaTv{Wr)Q=MP?jemdPHi;n$b%xyAvC*9~#dD@dby>v>QvEHZ*y;hRJDn3;kJM*c!62
zTvxsV_UwUotGZN?;-waL-Q%3RBWB${y>^TE7SVO$Lkbjdm_X!^2S-yRyAE9X%DzPa
z-XPv1r00tT?>VV?h!>^~Lf#;MW5>2X$PAQnhoxJXLmi|WH-6`!L^*FEfa7<c9u9)G
zA=K<VViMLlyzP^$y`)(Z$i6BX+7~5DE4o37bcpXm@Lv$17@ylVX%7#2g4IqB-`8|5
z^DEJ~hRdtHLp>PM>(5|x9%dguv6VSviq(F;es~^fmo(_FZDfW?_EVm}_Fit_f?MqN
zr_;pO8lc5!9N}Tj-Ju93LC0>0+-%HJ)Pkrq*P+O?zPAz{7WU<$zm!TUAlHrS8;W)-
zJ(UUwIjoK&wiy7w62vC>IDi(aq!WPNTO<bFuuVGu0$yTifbtk68=X&}Jk#lC)F(DV
zZ*Y3D#2V8TX1}uH^%S%q0bGog4%3Ij?_uZv%;ngq098QI`N%fY%Lai+fi&*Knz8%3
zB9T)@QH!WOKQiz2smT@9_NMLNztF2CSW0e-*w43@%m!MFlkT)92IzTlMQ^gVl-dJP
zKgY+hG(Ax6H2wpw%LxKM^&sXY;lIrKjgW^d6vhzAP_W{xke><(l;N+X8LiF3zQ5)*
zr8W2}eU#6aAHR0#z&zmGjirfAVpLwnk$&y?4aq}9gBFn-OA$J6Mf=5%mqcjRtEcmo
z@7I7ce`5FT((ALw0}j837V;7uCQmn^E|=#9<Y0UIfna?u_(VV&MREZ~KQM9_E`F>_
z`Mn`Zym&A|0K9rhe!Q-T7~<{eUIL&x9<|^hE&|P<zgzLgG!iEb2=$?iAGY8SW^zK>
jqT>AE;m@?G-Lc_N?^PZ?go%1LdickC{b&PD82i5feo4$A

literal 0
HcmV?d00001

diff --git a/src/gui/main_window.py b/src/gui/main_window.py
index 02338f9..098a69d 100644
--- a/src/gui/main_window.py
+++ b/src/gui/main_window.py
@@ -256,7 +256,14 @@ class MainWindow:
             # Clear current environment
             if self.environment:
                 self.environment.cleanup()
-            # Note: Robot cleanup is handled by ArmController internally
+            
+            # Clear current robot
+            if self.arm_controller and hasattr(self.arm_controller, 'robot_loader') and self.arm_controller.robot_loader:
+                if self.arm_controller.robot_loader.robot_id is not None:
+                    try:
+                        p.removeBody(self.arm_controller.robot_loader.robot_id, physicsClientId=self.physics_client)
+                    except:
+                        pass  # Robot might already be removed
             
             # Create new ConfigLoader with updated configuration
             self.config_loader = ConfigLoader()
diff --git a/src/robot/robot_loader.py b/src/robot/robot_loader.py
index 5a40bd6..cb77070 100644
--- a/src/robot/robot_loader.py
+++ b/src/robot/robot_loader.py
@@ -26,6 +26,14 @@ class RobotLoader:
         
     def load_robot(self) -> int:
         """Load robot URDF model into pybullet simulation"""
+        # Clear any existing robot
+        if self.robot_id is not None:
+            try:
+                p.removeBody(self.robot_id, physicsClientId=self.physics_client)
+            except:
+                pass  # Robot might already be removed
+            self.robot_id = None
+        
         model_path = self.robot_config['model_path']
         
         if not os.path.exists(model_path):
diff --git a/src/simulation/environment.py b/src/simulation/environment.py
index 2f998a7..ec8409b 100644
--- a/src/simulation/environment.py
+++ b/src/simulation/environment.py
@@ -20,6 +20,7 @@ class Environment:
         self.transport_object_id: Optional[int] = None
         self.ground_plane_id: Optional[int] = None
         self._wall_part_ids: List[int] = []  # 记录所有墙体部件ID
+        self.task_marker_ids: Dict[str, int] = {}  # 存储A/B点标记ID
         
     def setup_simulation(self) -> None:
         """Setup basic simulation environment"""
@@ -60,6 +61,15 @@ class Environment:
         self._wall_part_ids = []
         self.wall_id = None
     
+    def _clear_task_markers(self) -> None:
+        """Clear existing task markers"""
+        for marker_id in self.task_marker_ids.values():
+            try:
+                p.removeBody(marker_id, physicsClientId=self.physics_client)
+            except:
+                pass  # Marker might already be removed
+        self.task_marker_ids.clear()
+    
     def create_wall_with_hole(self) -> List[int]:
         """Create wall with hole using:
            - Top/Bottom: full wall width
@@ -258,10 +268,14 @@ class Environment:
         # Create transport object
         transport_obj_id = self.create_transport_object()
         
+        # Create task point markers
+        task_markers = self.create_task_markers()
+        
         return {
             'ground_plane_id': ground_id,
             'wall_parts': wall_parts,
-            'transport_object_id': transport_obj_id
+            'transport_object_id': transport_obj_id,
+            'task_markers': task_markers
         }
     
     def get_transport_object_pose(self) -> Tuple[List[float], List[float]]:
@@ -335,6 +349,60 @@ class Environment:
             'dimensions': self.wall_config['dimensions']
         }
     
+    def create_task_markers(self) -> Dict[str, int]:
+        """Create visual markers for task points A and B"""
+        # Clear existing markers
+        self._clear_task_markers()
+        
+        # Get task points from config
+        task_points = self.config_loader.get_task_points()
+        point_a_pos = task_points['point_A']['position']
+        point_b_pos = task_points['point_B']['position']
+        
+        # Create point A marker (green sphere)
+        a_visual = p.createVisualShape(
+            p.GEOM_SPHERE,
+            radius=0.05,
+            rgbaColor=[0.0, 1.0, 0.0, 0.8],  # Semi-transparent green
+            physicsClientId=self.physics_client
+        )
+        a_collision = p.createCollisionShape(
+            p.GEOM_SPHERE,
+            radius=0.005,  # Very small collision volume to avoid interference
+            physicsClientId=self.physics_client
+        )
+        a_id = p.createMultiBody(
+            baseMass=0,  # Static object
+            baseCollisionShapeIndex=a_collision,
+            baseVisualShapeIndex=a_visual,
+            basePosition=point_a_pos,
+            physicsClientId=self.physics_client
+        )
+        self.task_marker_ids['point_A'] = a_id
+        
+        # Create point B marker (red sphere)
+        b_visual = p.createVisualShape(
+            p.GEOM_SPHERE,
+            radius=0.05,
+            rgbaColor=[1.0, 0.0, 0.0, 0.8],  # Semi-transparent red
+            physicsClientId=self.physics_client
+        )
+        b_collision = p.createCollisionShape(
+            p.GEOM_SPHERE,
+            radius=0.005,
+            physicsClientId=self.physics_client
+        )
+        b_id = p.createMultiBody(
+            baseMass=0,
+            baseCollisionShapeIndex=b_collision,
+            baseVisualShapeIndex=b_visual,
+            basePosition=point_b_pos,
+            physicsClientId=self.physics_client
+        )
+        self.task_marker_ids['point_B'] = b_id
+        
+        return self.task_marker_ids
+    
     def reset_environment(self) -> None:
         """Reset environment to initial state"""
         if self.transport_object_id is not None:
@@ -344,7 +412,10 @@ class Environment:
     
     def cleanup(self) -> None:
         """Clean up environment objects"""
-        # Clean up wall parts first
+        # Clean up task markers first
+        self._clear_task_markers()
+        
+        # Clean up wall parts
         self._clear_wall()
         
         objects_to_remove = []