A Careful Control Loop
Forward kinematics is one lens on kinematics and robot motion. We study it because an embodied agent needs decisions that survive contact with noisy sensors, delayed effects, and changing environments.
This section develops the technical contract for Forward kinematics into a usable mental model. First we define the object of study, then we connect it to the agent loop, then we test it with a compact implementation.
The key question in Forward kinematics is practical: what must the agent know, what can it observe, what action is available, and what evidence shows that the action worked under the stated conditions?
A representation earns its place when it changes the measurable action interface. In Forward kinematics, the reader should keep asking which decision becomes easier, safer, or more reliable.
Theory
For Forward kinematics, the practical design rule is to make the interface inspectable before optimization begins: inputs, outputs, units, latency, bounds, and failure labels should all be visible in the saved artifact.
Forward kinematics answers the easier direction of the arm-motion question: given joint values, where is the tool? It is deterministic once the chain, joint values, and tool frame are fixed. That makes it the baseline oracle for inverse kinematics, trajectory tracking, calibration, and replay debugging.
The product-of-transforms view is useful because each link contributes a local transform, while the final pose lives in the base frame. For a planar two-link arm, the end-effector position is $x=l_1\cos q_1+l_2\cos(q_1+q_2)$ and $y=l_1\sin q_1+l_2\sin(q_1+q_2)$. The same composition idea scales to spatial arms with homogeneous transforms.
The mechanism in Forward kinematics is the contract between representation and action. Name what enters the module, what leaves it, which assumptions make that transformation valid, and which log would reveal a bad handoff.
Worked Example
The example computes forward kinematics for a planar two-link arm by chaining standard Denavit-Hartenberg transforms, then cross-checks the result against the closed-form expression $x=l_1\cos q_1+l_2\cos(q_1+q_2)$, $y=l_1\sin q_1+l_2\sin(q_1+q_2)$. Agreement between the general chain and the hand formula is the oracle every IK and control test reuses.
import numpy as np
def dh(theta, d, a, alpha):
"""Standard (distal) DH homogeneous transform."""
ct, st = np.cos(theta), np.sin(theta)
ca, sa = np.cos(alpha), np.sin(alpha)
return np.array([[ct, -st*ca, st*sa, a*ct],
[st, ct*ca, -ct*sa, a*st],
[0, sa, ca, d],
[0, 0, 0, 1]])
l1, l2 = 0.5, 0.4
q = np.deg2rad([35.0, 60.0])
# DH table for a planar RR arm: alpha=0, d=0, a=link length
T = dh(q[0], 0, l1, 0) @ dh(q[1], 0, l2, 0)
p_chain = T[:3, 3]
# Closed-form planar FK
x = l1*np.cos(q[0]) + l2*np.cos(q[0]+q[1])
y = l1*np.sin(q[0]) + l2*np.sin(q[0]+q[1])
p_closed = np.array([x, y, 0.0])
print("FK via DH chain :", np.round(p_chain, 6))
print("FK closed form :", np.round(p_closed, 6))
print("match?", np.allclose(p_chain, p_closed))
# End-effector orientation = sum of joint angles for a planar arm:
print("tool heading (deg):", round(np.rad2deg(np.arctan2(T[1,0], T[0,0])), 3),
"vs", round(np.rad2deg(q[0]+q[1]), 3))
The position match validates the DH table, and the heading check validates orientation, which a position-only test would miss. This two-link FK is reused as ground truth in the inverse-kinematics example in Section 5.6.
The small forward-kinematics fragment should make transform composition auditable: joint order, axis convention, link frame, base frame, and tool frame. Pinocchio and Drake then provide maintained derivatives and model parsing for real articulated systems.
Practical Recipe
- Write the observation, action, and success metric before choosing a model.
- Build a baseline that is simple enough to debug by inspection.
- Add the library implementation only after the baseline behavior is understood.
- Record failures as structured cases: perception error, state error, planning error, control error, or evaluation error.
- Run at least one perturbation test before trusting the result.
The common mistake in Forward kinematics is to celebrate the component score before checking the closed-loop handoff. The failure usually appears at the boundary: stale state, wrong frame, delayed action, saturated actuator, or metric that ignores the real task cost.
A robotics team using Forward kinematics should log not only final success, but intermediate observations, chosen actions, controller status, and recovery events. The logs reveal whether the method is solving the task or merely passing the easiest episodes.
A good embodied system makes forward kinematics visible twice: once in the design sketch and once in the replay artifact. The second view keeps the first one honest.
For Forward kinematics, treat frontier claims as hypotheses until they expose enough detail to reproduce the result: data boundary, embodiment, controller interface, evaluation panel, and failure cases.
Can you name the observation, state estimate, action, success metric, and most likely failure mode for Forward kinematics? If not, the system boundary is still too vague.
Production Pattern
Forward kinematics 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.
Use forward kinematics as the baseline oracle for every inverse-kinematics or control experiment. 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.
For Forward kinematics, kinematics maps joint or body motion into task-space motion without explaining forces. Preserve joint limits, frame conventions, velocity units, and singularity margins in the artifact.
| Tool or Library | What It Handles | Verification Check |
|---|---|---|
| Pinocchio | computes articulated-body kinematics, dynamics, and derivatives | Verify model frames, joint ordering, and derivative convention against the URDF. |
| Robotics Toolbox for Python | supports practical work on Forward kinematics | Verify the library output against the hand-built baseline on one small case. |
| MoveIt 2 | supports practical work on Forward kinematics | Verify the library output against the hand-built baseline on one small case. |
| Drake | models dynamical systems, multibody plants, optimization, and controllers | Verify scalar type, plant finalization, frame convention, and solver status. |
| ROS 2 control | supports practical work on Forward kinematics | Verify the library output against the hand-built baseline on one small case. |
Use this recipe when turning Forward kinematics 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.
- Write the joint vector, frame target, velocity convention, and constraint set before solving.
- Check forward kinematics on a known posture, then perturb one joint and inspect the end-effector delta.
- Compare an analytic or numerical Jacobian with Pinocchio, Robotics Toolbox, or Drake on the same robot model.
- Log residual error, joint-limit distance, manipulability, and solver iteration count in one artifact.
- Treat singularities and infeasible targets as design signals, not as solver annoyances.
For Forward kinematics, compare methods only through one saved artifact that preserves the inputs, outputs, units, timestamps, latency budget, configuration, seed, metric definition, and failure labels relevant to this section. The comparison is meaningful only when the same script evaluates the same panel.
Extend the section exercise by adding one perturbation specific to Forward kinematics and one latency or uncertainty check. Save the result in the EvidenceRecord schema, then explain which library output you trust and why.
Kinematic failures often arrive as a plausible pose with an impossible motion. Inspect link lengths, joint offsets, tool transforms, axis signs, and radians versus degrees before blaming the planner. For this section, first reproduce one tiny forward-kinematics case by hand, then rerun it through Pinocchio, Robotics Toolbox for Python, MoveIt 2, Drake, or ROS 2 tf2. If the two disagree, inspect conventions and timing before changing the model.
Technical Core
Forward kinematics needs a topic-native core: variables, equations or system contracts, an algorithmic procedure, an expected output, and a failure diagnosis. Figure 5.5.T summarizes the chain this section must preserve when moving from a teaching example to a real embodied system.
$T_{0e}(q)=T_{01}(q_1)T_{12}(q_2)\cdots T_{n e}(q_n)=\prod_i \exp(\widehat\xi_i q_i)M$
The matrix product says that each joint changes the coordinate frame seen by every downstream link. The product-of-exponentials form uses screw axes $\xi_i$ and a home configuration $M$, while Denavit-Hartenberg notation uses a structured table of link offsets and twists. Either convention works only if it is used consistently.
- Evaluate the home pose $q=0$ and compare it with the robot drawing, URDF, or CAD model.
- Move one joint by a small known angle and compute the expected downstream displacement.
- Compute the full base-to-tool transform and log both translation and orientation, not only $(x,y,z)$.
- Compare the hand-built result with Pinocchio, Drake, Robotics Toolbox, or ROS 2 tf2 on the same model and joint vector.
| Contract Field | What To Specify | Why It Matters |
|---|---|---|
| State and observation | Variables, units, timestamps, frames, and uncertainty. | Prevents a model score from being mistaken for robot capability. |
| Action interface | Command type, limits, update rate, and safety fallback. | Makes the learned or planned output executable. |
| Evidence artifact | Trace, metric, configuration, seed, and failure label. | Allows baseline and library path to be compared in one pass. |
| Tool path | Modern Robotics, Pinocchio, Drake, ROS 2 tf2, MoveIt, NumPy | Shows the practical library route after the mechanism is understood. |
Expected output is a reproducible transform $T_{0e}$ whose translation, rotation, and intermediate frames match the maintained-library result on the same joint vector. A correct forward-kinematics trace is the artifact you return to when inverse kinematics, control, or perception appears inconsistent.
Forward kinematics fails quietly when a fixed tool offset is forgotten, joint angles are passed in degrees to a radians API, link-frame axes are reversed, or the code compares only tool position while orientation is wrong.
Section References
Core references for Forward kinematics: Modern Robotics; Murray, Li, and Sastry; Siciliano et al.; LaValle; and official documentation for Drake, MuJoCo, Pinocchio, CasADi, python-control, GTSAM, ROS 2, and OpenCV as applicable.
Use these references to check notation, frame conventions, units, solver assumptions, and maintained-library behavior.
Forward kinematics is useful when it makes the perception-action loop more reliable, not when it merely adds a more impressive model name.
Design a method-matched experiment for Forward kinematics. Specify the environment, observations, actions, metric, one perturbation, and the library output you would compare against the hand-built baseline.