In previous lessons we considered learning a monolithic policy from demonstrations via behavior cloning or inverse reinforcement learning. Many manipulation tasks, however, are naturally decomposed into skills or subtasks such as reach, grasp, lift, and place. Skill segmentation and library building address two questions:

• Segmentation: Given a continuous demonstration trajectory \( \{(x_t,u_t)\}_{t=1}^T \), where are the skill boundaries?
• Library building: Given many segmented demonstrations, what reusable skill templates should we store, and how do we parameterize them?

We assume demonstrations are recorded as state–action sequences \( x_t \in \mathbb{R}^n \), \( u_t \in \mathbb{R}^m \), with kinematics and dynamics known from the basic robotics course. The advanced aspects here are temporal abstraction and probabilistic modeling of segments.

flowchart TD
  D["Raw demonstrations (x_t, u_t)"] --> FEAT["Feature extraction phi(x_t, u_t)"]
  FEAT --> SEG["Skill segmentation: find boundaries tau_k"]
  SEG --> SEGTRJ["Segmented trajectories xi_k"]
  SEGTRJ --> LIB["Cluster / parametrize segments into skills"]
  LIB --> SKLIB["Skill library {Skill_1, ..., Skill_M}"]
  SKLIB --> REUSE["Reuse & sequence skills in new tasks"]
        

Mathematically, segmentation can be cast as:

\[ \boldsymbol{\xi} = \{(x_t,u_t)\}_{t=1}^T,\quad S = \{\tau_1,\dots,\tau_{K-1}\},\quad 1 = \tau_0 < \tau_1 < \dots < \tau_{K-1} < \tau_K = T. \]

Each segment \( k \) is \( \boldsymbol{\xi}_k = \{(x_t,u_t)\}_{t=\tau_{k-1}+1}^{\tau_k} \), and will later be mapped to a skill type in the library.

A basic formalization views segmentation as selecting change points to optimize a trade-off between segment fit and model complexity. Suppose each segment \( \boldsymbol{\xi}_k \) is modeled by parameters \( \theta_k \) and has likelihood \( p(\boldsymbol{\xi}_k \mid \theta_k) \). Given a segmentation \( S \), the total log-likelihood is:

\[ \log p(\boldsymbol{\xi} \mid S,\{\theta_k\}) = \sum_{k=1}^K \log p(\boldsymbol{\xi}_k \mid \theta_k). \]

We penalize having many segments by a constant \( \beta > 0 \) (per segment), and consider the objective:

\[ J(S,\{\theta_k\}) = \sum_{k=1}^K \log p(\boldsymbol{\xi}_k \mid \theta_k) - \beta K. \]

For many simple models (e.g. Gaussian in feature space, linear dynamical systems with segmentwise parameters), the maximum-likelihood estimate of \( \theta_k \) is available in closed form given the segment. Then we can define a segment cost

\[ C(i,j) = -\max_{\theta} \log p\Big( \{(x_t,u_t)\}_{t=i}^j \mid \theta \Big), \]

meaning that a lower cost indicates a better fit for the subsequence \( t=i,\dots,j \) under a single skill. The segmentation problem reduces to:

\[ \min_{S} \sum_{k=1}^K C(\tau_{k-1}+1,\tau_k) + \beta K. \]

This optimization can be solved by dynamic programming in \( \mathcal{O}(T^2) \) time for general segment costs, or faster for some restricted cost structures.

Define the optimal cost up to time \( j \) as \( D(j) \), with \( D(0)=0 \). The dynamic-programming recursion for the best segmentation ending at \( j \) is:

\[ D(j) = \min_{0 \leq i < j} \Big\{ D(i) + C(i+1,j) + \beta \Big\},\quad j=1,\dots,T. \]

Storing the minimizer \( i^* \) for each \( j \) allows backtracking to recover the optimal change points. Under mild conditions (additive segment costs, fixed penalty \( \beta \)), this dynamic program is globally optimal.

A more structured probabilistic view introduces a discrete skill label \( z_t \in \{1,\dots,M\} \) at each time step, and models the demonstration using a hidden Markov model (HMM):

\[ p(z_1) \prod_{t=2}^T p(z_t \mid z_{t-1}) \prod_{t=1}^T p(x_t,u_t \mid z_t,\theta_{z_t}). \]

