ykcheungab
243 lines
9.9 KiB
Markdown
243 lines
9.9 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°** | `YAW_AXIS`, `finger_yaw_limit_deg` |
|
||
| Pitch axis | `y`, travel 360° | `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` |
|
||
|
||
### 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. 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
|
||
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) | **22 / 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`.
|
||
|
||
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 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.
|
||
|
||
### The two pitch bindings are one family
|
||
|
||
`PITCH_AXIS` is `y`, while the trunnion drawing shows the pitch arc about `x`.
|
||
This turns out not to matter:
|
||
|
||
```
|
||
R_y(p) · R_z(t) ≡ R_x(p) · R_z(t − 90°)
|
||
```
|
||
|
||
agreeing to machine precision in alignment for every yaw and pitch tried. So the
|
||
axis binding is a **naming and zero-offset choice**: the two reach exactly the
|
||
same set of alignments, and the 90° difference is absorbed when the yaw stage is
|
||
zeroed at commissioning. `PITCH_AXIS` is documentation, not a correctness
|
||
question — which is why the tests pin the identity rather than a value.
|
||
|
||
## 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 pose leaves the sensing face pointing along the tool |
|
||
| `in_range` | whether the pose is inside the stage travel |
|
||
| `reason` | why not, when it is not |
|
||
|
||
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.
|
||
|
||
## History: the sign-blind residual
|
||
|
||
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°".
|
||
|
||
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.
|
||
|
||
## Still open
|
||
|
||
**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.
|
||
|
||
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.
|
||
|
||
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)) # 1010 / 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.
|