In previous chapters we modeled manipulator dynamics using deterministic parameters: link masses, centers of mass, inertia tensors, and friction coefficients. In practice these parameters are never known exactly: they result from CAD approximations, manufacturing tolerances, payload variations, and imperfect identification experiments. Probabilistic dynamics explicitly models this uncertainty to obtain distributions over joint torques and motions instead of single deterministic predictions.

Recall the standard rigid-body dynamics for an \(n\)-DOF manipulator:

\[ \mathbf{M}(\mathbf{q},\boldsymbol{\theta})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}},\boldsymbol{\theta})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q},\boldsymbol{\theta}) + \mathbf{f}(\dot{\mathbf{q}},\boldsymbol{\theta}) = \boldsymbol{\tau} + \mathbf{w} \]

where \( \mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}} \in \mathbb{R}^n \), \( \boldsymbol{\theta} \in \mathbb{R}^p \) is the vector of inertial and friction parameters, \( \boldsymbol{\tau} \) are joint torques, and \( \mathbf{w} \) lumps unmodeled dynamics and measurement errors.

In probabilistic dynamics we promote \( \boldsymbol{\theta} \) and possibly \( \mathbf{w} \) to random variables. A typical assumption is a Gaussian prior on \( \boldsymbol{\theta} \):

\[ \boldsymbol{\theta} \sim \mathcal{N}\!\big( \bar{\boldsymbol{\theta}},\ \boldsymbol{\Sigma}_{\theta} \big), \]

leading to a random torque response \( \boldsymbol{\tau} \) for any given trajectory \( \mathbf{q}(t) \).

flowchart TD
  P["Physical robot"] --> N["Nominal parameters theta_hat"]
  P --> D["Data: (q, qdot, qddot, tau)"]
  N --> M["Deterministic model"]
  D --> U["Estimate parameter distribution p(theta)"]
  M --> PD["Predict tau from theta_hat"]
  U --> PD
  PD --> OUT["Probabilistic dynamics: tau ~ N(mu_tau, Sigma_tau)"]
        

From Chapter 13 and 19, the dynamic parameters \( \boldsymbol{\theta} \) typically contain for each link: mass, first moments, and independent inertia-tensor entries, optionally augmented with friction and motor parameters. We now treat \( \boldsymbol{\theta} \) as a random vector on a probability space \( (\Omega,\mathcal{F},\mathbb{P}) \):

\[ \boldsymbol{\theta} : \Omega \rightarrow \mathbb{R}^p. \]

Its first- and second-order statistics are

\[ \bar{\boldsymbol{\theta}} = \mathbb{E}[\boldsymbol{\theta}], \qquad \boldsymbol{\Sigma}_{\theta} = \mathbb{E}\!\big[ (\boldsymbol{\theta}-\bar{\boldsymbol{\theta}}) (\boldsymbol{\theta}-\bar{\boldsymbol{\theta}})^{\top} \big]. \]

The linear-in-parameters form of rigid-body dynamics derived in Chapter 19 is

\[ \boldsymbol{\tau} = \mathbf{Y}(\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}})\, \boldsymbol{\theta}, \]

where \( \mathbf{Y} \in \mathbb{R}^{n \times p} \) is the regressor matrix determined by the configuration and its derivatives. This representation is crucial: it makes the map from \( \boldsymbol{\theta} \) to \( \boldsymbol{\tau} \) affine and thus analytically tractable for many probability distributions.

When model errors \( \mathbf{w} \) and measurement noise are modeled as a separate random term \( \boldsymbol{\varepsilon} \),

\[ \boldsymbol{\tau} = \mathbf{Y}\boldsymbol{\theta} + \boldsymbol{\varepsilon}, \qquad \boldsymbol{\varepsilon} \sim \mathcal{N}(\mathbf{0},\boldsymbol{\Sigma}_{\varepsilon}), \]

we obtain a linear-Gaussian model that is the basis for Bayesian parameter estimation and torque prediction.

Consider the linear model without noise for a fixed regressor \( \mathbf{Y} \):

\[ \boldsymbol{\tau} = \mathbf{Y}\boldsymbol{\theta}. \]

