For a rigid manipulator, the state is typically \( \mathbf{x}_r = (\mathbf{q}_r, \dot{\mathbf{q}}_r) \in \mathbb{R}^{2n_r} \), where \( \mathbf{q}_r \) are joint angles or generalized coordinates. For a deformable object, the configuration is no longer given by a finite vector of rigid poses but by a continuous field. Let \( \Omega \subset \mathbb{R}^3 \) denote the reference (material) domain of the object, and let \( \boldsymbol{\phi}(\mathbf{X}, t): \Omega \times \mathbb{R} \to \mathbb{R}^3 \) map each material point \( \mathbf{X} \) to its position in space at time \( t \).

\[ \text{Full continuous state}:\quad \mathcal{S}(t) = \Big( \mathbf{q}_r(t), \dot{\mathbf{q}}_r(t), \boldsymbol{\phi}(\cdot, t), \partial_t \boldsymbol{\phi}(\cdot, t) \Big). \]

This is an infinite-dimensional object, impossible to represent exactly on a digital computer. Any usable representation must therefore:

• Choose a finite-dimensional parameterization of the configuration field \( \boldsymbol{\phi} \), and
• Specify which quantities (positions, velocities, sometimes strains) form the state used in planning and control.

At a high level, we will construct a finite-dimensional vector \( \mathbf{s}_d(t) \in \mathbb{R}^{n_s} \) that approximates the deformable configuration and then consider the combined robot+object state

\[ \mathbf{x}(t) = \begin{bmatrix} \mathbf{q}_r(t) \\ \dot{\mathbf{q}}_r(t) \\ \mathbf{s}_d(t) \end{bmatrix} \in \mathbb{R}^{2n_r + n_s}. \]

flowchart TD
  W["Physical object (cloth, rope)"] --> F["Continuous field phi(X, t)"]
  F --> D["Discretization: mesh / curve / basis"]
  D --> S["Finite state s_d(t)"]
  S --> X["Combine with robot state x(t)"]
  X --> P["Planner / controller"]
        

The rest of this lesson focuses on concrete constructions of \( \mathbf{s}_d(t) \) and their mathematical properties.

A standard approach from continuum mechanics and finite elements is to approximate the configuration field by a finite set of basis functions:

\[ \boldsymbol{\phi}(\mathbf{X}, t) \approx \sum_{k=1}^{n_b} q_k(t)\,\boldsymbol{\psi}_k(\mathbf{X}) = \boldsymbol{\Psi}(\mathbf{X})\,\mathbf{q}(t), \]

where \( \boldsymbol{\psi}_k: \Omega \to \mathbb{R}^3 \) are vector-valued basis functions and \( \mathbf{q}(t) = (q_1(t),\dots,q_{n_b}(t))^\top \) are generalized coordinates.

Typical choices of basis functions include:

• Piecewise-linear finite-element shape functions on a tetrahedral/hexahedral mesh.
• Modal bases (eigenmodes of a linearized elastic operator).
• Data-driven bases obtained by PCA/SVD from a set of example shapes.

The corresponding finite-dimensional configuration vector is \( \mathbf{q}(t) \in \mathbb{R}^{3n_n} \) if we stack the 3D coordinates of \( n_n \) nodes:

\[ \mathbf{q}(t) = \begin{bmatrix} \mathbf{p}_1(t) \\ \vdots \\ \mathbf{p}_{n_n}(t) \end{bmatrix} = \begin{bmatrix} x_1(t) \\ y_1(t) \\ z_1(t) \\ \vdots \\ x_{n_n}(t) \\ y_{n_n}(t) \\ z_{n_n}(t) \end{bmatrix} \in \mathbb{R}^{3n_n}. \]

With velocities \( \dot{\mathbf{q}}(t) \), we obtain a finite-dimensional deformable state vector

\[ \mathbf{s}_d(t) = \begin{bmatrix} \mathbf{q}(t) \\ \dot{\mathbf{q}}(t) \end{bmatrix} \in \mathbb{R}^{6n_n}. \]

As \( n_n \) increases, this state becomes more accurate but also more expensive. Later we will reduce its dimension while preserving task-relevant structure.

