orisys-app/apps/autocal/docs/inverse-kinematics.md

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 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

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 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 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:

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:

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.