Assume \( \boldsymbol{\theta} \) has mean \( \bar{\boldsymbol{\theta}} \) and covariance \( \boldsymbol{\Sigma}_{\theta} \). Then:

\[ \mathbb{E}[\boldsymbol{\tau}] = \mathbf{Y}\,\bar{\boldsymbol{\theta}}, \qquad \operatorname{Cov}(\boldsymbol{\tau}) = \mathbf{Y}\,\boldsymbol{\Sigma}_{\theta}\,\mathbf{Y}^{\top}. \]

Proof. Using linearity of expectation,

\[ \mathbb{E}[\boldsymbol{\tau}] = \mathbb{E}[\mathbf{Y}\boldsymbol{\theta}] = \mathbf{Y}\,\mathbb{E}[\boldsymbol{\theta}] = \mathbf{Y}\,\bar{\boldsymbol{\theta}}. \]

For the covariance,

\[ \begin{aligned} \operatorname{Cov}(\boldsymbol{\tau}) &= \mathbb{E}\big[ (\boldsymbol{\tau}-\mathbb{E}[\boldsymbol{\tau}]) (\boldsymbol{\tau}-\mathbb{E}[\boldsymbol{\tau}])^{\top} \big] \\ &= \mathbb{E}\big[ \mathbf{Y}(\boldsymbol{\theta}-\bar{\boldsymbol{\theta}}) (\boldsymbol{\theta}-\bar{\boldsymbol{\theta}})^{\top}\mathbf{Y}^{\top} \big] \\ &= \mathbf{Y}\, \mathbb{E}\big[ (\boldsymbol{\theta}-\bar{\boldsymbol{\theta}}) (\boldsymbol{\theta}-\bar{\boldsymbol{\theta}})^{\top} \big]\, \mathbf{Y}^{\top} \\ &= \mathbf{Y}\,\boldsymbol{\Sigma}_{\theta}\,\mathbf{Y}^{\top}. \end{aligned} \]

If in addition \( \boldsymbol{\theta} \sim \mathcal{N}(\bar{\boldsymbol{\theta}},\boldsymbol{\Sigma}_{\theta}) \), then by the closure of the Gaussian family under affine maps,

\[ \boldsymbol{\tau} \sim \mathcal{N}\!\big( \mathbf{Y}\bar{\boldsymbol{\theta}},\ \mathbf{Y}\boldsymbol{\Sigma}_{\theta}\mathbf{Y}^{\top} \big). \]

With additive Gaussian torque noise \( \boldsymbol{\varepsilon}\sim \mathcal{N}(\mathbf{0},\boldsymbol{\Sigma}_{\varepsilon}) \), the total covariance becomes \( \mathbf{Y}\boldsymbol{\Sigma}_{\theta}\mathbf{Y}^{\top} + \boldsymbol{\Sigma}_{\varepsilon} \).

Least-squares identification in Chapter 19 produced a point estimate \( \hat{\boldsymbol{\theta}} \). A probabilistic treatment instead returns a posterior distribution over \( \boldsymbol{\theta} \) conditioned on observed data.

Suppose we have \( N \) samples (e.g., from an identification experiment), each with regressor \( \mathbf{Y}_k \in \mathbb{R}^{n \times p} \) and measured torque \( \boldsymbol{\tau}_k \in \mathbb{R}^n \):

\[ \boldsymbol{\tau}_k = \mathbf{Y}_k \boldsymbol{\theta} + \boldsymbol{\varepsilon}_k,\qquad \boldsymbol{\varepsilon}_k \sim \mathcal{N}(\mathbf{0},\boldsymbol{\Sigma}_{\varepsilon}), \]

with \( \boldsymbol{\varepsilon}_k \) i.i.d. and independent of \( \boldsymbol{\theta} \). Stack all samples:

\[ \mathbf{Y}_{\text{all}} = \begin{bmatrix} \mathbf{Y}_1 \\ \vdots \\ \mathbf{Y}_N \end{bmatrix}, \qquad \boldsymbol{\tau}_{\text{all}} = \begin{bmatrix} \boldsymbol{\tau}_1 \\ \vdots \\ \boldsymbol{\tau}_N \end{bmatrix}. \]