For many robotic manipulation tasks, deformables are modeled as networks of point masses connected by springs (mass–spring systems) or as standard finite-element meshes. Let \( G = (V,E) \) be a graph whose vertices correspond to nodes and edges to springs. Each vertex \( i \in V \) has a position \( \mathbf{p}_i(t) \in \mathbb{R}^3 \) and velocity \( \mathbf{v}_i(t) \in \mathbb{R}^3 \).

\[ \mathbf{q}(t) = \begin{bmatrix} \mathbf{p}_1(t) \\ \dots \\ \mathbf{p}_{n_n}(t) \end{bmatrix}, \qquad \dot{\mathbf{q}}(t) = \begin{bmatrix} \mathbf{v}_1(t) \\ \dots \\ \mathbf{v}_{n_n}(t) \end{bmatrix}, \qquad \mathbf{s}_d(t) = \begin{bmatrix} \mathbf{q}(t) \\ \dot{\mathbf{q}}(t) \end{bmatrix} \in \mathbb{R}^{6n_n}. \]

The potential energy of a mass–spring network is typically

\[ V(\mathbf{q}) = \frac{1}{2} \sum_{(i,j)\in E} k_{ij} \big( \lVert \mathbf{p}_i - \mathbf{p}_j \rVert - \ell_{ij}^0 \big)^2, \]

where \( k_{ij} \) is the stiffness and \( \ell_{ij}^0 \) the rest length. The kinetic energy is \( T(\dot{\mathbf{q}}) = \tfrac{1}{2}\dot{\mathbf{q}}^\top \mathbf{M} \dot{\mathbf{q}} \) with diagonal mass matrix \( \mathbf{M} \). The Euler–Lagrange equations yield a second-order ODE in \( \mathbf{q} \).

For state-space formulations compatible with rigid-body planning, we rewrite this as a first-order system:

\[ \dot{\mathbf{s}}_d(t) = \begin{bmatrix} \dot{\mathbf{q}}(t) \\ \mathbf{M}^{-1} \big( -\nabla_{\mathbf{q}} V(\mathbf{q}(t)) + \mathbf{f}_{\text{ext}}(t) \big) \end{bmatrix} = \mathbf{f}_d(\mathbf{s}_d(t), \mathbf{u}(t)), \]

where \( \mathbf{f}_{\text{ext}} \) encodes contact forces from the rigid robot and the environment. In this lesson, we focus on how \( \mathbf{s}_d(t) \) is defined, not on the precise form of \( \mathbf{f}_d \).

The nodal state dimension \( 6n_n \) may be extremely large for fine meshes. One classical way to reduce it is modal reduction, based on linearizing dynamics around an equilibrium and computing vibration modes.

Consider the linearized equation

\[ \mathbf{M} \ddot{\mathbf{q}} + \mathbf{K} \mathbf{q} = \mathbf{0}, \]

where \( \mathbf{K} \) is the stiffness matrix. Seeking solutions of the form \( \mathbf{q}(t) = \boldsymbol{\phi} \cos(\omega t) \) leads to the generalized eigenproblem

\[ \mathbf{K}\boldsymbol{\phi}_i = \lambda_i \mathbf{M} \boldsymbol{\phi}_i, \quad \lambda_i = \omega_i^2. \]

Sorting eigenpairs by increasing \( \lambda_i \) and keeping the first \( r \) modes yields a basis \( \boldsymbol{\Phi} = [\boldsymbol{\phi}_1,\dots,\boldsymbol{\phi}_r] \in \mathbb{R}^{3n_n \times r} \). We approximate the configuration by

\[ \mathbf{q}(t) \approx \mathbf{q}_0 + \boldsymbol{\Phi} \mathbf{z}(t), \]

where \( \mathbf{z}(t) \in \mathbb{R}^r \) are modal coordinates and \( \mathbf{q}_0 \) is a rest configuration. The reduced state is then

\[ \mathbf{s}_d^{\text{modal}}(t) = \begin{bmatrix} \mathbf{z}(t) \\ \dot{\mathbf{z}}(t) \end{bmatrix} \in \mathbb{R}^{2r}, \qquad r \ll 3n_n. \]

