From 855f917d24665b3efb114d42949ac66c4dd5bfad Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Tue, 16 Jun 2026 19:46:33 +0200 Subject: [PATCH] Reihenfolge sinnvoll gestalten --- scripts/5_pose_estimation.py | 33 ++++++++++++++++-- .../5_pose_estimation.cpython-311.pyc | Bin 42147 -> 43578 bytes scripts/robot_1781069752019.json | 2 +- 3 files changed, 31 insertions(+), 4 deletions(-) diff --git a/scripts/5_pose_estimation.py b/scripts/5_pose_estimation.py index 4d181aa..6c23805 100644 --- a/scripts/5_pose_estimation.py +++ b/scripts/5_pose_estimation.py @@ -230,11 +230,29 @@ def analyze_chain(fk: RobotFK) -> Dict[str, Any]: for x in pending: var_block[x] = len(blocks) - 1 + # subtree_markers[L] = L's own markers + all descendants' markers. Lets + # observability() credit a block whose own link saw nothing this capture + # but whose CHILD link did (e.g. Ellbow has no visible markers, but Arm2's + # markers still constrain z through the chain — same idea as 4b's + # Fallback-1, just for confidence reporting here, not for the fit itself). + children: Dict[str, List[str]] = defaultdict(list) + for ln, ld in links.items(): + p = ld.get("parent") + if p: + children[p].append(ln) + subtree_markers: Dict[str, List[int]] = {} + for ln in reversed(topo): + ids = list(link_markers.get(ln, [])) + for c in children.get(ln, []): + ids.extend(subtree_markers.get(c, [])) + subtree_markers[ln] = ids + return { "ordered_vars": ordered_vars, "var_type": var_type, "var_links": dict(var_links), "link_markers": link_markers, + "subtree_markers": subtree_markers, "blocks": blocks, } @@ -518,16 +536,25 @@ def observability(chain: Dict[str, Any], obs: Dict[int, Dict[str, Any]]) -> Dict driven by markers-per-variable in that block: high : >= 2 markers per variable (well over-determined) medium : >= 1 marker per variable - low : fewer markers than variables (under-determined — distrust!) - none : no markers at all (variable left at 0) + low : fewer markers than variables (under-determined — distrust!), + OR no own markers seen but a child link's markers were + (indirect evidence through the chain, e.g. Ellbow via Arm2) + none : no markers at all, not even indirectly (variable left at 0) """ info: Dict[str, Dict[str, Any]] = {} + subtree_markers = chain.get("subtree_markers", {}) for block in chain["blocks"]: seen = [m for m in block["markers"] if m in obs] + indirect = False + if not seen and block["anchor"]: + seen = [m for m in subtree_markers.get(block["anchor"], []) if m in obs] + indirect = bool(seen) nvars = max(1, len(block["vars"])) ratio = len(seen) / nvars if len(seen) == 0: conf = "none" + elif indirect: + conf = "low" # indirect/coupled through a child link, not direct elif ratio >= 2.0: conf = "high" elif ratio >= 1.0: @@ -537,7 +564,7 @@ def observability(chain: Dict[str, Any], obs: Dict[int, Dict[str, Any]]) -> Dict for v in block["vars"]: info[v] = {"observable": len(seen) > 0, "n_markers": len(seen), "block_vars": len(block["vars"]), "confidence": conf, - "block_anchor": block["anchor"]} + "block_anchor": block["anchor"], "indirect": indirect} return info diff --git a/scripts/__pycache__/5_pose_estimation.cpython-311.pyc b/scripts/__pycache__/5_pose_estimation.cpython-311.pyc index 63ef7df50d6f18eb81a8914b057e66fc1e12bc62..eaf25667b6d96cc241a65f28b0defc9e41f4dcb3 100644 GIT binary patch delta 4875 zcma)A4Nz3q6@G7b-(%TbmjC5fd4Pxuh%6c;O5#ru{0qjPXky0TF8hF$Wf$*zfZ!si zX}dVq09PY1)?m{~V~nkK(`htL{%R-WuLVou=0&HOrtS1+qS!P)lhL&2+(l87PWlGF zJ@?#m&pq$lbIv`_r{C0_8Pi#R787IQ;Az|A*m8CIDeLDMx-X+bVrGI!*GkHc(s?5CgV=p#qfcZ4Sf)%xVUmnlr_03s~JThsCv=-5RRQc~oc8#1zg)Om6oQ-g#Cr5W(Q*jb)&|N5o5-<$ff0llQJ(Qrw!WsYxmcNQ>TS93c?Ag<1ywa zi#X0jfDYN;<_D;^AXgj&7a94`fr6uYevr;B&d%xqjwY&puSZesF=POSk+XUNsYA5A zc%yg>$QV9!p*Tuseh%5sBb=arFPSZbfRsYnj(0`gL9fg`koId}*m+WbKYFqmeuxX< z1pRS&CBGo_r}7ehF|Hyt2&mg`Vj4A_qLLHHEJR=ta|kFc%2%tXPE{t%rk8N?Il6A% zJ)mb`UN)cCjY5VdxkDiqoU|jL0W9E%8>yW%JU>Td`|ca4u$h@E_A)Qf?25wT*HP#k z!fydIz2f&WXJ~qPTV2E|w1vEe@DlB;m?OLgWML>&@s^%{k?vUNh*tf*QvVl7$#QCUL%05jwi!Y^r5O||d^kaB2k&AzBR4mO$c>t|)1fznPs zLh)h*cCy=$Vx_K=u3BF+1&=5a5FZ0|$C1+O4-`zE={b6Ny(1r^tA>1l%l-*qkB$WU zq)(ZP2Wi!Yn%Q^w=ca4Oq!pJ@5xOXno{!QuHq7Ebr(bNS5cFXFsZjZYhk47_D8S+C z@O|mCtg97F?5#G zW)_iTXFo*mccyM&iv_3Z^p$Et9q}s)3lvDn)@Bw2T9jLEfV)Sr$CF9>*emm>3l(a- zQ!_Yy4T`2~S2W&3dg&|9R8fcXC+Q{UG}ASFGXyPUb{!YK<2*(@1^gHE>xM&%FmVW` zkUeUm4kF_U0`uk_ITHmFm@n`|{Pa5gpfQ)fMz1%{F|&f9!=*ktICerv{D26-S87g3!j@Qv^o_qO!&?_r*x-s?aR`Ng7t_C&5Al#mDobuK5(<-yE0CkO4IVX1h)Wfkf1C8o;bOJ%w@7sAggD3NIm)7YNc(clv{FQxhI8NAzxUSF|rJAOd9p zDk)%e$22kBz_9|y44l}ToH*d&>;C0(Y7auX;wpz_Et?%Qme89)WMI3OUp z;}44gUBJW|zN$WcuQi~9CNH5?)rGSC`}po66V&(HpH*ulpI`E~`=p6VDk-w;lU%K; zHPM24fymCEQq)H`rws)j+ z?8LkDJCb|J*6FrfI4L4wc__vcn+H%u$p5$4GaVKyS+L~G9&RLYUF2^td)P5@|Jvd5 z;qtykh{NR(-I&~DD*{G`X7KravZnX?+Z$kr&qB@6=xJ!27>+N{#Aey;X>Eqy`<1Qt zbZf>NuWvU*Zc_&=VwYy5VwfXWBUVJs;PKV_Bd);o)eLxcqy$T(;A6MGK%A<_kJqiQ zp4|ls#4smqz8M6pZfP-o7qkU!PFI!3>rp#M8K_9Y_udW!y!Uu2mv8T>4BIjedImfr zlH+9QsI6?wR@SpB9B=E}wm)l}OBQPQs|eeEUOwOMibYKC9VrrUP{RrtnBaH-x*v= zkCE?1CqwHSod&AuU&$O}4G-OZbPZ1>Wme{^OUDIW3bGJ`m3{HO)xqkkdK3uND^7uL zrNi4wGcTe`6A`c=+l5Gbk%EjeYU{|J-H&7m0&8e?uo;XWygdmv2m~8M*5DHCP|3S= zONZpx38dX}OI2mHlx@cv8rGBIKAHhM-bM!KOC1Y@N>DmK^q-EqjQOWg0j~+r9C=#o zID8Evi{ooe(|1J2^o3H)t+(i1Scr zfNrGH61+q-zC+_Xi4D0)xNS0m4FO{ZEJvvb;~AhdJznd@C|-pVh4dAgKLx$tX(%r) zt^bObIxP^CGbXCgJ*0^0_ZKawfEAsl8?|SiMCPN4VN{Wi6ne!jXhNIQ+bU~@R-Z>D zg*d^sDx$vq>!z?JOi>*1yZmb8d!u-Z;`f2K{+mU3SA1nYi(?gGEt;e(^c;1>wWTn-sMtW@*a&&$d z@uO{b0cd*F*+3K=GJDscgwfxs*0wmIaE_oJG)qD6!iRAZei)k#&Gx5Wdwd_VdVNHnQCeFtm%2s+e=TB%9C0c~c}O?=Ykg#o3yOF*t-ZcSo4u z7?wcn#aI?Gsv5Union!kd4_Ojw37}$C1thZ+%nXK-7eXIuo?iGT=~1Am+{KeXzo#;0xOdH9cmZ(xm{E`Iu4@-#kfoVy+F=V|&+d;bfEF9CA^ delta 3895 zcmai13s98T72f;r{{Jp4peP~;DT^rh;{#B1EaBnXI^vRO(|`3OkFj$!Dw3)1kebKnT2pP3w$VH^6Jt6i(m&DEPTMhQ(vWnpxjzwNlW_;W zJ@?#m&pr3N?%h)tr2}WBl&6xD6D|08drirjNXyZb>seBn5?8W^DM~as>eMsgnTJLt zMXk{kUsAWc#G)q0r7@35k}AWt{K*MQT_9B7sHw^DLH>eFJ`OdhX?!()@pTA>O%FAP zcq{y%pkQd2xGe-aK?Gt2`LdRVG|Y6Q!Ji67I5*>9AO33xtnr%#G3%H<#gaL;H!b_} zh|FE;FFe~ly%}SAhh_9Rl_aIlVghWK@Fv>=izenNfNQT?;w=;RSs6e^Np8-1MtzA(;}FO5ajg4qPbP zk$DUmD&vLpW#udIi`NnK!20{=u!-^0_ZPESw2He4NVl3OG#Zko^B!W3ClF5Bfs|nn zdNkdu`?;{`1Wg`-sTCEhIIdOXva#(XWNY-V(|8h1ssv^Na}Y4zeg~vS|}jAVOafsntPt96G8OF&~_-E^&(Z zM@Vcf%nX;Qq)4oIy5EmWh(UI1Rs@%;gHIsNNw~T&Kam1hA8Kl@Y=9>hH4hV34EFye z$Z^kO0a)qY|DbS#@VvwHyyB^A@p8?udg_`C*;D7${QS41dms{GUx(P&g?EG-;}>oItHSxEt< zn44cDHGfA?6U>w!@TqVyKtZj03Wbah_~(vm_<$83kqEiSEYc0oQ(MG7hu+$9`9H}0 zWju3vH%q!n0=Edhvc&)J;||%mn+Ed{K9ZrPJ_x7k(uY%!`cL38GxN{zeti-90EX8e zxzC2)Fs$fHt|0X-a>`}$mjj0S(w&S*kqLXC#Fz1~SS)*WZ*a09*Yl93iL}8g4L6CD zFzi16D(?4dYAPSFRyg1*GL;k>%xl=Z!Bv_ewQ2@y;6H=Cz6|9y%^!i6ePa^8B$D8b z_$R(2@;#z+G*HO?1|P2Jnn|U^si;(Ye{Uql#{|N+B0WVvBa1XU0#y)xOh3H@ryBFv z$8e>w6bq+p*(gXUPk?EGg~g(yXef^zg<|}5OZ^mSq5JNXL8GM47+eliqHPyyM#rcG z)fE1n<@a5c$FI>A6w^V4mpZ8MMCyp77RM9WC7S3zXw5$fKC@VIExeD&^8{w?5Yb@v%&tHa~MLkG#@u?PM#NPkPJ| zwfd}Zv^gusrsImQKR%mIU9$g!GBT`PTVR!SE1ViVDlsazIW3lU#T>eQN~1)g-0HKp z+kJd8ETZ) zB@e9grF6?tDe5Rk@y&BLpN>lSkQfp^%i?GFRtDGfJaC1xScI;HdnD!fmi%!kcod#R zB{T^Y@-hO-A-s(VblEnAXK)FQW9|JS^+CsvkQxGM+voS|3%)74ohE}r9kbN)0>*UEwVkQ&oV@w zE}&C}iNS2C(*}i+*$|CPQjl*=m=4DxMI-xUX(SF2I_GRl-8jE!Q3TJf&z-WF$YO#*f~c4wcTh{?)U`QX-Qw-R+lhJuSj<%- z3M^?*ii^Y++lekJUJJC3VGGr1Eqn_sj#bGOsM!^NA~w}A_BhU|%#iWMYG_73t{_>~ ztdYxI{XOv{eDQcOdl>RMr?D5Hwlmv#250%J1a{caIhV!Y_0C*pB^g{rFa!S8S)wc^ z(hY?hC(E@+&Wn3DUQbq55cg4dWas4}L>l%czwX7W9C-_NarO$lxT_>Kg@mX++cFa8 zYI;KxzNLJwHKB%}?rQM{!hWp`8`mZjFxWbSweeJ1kVY1bAV?>m=;Dz`%4FlXH`RB_i$`$9lNtwK6LM%P&E~$+7ey;H*%Sa zmy&|hq#&0la+fL_varptg@X;c&}?p$5B9DfDq_i0@OGs$J4CuhD<^3A3$V9qo?h&y>wO~Y%y8E*!@O5PY?(@M-wGh??g&NhMI8zKf284~{Y#0y%dq!|C(IczjC{v?34{ueMpaE2&KLCv60&m<7)OK5w^q?d#DkSixda|_63dcp7r!9oOlxZsbKFzh`Pa2QIn zm-~Y{q#iD0uR;0YX))2jfR-I3c#c4HtaphL4NSDMFKH;QPO45$orqIPb4oG8;qe5$ zO@5EZ{MjMyf(f|d-;VO8kS`qYS8_l({Ih6fcvtpW8Do96Viy1W@OhT$W(n!qKNU{tquc(tZE{ diff --git a/scripts/robot_1781069752019.json b/scripts/robot_1781069752019.json index a9d922e..fa5031d 100644 --- a/scripts/robot_1781069752019.json +++ b/scripts/robot_1781069752019.json @@ -288,7 +288,7 @@ "name": "Joint1", "type": "revolute", "axis": [-1, 0, 0], - "origin": [110, 107.457, 37.242], + "origin": [110, 101.1, 55.2], "rotation": [0, 0, 0], "variable": "y" },