robot-sysid

Robotic Arm System Identification via Spatial Dynamics Regressors

Python MuJoCo SymPy NumPy Least Squares Spatial Dynamics

Overview

A generic, robot-agnostic system identification tool that recovers the 10 standard rigid-body inertial parameters (mass, center-of-mass moments, and inertia tensor) plus optional Coulomb and viscous friction coefficients for a robot's terminal link from MuJoCo simulation data.

Common use case: you've attached a payload or tool to your robot's end-effector and need to identify the composite inertial properties for accurate dynamic modeling and control.

Technical Highlights

Robot-Agnostic

Works with any MuJoCo XML model. Automatically detects the terminal joint from the kinematic tree — not limited to specific robot types.

Symbolic Regressor Derivation

Uses SymPy to analytically derive the spatial dynamics regressor matrix, ensuring mathematical correctness and avoiding numerical differentiation errors.

Friction Identification

Optionally identifies Coulomb and viscous friction coefficients alongside the 10 inertial parameters, extending the parameter vector to 12 dimensions.

Hardware Export

Generates trajectory files for Damiao motors with automatic limit clamping, enabling the same excitation trajectory to run on real hardware.

Analytical Derivatives

Trajectory velocities and accelerations are computed exactly as closed-form derivatives of position sinusoids — no finite differencing.

Quality Metrics

Reports RMSE, condition number, and matrix rank to assess identification quality and regressor conditioning.

Pipeline

Parse — Load a MuJoCo XML model and extract the kinematic chain
Generate — Create multi-frequency sinusoidal excitation trajectories
Simulate — Run MuJoCo simulation, collecting spatial velocity, acceleration, and torque
Solve — Compute the regressor matrix $Y$ and solve $\theta = Y^\dagger \tau$ via least-squares
Export — Output identified parameters as JSON and an updated MuJoCo XML

Mathematical Formulation

Spatial Dynamics Regressor

The tool establishes a linear relationship between joint torques and inertial parameters:

\tau = Y\theta

where $\tau$ is the joint torque, $Y$ is the regressor matrix, and $\theta$ is the parameter vector:

\theta = [m,\; h_x,\; h_y,\; h_z,\; I_{xx},\; I_{xy},\; I_{xz},\; I_{yy},\; I_{yz},\; I_{zz}]^T

Spatial Inertia Matrix

The spatial inertia matrix $G_a \in \mathbb{R}^{6 \times 6}$ encodes the mass distribution:

G_a = \begin{bmatrix} \bar{I} & [h]_\times \\ -[h]_\times & mI_3 \end{bmatrix}

where $\bar{I}$ is the inertia tensor, $h = m \cdot c$ is the first moment of mass, and $[h]_\times$ is the skew-symmetric matrix:

[h]_\times = \begin{bmatrix} 0 & -h_z & h_y \\ h_z & 0 & -h_x \\ -h_y & h_x & 0 \end{bmatrix}

Newton-Euler Dynamics

The 6D wrench is governed by the spatial Newton-Euler equation:

F_a = G_a \dot{V}_a - [\text{ad}_{V_a}]^T G_a V_a

where $V_a = [\omega^T, v^T]^T$ is the spatial velocity, $\dot{V}_a$ is the spatial acceleration, and $[\text{ad}_{V_a}]^T$ is the adjoint transpose capturing gyroscopic effects:

[\text{ad}_{V_a}]^T = \begin{bmatrix} -[\omega]_\times & -[v]_\times \\ 0_{3\times3} & -[\omega]_\times \end{bmatrix}

Full 6×10 Regressor Matrix

The regressor $Y$ is obtained by computing the Jacobian $\frac{\partial F_a}{\partial \theta}$. Each row corresponds to one component of the 6D wrench $F_a = [n_x, n_y, n_z, f_x, f_y, f_z]^T$ and each column to one inertial parameter:

Y = \begin{bmatrix} 0 & 0 & \dot{v}_z + \omega_x v_y - \omega_y v_x & -\dot{v}_y + \omega_x v_z - \omega_z v_x & \dot{\omega}_x & \dot{\omega}_y - \omega_x\omega_z & \dot{\omega}_z + \omega_x\omega_y & -\omega_y\omega_z & \omega_y^2 - \omega_z^2 & \omega_y\omega_z \\ 0 & -\dot{v}_z - \omega_x v_y + \omega_y v_x & 0 & \dot{v}_x + \omega_y v_z - \omega_z v_y & \omega_x\omega_z & \dot{\omega}_x + \omega_y\omega_z & -\omega_x^2 + \omega_z^2 & \dot{\omega}_y & \dot{\omega}_z - \omega_x\omega_y & -\omega_x\omega_z \\ 0 & \dot{v}_y - \omega_x v_z + \omega_z v_x & -\dot{v}_x - \omega_y v_z + \omega_z v_y & 0 & -\omega_x\omega_y & \omega_x^2 - \omega_y^2 & \dot{\omega}_x - \omega_y\omega_z & \omega_x\omega_y & \dot{\omega}_y + \omega_x\omega_z & \dot{\omega}_z \\ \dot{v}_x + \omega_y v_z - \omega_z v_y & -\omega_y^2 - \omega_z^2 & -\dot{\omega}_z + \omega_x\omega_y & \dot{\omega}_y + \omega_x\omega_z & 0 & 0 & 0 & 0 & 0 & 0 \\ \dot{v}_y - \omega_x v_z + \omega_z v_x & \dot{\omega}_z + \omega_x\omega_y & -\omega_x^2 - \omega_z^2 & -\dot{\omega}_x + \omega_y\omega_z & 0 & 0 & 0 & 0 & 0 & 0 \\ \dot{v}_z + \omega_x v_y - \omega_y v_x & -\dot{\omega}_y + \omega_x\omega_z & \dot{\omega}_x + \omega_y\omega_z & -\omega_x^2 - \omega_y^2 & 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix}

Joint Torque Projection

For a revolute joint with axis $a = [a_x, a_y, a_z]^T$, the scalar joint torque is:

\tau = a^T \begin{bmatrix} Y_{1,1:10} \\ Y_{2,1:10} \\ Y_{3,1:10} \end{bmatrix} \theta

This gives a $1 \times 10$ regressor row per timestep. Stacking all timesteps yields the full system $\tau = Y\theta$.

Friction Extension

When friction is included, two columns are appended:

Y_{\text{friction}} = \begin{bmatrix} Y_{\text{inertial}} & \text{sign}(\dot{q}) & \dot{q} \end{bmatrix}

giving the total torque model:

\tau_{\text{total}} = a^T \begin{bmatrix} Y_{1:3} \end{bmatrix} \theta_{\text{inertial}} + f_c \cdot \text{sign}(\dot{q}) + f_v \cdot \dot{q}

Least-Squares Solution

The parameter vector is recovered via the Moore-Penrose pseudoinverse:

\theta = Y^\dagger \tau