Under the assumption of \( \mathbf{M} \)-orthonormal modes \( \boldsymbol{\phi}_i^\top \mathbf{M} \boldsymbol{\phi}_j = \delta_{ij} \), inserting the expansion into the Lagrangian and using standard manipulations shows that the dynamics decouple into independent oscillators:

\[ \ddot{z}_i + \lambda_i z_i = 0,\quad i = 1,\dots,r, \]

so that modal coordinates have a direct mechanical interpretation. In practice, when the object interacts with a robot, the modes become coupled again via contact forces, but the dimension reduction remains valid.

Many deformable objects used in manipulation (cables, ropes, cloth) admit simple graph or grid structures. For a rope, \( G \) is a chain; for rectangular cloth, \( G \) is a 2D grid graph. Let \( \mathbf{q} \) stack node positions as before and let \( \mathbf{A} \) be the adjacency matrix of \( G \).

The graph Laplacian \( \mathbf{L} = \mathbf{D} - \mathbf{A} \), with \( \mathbf{D} \) the degree matrix, induces a natural smoothness energy

\[ E_{\text{graph}}(\mathbf{q}) = \frac{1}{2} \sum_{(i,j)\in E} w_{ij}\,\lVert \mathbf{p}_i - \mathbf{p}_j \rVert^2 = \frac{1}{2} \mathbf{q}^\top \mathbf{K}_G \mathbf{q}, \]

where \( w_{ij} \) are edge weights and \( \mathbf{K}_G \) is a block-structured matrix constructed from \( \mathbf{L} \). This energy penalizes sharp bends and folds and is therefore aligned with intuitive task goals such as “keep the rope straight” or “avoid excessive wrinkling of cloth”.

In graph-based state representations, we often keep both:

• the geometric state \( \mathbf{s}_d(t) = [\mathbf{q}(t); \dot{\mathbf{q}}(t)] \), and
• the topological descriptor (graph connectivity, boundary edges, etc.).

For planning and control, it is sometimes advantageous to define the state as \( \mathbf{s}_d = (\mathbf{q}, \dot{\mathbf{q}}, G) \), where \( G \) is fixed but explicitly available to collision checking and constraint generation.

In many robotic systems, we do not observe node-level coordinates directly; instead we observe depth images, point clouds, or RGB frames \( \mathbf{o}_t \). An increasingly popular approach is to learn a latent state \( \mathbf{z}_t \in \mathbb{R}^r \) and encode the deformable configuration via

\[ \mathbf{z}_t = f_\theta(\mathbf{o}_t), \qquad \hat{\mathbf{o}}_t = g_\theta(\mathbf{z}_t), \]

where \( f_\theta \) and \( g_\theta \) are neural encoder and decoder networks. A typical training objective is a reconstruction loss

\[ \mathcal{L}(\theta) = \sum_{t} \big\lVert \mathbf{o}_t - g_\theta(f_\theta(\mathbf{o}_t)) \big\rVert^2 + \lambda\,R(\theta), \]

where \( R(\theta) \) is a regularizer inducing smoothness or disentanglement and \( \lambda > 0 \) is a weight.

For robotics, we require more than reconstruction: the latent must be dynamically useful. A common strategy is to impose approximate linear dynamics in latent space

\[ \mathbf{z}_{t+1} \approx \mathbf{A}\mathbf{z}_t + \mathbf{B}\mathbf{u}_t, \]

and jointly learn \( \mathbf{A},\mathbf{B} \) and the encoder/decoder so that classical planning and control tools can be applied in the reduced space.

flowchart TD
  O["Sensor data o_t (image, cloud)"] --> ENC["Encoder f_theta"]
  ENC --> Z["Latent state z_t"]
  Z --> DYN["Latent dynamics A z_t + B u_t"]
  Z --> DEC["Decoder g_theta"]
  DEC --> OREC["Reconstructed observation"]
        

The resulting state for the combined robot–object system is \( \mathbf{x}_t = (\mathbf{q}_r, \dot{\mathbf{q}}_r, \mathbf{z}_t) \), where \( \mathbf{z}_t \) is either a modal coordinate vector or a learned representation.

We now implement a simple rope representation as a chain of nodes using Python and NumPy. We treat the rope as \( n \) point masses connected by springs. The state vector is \( \mathbf{s}_d = [\mathbf{q};\dot{\mathbf{q}}] \).

