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 solves for 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. The regressor matrix is typically rank-deficient (e.g., rank 7 for the Kinova example), meaning not all parameters are independently identifiable — the tool reports the number of identifiable base parameters as a quality metric and handles rank deficiency gracefully via minimum-norm least-squares.

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, appending two columns to the regressor.

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 & Base Parameters

The parameter vector is recovered via the Moore-Penrose pseudoinverse, which handles rank-deficient regressors gracefully (minimum-norm solution):

\theta = Y^\dagger \tau

Due to the structure of the dynamics equations, the regressor is typically rank-deficient — not all 10 inertial parameters are independently identifiable from single-joint torque measurements. The identifiable combinations are called base parameters. The tool reports the number of base parameters (matrix rank) as a quality metric; for well-conditioned data this should be close to 10.