Tutorial on Programming Humanoid Robots

International Humanoid Forum — February 21, 2025

Sylvain Calinon

Idiap Research Institute

Why is it hard to program a humanoid robot?

Forward kinematics

Inverse kinematics (IK)

\begin{align*} \qquad \bm{\dot{x}} &= \arg\min_{\bm{\dot{x}}} \; \|\bm{J}\bm{\dot{x}}-\bm{\dot{f}}\|^2 \qquad \\ &= \underbrace{{(\bm{J}^\trsp \bm{J})}^{-1} \bm{J}^\trsp}_{\color{red}\bm{J}^\psin} \; \bm{\dot{f}} \end{align*}

Damped inverse kinematics

\begin{align*} \bm{\dot{x}} &= \arg\min_{\bm{\dot{x}}} \; \|\bm{J}\bm{\dot{x}}-\bm{\dot{f}}\|^2 + {\color{red} \lambda \|\bm{\dot{x}}\|^2} \\ &= {(\bm{J}^\trsp \bm{J} + {\color{red} \lambda\bm{I}})}^{-1} \bm{J}^\trsp \; \bm{\dot{f}} \end{align*}

Weighted inverse kinematics

\begin{align*} \bm{\dot{x}} &= \arg\min_{\bm{\dot{x}}} \; \|\bm{J}\bm{\dot{x}}-\bm{\dot{f}}\|^2_{{\color{red}\bm{W}}} \\ &= (\bm{J}^\trsp {\color{red}\bm{W}} \bm{J})^{-1} \bm{J}^\trsp {\color{red}\bm{W}} \; \bm{\dot{f}} \end{align*}

Prioritized inverse kinematics

\begin{align*} \bm{\dot{x}} &= \arg\min_{\bm{\dot{x}}} \; {\color{red}c(\bm{x})} \quad\text{s.t.}\quad \bm{\dot{f}}=\bm{J}\bm{\dot{x}} \\ &= \bm{J}^\psin\!(\bm{x}) \; \bm{\dot{f}} {\color{red} \;+\; \bm{N}\!(\bm{x}) \; \frac{\partial c(\bm{x})}{\partial\bm{x}}} \end{align*}