import numpy as np

class DeformableRopeState:
    """
    Rope modeled as n point masses in R^3.
    State s = [q; qdot] with q, qdot in R^{3n}.
    """
    def __init__(self, n_nodes: int):
        self.n = n_nodes
        # Positions and velocities as arrays of shape (n, 3)
        self.positions = np.zeros((n_nodes, 3), dtype=float)
        self.velocities = np.zeros((n_nodes, 3), dtype=float)

    @property
    def q(self) -> np.ndarray:
        """Flattened position vector in R^{3n}."""
        return self.positions.reshape(-1)

    @property
    def qdot(self) -> np.ndarray:
        """Flattened velocity vector in R^{3n}."""
        return self.velocities.reshape(-1)

    def state_vector(self) -> np.ndarray:
        """Return s = [q; qdot] in R^{6n}."""
        return np.concatenate([self.q, self.qdot], axis=0)

    def from_state_vector(self, s: np.ndarray):
        """Load positions and velocities from s in R^{6n}."""
        assert s.shape[0] == 6 * self.n
        q = s[:3*self.n]
        qdot = s[3*self.n:]
        self.positions = q.reshape(self.n, 3)
        self.velocities = qdot.reshape(self.n, 3)

def build_chain_adjacency(n_nodes: int):
    """
    Build edge list for a chain graph 1--2--...--n.
    Returns list of (i, j) pairs with 0-based indices.
    """
    edges = [(i, i + 1) for i in range(n_nodes - 1)]
    return edges

def stiffness_matrix_from_edges(n_nodes: int, edges, k_spring: float) -> np.ndarray:
    """
    Construct a simple scalar stiffness matrix for a 1D chain,
    then lift it to 3D by Kronecker product with I_3.
    """
    L = np.zeros((n_nodes, n_nodes), dtype=float)
    for i, j in edges:
        L[i, i] += 1.0
        L[j, j] += 1.0
        L[i, j] -= 1.0
        L[j, i] -= 1.0
    # Scalar Laplacian scaled by k_spring
    K_scalar = k_spring * L
    # Lift to 3D: K = kron(K_scalar, I_3)
    K = np.kron(K_scalar, np.eye(3))
    return K

# Example usage
n = 10
rope = DeformableRopeState(n)
edges = build_chain_adjacency(n)
K = stiffness_matrix_from_edges(n, edges, k_spring=10.0)

print("State dimension:", rope.state_vector().shape[0])  # should be 6n

# Modal basis via eigen-decomposition of K (assuming unit masses)
eigvals, eigvecs = np.linalg.eigh(K)
# Keep r lowest non-zero modes (ignore the rigid-motion modes)
r = 4
Phi = eigvecs[:, 3:3+r]  # skip 3 smallest eigenvalues (approx rigid modes)

def project_to_modal(q: np.ndarray, q0: np.ndarray, Phi: np.ndarray) -> np.ndarray:
    """
    Compute modal coordinates z given full configuration q and rest q0.
    Phi has shape (3n, r).
    """
    return Phi.T @ (q - q0)

def reconstruct_from_modal(z: np.ndarray, q0: np.ndarray, Phi: np.ndarray) -> np.ndarray:
    """
    Reconstruct configuration from modal coordinates.
    """
    return q0 + Phi @ z
      

This snippet constructs (i) a nodal state representation, (ii) a graph-based stiffness matrix \( \mathbf{K}_G \), and (iii) a low-rank modal basis \( \boldsymbol{\Phi} \) via eigen-decomposition of \( \mathbf{K}_G \).

In C++, Eigen is a common linear algebra library for robotics. The following code illustrates a rope state representation compatible with ROS or other middleware.

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

struct DeformableRopeState {
  std::size_t n;
  std::vector<Eigen::Vector3d> positions;
  std::vector<Eigen::Vector3d> velocities;

  explicit DeformableRopeState(std::size_t n_nodes)
      : n(n_nodes),
        positions(n_nodes, Eigen::Vector3d::Zero()),
        velocities(n_nodes, Eigen::Vector3d::Zero()) {}