Viterbi decoding computes the most likely skill sequence \( z_{1:T}^* \). In log-domain, the recursion is:

\[ \delta_t(j) = \max_i \Big( \delta_{t-1}(i) + \log A_{ij} \Big) + \log p(x_t,u_t \mid z_t=j), \]

where \( A_{ij} = p(z_t=j \mid z_{t-1}=i) \) and \( \delta_t(j) \) stores the best path ending in state \( j \) at time \( t \). The resulting skill sequence defines a segmentation via runs of constant \( z_t \).

HMMs implicitly assume geometric state-duration distributions. To encode more realistic dwell times (for skills like “grasp” that should persist), one may use hidden semi-Markov models (HSMMs), where each skill emits a variable-length segment with explicit duration distribution.

flowchart TD
  XU["Time series (x_t, u_t)"] --> MODEL["Choose model: DP cost or HMM/HSMM"]
  MODEL --> DP["DP over change points D(j)"]
  MODEL --> VIT["Viterbi over skill labels z_t"]
  DP --> BND["Boundaries tau_k"]
  VIT --> BND
  BND --> SEG["Segments xi_k with skill IDs z_k"]
        

Once segments \( \boldsymbol{\xi}_k \) are extracted, the next step is to construct a compact library of reusable skills. The key idea is to map each segment into a feature space and then cluster segments so that each cluster corresponds to a skill type.

Let \( \boldsymbol{\phi}(\boldsymbol{\xi}_k) \in \mathbb{R}^d \) be a feature vector for segment \( k \) (e.g., via resampling the trajectory to a fixed length, stacking joint positions and velocities). For a library of \( M \) skills with prototypes \( \mu_s \), a k-means type objective is:

\[ \min_{\{\mu_s\},\mathbf{z}} \sum_{k=1}^K \left\| \boldsymbol{\phi}(\boldsymbol{\xi}_k) - \mu_{z_k} \right\|_2^2, \]

where \( z_k \in \{1,\dots,M\} \) is the assigned skill label. A probabilistic variant uses Gaussian mixture models (GMMs), leading to soft assignments and likelihood-based selection of \( M \).

The library is then the collection \( \mathcal{L} = \{\text{Skill}_s(\cdot;\phi_s)\}_{s=1}^M \), where \( \phi_s \) denotes the parameters of the skill representation (e.g., dynamic movement primitive weights, local linear dynamics, or polynomial trajectories).

In later TAMP or planning modules, high-level planners operate over this discrete set of skills, composing them to solve long-horizon tasks.

A widely used parametric representation for skills is the dynamic movement primitive (DMP), which encodes a stable nonlinear dynamical system driving a point attractor from start \( y_0 \) to goal \( g \). For one degree of freedom, the DMP reads:

\[ \tau \dot{v} = \alpha_v \big(\beta_v (g - y) - v\big) + f(s),\quad \tau \dot{y} = v, \]

\[ \tau \dot{s} = -\alpha_s s, \]

where \( s \in [0,1] \) is a canonical phase variable, \( v \) is a velocity-like auxiliary variable, and \( f(s) \) is a nonlinear forcing term:

\[ f(s) = \frac{\sum_{i=1}^M w_i \psi_i(s)}{\sum_{i=1}^M \psi_i(s)} s,\quad \psi_i(s) = \exp\big(-h_i (s - c_i)^2\big). \]

Here \( w_i \) are learnable weights, and \( c_i,h_i \) define Gaussian basis functions in phase space. For a given demonstration in joint space, one can compute the desired \( f(s) \) that would reproduce the motion and then solve a linear least-squares problem for the weights \( w_i \). The resulting weights form part of the skill-parameter vector \( \phi_s \) stored in the library.

For multi-DOF manipulators, each joint can have its own DMP, or a low-dimensional task-space DMP can be mapped to joint space via inverse kinematics.

We now sketch a Python implementation of change-point detection using dynamic programming, and a simple feature-based clustering into a skill library. This core logic can later be embedded into a ROS node using rospy and integrated with motion planning stacks such as MoveIt.

import numpy as np
from numpy.linalg import lstsq
from sklearn.cluster import KMeans

