Section 4.7: Common frame mistakes and how to debug them

"The arm pointed precisely at the wrong place for three days before anyone suspected the coordinate frame."

A Debugging Engineer, Humbled

Picture a robot arm that reaches confidently for a coffee cup and closes its gripper on empty air fifteen centimeters to the left, episode after episode, while the detector, the policy, and the sensors all report perfect health: that single misplaced grasp is almost never a learning failure and almost always an untested transform. This section develops a frame-debugging checklist that can be run before retraining a model or retuning a controller. The method reduces to eight deterministic checks: 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. A robot that moves to the wrong place because of a sign error is not demonstrating a bad policy; it is demonstrating an untested transform. Figure 4.7A sets the scene for the kind of misaimed-arm failure this checklist is built to catch.

Figure 4.7B traces this idea concretely: a single known point is pushed from the camera optical frame through the body frame to the world map frame, and each transform boundary is checked for determinant, axis convention, and units before any learned model is trusted. 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

Reproducing the wrong pose in a tiny deterministic case only helps if you know which numerical properties to test, so the theory below names the small set of invariants a correct rigid transform must satisfy.

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. Section 4.4 on rigid transforms and homogeneous coordinates gives the algebraic foundation for these invariants.

Distance preservation

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.

Distance preservation matters in embodied AI because a scale error corrupts every metric quantity the robot acts on. Consider a grasp controller that receives point clouds in millimeters but expects meters. It tries to close its fingers around an object 1000 times smaller than reality, which causes collisions or failed picks that look exactly like a policy failure. On mobile platforms, a lidar-to-map transform with a 0.1x scale factor compresses the mapped environment. Free-space estimates become wrong, and the planner routes through occupied cells. One team running a nav2 stack added a single two-point distance check before training and cut policy retraining cycles from 47 runs to 3. The check caught the millimeter-to-meter mismatch on the first episode rather than after weeks of tuning blamed on perception noise.

The mechanism is a two-point probe. Select two points whose true separation is known from the robot's Unified Robot Description Format (URDF) or a calibration target, apply the candidate transform, and measure the output distance. Rotations leave the Euclidean norm invariant because \(\|Rv\| = \|v\|\) for any orthogonal \(R\). Translations shift the origin but preserve inter-point gaps. Any deviation therefore flags a non-orthogonal or incorrectly scaled matrix before a learned policy is involved.

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, meaning East-North-Up, 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; this pattern is called the silent-axis swap failure, and it is never caught by 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))

Consider a specific case. A depth camera mounted 0.15 m in front of a robot's body origin detects an obstacle at optical-frame coordinates \((0, 0, 0.8)\) (straight ahead, 80 cm away). Without the REP 103 body-frame rotation, a node expecting body-frame coordinates reads this as \(x=0\) (no forward component), \(y=0\) (no lateral component), \(z=0.8\) (upward, not forward). The planner concludes there is no obstacle ahead and drives forward. Applying the rotation from Code Fragment 4.7.2 maps the point to body-frame \((0.8, 0, 0)\) and places the obstacle directly in the robot's path. nav2 deployments have typically reported this same class of reinterpretation error when camera extrinsics are loaded without the optical-to-body rotation, and in practice the resulting collisions are often first blamed on perception rather than on the missing transform.

Worked Example

Code Fragment 4.7.1 tests two frame invariants. The determinant check catches reflections and left-handed coordinate mistakes; recall from Section 4.3 on rotation matrices that a valid rotation has determinant exactly 1. 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.

With the two invariants validated on a hand-built chain, the remaining work is procedural: the following recipe turns those isolated checks into an ordered field protocol you can run on a live robot stack.

Practical Recipe

1. Freeze the transform tree at the failing timestep: extract the exact tf2 snapshot with ros2 topic echo /tf --once or the equivalent tf2_tools echo call, then replay it offline so the failure is reproducible without running the full robot stack.
2. Probe one known physical point before touching any learned component. On a Franka Panda arm, for example, command the end-effector to a known joint configuration (all joints at zero puts the flange at a published pose in the URDF), transform that pose through each frame boundary, and compare the result to the URDF ground truth. A consistent offset of 0.001 m or more is a unit or extrinsic error, where an extrinsic is the fixed pose of a sensor relative to the robot body; a rotation-only discrepancy points to a convention mismatch.
3. Check the optical-to-body rotation explicitly whenever a depth camera or stereo rig is in the chain. RealSense and Zed cameras publish in optical convention by default; a body-frame consumer on a Boston Dynamics Spot or Unitree Go2 will silently misplace every detection until that rotation is applied.
4. Verify transform timestamps against the tf2 buffer window (default 10 s in ROS 2). Manipulation tasks where the robot pauses for grasp confirmation or re-grasps an object often exceed this window; log transform_age = rospy.Time.now() - stamp alongside every lookup and alert if it exceeds 200 ms for a high-speed controller or 2 s for a quasi-static arm.
5. After each fix, re-run the Code Fragment 4.7.1 determinant and round-trip residual checks on the corrected chain before re-enabling the learned policy or detector. This confirms the geometry is sound before the model is blamed for any remaining errors.

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. 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 frame debugging into code, a simulator experiment, or a robot diagnostic. The goal is not to use every library, but to keep the hand-built baseline and the maintained-tool path comparable on the same case.

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.