  Eigen::VectorXd q() const {
    Eigen::VectorXd q_vec(3 * n);
    for (std::size_t i = 0; i < n; ++i) {
      q_vec.segment<3>(3 * i) = positions[i];
    }
    return q_vec;
  }

  Eigen::VectorXd qdot() const {
    Eigen::VectorXd qd_vec(3 * n);
    for (std::size_t i = 0; i < n; ++i) {
      qd_vec.segment<3>(3 * i) = velocities[i];
    }
    return qd_vec;
  }

  Eigen::VectorXd stateVector() const {
    Eigen::VectorXd s(6 * n);
    s << q(), qdot();
    return s;
  }

  void fromStateVector(const Eigen::VectorXd& s) {
    assert(static_cast<std::size_t>(s.size()) == 6 * n);
    Eigen::VectorXd q_vec = s.head(3 * n);
    Eigen::VectorXd qd_vec = s.tail(3 * n);
    for (std::size_t i = 0; i < n; ++i) {
      positions[i] = q_vec.segment<3>(3 * i);
      velocities[i] = qd_vec.segment<3>(3 * i);
    }
  }
};

Eigen::MatrixXd stiffnessMatrixChain(std::size_t n_nodes, double k_spring) {
  Eigen::MatrixXd L = Eigen::MatrixXd::Zero(n_nodes, n_nodes);
  for (std::size_t i = 0; i + 1 < n_nodes; ++i) {
    L(i, i) += 1.0;
    L(i + 1, i + 1) += 1.0;
    L(i, i + 1) -= 1.0;
    L(i + 1, i) -= 1.0;
  }
  Eigen::MatrixXd K_scalar = k_spring * L;
  // Kronecker product with I_3
  Eigen::MatrixXd K = Eigen::kroneckerProduct(K_scalar, Eigen::Matrix3d::Identity());
  return K;
}
      

The combination of stateVector() and stiffnessMatrixChain provides a C++ equivalent of the Python representation, suitable for integration with model-based controllers.

For Java-based simulation or high-level control, we can define a cloth state as a grid of nodes. The following uses javax.vecmath.Vector3d for 3D vectors.

import javax.vecmath.Vector3d;
import java.util.Arrays;

public class ClothState {
    private final int nx;
    private final int ny;
    private final Vector3d[][] positions;
    private final Vector3d[][] velocities;

    public ClothState(int nx, int ny) {
        this.nx = nx;
        this.ny = ny;
        this.positions = new Vector3d[nx][ny];
        this.velocities = new Vector3d[nx][ny];
        for (int i = 0; i < nx; ++i) {
            for (int j = 0; j < ny; ++j) {
                positions[i][j] = new Vector3d(0.0, 0.0, 0.0);
                velocities[i][j] = new Vector3d(0.0, 0.0, 0.0);
            }
        }
    }

    public int stateDimension() {
        // 3 coordinates per node, 2 (position, velocity)
        return 6 * nx * ny;
    }

    public double[] toStateVector() {
        double[] s = new double[stateDimension()];
        int idx = 0;
        for (int i = 0; i < nx; ++i) {
            for (int j = 0; j < ny; ++j) {
                Vector3d p = positions[i][j];
                s[idx++] = p.x;
                s[idx++] = p.y;
                s[idx++] = p.z;
            }
        }
        for (int i = 0; i < nx; ++i) {
            for (int j = 0; j < ny; ++j) {
                Vector3d v = velocities[i][j];
                s[idx++] = v.x;
                s[idx++] = v.y;
                s[idx++] = v.z;
            }
        }
        return s;
    }

    public void fromStateVector(double[] s) {
        if (s.length != stateDimension()) {
            throw new IllegalArgumentException("Invalid state length");
        }
        int idx = 0;
        for (int i = 0; i < nx; ++i) {
            for (int j = 0; j < ny; ++j) {
                positions[i][j].x = s[idx++];
                positions[i][j].y = s[idx++];
                positions[i][j].z = s[idx++];
            }
        }
        for (int i = 0; i < nx; ++i) {
            for (int j = 0; j < ny; ++j) {
                velocities[i][j].x = s[idx++];
                velocities[i][j].y = s[idx++];
                velocities[i][j].z = s[idx++];
            }
        }
    }

