SO-101
Course overview/Chapter 04 · FK and IK
中文
Chapter 04Kinematics~18 min

FK and IK

Companion pages: Joint Control (where FK shows up) + End-Effector Pose (where IK shows up)

9 sections 1 figures 11 code blocks Forward kinematicsInverse kinematicsIteration
FK and IK

1. A Two-Way Chain, but Only One Direction Is Measured#

text
servo encoder
    │  reading (angle)
    ▼
joint angles ────── FK ──────▶ pose (X / Y / Z / pitch / roll)
    ▲                          │
    └─────────── IK ───────────┘

The two arrows look symmetric, but they are fundamentally different:

  • FK is a computation: given six joint angles, you multiply through the link geometry and there is always exactly one answer, available instantly.
  • IK is a search: given a pose, you work back to joint angles by iterative approximation — it can fail, and there may be more than one solution.

More importantly: the only pose-related quantity the hardware reports is joint angles (voltage, temperature and current are also read, but they are not pose). A pose is never handed to you by a sensor; it is computed by FK.

2. What the Hardware Actually Gives You#

Inside each servo is a magnetic encoder that reports a tick count. lerobot converts that count into an angle using the calibration file, so what starm receives is just these six angles:

python
arm.get_joints()
# {'shoulder_pan': 4.9, 'shoulder_lift': -5.0, ..., 'gripper': 30.0}

The pose, by contrast, is computed on the fly from those six angles:

python
def get_pose(self) -> Pose:
    return self.kinematics.pose(self.get_joints())   # arm.py

There is no such thing as a pose sensor. That has a useful corollary: pose error cannot come from an inaccurate pose measurement. It can only come from two places — inaccurate joint angles (calibration error), or inaccurate link geometry (URDF / TCP offset).

3. FK: Joint Angles → Pose#

How it is computed#

The arm is a serial chain, and each joint contributes one 4×4 transform:

text
T_joint = fixed mounting transform (the origin in the URDF) × rotation θ about that axis

Multiply from the base all the way out to the gripper, then add one fixed TCP offset at the end:

text
T_base→TCP = T_shoulder_pan × T_shoulder_lift × T_elbow_flex × T_wrist_flex × T_wrist_roll × T_TCP_offset

That is the whole of it in code (starm/kinematics.py):

python
def fk(self, joints):
    frames = {"base": np.eye(4)}
    for name in JOINT_NAMES:
        joint = self._joints[name]
        angle = np.radians(joints.get(name, 0.0))
        frames[joint.child] = frames[joint.parent] @ joint.fixed @ _rot_z(angle)
    return frames

Measured at the zero pose#

With all six joints at 0°, the position of each link origin in the base frame (mm):

Link x y z
base 0.0 0.0 0.0
shoulder 20.8 −23.1 94.9
upper_arm 2.5 −53.5 149.1
lower_arm 2.5 −81.5 261.7
wrist 2.5 −216.4 266.9
gripper 20.6 −277.5 266.9
jaw 39.4 −300.9 287.1
TCP 20.4 −381.1 256.8

Note that the TCP sits 80 mm further out in y than the jaw — that is the ~104 mm TCP offset.

How much each joint moves the end effector#

Distance the TCP travels when a single joint is rotated by 10°, with all other joints fixed:

Joint +10° TCP displacement
shoulder_pan 62.4 mm
shoulder_lift 60.1 mm
elbow_flex 52.2 mm
wrist_flex 28.8 mm
wrist_roll 1.8 mm

The first three are easy to understand: they are the long, proximal joints far from the end effector. wrist_roll moves the TCP by only 1.8 mm because spinning about the approach axis barely changes the TCP position — that 1.8 mm comes entirely from the lateral offset of the TCP relative to the wrist axis.

This table also explains why calibration error hurts far more at the shoulder than at the wrist.

4. IK: Pose → Joint Angles#

Why not a closed-form solution#

This arm has 5 DoF, plus geometric offsets and joint limits, so a closed-form expression would be both ugly and prone to trouble at the boundaries. The SDK uses numerical iteration: Levenberg–Marquardt, converging step by step from the current pose.

Four unknowns are solved#

text
unknowns : shoulder_pan, shoulder_lift, elbow_flex, wrist_flex
residuals: x, y, z, vertical component of the approach direction

wrist_roll is not part of the solve — it is simply taken from the target roll. That is how "5 pose components → 5 joint angles" works out:

Pose component Handled by
x, y, z Solved iteratively by the first four joints
pitch Same (it appears through the approach component)
roll Assigned directly to wrist_roll

One detail: the residual does not use the pitch angle#

If pitch were used directly as the residual, pitch = 90° (straight down) would be a singularity — the angle is insensitive to orientation changes there, and the Jacobian degenerates. The code uses the z component of the approach vector instead:

python
target_approach_z = -np.sin(np.radians(target.pitch))
# residual = [position error (3), w × (approach_z - target_approach_z)]

The components of a direction vector are smooth over the whole sphere, including the straight-down point. So pitch=90 solves normally; only the reachable height is limited (about 80 mm above the table).

Two solvers#

The SDK provides two IK implementations. The default is "lm"; you can change it in the config, or specify it per call:

Solver How the orientation residual is computed Best for
"lm" (default) The vertical component of the approach vector, approach_z Most stable for cold starts and distant targets
"riemann" Geodesic rotation error on SO(3) (the angle rotated about the pitch axis) Uniform cost per degree, e.g. straight-down grasps

Why the second one exists: near pitch = 90°, the gradient of the approach_z residual approaches zero — the vertical component no longer responds to pitch, and the solver can only push through by re-seeding. "riemann" expresses the orientation error on SO(3): the error magnitude is exactly the angle the gripper has to rotate, so it has a gradient at any pitch, and the cost per degree does not change with where the arm is pointing.

Both solve the same four unknowns, with an identical interface:

python
arm.ik(pose)                        # default solver from config (ik_solver)
arm.ik(pose, solver="riemann")      # use the geodesic variant this time

When "riemann" fails it falls back to "lm" with the same seed, so choosing it is never worse than the default.

You can switch it in the UI too: the Connection page has an IK Solver card. Pick from the dropdown and the change takes effect without disconnecting and reconnecting — the IK precheck and every move after it use the new solver immediately (it mutates the same config object).

When to switch: the default lm nails most poses on the first try. If you keep hitting Unreachable on straight-down grasps, switching to riemann often solves it — it is not sensitive at pitch = 90.

The seed: start from the current pose#

The solver always starts from the current joint angles. Two benefits:

  1. Incremental motion (straight-line paths, slider drags) keeps the existing configuration instead of suddenly jumping to a different arm posture.
  2. Fast convergence.

If the seed does not converge, a few fixed poses are tried as fallbacks; only when all of them fail does it raise Unreachable.

What failure looks like#

python
arm.move_pose(y=-300, z=230, pitch=90)
# Unreachable: no solution for Pose(x=0.0, y=-300.0, z=230.0, pitch=90.0, roll=0.0):
# closest attempt was 1.4 mm and 43.5 deg away; wrist_flex is at its upper limit (95.0 deg).
# Pointing straight down costs reach height: at pitch 90 the arm only reaches about 80 mm
# above the table

The error tells you three things: how far off the result was, which joint is stuck, and why. This is not bad luck — the pose is geometrically out of reach.

INTERACTIVE LAB

IK solver

In-browser LM iteration

Give it a target pose and watch the inverse solution walk toward it from the current pose, step by step. The residual is "position error + vertical component of the approach direction", the same parameterization as the SDK; this is a simplified implementation for observing behavior, not a stand-in for the controller.

Target TCPSolved poseTable
Awaiting solveDrag the target, or press "Solve IK"
"High, pointing down" is the failure case from Chapter 4: at pitch = 90 the arm tops out about 80 mm above the table. There is no safety layer here, so it only reports "geometrically unreachable" and never actually moves.

5. The Two Directions Are Not Symmetric#

FK IK
Input → output 6 joint angles → 1 pose 1 pose → 6 joint angles
Always succeeds? Yes May fail (Unreachable)
Unique? Yes May have multiple solutions (in practice essentially unique on this arm)
How Direct matrix chain Iterative approximation, up to 100 iterations
Speed Microseconds Milliseconds
Side effects? None, pure computation The solve itself has none, but the command still has to pass the safety layer before it is sent

IK precheck passed only means "geometrically reachable"; it does not mean "executable" — joint limits, workspace and velocity limits all come afterwards.

6. A Full Round Trip#

python
target = Pose(x=0.0, y=-260.0, z=150.0, pitch=65.0, roll=0.0)

joints = k.ik(target, seed=zero)     # IK: pose → joint angles
back   = k.pose(joints)              # FK: joint angles → pose

Measured:

Step Result
Target pose (0, −260, 150), pitch 65
Joint angles from IK pan 4.9° / lift −5.0° / elbow −9.9° / wrist_flex 80.0° / wrist_roll 0°
Pose recovered by FK (0.00, −260.00, 150.00), pitch 65.00
Position error 0.005 mm (5 µm)

The numerical solution is not "exact", but the error is on the order of micrometres — far below the mechanical error of a 3D-printed arm.

7. Seeing Both Directions in the UI#

The two tabs in the console are really the two directions of the same mapping:

Tab What you manipulate What happens behind the scenes
Joint Control Drag the six joint sliders FK: the backend computes the pose from the new joint angles; telemetry and the 3D model follow
End-Effector Pose Type five pose numbers IK: click IK Precheck to solve for joint angles and show them without moving the robot

An exercise: on Joint Control, drag wrist_flex to 90°, then switch to End-Effector Pose and check whether pitch in the telemetry is also 90 — that is FK at work. Now enter z = 230 on End-Effector Pose and click IK Precheck; you get Unreachable — that is IK failing.

8. Common Misconceptions#

Claim True? Explanation
"The pose is read directly from a sensor" ✗ The only pose-related hardware reading is joint angles; the pose is computed by FK
"IK is just the inverse of FK; the two are symmetric" ✗ FK always has a unique solution; IK may have none and depends on iteration
"If IK finds a solution, the arm can definitely move" ✗ It still has to pass the safety layer: limits, workspace, velocity, e-stop
"IK always has multiple solutions" ≈ Possible in theory, but with 5 DoF and joint limits this arm is essentially unique in practice
"An IK failure means a bug in the code" ✗ It is usually a pose that is genuinely out of reach

References#