def segment_cost_gaussian(traj, lam=1e-6):
    """
    traj: array of shape (L, d) with features phi(x_t, u_t) for a candidate segment.
    Fits a Gaussian with segment-specific mean and shared spherical covariance.
    Returns negative log-likelihood up to a constant.
    """
    L, d = traj.shape
    mu = traj.mean(axis=0)
    resid = traj - mu
    var = (resid ** 2).sum() / (L * d) + lam
    # Negative log-likelihood up to constants
    return 0.5 * L * d * np.log(var)

def build_segment_cost_matrix(phi, max_len=None):
    """
    phi: array of shape (T, d).
    Returns C[i, j] = segment_cost_gaussian(phi[i:j+1]) for all i <= j.
    Optionally restrict segment length by max_len for efficiency.
    """
    T, d = phi.shape
    C = np.full((T, T), np.inf)
    for i in range(T):
        max_j = T if max_len is None else min(T, i + max_len)
        for j in range(i, max_j):
            C[i, j] = segment_cost_gaussian(phi[i:j+1])
    return C

def segment_dp(phi, beta, max_len=None):
    """
    Dynamic-programming segmentation on features phi.
    beta: penalty per segment.
    Returns list of (start, end) indices for each segment.
    """
    T = phi.shape[0]
    C = build_segment_cost_matrix(phi, max_len=max_len)
    D = np.full(T + 1, np.inf)
    prev = np.full(T + 1, -1, dtype=int)
    D[0] = 0.0

    for j in range(1, T + 1):
        best_val = np.inf
        best_i = -1
        for i in range(0, j):
            cost = D[i] + C[i, j - 1] + beta
            if cost < best_val:
                best_val = cost
                best_i = i
        D[j] = best_val
        prev[j] = best_i

    # Backtrack
    segments = []
    j = T
    while j > 0:
        i = prev[j]
        segments.append((i, j - 1))
        j = i
    segments.reverse()
    return segments

def build_skill_library(phi, segments, num_skills):
    """
    phi: (T, d) feature matrix.
    segments: list of (i, j) index pairs.
    num_skills: desired library size M.
    Returns cluster assignments and cluster centers.
    """
    seg_feats = []
    for (i, j) in segments:
        # Simple feature: mean of features over the segment
        seg_feats.append(phi[i:j+1].mean(axis=0))
    seg_feats = np.vstack(seg_feats)

    km = KMeans(n_clusters=num_skills, n_init=10)
    z = km.fit_predict(seg_feats)
    centers = km.cluster_centers_
    return z, centers

# Example usage: phi_t is concatenated joint angle and velocity over time
T = 200
d = 7 * 2
phi = np.random.randn(T, d)
segments = segment_dp(phi, beta=5.0, max_len=40)
z, centers = build_skill_library(phi, segments, num_skills=4)
print("Segments:", segments)
print("Skill assignments:", z)
      

In a practical robotics system, phi would be constructed from measured joint states and end-effector poses (e.g. subscribed via rospy.Subscriber), and each cluster center would parameterize a DMP or other primitive to be stored in the robot's skill library.

In C++, a segmentation module can be implemented using Eigen for linear algebra and integrated into a ROS node using roscpp. Below is a simplified example that computes segment mean features and performs a greedy segmentation based on a variance threshold.

#include <vector>
#include <Eigen/Dense>

using Eigen::MatrixXd;
using Eigen::VectorXd;

struct Segment {
  int start;
  int end;  // inclusive
};

double segmentVariance(const MatrixXd& phi, int i, int j) {
  // phi: T x d
  int L = j - i + 1;
  MatrixXd seg = phi.block(i, 0, L, phi.cols());
  VectorXd mu = seg.colwise().mean();
  MatrixXd centered = seg.rowwise() - mu.transpose();
  double var = (centered.array() * centered.array()).sum() / (L * phi.cols());
  return var;
}

std::vector<Segment> greedySegment(const MatrixXd& phi, double var_threshold, int max_len) {
  std::vector<Segment> segments;
  int T = static_cast<int>(phi.rows());
  int i = 0;
  while (i < T) {
    int j = std::min(i + max_len - 1, T - 1);
    // shrink j until variance is below threshold
    while (j > i) {
      double var = segmentVariance(phi, i, j);
      if (var < var_threshold) break;
      --j;
    }
    Segment s{i, j};
    segments.push_back(s);
    i = j + 1;
  }
  return segments;
}