Assume a Gaussian prior \( \boldsymbol{\theta} \sim \mathcal{N}(\boldsymbol{\theta}_0,\boldsymbol{\Sigma}_0) \). The posterior is again Gaussian with parameters (Bayesian linear regression):

\[ \boldsymbol{\Sigma}_{\text{post}}^{-1} = \boldsymbol{\Sigma}_0^{-1} + \mathbf{Y}_{\text{all}}^{\top} \boldsymbol{\Sigma}_{\varepsilon}^{-1} \mathbf{Y}_{\text{all}}, \]

\[ \boldsymbol{\theta}_{\text{post}} = \boldsymbol{\Sigma}_{\text{post}} \left( \boldsymbol{\Sigma}_0^{-1}\boldsymbol{\theta}_0 + \mathbf{Y}_{\text{all}}^{\top} \boldsymbol{\Sigma}_{\varepsilon}^{-1} \boldsymbol{\tau}_{\text{all}} \right). \]

As \( \boldsymbol{\Sigma}_0 \) becomes very large (weak prior), the posterior mean approaches the ordinary least-squares estimate. The posterior covariance \( \boldsymbol{\Sigma}_{\text{post}} \) quantifies how well the experiment excited the parameters.

The predictive torque distribution at a new regressor \( \mathbf{Y}_* \) is then

\[ \boldsymbol{\tau}_* \mid \text{data} \sim \mathcal{N}\!\big( \mathbf{Y}_*\boldsymbol{\theta}_{\text{post}},\ \mathbf{Y}_*\boldsymbol{\Sigma}_{\text{post}}\mathbf{Y}_*^{\top} + \boldsymbol{\Sigma}_{\varepsilon} \big), \]

combining epistemic uncertainty (in \( \boldsymbol{\theta} \)) and aleatoric torque noise.

flowchart TD
  PR["Prior: theta ~ N(theta_0, Sigma_0)"] --> LK["Likelihood from data (Y_k, tau_k)"]
  D["Logged samples (Y_k, tau_k)"] --> LK
  LK --> PO["Posterior N(theta_post, Sigma_post)"]
  PR --> PO
  PO --> PD["Predictive torque distribution at new Y_*"]
        

For a given torque command profile \( \boldsymbol{\tau}(t) \), the state \( \mathbf{x} = [\mathbf{q}^{\top},\dot{\mathbf{q}}^{\top}]^{\top} \) evolves according to

\[ \dot{\mathbf{x}} = \mathbf{f}\big(\mathbf{x},\boldsymbol{\tau},\boldsymbol{\theta}\big), \]

where \( \mathbf{f} \) encodes the manipulator equations of motion (from Chapters 10–12). With uncertain \( \boldsymbol{\theta} \), the resulting state trajectory \( \mathbf{x}(t) \) becomes random.

For small parameter uncertainty one can approximate the effect by first-order linearization about the mean \( \bar{\boldsymbol{\theta}} \) and nominal trajectory \( \bar{\mathbf{x}}(t) \) satisfying \( \dot{\bar{\mathbf{x}}} = \mathbf{f}(\bar{\mathbf{x}},\boldsymbol{\tau},\bar{\boldsymbol{\theta}}) \).

\[ \delta \dot{\mathbf{x}}(t) \approx \mathbf{A}(t)\,\delta \mathbf{x}(t) + \mathbf{B}_{\theta}(t)\,\delta\boldsymbol{\theta}, \]

\[ \mathbf{A}(t) = \left.\frac{\partial \mathbf{f}}{\partial \mathbf{x}}\right| _{(\bar{\mathbf{x}}(t),\boldsymbol{\tau}(t),\bar{\boldsymbol{\theta}})}, \qquad \mathbf{B}_{\theta}(t) = \left.\frac{\partial \mathbf{f}}{\partial \boldsymbol{\theta}}\right| _{(\bar{\mathbf{x}}(t),\boldsymbol{\tau}(t),\bar{\boldsymbol{\theta}})}. \]

Discretizing with step \( \Delta t \) yields approximately

