orisys-tech/orisys-app
3
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

修正朝上判断、接触高度与过期姿态目标

Browse Source 
朝上判断误用平台轴、接触高度报旋转前数值、多点对准复用缓存姿态，且多点会话被复核入口拒绝；
现按修正计划改正并接通 autocal-multipoint-1.0 只读复核，缺失帧源如实报告而不补零。

离线验证：新回归在 dc5a6f8 上失败、此处通过，panel_check 通过，两截图点接触高度复现
19.0510 / 21.5781 mm；实机项未做，详见 .scratch/autocal-motion-fix/verification.md。
This commit is contained in:
ykcheungab
2026-10-15 06:52:28 +08:00
parent 5339624ffa
commit bcc55a3b97
29 changed files with 2179 additions and 209 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/autocal/docs/inverse-kinematics.md
+79 -40
View File
@@ -17,7 +17,7 @@ the contact point ends up, and Z is reported, never solved.
|---|---|---|
| Tool axis | `z` | `motion/frames.py` |
| Yaw axis | `z`, travel **±180°** | `YAW_AXIS`, `finger_yaw_limit_deg` |
| Pitch axis | `y`, travel 360° | `PITCH_AXIS`, `finger_pitch_limit_deg` |
| Pitch axis | `y`, travel **90°** | `PITCH_AXIS`, `finger_pitch_limit_deg` |
| Stage order | `R_pitch @ R_yaw` | `stage_rotation` |
| Mount map | sensor `x→x, y→z, z→-y` | `AXIS_MAP` |
| Aim direction | `(0, 0, +1)` in the platform frame | `approach_sign = +1` |
@@ -29,9 +29,10 @@ tool feeds **down** onto it. So the sensing face — and the surface normal —
along **`+Z`**, and the solver's aim is to bring the normal onto `+Z`.
`approach_sign` was **`-1`**, aiming every normal at `-Z` and asking the machine to
turn the sensor over. It survived because the residual is sign-blind
(`arccos` maps a face turned fully over to `0.0000°`, the same as perfectly
aligned) and because every test asserted only that the residual was *small*. The
turn the sensor over. Nothing caught it, because the residual is measured against
the aim vector itself: `acos(turned · tool_direction)` reports `0.0000°` for a face
that landed *on the aim*, and the aim had been flipped — so a pose turned fully over
scored as perfect. Every test asserted only that the residual was *small*. The
failure was visible **only** as the model flipping upside down.
Measured over the export, counting vertices whose face ends up pointing away from
@@ -40,12 +41,15 @@ the tool:
| `approach_sign` | Face ends inverted | Nominal pose (`yaw = pitch = 0`) |
|---|---|---|
| `-1` (was) | **981 / 1032** | pitch **180°** needed; lands on `(0, 0, -1)` |
| `+1` (now) | **22 / 1032** | **no rotation**; lands on `(0, 0, +1)` |
| `+1` (now) | **0 / 1032** | **no rotation**; lands on `(0, 0, +1)` |
The 22 are the rim of the dome: their normals sit 60–90° off the tool axis, so
aligning them necessarily rolls the body about that normal and the sign is
decided by rounding. They are edge-on, not turned over — the most negative
`uprightness` among them is a hair past zero, not `-1`.
An earlier revision of this document reported 22 vertices not `upright` under `+1`.
That count came from `uprightness` comparing a **platform Y** direction — a
horizontal axis — so a pose that merely rolled the body about the vertical was
reported as turned over. `motion-correction-plan.zh.md` task 1 replaced the
reference with the post-mount nominal up (the tool axis) in the same frame; with
that reference the exported model reports `upright` for **all 1032 vertices**, and
the corrected flag does not change which branch any of them selects.
The nominal pose needing **no rotation at all** is the tell: a machine that has to
make a half-turn to reach its own commissioned configuration has the polarity
@@ -127,9 +131,9 @@ The closed form is **~200,000× more accurate** and **~7× faster**. Verified at
That residual is the floating-point floor: `acos` near 1 loses about `sqrt(eps)`
in radians, so "exact" cannot be reported as more precise than this.
All 1032 vertices are **in range**, and 1010 report `upright`. The 22 that do not
are the rim of the dome, where the normal is 60–90° off the tool axis — see
*the face is aimed at +Z* above and *still open* below.
All 1032 vertices are **in range**, and all 1032 report `upright` under the
tool-axis reference task 1 introduced. Reachability is the yaw limit, not the rim:
see *the face is aimed at +Z* above and *the two exact poses of one normal* below.
### The two pitch bindings are one family
@@ -154,43 +158,78 @@ everything the machine needs to know:
| Field | Meaning |
|---|---|
| `residual_deg` | angle between the rotated normal and the tool direction |
| `upright` | whether the pose leaves the sensing face pointing along the tool |
| `upright` | whether the mounted body's up axis still points along the tool |
| `in_range` | whether the pose is inside the stage travel |
| `reason` | why not, when it is not |
| `mounted_point_mm` | the picked point after mounting, before the stages turn |
| `contact_z_mm` | the contact height **after** the stages turn |
| `contact_point_mm` | where the move ends: `(tool_x, tool_y, contact_z_mm)` |
| `branch_reason` | which key chose this branch among the exact ones |
A pose that aligns perfectly and turns the sensor over is a different thing from
one that aligns perfectly and does not, and the residual alone cannot tell them
apart. Likewise a pose needing more travel than the stage has: `in_range` says so,
and the solver reports the **best pose the limits do allow** with its real
residual, rather than an angle the machine cannot reach or a residual it cannot
achieve.
`upright` used to be measured against the platform's Y axis, which reports a body
rolled about the vertical as turned over; it now compares the tool axis with the
same axis after the stages turn. A pose that aligns perfectly and turns the sensor
over is a different thing from one that aligns perfectly and does not, and the
residual alone cannot tell them apart — but the up test can only do that if it uses
the axis the machine actually calls up.
## History: the sign-blind residual
Three positions belong to one solve and they are not interchangeable: where the
point sits on the mounted sensor, where the stages leave it, and where the tool has
to be. `mounted_point_mm` answers the first, `contact_z_mm`/`contact_point_mm` the
second and third. The contact height is the **rotated** one: mounting a plate or
offsetting the mount shifts the presented point, so the height after the stages
turn is what the press axis has to reach.
The previous residual was `acos(turned · tool_direction)`, which is **sign-blind**.
A rotation putting the normal *on* the tool axis and one putting it *opposite*
both give `|cos| = 1`, so both scored `0.0000°`. That is how an upside-down pose
read as perfect, and it is why this document previously distrusted the solver's
"100% within 0.5°".
### From contact height to a press axis command
A geometry height is not a controller coordinate. `motion/pose_move.py::PressAxis`
holds the measured relation (`zero_mm` is the reading where the tool meets the
table's zero plane, `rising` the drive direction) and turns a contact height into
an axis target only when it is marked `calibrated`. An uncalibrated press axis is
**held** — `press_targets` removes it from the move and reports the role as held —
rather than being commanded with a geometric number. The other four axes move
either way.
## History: a residual that measured the wrong aim
The previous residual was `acos(turned · tool_direction)`, where `tool_direction`
carried `approach_sign`. With the sign wrong, the aim itself pointed at `-Z`, so a
rotation putting the normal *on* that aim and one putting it on `+Z` were not
distinguished: the first scored `0.0000°` because it landed exactly where the
flipped aim asked. It is not that `arccos` cannot tell a direction from its
opposite — `acos(-1)` is `180°` — it is that the reference it measured against had
been inverted. An upside-down pose read as perfect, and that is why this document
previously distrusted the solver's "100% within 0.5°".
The angle itself is correct and is still what `residual_deg` reports; what was
missing was `upright`, which now travels with every pose so the distinction cannot
be lost. The lesson is recorded rather than deleted: a scalar that cannot express
the failure mode it is used to detect will not detect it.
missing was an aim the machine could trust plus `upright`, which now travels with
every pose so the distinction cannot be lost. The lesson is recorded rather than
deleted: a scalar that cannot express the failure mode it is used to detect will
not detect it.
## Still open
## The two exact poses of one normal
**22 of 1032 vertices report not-upright** (16 by more than the tie band). These
are the rim of the dome, where the surface normal is 60–90° from the tool axis.
Aligning an edge-on face necessarily rolls the body about that normal, and which
side of zero it lands on is settled by rounding — the most negative uprightness is
`-0.997` for a vertex whose normal is `90.4°` from the tool axis, i.e. a face being
presented side-on rather than turned over.
Every solvable normal has two exact poses, and they are a **180° roll about the
vertical**, not an upright/inverted pair: `R_branch2 = R_z(180°) @ R_branch1`
agrees with the measured branches to `2.2e-16`. Both leave the body up (the same
tool-axis component), both reach the same contact height, and both put the contact
back under the tool — the table XY differs, because the roll moves the point
sideways and the table compensates.
They are not a solver defect: the closed form is exact for all of them
(`residual < 1.3e-06°`). Whether they matter is a coverage question — a contact
made on the rim is the least well-conditioned one on the patch anyway — and it is
worth a look at the bench rather than a change in the maths.
So the choice is a machine convention, and the solver states it instead of hiding
it: among the branches with the best residual (within `1e-6°`), it prefers one that
is in range, then upright, then the smaller total travel `|yaw| + |pitch|`, and
`Pose.branch_reason` reports which of those keys decided (`only`, `alignment`,
`in_range`, `upright`, `smallest`). An equivalent solution never wins on a
floating-point residue.
On the two bench screenshots this picks `yaw +36.42° / pitch −47.69°` on the right
and `yaw −61.32° / pitch +45.64°` on the left; the other exact pair is
`−143.58° / +47.69°` and `+118.68° / −45.64°`. A fixed approach sign on the rim
(which of the two the operator wants there) still needs the platform lead's rule —
it is not something the solver may invent, and it is not derivable from the UV
position. A contact made on the rim is the least well-conditioned one on the patch
anyway, so it is worth a look at the bench rather than a change in the maths.
The polarity question that used to live here is **settled**: see *the face is aimed
at +Z* above.
@@ -212,7 +251,7 @@ normals, points = bundle.vertex_normals(), bundle.vertices_mm
analytical = [stack.solve(p, n, method="analytical") for p, n in zip(points, normals)]
residuals = np.array([pose.residual_deg for pose in analytical])
print(residuals.max()) # ~1.2e-06
print(sum(pose.upright for pose in analytical), "/", len(analytical)) # 1010 / 1032
print(sum(pose.upright for pose in analytical), "/", len(analytical)) # 1032 / 1032
print(all(pose.in_range for pose in analytical)) # True
```