// In a ROS node, phi could be filled from sensor_msgs/JointState callbacks,
// and the resulting segments published as a custom message or used to
// parameterize movement primitives in a manipulation pipeline.
      

For a full dynamic-programming solution in C++, the recursion from Section 3 can be implemented with nested loops over indices and a precomputed cost matrix, similarly to the Python code, but using Eigen::MatrixXd.

Java is less common for low-level robot control but is used in higher-level orchestration and in some ROSJava-based systems. The following example illustrates a simple k-means based skill library over pre-segmented demonstrations.

public class SkillLibrary {
    private double[][] centers; // M x d

    public SkillLibrary(int numSkills, int dim) {
        centers = new double[numSkills][dim];
    }

    public int[] kMeans(double[][] segFeatures, int numIters) {
        int K = centers.length;
        int N = segFeatures.length;
        int d = segFeatures[0].length;
        int[] z = new int[N];

        // Initialize centers (first K segments)
        for (int k = 0; k < K; ++k) {
            System.arraycopy(segFeatures[k], 0, centers[k], 0, d);
        }

        for (int it = 0; it < numIters; ++it) {
            // Assignment step
            for (int n = 0; n < N; ++n) {
                z[n] = argMinDist(segFeatures[n]);
            }
            // Update step
            double[][] newCenters = new double[K][d];
            int[] counts = new int[K];
            for (int n = 0; n < N; ++n) {
                int k = z[n];
                counts[k]++;
                for (int j = 0; j < d; ++j) {
                    newCenters[k][j] += segFeatures[n][j];
                }
            }
            for (int k = 0; k < K; ++k) {
                if (counts[k] == 0) continue;
                for (int j = 0; j < d; ++j) {
                    newCenters[k][j] /= counts[k];
                }
                centers[k] = newCenters[k];
            }
        }
        return z;
    }

    private int argMinDist(double[] x) {
        int bestK = 0;
        double bestDist = Double.POSITIVE_INFINITY;
        for (int k = 0; k < centers.length; ++k) {
            double d2 = 0.0;
            for (int j = 0; j < x.length; ++j) {
                double diff = x[j] - centers[k][j];
                d2 += diff * diff;
            }
            if (d2 < bestDist) {
                bestDist = d2;
                bestK = k;
            }
        }
        return bestK;
    }

    public double[][] getCenters() {
        return centers;
    }
}
      

In a ROSJava context, segment features could be received via topics from lower-level controllers, clustered using this class, and then mapped to higher-level skill identifiers for planning.

MATLAB, together with the Robotics System Toolbox and Simulink, is widely used to prototype segmentation and skill execution. Below is a MATLAB function implementing dynamic-programming segmentation on a feature matrix phi.

function segments = dp_segment(phi, beta, maxLen)
% phi: T x d feature matrix
% beta: penalty per segment
% maxLen: optional maximum segment length

[T, ~] = size(phi);
if nargin < 3
    maxLen = T;
end

% Precompute segment costs
C = inf(T, T);
for i = 1:T
    maxJ = min(T, i + maxLen - 1);
    for j = i:maxJ
        seg = phi(i:j, :);
        mu = mean(seg, 1);
        centered = seg - mu;
        varSeg = sum(centered(:).^2) / numel(seg);
        C(i, j) = 0.5 * numel(seg) * log(varSeg);
    end
end

D = inf(T + 1, 1);
prev = -ones(T + 1, 1);
D(1) = 0.0;

for j = 2:T+1
    bestVal = inf;
    bestI = -1;
    for i = 1:j-1
        cost = D(i) + C(i, j - 1) + beta;
        if cost < bestVal
            bestVal = cost;
            bestI = i;
        end
    end
    D(j) = bestVal;
    prev(j) = bestI;
end

% Backtrack
segments = [];
j = T + 1;
while j > 1
    i = prev(j);
    segments = [i, j - 1; segments]; %#ok<AGROW>
    j = i;
end
end
      

In Simulink, a typical architecture is:

• A block that reads joint states and end-effector pose, computes features \( \boldsymbol{\phi}(x_t,u_t) \).
• A MATLAB Function block implementing dp_segment periodically on a sliding window of data.
• A library of DMP blocks, each parameterized by stored weights \( w_i \), selectable via a skill index input.

The Robotics System Toolbox can be used to compute forward/inverse kinematics for mapping DMP outputs to joint commands.

Mathematica is convenient for prototyping segmentation algorithms symbolically and numerically, especially when experimenting with different cost functions. Below is a simple implementation of a dynamic-programming segmentation with quadratic reconstruction cost.

(* phi: list of T feature vectors in R^d *)
segmentCost[phi_List, i_Integer, j_Integer] := Module[
  {seg, mu, centered, var},
  seg = phi[[i ;; j]];
  mu = Mean[seg];
  centered = seg - mu;
  var = Total[centered.centered]/(Length[seg] * Length[mu]);
  0.5 * Length[seg] * Length[mu] * Log[var]
];

dpSegment[phi_List, beta_?NumericQ] := Module[
  {T, D, prev, C, i, j, cost, bestVal, bestI, segments},
  T = Length[phi];
  D = Table[Infinity, {T + 1}];
  prev = Table[-1, {T + 1}];
  D[[1]] = 0.0;

  (* Precompute costs lazily *)
  C[i_, j_] := segmentCost[phi, i, j];

  For[j = 2, j <= T + 1, j++,
    bestVal = Infinity;
    bestI = -1;
    For[i = 1, i <= j - 1, i++,
      cost = D[[i]] + C[i, j - 1] + beta;
      If[cost < bestVal,
        bestVal = cost;
        bestI = i;
      ];
    ];
    D[[j]] = bestVal;
    prev[[j]] = bestI;
  ];

  segments = {};
  j = T + 1;
  While[j > 1,
    i = prev[[j]];
    segments = Prepend[segments, {i, j - 1}];
    j = i;
  ];
  segments
]

(* Example usage with random 2D features *)
phi = Table[RandomReal[{-1, 1}, 2], {t, 1, 200}];
segments = dpSegment[phi, 5.0];
Print[segments];
      

Mathematica's visualization tools can be used to overlay discovered segment boundaries on joint-space trajectories, which is useful for debugging segmentation logic before porting to real-time robot code.

Problem 1 (Optimality of the DP Segmentation Recursion). Consider the cost functional \( J(S) = \sum_{k=1}^K C(\tau_{k-1}+1,\tau_k) + \beta K \) over all segmentations with change points \( S \). Show that the dynamic-programming recursion

\[ D(j) = \min_{0 \leq i < j} \left\{ D(i) + C(i+1,j) + \beta \right\},\quad D(0) = 0 \]

indeed yields the globally optimal segmentation.

Solution. For any segmentation ending at time \( j \) with last segment starting at \( i+1 \), the total cost can be written as \( J = J_{\text{prefix}} + C(i+1,j) + \beta \), where \( J_{\text{prefix}} \) is the cost of segmenting times \( 1,\dots,i \). By the principle of optimality, if the last segment starts at \( i+1 \) in an optimal segmentation, then the prefix segmentation must minimize cost among all segmentations of \( 1,\dots,i \). Thus \( J_{\text{prefix}} = D(i) \). Minimizing over possible \( i \) gives exactly the recursion for \( D(j) \). Since the recursion considers all possible last-change indices at each step and composes optimal prefixes, the resulting segmentation is globally optimal.

Problem 2 (Relation between HMM and Change-Point Models). Let \( z_t \) be the HMM skill label at time \( t \). Show that if the transition matrix satisfies \( A_{ii} = 1 - \alpha_i \), \( A_{ij} = \alpha_i q_{ij} \) for \( j \neq i \) with \( \sum_{j \neq i} q_{ij} = 1 \), then the distribution over segment lengths for skill \( i \) is geometric with parameter \( \alpha_i \).

Solution. The duration of a run in state \( i \) is the number of consecutive time steps spent in \( i \) before transitioning away. At each step, the probability of staying is \( 1 - \alpha_i \) and the probability of leaving is \( \alpha_i \). Therefore,

\[ p(\text{duration} = \ell \mid z_{\text{enter}} = i) = (1 - \alpha_i)^{\ell-1} \alpha_i, \]