    @Override
    public String toString() {
        return "ClothState(dim=" + stateDimension() + ")";
    }
}
      

This representation can be connected to a physics engine (e.g., jBullet soft bodies) or used in high-level planners that only need access to node positions and velocities.

In MATLAB, we can define the rope state as a vector and interface it with Simulink via a State-Space block or a custom S-Function. The snippet below constructs a simple state vector and a mass–spring matrix.

function demo_rope_state()
    n = 8;              % number of nodes
    m = 0.05;           % mass per node
    k_spring = 20.0;    % spring stiffness
    x0 = zeros(6*n, 1); % initial state [q; qdot]

    % Mass matrix M and stiffness K for 1D chain, lifted to 3D
    M_scalar = m * eye(n);
    L = zeros(n, n);
    for i = 1:(n-1)
        L(i, i)     = L(i, i) + 1;
        L(i+1, i+1) = L(i+1, i+1) + 1;
        L(i, i+1)   = L(i, i+1) - 1;
        L(i+1, i)   = L(i+1, i) - 1;
    end
    K_scalar = k_spring * L;

    M = kron(M_scalar, eye(3));
    K = kron(K_scalar, eye(3));

    % Continuous-time state-space: xdot = [qdot; -M^{-1} K q + M^{-1} f_ext]
    % In Simulink, implement this as a custom block.
    A = [zeros(3*n) eye(3*n); -M\K zeros(3*n)];
    B = [zeros(3*n, 3*n); M\eye(3*n)];  % external forces as input
    C = eye(6*n);
    D = zeros(6*n, 3*n);

    sys = ss(A, B, C, D);
    disp(sys);
end
      

In Simulink, the matrices A, B, C, D can be used in a State-Space block to simulate the rope state. Coupling to a rigid robot is handled by connecting robot contact forces into the input channels representing \( \mathbf{f}_{\text{ext}} \).

Mathematica is well-suited for symbolic derivations of reduced-order models. The following code constructs a simple mass–spring system, computes eigenmodes, and defines the reduced mapping between full coordinates and modal coordinates.

(* Number of nodes *)
n = 5;

(* Scalar Laplacian for 1D chain *)
L = ConstantArray[0, {n, n}];
Do[
  L[[i, i]] += 1;
  L[[i + 1, i + 1]] += 1;
  L[[i, i + 1]] -= 1;
  L[[i + 1, i]] -= 1;
, {i, 1, n - 1}];

k = 10.0;
m = 1.0;
Kscalar = k L;
Mscalar = m IdentityMatrix[n];

(* Lift to 3D by Kronecker product *)
K = KroneckerProduct[Kscalar, IdentityMatrix[3]];
M = KroneckerProduct[Mscalar, IdentityMatrix[3]];

(* Generalized eigenproblem K phi = lambda M phi *)
valsVecs = Eigensystem[{K, M}];
lambdaList = valsVecs[[1]];
phiList = valsVecs[[2]];

(* Sort by eigenvalues and take first r modes after rigid motions *)
sorted = Transpose[{lambdaList, phiList}];
sorted = SortBy[sorted, First];
r = 3;
(* Skip first 3 eigenvalues (approx rigid motions in 3D) *)
Phi = sorted[[4 ;; 4 + r - 1, 2]];
PhiMat = Transpose@ArrayReshape[Phi, {r, 3 n}];

(* Define q0 and modal mapping *)
q0 = ConstantArray[0.0, 3 n];

zFromQ[q_] := PhiMat.(q - q0);
qFromZ[z_] := q0 + Transpose[PhiMat].z;
      

This code yields symbolic expressions for the reduced basis and the maps \( \mathbf{q} \mapsto \mathbf{z} \) and \( \mathbf{z} \mapsto \mathbf{q} \), which can then be exported and used inside a numeric controller.

Problem 1 (State Dimension of a Rope): A rope is represented as \( n \) nodes in 3D, each with position \( \mathbf{p}_i \in \mathbb{R}^3 \) and velocity \( \mathbf{v}_i \in \mathbb{R}^3 \). Define the stacked state \( \mathbf{s}_d = [\mathbf{q};\dot{\mathbf{q}}] \). What is the dimension of \( \mathbf{s}_d \)? Show how to write a matrix that extracts the position of node \( i \) from \( \mathbf{s}_d \).

