Lab 1 closed with a 2-link planar arm drawing a clean Cartesian square. The math stayed on a whiteboard — two joints, analytic IK, a Jacobian you can invert in your head. Lab 2 drops that pedagogical comfort and picks up a real 6-DOF UR5e industrial manipulator. Wrist axes align, the mass matrix stops being diagonal, and a naive position controller can produce 133 mm of tracking error on a trajectory a human would consider slow. This post is about the engineering moves required to get from there to 0.088 mm RMS on a 3D cube.
Problem
Going from 2 DOF to 6 DOF is not a linear jump in difficulty. Three things change at once. First, singularities become structural rather than cosmetic: the UR5e has wrist, shoulder, and elbow singularities that are guaranteed to appear inside the reachable workspace, and any IK solver that does not explicitly damp them will blow up. Second, rigid-body dynamics stop being a rounding error — the mass matrix M(q) swings by more than an order of magnitude across the workspace, and Coriolis coupling between joints is no longer something you can lump into a gain. Third, the robot now has both position and orientation to track, which means the error lives in SE(3), not R². Every module from Lab 1 — FK, Jacobian, IK, control — needs a 3D rewrite.
The other shift is tooling. Hand-written trigonometry gets you through a 2-link arm. For a 6-DOF manipulator you want an analytical library that speaks URDF, computes forward kinematics and Jacobians in microseconds, and gives you RNEA/CRBA/ABA out of the box. This lab introduces the dual-engine architecture that carries through the rest of the series: Pinocchio as the analytical brain (kinematics, dynamics, Jacobians) and MuJoCo as the physics simulator (contacts, actuators, integration). Every quantity Pinocchio produces is cross-validated against MuJoCo before it is trusted.
Theory
Forward Kinematics
For a 6-DOF serial chain, FK is the product of per-joint homogeneous transforms:
T_base_to_ee = T_1(q_1) · T_2(q_2) · T_3(q_3) · T_4(q_4) · T_5(q_5) · T_6(q_6) · T_tool
Each link transform separates fixed geometry from the variable joint angle:
T_fixed = Rot_x(alpha) · Trans_x(a) · Trans_z(d)
T_joint = T_fixed · Rot_z(theta)
The upper-left 3×3 block of the result is the end-effector rotation R; the upper-right 3×1 is the position p. The matrix encodes both "where" and "how the tool is pointing" in a single object. At the UR5e home pose the implementation reports an EE position of (-0.8172, -0.3679, 0.0628) m — the first smoke test that the chain is at least internally consistent.
Jacobian and Singularities
The geometric 6×6 Jacobian maps joint velocity to end-effector twist:
[v; ω] = J(q) · q̇
For a revolute joint i, the column is built from the joint axis z_i expressed in world frame and the vector from the joint origin to the EE:
J_linear[:, i] = z_i × (p_ee - p_i)
J_angular[:, i] = z_i
The lab computes this three independent ways — hand-rolled geometric, Pinocchio's computeFrameJacobian in LOCAL_WORLD_ALIGNED, and central finite differences with log3 on the rotation error — and checks that all three agree to Frobenius norm < 1e-4. That triple cross-check is the only way to trust a Jacobian implementation.
Manipulability uses Yoshikawa's index w = sqrt(det(J · Jᵀ)). The UR5e has three classical singularities: wrist (|q5| < 0.05 rad, axes 4 and 6 align), shoulder (EE xy-distance from base z-axis < 0.02 m), and elbow (|q3| ≈ 0 or |q3 - π| ≈ 0, links 2 and 3 collinear). Near any of them, det(J) → 0, the condition number explodes, and naive pseudo-inverse IK demands infinite joint velocity.
Damped Least Squares IK
Pseudo-inverse IK (dq = pinv(J) · e) works until it meets a singularity, at which point it diverges. DLS replaces the pseudo-inverse with a damped normal equation:
dq = Jᵀ · (J · Jᵀ + λ² · I)^(-1) · e
The 6D error stacks position and orientation: e = [p_target - p; log3(R_target · Rᵀ)]. The damping term λ² · I regularizes the inversion — small λ away from singularities for fast convergence, large λ near singularities for stability. The lab uses an adaptive λ: halve it when the error decreases, double it when the error increases, clamped to [1e-4, 1.0]. This is a trust-region method in disguise, and on a 50-target benchmark it lifts the success rate from roughly 54% (pseudo-inverse) to 62% (DLS).
Dynamics: RNEA, CRBA, ABA
The manipulator equation is the usual:
M(q) · q̈ + C(q, q̇) · q̇ + g(q) = τ
Pinocchio supplies three algorithms that together cover every use case:
- CRBA — Composite Rigid Body Algorithm. Builds
M(q). O(n²). Only fills the upper triangle, so the code symmetrizes explicitly. - RNEA — Recursive Newton-Euler Algorithm. Inverse dynamics: given
(q, q̇, q̈), returns the requiredτ. O(n). Atq̇ = 0, q̈ = 0, RNEA reduces exactly tog(q). - ABA — Articulated Body Algorithm. Forward dynamics: given
(q, q̇, τ), returnsq̈ = M⁻¹(τ - C·q̇ - g). O(n), the algorithm MuJoCo itself uses under the hood.
The lab verifies the skew-symmetry property — M_dot - 2C must be skew-symmetric — by finite-differencing M and checking ||N + Nᵀ||_F ≈ 0. This is the passivity identity that underwrites every energy-based controller further down the series.
Implementation
Dual-Engine Architecture
Every module in this lab follows the same pattern: compute analytically in Pinocchio, simulate physically in MuJoCo, cross-validate, and only then trust the result. Pinocchio loads the UR5e URDF and exposes pin.forwardKinematics, pin.computeFrameJacobian, pin.crba, pin.rnea, and pin.aba. MuJoCo loads the Menagerie XML of the same robot and provides contact-aware physics, actuators, and integration. The two models must agree on geometry and inertias for the cross-checks to mean anything.
The gravity-vector cross-check is the bluntest version of this test:
import pinocchio as pin
# Pinocchio: analytical gravity via RNEA at zero velocity/acceleration
pin.forwardKinematics(model, data, q)
g_pin = pin.rnea(model, data, q, np.zeros(6), np.zeros(6))
# MuJoCo: qfrc_bias at zero velocity equals g(q)
sim.set_qpos(q)
sim.step_forward_kinematics()
g_mj = sim.data.qfrc_bias.copy()
# These must agree to < 0.01 Nm per joint
assert np.max(np.abs(g_pin - g_mj)) < 1e-2
When this check passes at five probe configurations (zeros, home, elbow_up, shoulder_only, random), both models are trustworthy. When it fails, the problem is almost always a URDF/XML inertial mismatch and nothing downstream will work until it is fixed.
The Menagerie Position-Servo Trap
The MuJoCo Menagerie UR5e ships with general actuators configured as PD position servos:
τ = Kp · (ctrl - qpos) - Kd · qvel
Sending ctrl = q_desired directly — the obvious thing to do — produces two failure modes. First, gravity droop: the servo has to fight gravity through position error, so it settles with a steady-state offset proportional to g(q) / Kp. Second, tracking lag: the damping term -Kd · qvel drags against any desired velocity, so the servo chases a moving reference with a delay proportional to qd_des. On the first cube attempt, these two effects stacked with a table collision and produced 133 mm RMS error — two orders of magnitude worse than the 2-link arm from Lab 1. The system was unusable.
Feedforward Gravity Compensation
The fix is to pre-load ctrl with the exact offset that cancels both effects. Read g(q) out of MuJoCo as data.qfrc_bias (equivalent to Pinocchio's computeGeneralizedGravity at zero velocity), add a velocity feedforward term to cancel the damping drag, and the effective closed-loop law collapses to a clean PD + gravity:
# Gravity compensation + velocity feedforward
g_q = data.qfrc_bias.copy()
ctrl = q_des + g_q / Kp + (Kd * qd_des) / Kp
data.ctrl[:] = ctrl
Substituting back into the servo's torque equation:
τ ≈ Kp · (q_des - qpos) - Kd · (qvel - qd_des) + g(q)
The position error term and the velocity-error term now operate on a gravity-compensated plant. No steady-state droop, no tracking lag. Combined with relocating the cube center from [0.4, 0.0, 0.50] to [0.3, 0.2, 0.55] to keep the upper arm clear of the table, this single change brought RMS error from 133 mm to 0.088 mm — a roughly 1500× improvement with fewer than ten lines of code.
Results
The capstone demo draws a full 3D cube — 8 vertices, 12 edges, traversed as a 13-waypoint Eulerian path with no lifting — at 10 cm side length, over 24 s of simulated time. DLS IK solves each waypoint with sequential seeding; 12 quintic segments chain the waypoints with zero boundary velocity and acceleration; the GC+VFF position controller tracks the reference in MuJoCo.
| Metric | Value |
|---|---|
| Cube side length | 10 cm |
| Trajectory duration | 24.0 s |
| Quintic segments | 12 |
| RMS Cartesian error | 0.088 mm |
| Max Cartesian error | 0.234 mm |
| Max actuator torque | 16.50 N·m |
| Joint torque limit | 150 N·m |
| Peak torque utilization | ~11% |
| Pinocchio vs MuJoCo g(q) | < 0.01 N·m |
| Jacobian cross-check | < 1e-4 Frob. |
| IK waypoint error | < 0.1 mm |
The controller is nowhere near saturating the actuators — peak torque is about 11% of the joint limit on the loaded joints. The error budget is dominated by the integration step and quintic-polynomial discretization, not by dynamics modeling, which is exactly where you want the bottleneck to be.
Key Takeaways
- Dual-engine architecture is non-negotiable. Pinocchio for analytical FK/Jacobian/dynamics, MuJoCo for physics. Every analytical quantity gets cross-validated against the simulator before it is trusted. This single discipline catches URDF mismatches, frame-convention bugs, and inertial typos before they poison downstream control.
- DLS with adaptive damping is the right default IK for industrial arms. Pseudo-inverse is a textbook exercise; DLS is what ships. The adaptive λ behaves like a trust region and lifts success rate on random reachable targets from ~54% to ~62%.
- Read your actuator model before writing your controller. The Menagerie UR5e uses PD position servos, not direct torque. Assuming torque control when the plant is a position servo cost 133 mm of tracking error on the first attempt. Match the feedforward to the actuator.
- Feedforward beats feedback gains every time. Adding
g(q) / KpandKd · qd_des / Kpto the reference is cheap, principled, and dropped RMS error by three orders of magnitude. Trying to fix the same problem by crankingKpwould have saturated the actuators and rung the system. - Collisions destroy tracking silently. The 133 mm initial error was half controller design, half the forearm clipping the table. Always inspect whether the planned path geometrically clears the workspace before blaming the controller.
- Cross-validation is the fastest debugger. When Pinocchio RNEA and MuJoCo
qfrc_biasagree to < 0.01 N·m across five probe configurations, both models are trustworthy. When they disagree, nothing downstream will work and you know exactly where to look.