\[ \delta \mathbf{x}_{k+1} \approx \mathbf{\Phi}_k\,\delta \mathbf{x}_k + \mathbf{\Gamma}_k\,\delta\boldsymbol{\theta}, \]

where \( \mathbf{\Phi}_k \) is the state transition matrix and \( \mathbf{\Gamma}_k \) the parameter sensitivity. Assuming \( \delta\boldsymbol{\theta} \) is time-invariant and Gaussian, the state covariance can be propagated via

\[ \boldsymbol{\Sigma}_{x,k+1} \approx \mathbf{\Phi}_k \boldsymbol{\Sigma}_{x,k} \mathbf{\Phi}_k^{\top} + \mathbf{\Gamma}_k \boldsymbol{\Sigma}_{\theta} \mathbf{\Gamma}_k^{\top}. \]

This linearization-based propagation is often combined with Monte Carlo simulation to validate approximations.

We illustrate probabilistic dynamics on a simple 1-DOF rotary joint (e.g., a pendulum) whose dynamics in a simplified linear-in-parameters form is

\[ \tau = a\,\ddot{q} + b\,\sin(q), \]

where \( a \) and \( b \) are uncertain physical parameters collected in \( \boldsymbol{\theta} = [a\ b]^{\top} \). The regressor is \( \mathbf{Y} = [\ddot{q}\ \ \sin(q)] \).

import numpy as np

# Regressor for 1-DOF rotary joint
def Y_pendulum(q, qd, qdd):
    return np.array([[qdd, np.sin(q)]])  # shape (1, 2)

# Parameter distribution (Gaussian)
theta_mean = np.array([1.5, 0.8])                # a, b
Sigma_theta = np.array([[0.04, 0.0],
                        [0.0,  0.01]])           # covariance

# Operating point
q   = 0.5       # rad
qd  = 0.0       # rad/s
qdd = 2.0       # rad/s^2

Y = Y_pendulum(q, qd, qdd)                       # (1, 2)

# Analytical mean and variance of tau
tau_mean = (Y @ theta_mean)[0]
tau_var  = (Y @ Sigma_theta @ Y.T)[0, 0]

print("Analytical E[tau] =", tau_mean)
print("Analytical Var[tau] =", tau_var)

# Monte Carlo check
rng = np.random.default_rng(0)
Nmc = 100000
theta_samples = rng.multivariate_normal(theta_mean, Sigma_theta, size=Nmc)
tau_samples = (Y @ theta_samples.T).ravel()

print("MC mean approx =", tau_samples.mean())
print("MC var approx  =", tau_samples.var())

# Example: include additive torque noise (independent Gaussian)
sigma_eps = 0.05
tau_noisy_samples = tau_samples + rng.normal(0.0, sigma_eps, size=Nmc)
tau_total_var = tau_var + sigma_eps**2
print("Total theoretical Var[tau+eps] =", tau_total_var)
      

In more complex manipulators, one would obtain \( \mathbf{Y} \) using a robotics dynamics library (e.g., roboticstoolbox-python or pinocchio) and apply the same mean/variance formulas \( \mathbf{Y}\bar{\boldsymbol{\theta}} \) and \( \mathbf{Y}\boldsymbol{\Sigma}_{\theta}\mathbf{Y}^{\top} \).

In C++, probabilistic dynamics can be implemented using Eigen for linear algebra together with a dynamics library such as RBDL or Pinocchio. Below we keep the same 1-DOF example and focus on the linear algebra.

#include <iostream>
#include <Eigen/Dense>
#include <random>