Solution:

Each node contributes 3 position coordinates and 3 velocity coordinates, so the total dimension is \( 6n \). Let

\[ \mathbf{q} = \begin{bmatrix} \mathbf{p}_1 \\ \vdots \\ \mathbf{p}_n \end{bmatrix} \in \mathbb{R}^{3n}, \qquad \dot{\mathbf{q}} = \begin{bmatrix} \mathbf{v}_1 \\ \vdots \\ \mathbf{v}_n \end{bmatrix} \in \mathbb{R}^{3n}, \qquad \mathbf{s}_d = \begin{bmatrix} \mathbf{q} \\ \dot{\mathbf{q}} \end{bmatrix} \in \mathbb{R}^{6n}. \]

To extract the position of node \( i \), define a selection matrix \( \mathbf{S}_i \in \mathbb{R}^{3 \times 6n} \) such that \( \mathbf{S}_i \mathbf{s}_d = \mathbf{p}_i \). Concretely, \( \mathbf{S}_i \) has an identity block in columns \( 3(i-1)+1,\dots,3i \) and zeros elsewhere.

\[ \mathbf{S}_i = \begin{bmatrix} \mathbf{0}_{3 \times 3(i-1)} & \mathbf{I}_3 & \mathbf{0}_{3 \times (6n - 3i)} \end{bmatrix}. \]

Problem 2 (Matrix Form of Spring Energy): Consider a 1D chain of \( n \) masses with scalar positions \( x_i \in \mathbb{R} \), each pair \( (i,i+1) \) connected by a spring of stiffness \( k \) and zero rest length. Show that the total potential energy can be written as \( V(\mathbf{x}) = \tfrac{1}{2} \mathbf{x}^\top k\mathbf{L}\mathbf{x} \), where \( \mathbf{L} \) is the graph Laplacian of the chain.

Solution:

By definition of spring energy,

\[ V(\mathbf{x}) = \frac{1}{2} \sum_{i=1}^{n-1} k (x_{i+1} - x_i)^2. \]

Expanding the squared term gives

\[ (x_{i+1} - x_i)^2 = x_i^2 + x_{i+1}^2 - 2 x_i x_{i+1}. \]

Grouping coefficients of \( x_i^2 \) and \( x_i x_j \) yields exactly the quadratic form defined by the Laplacian \( \mathbf{L} \) of the chain graph, with entries \( L_{ii} = \deg(i) \) and \( L_{ij} = -1 \) if \( i \) and \( j \) are neighbors. Thus

\[ V(\mathbf{x}) = \frac{1}{2} k \mathbf{x}^\top \mathbf{L} \mathbf{x}. \]

Problem 3 (Orthogonality of Modes): Let \( \mathbf{M},\mathbf{K} \) be symmetric positive definite matrices corresponding to mass and stiffness. Suppose \( \boldsymbol{\phi}_i \) are generalized eigenvectors satisfying \( \mathbf{K}\boldsymbol{\phi}_i = \lambda_i \mathbf{M}\boldsymbol{\phi}_i \) and normalized such that \( \boldsymbol{\phi}_i^\top \mathbf{M} \boldsymbol{\phi}_j = \delta_{ij} \). Show that in modal coordinates \( \mathbf{q} = \boldsymbol{\Phi} \mathbf{z} \) with \( \boldsymbol{\Phi} = [\boldsymbol{\phi}_1,\dots,\boldsymbol{\phi}_r] \), the quadratic potential energy becomes diagonal: \( V(\mathbf{q}) = \tfrac{1}{2}\sum_{i=1}^r \lambda_i z_i^2 \).

Solution:

The potential energy is

\[ V(\mathbf{q}) = \frac{1}{2}\mathbf{q}^\top \mathbf{K} \mathbf{q} = \frac{1}{2} \mathbf{z}^\top \boldsymbol{\Phi}^\top \mathbf{K} \boldsymbol{\Phi} \mathbf{z}. \]

