283 lines
13 KiB
Markdown
283 lines
13 KiB
Markdown
# Inverse kinematics: the simulated 5-axis pose
|
||
|
||
How AutoCal turns a picked surface point into the X, Y, yaw and pitch a run would
|
||
command. The solve is **closed form** — two `atan2` calls, exact to floating
|
||
point, no search.
|
||
|
||
Everything below is measured, not reasoned. The commands that reproduce each
|
||
number are at the end.
|
||
|
||
## The machine
|
||
|
||
An XY table with a yaw stage and a pitch stage stacked on it, and a separate Z
|
||
axis carrying the tool. Yaw and pitch are solved for; X and Y follow from where
|
||
the contact point ends up, and Z is reported, never solved.
|
||
|
||
| Item | Value | Where |
|
||
|---|---|---|
|
||
| Tool axis | `z` | `motion/frames.py` |
|
||
| Yaw axis | `z`, travel **±180°**, positive is clockwise from above | `YAW_AXIS`, `YAW_COMMAND_SIGN` |
|
||
| Pitch axis | `x`, travel **90°**, positive nods the dome toward `−Y` | `PITCH_AXIS`, `PITCH_COMMAND_SIGN` |
|
||
| 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` |
|
||
|
||
### The face is aimed at +Z, and this was wrong
|
||
|
||
The sensor sits at the **bottom of its shell with its sensing face up**, and the
|
||
tool feeds **down** onto it. So the sensing face — and the surface normal — point
|
||
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. 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
|
||
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) | **0 / 1032** | **no rotation**; lands on `(0, 0, +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
|
||
backwards. `test_the_nominal_normal_needs_no_rotation_and_lands_on_plus_z` pins
|
||
this, and `test_the_normal_is_aligned_to_plus_z_not_minus_z` checks the direction
|
||
over the whole export. Both assert the direction landed in, which is the thing the
|
||
residual cannot express.
|
||
|
||
The yaw figure is **±180°, not ±90°** — confirmed at the bench after an earlier
|
||
value of 90 sent this document, and the solver, down a wrong path. With a
|
||
half-turn available, **every normal on the export is reachable**; the reachability
|
||
question below was an artefact of the wrong limit.
|
||
|
||
The solver's whole job is to make
|
||
|
||
```
|
||
R_pitch @ R_yaw @ n_platform == tool_direction
|
||
```
|
||
|
||
and to report the angle by which it fails. That angle is the **residual**.
|
||
|
||
## The closed form
|
||
|
||
### Yaw first, because the pitch cannot help with one component
|
||
|
||
The pitch stage turns about the pitch axis, so it **cannot change the normal's
|
||
component along that axis**. Whatever that component is after the yaw, it stays
|
||
that way through the pitch. So the yaw must bring it onto the tool's:
|
||
|
||
```
|
||
n_pinned(yaw) = tool_pinned
|
||
```
|
||
|
||
That component after a yaw is `a·cos(y) + b·sin(y)` — one sinusoid — so it has at
|
||
most two roots in a turn:
|
||
|
||
```
|
||
y = atan2(b, a) ± acos(tool_pinned / hypot(a, b))
|
||
```
|
||
|
||
The coefficients `a` and `b` are read straight off the rotation primitive, by
|
||
evaluating it at yaw 0 and yaw 90 degrees. No basis is written out by hand: every
|
||
sign error made while deriving this module came from transcribing the rotation
|
||
convention locally, and there is no convention left to transcribe wrong this way.
|
||
|
||
If `|tool_pinned| > hypot(a, b)` the normal is **unreachable at any pitch**, which
|
||
is reported rather than hidden.
|
||
|
||
### Then pitch, in one `atan2`
|
||
|
||
With the yaw fixed, the pitch rotates the remaining pair onto the tool's:
|
||
|
||
```
|
||
pitch = atan2(u₁w₂ − u₂w₁, u₁w₁ + u₂w₂)
|
||
```
|
||
|
||
where `(u₁, u₂)` is the yawed normal and `(w₁, w₂)` the tool, both taken on the
|
||
pair the pitch actually turns.
|
||
|
||
### Two branches
|
||
|
||
Both yaw roots satisfy the first condition. They reach the same normal in
|
||
opposite senses, so one usually leaves the sensing face up and the other turns it
|
||
over. Both are exact; the choice between them is a machine decision, not a
|
||
numerical one.
|
||
|
||
## Results
|
||
|
||
Measured on the real 1032-vertex export
|
||
(`…\orisys-sensor-design\build\export\finger\r01\compute`):
|
||
|
||
| | Worst residual | Time for 1032 | Per solve |
|
||
|---|---|---|---|
|
||
| Closed form (`method="analytical"`) | **1.2e-06°** | **148 ms** | 0.126 ms |
|
||
| Sweep (`method="search"`) | 0.238° | 918 ms | 0.910 ms |
|
||
|
||
The closed form is **~200,000× more accurate** and **~7× faster**. Verified at
|
||
`1.2e-06°` worst over all 1032 vertices and `1.5e-06°` over 5000 random normals.
|
||
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 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 pitch axis is the left-right axis
|
||
|
||
The right-hand identity
|
||
|
||
```
|
||
R_y(p) · R_z(t) ≡ R_x(p) · R_z(t − 90°)
|
||
```
|
||
|
||
still says the two bindings can reach the same alignments. It does not say the
|
||
drive can use either one. Measured from the commissioned zero on 2026-10-15:
|
||
pitch is about platform `x`. An executed pose matched the preview only after
|
||
the pitch sign was reversed, so positive pitch is the right-hand sense about
|
||
`+X` and nods the dome toward the operator. Axis 2 at `+10°` turned the tip clockwise from above, so positive yaw is
|
||
the opposite of the right-hand sense about `+Z`. The solver, the preview and
|
||
the number sent to the drive are that commanded angle.
|
||
|
||
## What the solver reports
|
||
|
||
`Pose` carries more than angles and a residual, because those two cannot express
|
||
everything the machine needs to know:
|
||
|
||
| Field | Meaning |
|
||
|---|---|
|
||
| `residual_deg` | angle between the rotated normal and the tool direction |
|
||
| `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 |
|
||
|
||
`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.
|
||
|
||
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.
|
||
|
||
### 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 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.
|
||
|
||
## The two exact poses of one normal
|
||
|
||
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.
|
||
|
||
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.
|
||
|
||
## Reproducing the numbers
|
||
|
||
Interpreter `C:\Users\cheun\anaconda3\envs\py310\python.exe`, repo root
|
||
`D:\orisys_ws\orisys-app`:
|
||
|
||
```python
|
||
from orisys_autocal.motion.kinematics import PlatformStack
|
||
from orisys_autocal.sensor import load_bundle
|
||
import numpy as np
|
||
|
||
bundle = load_bundle(r"…\finger\r01\compute")
|
||
stack = PlatformStack()
|
||
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)) # 1032 / 1032
|
||
print(all(pose.in_range for pose in analytical)) # True
|
||
```
|
||
|
||
The closed form on a single normal, with no data needed:
|
||
|
||
```python
|
||
unit = np.array([0.3, 0.5, -0.81]); unit /= np.linalg.norm(unit)
|
||
for branch in stack.alignment_branches(unit, stack.tool_direction):
|
||
print(branch.describe())
|
||
```
|
||
|
||
Tests and checks:
|
||
|
||
```
|
||
pytest apps/autocal/tests
|
||
python apps/autocal/tools/panel_check.py
|
||
```
|
||
|
||
## What is not affected
|
||
|
||
The **UV picker** and the **pixel → 3D mapping** are independent of this and are
|
||
verified separately: a click's 3D point, normal and face come from one triangle
|
||
test, and the lookup tables are unchanged by the texture. This document is about
|
||
which pose is reported and drawn.
|
||
|
||
Also out of scope: the sensor origin versus the stage rotation centre, which is a
|
||
lever-arm error in the reported X/Y rather than in the angles.
|