Section 4.7: Common frame mistakes and how to debug them

A Careful Control Loop

This section develops a frame-debugging checklist that can be used before retraining a model or retuning a controller. The method is simple: validate identity, inverse, composition order, handedness, units, timestamps, and one known physical point. If any of those fail, the system has a geometry problem before it has a learning problem.

The key question is practical: can the team reproduce the wrong pose with a tiny deterministic case, or are they debugging a full robot episode with too many variables active at once?

Theory

The most useful invariant is the inverse check. If $T_{AB}$ maps coordinates from $B$ into $A$, then $T_{BA}=T_{AB}^{-1}$ and

$$T_{AB}T_{BA} \approx I.$$

The approximation accounts for floating-point error, not conceptual error. A large residual means one of three things is likely: the transform was inverted twice, the multiplication order is wrong, or the stored translation is expressed in the wrong parent frame.

A second invariant is distance preservation. A rigid transform can move and rotate two points, but the distance between them must stay constant. If a transform changes distances, it is not a rigid transform. Check scale before checking policy behavior.

ROS conventions (REP 103)

Many frame bugs are convention mismatches against a published standard. ROS REP 103 fixes the axis conventions so that independently written nodes agree without negotiation:

• Body and sensor frames: $x$ forward, $y$ left, $z$ up (a right-handed frame attached to the robot).
• World and map frames: ENU, that is $x$ east, $y$ north, $z$ up.
• Units: meters, radians, and seconds throughout.

A common mistake is to feed a camera optical frame (OpenCV convention: $x$ right, $y$ down, $z$ forward) directly into a body-frame consumer that expects $x$ forward, $y$ left, $z$ up. The fix is an explicit transform between the two conventions, never a silent reinterpretation of the same array. Code Fragment 4.7.2 shows the rotation that maps an optical-frame vector into a REP 103 body frame.

# Convert a camera optical-frame vector (x right, y down, z forward, OpenCV)
# into a REP 103 body frame (x forward, y left, z up).
import numpy as np

# Columns are the optical axes expressed in body coordinates:
#   optical +x (right)   -> body -y
#   optical +y (down)    -> body -z
#   optical +z (forward) -> body +x
R_body_optical = np.array([
    [0.0, 0.0, 1.0],
    [-1.0, 0.0, 0.0],
    [0.0, -1.0, 0.0],
])

v_optical = np.array([0.0, 0.0, 1.0])          # "straight ahead" for the camera
v_body = R_body_optical @ v_optical
print("forward in body frame:", v_body.round(3).tolist())
print("valid rotation? det =", round(float(np.linalg.det(R_body_optical)), 6))

Worked Example

Code Fragment 4.7.1 tests two frame invariants. The determinant check catches reflections and left-handed coordinate mistakes. The round-trip residual catches inverse and order errors that otherwise produce plausible but wrong positions.

# Run two frame sanity checks before blaming perception or control.
# A valid rigid transform has det(R)=1 and a tiny round-trip residual.
# The test uses one known point so the failure is easy to reproduce.
import numpy as np

rotation = np.array([
    [0.0, -1.0, 0.0],
    [1.0, 0.0, 0.0],
    [0.0, 0.0, 1.0],
])
translation = np.array([0.5, 0.0, 1.0])

transform = np.eye(4)
transform[:3, :3] = rotation
transform[:3, 3] = translation

point_local = np.array([0.2, 0.1, 0.0, 1.0])
point_world = transform @ point_local
point_recovered = np.linalg.inv(transform) @ point_world

determinant = np.linalg.det(rotation)
residual = np.linalg.norm(point_local - point_recovered)
print("determinant:", round(determinant, 6))
print("round-trip residual:", round(residual, 12))

Expected output: a determinant near 1 and a near-zero residual mean the transform is at least internally consistent. They do not prove that the transform is the right one for the robot, but they eliminate several common convention mistakes.

Practical Recipe

1. Write the observation, action, and success metric before choosing a model.
2. Build a baseline that is simple enough to debug by inspection.
3. Add the library implementation only after the baseline behavior is understood.
4. Record failures as structured cases: perception error, state error, planning error, control error, or evaluation error.
5. Run at least one perturbation test before trusting the result.

Production Pattern

Common frame mistakes and how to debug them sits inside the Part II robotics contract: geometry defines where things are, kinematics defines what motion is possible, dynamics defines what motion costs, control defines how errors are corrected, and sensing defines what the agent can know on time.

Debug frame errors by replaying one known point through the full chain, then checking signs, units, and timestamps. This makes the section useful to students, builders, and researchers at the same time: the idea has an intuitive role, a formal interface, a runnable check, and a failure mode that can be reproduced.

Use this recipe when turning Common frame mistakes and how to debug them into code, a simulator experiment, or a robot diagnostic. The point is not to use every library. The point is to keep the hand-built baseline and the maintained-tool path comparable.

1. Name every frame with a parent, child, unit convention, and timestamp policy.
2. Write one hand-checked transform chain and verify identity, inverse, and composition tests.
3. Run the same transform through ROS 2 tf2 or SciPy Rotation, then compare one point and one direction vector.
4. Record a frame audit with source sensor, latency, and expected sign convention.
5. Debug failed behavior by replaying the transform tree before changing policy or controller code.

Debug frame mistakes with a minimal geometric probe: one point, one vector, one pose, one timestamp, and one expected transform path. Then replay the same case through tf2 and visualization.