RobotGuide

Tools & simulation · topic G7 · 35 hours

Robotics math & optimization libraries

Robotics does not reinvent the wheel: kinematics goes through Pinocchio, optimization through CasADi, real-time MPC through acados, WBC through QP solvers. Mastering these libraries means borrowing a decade of someone else's implementation.

Engineering stage · Hardcore
Algorithm · Proficient System software · Hands-on Embedded & controls · Aware Mechanical · Aware

Tools
Tools

What to learn

  • Eigen + Sophus: SE(3)/SO(3) exponential maps, Lie-algebra derivatives, quaternion interpolation
  • Pinocchio: URDF parsing, RNEA (inverse dynamics), CRBA (mass matrix), ABA (forward dynamics), analytic Jacobians, automatic differentiation
  • KDL: venerable but with an awkward API and slow; when it is still in use (MoveIt's default IK is exactly it)
  • CasADi: symbolic modeling + automatic differentiation + IPOPT, the standard MPC recipe; warm starts and solver options affect real-time performance more than the model itself
  • Real-time optimal control: acados (C code generation, millisecond scale), OCS2 (legged MPC + WBC), Crocoddyl (DDP/iLQR)
  • QP solvers: OSQP, qpOASES, ProxQP, HPIPM; the practical limits of solving a 30-dimensional QP at 1 kHz
  • Drake: multibody dynamics with contact plus optimization — the best for verification and teaching

The lesson · plain words

Robot math libraries (Eigen/Pinocchio/CasADi) are the translation layer between algorithms and code. Understanding their trade-offs — dense or sparse, analytic derivatives or automatic differentiation — decides whether your algorithm takes 1 ms or 100 ms.

Ill-conditioning: the condition number decides everything

Watch the animation and drag κ (the axis ratio): the flatter the ellipse (the worse the conditioning), the slower gradient descent crawls, while L-BFGS stays fast. ICP registration, BA optimization, inverse kinematics solving — underneath, all of them are this exact race.

Engineering lesson: when optimization is slow, compute the condition number first (the Jacobian's singular values); if ill-conditioned, precondition (normalize/regularize) or switch to a second-order method (LM/GN). Hard-tuning the step size is solving the wrong problem.

Interactive figure · drag it and see

What this figure shows: three optimizers racing down the same valley as you drag the condition number κ (the axis ratio) — gray gradient descent crawls ever slower as the ellipse flattens, yellow momentum does better but still converges linearly, orange L-BFGS exploits curvature and converges within tens of steps. Takeaway: when optimization is slow, compute the Jacobian's condition number first; precondition or switch to a second-order method (LM/GN) instead of twisting the step size.

The division of labor among the trio

Eigen: the matrix-computation foundation (mostly dense); expression templates make operations run with zero temporary objects — the bedrock under every other library. Pinocchio: analytic rigid-body dynamics (M/C/g, Jacobians, RNEA), the compute source of 1 kHz WBC. CasADi: automatic differentiation + nonlinear optimization (the IPOPT frontend), the workhorse of MPC and trajectory optimization.

The selection logic: compute dynamics with Pinocchio, formulate optimization problems with CasADi, and both stand on Eigen. Stack the three together and you have the modern robot algorithm stack.

Analytic derivatives vs automatic differentiation

Analytic derivatives (hand-derived Jacobians): the fastest and auditable, but derivation is slow and error-prone. Autodiff (CasADi/autodiff): write the forward pass once and the derivatives come out automatically — slightly slower, but it saves your life. The engineering default: analytic on the hot path (Pinocchio already gives it to you), autodiff for prototypes.

Sparsity is the hidden leg-up: BA's Jacobian is >99% sparse, and sparse solving (SparseLU/Ceres) beats dense by hundreds of times. Once the data grows, «whether you can use sparse solvers» is the dividing line between dabbling and actually doing SLAM back-ends.

How deep to go

Write allocation-free expressions in Eigen; compute M/C/g and Jacobians with Pinocchio; define a QP/NLP in CasADi and solve it. With these three done, you can compute the dynamics behind the F1 simulation yourself.

Three questions — do you really get it?

1. Optimization iterations are slow — what do you check first?

  • AThe problem's condition number (the Jacobian's singular values)
  • BThe learning rate
  • CThe CPU temperature
Show answer

A. Ill-conditioning is the root of all slowness. Precondition or switch to a second-order method; do not keep twisting the step size.

2. What carries the dynamics computation of 1 kHz whole-body control (WBC)?

  • APinocchio's analytic rigid-body dynamics
  • BNumerical differentiation
  • CThe GPU
Show answer

A. The analytic RNEA/ABA implementations run in microseconds. Numerical differentiation is two orders of magnitude slower and noisy.

3. Large-scale BA optimization must use?

  • ADense LU
  • BSparse solvers (the Jacobian's sparsity is extremely high)
  • CGradient descent
Show answer

B. BA's Jacobian is >99% sparse. Sparse vs dense differs by hundreds of times — this is the entry gate of the SLAM back-end.

Keep going deeper

Links without a marker stay on this site; the rest point to the Chinese edition until its translation batch lands.

How it fails in the field

  • Deriving the Jacobian yourself, one sign wrong, the controller only blows up near singularities — two weeks of searching finds nothing
  • Using IPOPT for 1 kHz MPC, 8 ms per solve, forced down to 50 Hz, and then declaring «MPC does not fit this scenario»
  • Rebuilding QP matrices every cycle with no preallocation — 60% of the 1 kHz budget spent on allocating memory

You have learned enough when

Compute the inverse dynamics of a 6-axis arm with Pinocchio and match the actual torques of the same trajectory in Gazebo (error <10%); write a 20-step-horizon MPC in CasADi that solves in under 2 ms.

The Chinese edition carries the full topic page for Robotics math & optimization libraries, including the complete outline, depth requirements per career track, and further derivations: robotguide.dev/k/G7/.