Using \( \mathbf{K}\boldsymbol{\phi}_i = \lambda_i \mathbf{M}\boldsymbol{\phi}_i \), we have \( \boldsymbol{\Phi}^\top \mathbf{K} \boldsymbol{\Phi} = \boldsymbol{\Lambda} \), where \( \boldsymbol{\Lambda} = \operatorname{diag}(\lambda_1,\dots,\lambda_r) \), because

\[ \boldsymbol{\Phi}^\top \mathbf{K} \boldsymbol{\Phi} = \big[ \boldsymbol{\phi}_i^\top \mathbf{K} \boldsymbol{\phi}_j \big]_{ij} = \big[ \lambda_j \boldsymbol{\phi}_i^\top \mathbf{M} \boldsymbol{\phi}_j \big]_{ij} = \big[ \lambda_j \delta_{ij} \big]_{ij} = \boldsymbol{\Lambda}. \]

Therefore

\[ V(\mathbf{q}) = \frac{1}{2} \mathbf{z}^\top \boldsymbol{\Lambda} \mathbf{z} = \frac{1}{2} \sum_{i=1}^r \lambda_i z_i^2. \]

Problem 4 (Latent Linear Dynamics and Local Linearization): Suppose a learned latent state \( \mathbf{z}_t = f_\theta(\mathbf{o}_t) \) follows unknown nonlinear dynamics \( \mathbf{z}_{t+1} = \mathbf{g}(\mathbf{z}_t, \mathbf{u}_t) \). Show that near a nominal trajectory \( (\bar{\mathbf{z}}_t, \bar{\mathbf{u}}_t) \), the dynamics can be approximated by

\[ \mathbf{z}_{t+1} \approx \mathbf{A}_t \mathbf{z}_t + \mathbf{B}_t \mathbf{u}_t + \mathbf{c}_t, \]

and derive expressions for \( \mathbf{A}_t, \mathbf{B}_t, \mathbf{c}_t \) in terms of derivatives of \( \mathbf{g} \).

Solution:

Perform a first-order Taylor expansion of \( \mathbf{g} \) at \( (\bar{\mathbf{z}}_t, \bar{\mathbf{u}}_t) \):

\[ \mathbf{g}(\mathbf{z}_t, \mathbf{u}_t) \approx \mathbf{g}(\bar{\mathbf{z}}_t, \bar{\mathbf{u}}_t) + \mathbf{G}_z (\mathbf{z}_t - \bar{\mathbf{z}}_t) + \mathbf{G}_u (\mathbf{u}_t - \bar{\mathbf{u}}_t), \]

where \( \mathbf{G}_z = \left.\tfrac{\partial \mathbf{g}}{\partial \mathbf{z}}\right|_{(\bar{\mathbf{z}}_t,\bar{\mathbf{u}}_t)} \) and \( \mathbf{G}_u = \left.\tfrac{\partial \mathbf{g}}{\partial \mathbf{u}}\right|_{(\bar{\mathbf{z}}_t,\bar{\mathbf{u}}_t)} \). Rearranging, we obtain

\[ \mathbf{z}_{t+1} \approx \mathbf{G}_z \mathbf{z}_t + \mathbf{G}_u \mathbf{u}_t + \big( \mathbf{g}(\bar{\mathbf{z}}_t,\bar{\mathbf{u}}_t) - \mathbf{G}_z \bar{\mathbf{z}}_t - \mathbf{G}_u \bar{\mathbf{u}}_t \big). \]

Therefore,

\[ \mathbf{A}_t = \mathbf{G}_z, \quad \mathbf{B}_t = \mathbf{G}_u, \quad \mathbf{c}_t = \mathbf{g}(\bar{\mathbf{z}}_t,\bar{\mathbf{u}}_t) - \mathbf{G}_z \bar{\mathbf{z}}_t - \mathbf{G}_u \bar{\mathbf{u}}_t. \]

In this lesson we formalized the notion of state for deformable objects manipulated by rigid robots. Starting from infinite-dimensional configuration fields, we introduced finite-dimensional nodal states, graph-based and FEM-inspired structures, and modal reduction to obtain low-dimensional yet physically meaningful coordinates. We then discussed learned latent state representations derived from visual observations and implemented several representations in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica. These state representations are the foundation on which planning and control algorithms for deformable manipulation (addressed in the next lessons) will operate.