int main() {
    using Eigen::Matrix2d;
    using Eigen::Vector2d;
    using Eigen::RowVector2d;

    // Parameter distribution
    Vector2d theta_mean;
    theta_mean << 1.5, 0.8;

    Matrix2d Sigma_theta;
    Sigma_theta <<
        0.04, 0.0,
        0.0,  0.01;

    // Regressor Y(q, qd, qdd)
    double q   = 0.5;
    double qd  = 0.0;
    double qdd = 2.0;
    RowVector2d Y;
    Y << qdd, std::sin(q);

    // Analytical mean and variance of tau
    double tau_mean = (Y * theta_mean)(0);
    double tau_var  = (Y * Sigma_theta * Y.transpose())(0, 0);

    std::cout << "Analytical E[tau] = " << tau_mean << std::endl;
    std::cout << "Analytical Var[tau] = " << tau_var << std::endl;

    // Monte Carlo simulation
    std::mt19937 gen(0);
    std::normal_distribution<double> std_normal(0.0, 1.0);

    // Cholesky of Sigma_theta (since it is SPD)
    Eigen::LLT<Matrix2d> llt(Sigma_theta);
    Matrix2d L = llt.matrixL();

    const int Nmc = 100000;
    double mean_mc = 0.0;
    double m2_mc   = 0.0;

    for (int k = 0; k < Nmc; ++k) {
        // Sample z ~ N(0, I)
        Vector2d z;
        z << std_normal(gen), std_normal(gen);

        // theta_sample = theta_mean + L * z
        Vector2d theta_sample = theta_mean + L * z;

        double tau_sample = (Y * theta_sample)(0);

        // Online mean/variance (Welford)
        double delta = tau_sample - mean_mc;
        mean_mc += delta / static_cast<double>(k + 1);
        m2_mc   += delta * (tau_sample - mean_mc);
    }

    double var_mc = m2_mc / static_cast<double>(Nmc - 1);

    std::cout << "MC mean approx = " << mean_mc << std::endl;
    std::cout << "MC var approx  = " << var_mc << std::endl;

    return 0;
}
      

In a full robot implementation, Y would be obtained from a regressor function of RBDL or Pinocchio, and the same propagation formulas would apply.

Java does not have a de-facto standard robotics dynamics library, but probabilistic dynamics can be implemented using basic linear algebra (or libraries such as EJML). We again use the 1-DOF example with independent parameters \( a \) and \( b \).

import java.util.Random;

public class ProbabilisticDynamics1DOF {

    // Regressor Y(q, qd, qdd) = [qdd, sin(q)]
    static double[] regressor(double q, double qd, double qdd) {
        return new double[]{qdd, Math.sin(q)};
    }

    public static void main(String[] args) {
        // Parameter mean and (diagonal) covariance
        double[] thetaMean = {1.5, 0.8};
        double[] thetaStd  = {0.2, 0.1}; // sqrt of variances

        double q   = 0.5;
        double qd  = 0.0;
        double qdd = 2.0;
        double[] Y = regressor(q, qd, qdd); // length 2

        // Analytical mean and variance (independent parameters)
        double tauMean = Y[0] * thetaMean[0] + Y[1] * thetaMean[1];
        double tauVar  =
                Y[0] * Y[0] * thetaStd[0] * thetaStd[0]
              + Y[1] * Y[1] * thetaStd[1] * thetaStd[1];

        System.out.println("Analytical E[tau] = " + tauMean);
        System.out.println("Analytical Var[tau] = " + tauVar);

        // Monte Carlo sampling
        Random rng = new Random(0L);
        int Nmc = 100000;
        double meanMc = 0.0;
        double m2Mc   = 0.0;

        for (int k = 0; k < Nmc; ++k) {
            double aSample = thetaMean[0] + thetaStd[0] * rng.nextGaussian();
            double bSample = thetaMean[1] + thetaStd[1] * rng.nextGaussian();

            double tauSample = Y[0] * aSample + Y[1] * bSample;

            double delta = tauSample - meanMc;
            meanMc += delta / (k + 1);
            m2Mc   += delta * (tauSample - meanMc);
        }

        double varMc = m2Mc / (Nmc - 1);
        System.out.println("MC mean approx = " + meanMc);
        System.out.println("MC var approx  = " + varMc);
    }
}
      

Extending this to \( n \) DOF and correlated parameters only requires matrix operations (e.g., using EJML) to implement \( \mathbf{Y}\boldsymbol{\Sigma}_{\theta}\mathbf{Y}^{\top} \) and Gaussian sampling with a Cholesky factor.

MATLAB provides convenient support for multivariate normal sampling and Simulink integration. The following script computes an analytical torque distribution and validates it via Monte Carlo. It can be used offline or as a data source for Simulink blocks.

