added motion trajectory simulation
This commit is contained in:
ykcheungab
@@ -0,0 +1,242 @@
|
||||
# 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.
|
||||
Reference in New Issue
Block a user