which is exactly a geometric distribution with parameter \( \alpha_i \). Hence a standard HMM corresponds to a change-point model with geometrically distributed segment lengths.

Problem 3 (Stability of DMPs). Consider the DMP without forcing term, \( f(s) = 0 \):

\[ \tau \dot{v} = \alpha_v \big(\beta_v (g - y) - v\big),\quad \tau \dot{y} = v. \]

Show that for \( \alpha_v > 0 \), \( \beta_v > 0 \), the point \( (y,v) = (g,0) \) is an asymptotically stable equilibrium.

Solution. At equilibrium, \( \dot{y} = 0 \) and \( \dot{v} = 0 \). From \( \dot{y} = v/\tau \), we obtain \( v = 0 \), and from \( \dot{v} = \alpha_v(\beta_v(g-y) - v)/\tau \) with \( v=0 \), we obtain \( y = g \). Thus \( (g,0) \) is the unique equilibrium. Linearizing the system around \( (g,0) \) with state \( e_y = y - g \), \( e_v = v \), we obtain

\[ \tau \dot{e}_v = -\alpha_v \beta_v e_y - \alpha_v e_v,\quad \tau \dot{e}_y = e_v. \]

In matrix form \( \tau \dot{\mathbf{e}} = A \mathbf{e} \) with \( \mathbf{e} = (e_y,e_v)^\top \) and

\[ A = \begin{bmatrix} 0 & 1 \\ -\alpha_v \beta_v & -\alpha_v \end{bmatrix}. \]

The characteristic polynomial is \( \lambda^2 + \alpha_v \lambda + \alpha_v \beta_v = 0 \). Since \( \alpha_v > 0 \), \( \beta_v > 0 \), both coefficients are positive, and by the Routh–Hurwitz criterion, both eigenvalues have negative real part. Therefore the equilibrium is asymptotically stable.

Problem 4 (Choosing the Segment Penalty \( \beta \)). Consider a model where each segment cost is approximately proportional to its length, \( C(i,j) \approx c_0 (j-i+1) \), and suppose the true data are generated by a single skill. What happens if \( \beta \) is chosen too small or too large? Provide a qualitative explanation.

Solution. If \( \beta \) is too small relative to the typical reduction in cost achieved by splitting a segment (e.g., due to noise), the dynamic program will find many short segments because each additional segment only incurs a small penalty but allows a better fit, leading to over-segmentation. Conversely, if \( \beta \) is too large, introducing a new segment rarely compensates for its penalty, and the optimal solution will favor very few segments, potentially merging distinct skills (under-segmentation). Thus \( \beta \) controls the complexity of the segmentation and must be tuned to balance bias and variance.

Problem 5 (Feature Choice for Skill Clustering). Suppose we represent each segment \( \boldsymbol{\xi}_k \) by the time-averaged joint position and velocity vector \( \boldsymbol{\phi}(\boldsymbol{\xi}_k) \). Discuss a situation in which this feature representation might fail to distinguish two different skills, and propose a richer feature that could resolve the ambiguity.

Solution. Time-averaged joint positions and velocities discard temporal ordering and detailed trajectory shape. Two skills that visit the same part of the workspace but with different temporal profiles (e.g., “fast reach–slow retract” vs “slow reach–fast retract”) may have similar averages and be clustered together, even though they are qualitatively different. A richer feature could be to resample each segment to a fixed number of phases and concatenate the positions or velocities at each phase, yielding a higher-dimensional feature that preserves trajectory shape. Alternatively, coefficients of a basis expansion (e.g., Fourier or polynomial) of the joint trajectories over time could be used.

In this lesson we formalized skill segmentation as an optimization problem over change points and as probabilistic inference in HMM/HSMM models. We derived a dynamic-programming recursion that yields globally optimal segmentations for additive cost models, and showed how segmented demonstrations can be clustered into a reusable skill library. We introduced DMPs as a stable parametric representation of individual skills and provided implementation sketches in Python, C++, Java, MATLAB/Simulink, and Mathematica. These tools enable robots to acquire modular skills from demonstration, setting the stage for composing skills in task and motion planning and for integrating with reinforcement learning in subsequent chapters.