% Parameter mean and covariance
theta_mu     = [1.5; 0.8];
Sigma_theta  = [0.04 0.0;
                0.0  0.01];

% Operating point
q   = 0.5;
qd  = 0.0;
qdd = 2.0;

% Regressor for 1-DOF system
Y = [qdd, sin(q)];   % 1-by-2

% Analytical mean and variance
tau_mu  = Y * theta_mu;
tau_var = Y * Sigma_theta * Y.';  % 1-by-1

fprintf("Analytical E[tau] = %f\n", tau_mu);
fprintf("Analytical Var[tau] = %f\n", tau_var);

% Monte Carlo sampling
Nmc = 100000;
theta_samples = mvnrnd(theta_mu.', Sigma_theta, Nmc);   % Nmc-by-2
tau_samples   = (Y * theta_samples.').';               % Nmc-by-1

fprintf("MC mean approx = %f\n", mean(tau_samples));
fprintf("MC var approx  = %f\n", var(tau_samples, 1));

% Optional: export samples for use in Simulink
tau_time = [(0:Nmc-1).', tau_samples];  % time index, value
save("tau_samples.mat", "tau_time");
      

In Simulink, a typical setup is:

• Use a From Workspace block pointing to tau_time to feed a stochastic torque profile.
• Use a MATLAB Function block to implement the manipulator dynamics (with parameters in a Simulink.Parameter object).
• Repeat simulations with different parameter samples to obtain distributions over joint trajectories.

Wolfram Mathematica can represent the linear-in-parameters structure symbolically and compute mean/covariance expressions analytically before numerical evaluation.

(* Symbolic regressor and parameters *)
Clear[q, qd, qdd, a, b];
Y = { {qdd, Sin[q]} };                (* 1 x 2 regressor *)
theta = {a, b};                     (* parameter vector *)

(* Torque expression *)
tau = Y.theta // Simplify

(* Parameter mean and covariance (symbolic) *)
thetaMu = {Subscript[a, 0], Subscript[b, 0]};
SigmaTheta = { {Subscript[saa, 0], Subscript[sab, 0]},
              {Subscript[sab, 0], Subscript[sbb, 0]} };

(* Analytical mean and variance of tau *)
tauMu = (Y.thetaMu)[[1]] // Simplify
tauVar = (Y.SigmaTheta.Transpose[Y])[[1, 1]] // Simplify

(* Numeric example *)
qVal   = 0.5;
qdVal  = 0.0;
qddVal = 2.0;

thetaMuNum = {1.5, 0.8};
SigmaThetaNum = { {0.04, 0.0},
                 {0.0,  0.01} };

Ynum = Y /. {q -> qVal, qdd -> qddVal};

tauMuNum = (Ynum.thetaMuNum)[[1]];
tauVarNum = (Ynum.SigmaThetaNum.Transpose[Ynum])[[1, 1]];

Print["Analytical E[tau] = ", tauMuNum];
Print["Analytical Var[tau] = ", tauVarNum];

(* Monte Carlo sampling *)
distTheta = MultinormalDistribution[thetaMuNum, SigmaThetaNum];
thetaSamples = RandomVariate[distTheta, 100000];
tauSamples = (Ynum.#)& /@ thetaSamples // Flatten;

N@Mean[tauSamples]
N@Variance[tauSamples]
      

This environment is particularly powerful for deriving closed-form covariance and sensitivity expressions before implementing numerical approximations in other languages.

In this lesson we introduced probabilistic dynamics by modeling the dynamic parameter vector \( \boldsymbol{\theta} \) as a random variable. Using the linear-in-parameters representation of manipulator dynamics, we derived analytical expressions for the mean and covariance of the torque vector \( \boldsymbol{\tau} \), showed how Bayesian linear regression yields a posterior distribution over parameters, and outlined trajectory-level covariance propagation via linearization. Implementation examples in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica demonstrated how these concepts can be realized numerically and connected to existing robotics dynamics libraries.

Probabilistic dynamics forms a key bridge between modeling and robust or stochastic control, enabling quantification of risk and confidence in predicted robot motions. Subsequent lessons on differentiable physics and open modeling problems build on this probabilistic view.