Modern robots (industrial manipulators, mobile bases, multi-robot teams) execute complex trajectories planned using configuration-space planners, sampling-based methods, and optimization-based trajectory generators from earlier chapters. These methods are often evaluated in simulation and with limited real-world experiments, giving statistical confidence but not absolute guarantees.

Consider a robot with discrete-time state \( x_k \in \mathbb{R}^n \) and control \( u_k \in \mathbb{U} \), evolving as

\[ x_{k+1} = f(x_k, u_k, w_k), \quad w_k \in \mathbb{W}, \]

where \( w_k \) models disturbances and uncertainty (actuation errors, unmodeled friction, etc.). A closed-loop controller or planner induces a sequence \( \pi = x_0, x_1, x_2, \dots \) called a trace. A critical property (e.g. "no collision" or "never exceed joint limits") should hold for all traces compatible with dynamics and uncertainty, not only for a finite sample of scenarios.

Informally, a formal guarantee aims at:

\[ \text{Correctness:} \quad \forall \text{ closed-loop traces } \pi,\; \pi \in \varphi, \]

where \( \varphi \) is a mathematically defined specification (e.g. set of safe traces). This is qualitatively stronger than empirical statements such as "we observed no collision in 10,000 simulations."

flowchart TD
  RQ["Task & constraints (e.g., pick-and-place, no-collision, joint limits)"] --> M["Mathematical model of robot + environment"]
  M --> SPEC["Formal specification of desired behavior"]
  SPEC --> VER["Automatic verification / synthesis step"]
  VER --> CTRL["Controller or planner with proven guarantees"]
  CTRL --> MON["Optional runtime monitoring & fault handling"]
  MON --> DEP["Deployed robot in the real world"]
        

The rest of this chapter formalizes each block in this diagram, starting here with models, specifications, and the notion of a guarantee.

Formal methods for robotics rest on two mathematical objects: a system model and a specification.

A very common abstraction is a transition system:

\[ \mathcal{S} = (X, X_0, U, \longrightarrow), \]

where:

• \( X \): state space (e.g. configuration and velocities).
• \( X_0 \subseteq X \): set of initial states.
• \( U \): set of control inputs.
• \( \longrightarrow \subseteq X \times U \times X \): transition relation; we write \( x \xrightarrow{u} x' \) if state \( x' \) is reachable from \( x \) under control \( u \).

A (possibly infinite) sequence of states \( \pi = x_0 x_1 x_2 \dots \) is a run of \( \mathcal{S} \) if

\[ x_0 \in X_0, \quad \exists u_0, u_1, \dots\; \text{s.t. } x_k \xrightarrow{u_k} x_{k+1} \text{ for all } k \in \mathbb{N}. \]

A specification is any set of runs \( \varphi \subseteq X^{\mathbb{N}} \). Typical specifications encode:

• Safety: "something bad never happens" (e.g. robot never collides).
• Reachability: "eventually something good happens" (e.g. task is completed).

A controller policy \( \mu: X \to U \) maps states to controls, inducing a set of closed-loop runs \( \mathcal{T}(\mathcal{S}, \mu) \). We say that the controller satisfies the specification if

\[ \text{Correctness of } \mu: \quad \mathcal{T}(\mathcal{S}, \mu) \subseteq \varphi. \]

The purpose of formal verification is to prove such inclusions, or to automatically synthesize a policy \( \mu \) that satisfies them.

Many robotics constraints are naturally stated as safety properties. Let \( S_{\text{safe}} \subseteq X \) be a safe set (e.g. configurations without collision, joint limits respected, or a distance to obstacles above some threshold).

The corresponding safety property is:

\[ \varphi_{\text{safe}} = \left\{ \pi = x_0 x_1 x_2 \dots \in X^{\mathbb{N}} \,\middle|\, \forall k \in \mathbb{N},\; x_k \in S_{\text{safe}} \right\}. \]

To reason about safety, we introduce the reachable set. Given initial states \( X_0 \), define:

\[ \operatorname{Reach}_0(X_0) = X_0, \quad \operatorname{Reach}_{k+1}(X_0) = \left\{ x' \in X \,\middle|\, \exists x \in \operatorname{Reach}_k(X_0),\; \exists u \in U,\; x \xrightarrow{u} x' \right\}. \]

The union \( \operatorname{Reach}(X_0) = \bigcup_{k \in \mathbb{N}} \operatorname{Reach}_k(X_0) \) is the set of all states that can ever be visited. A sufficient condition for the safety specification to hold under policy \( \mu \) is:

\[ \operatorname{Reach}(X_0) \subseteq S_{\text{safe}}. \]

A set \( C \subseteq X \) is called an invariant (or controlled invariant) if whenever the state starts in \( C \) and we apply the controller, it remains in \( C \) for all time. Formally:

\[ x_0 \in C \;\Rightarrow\; \forall k \in \mathbb{N},\; x_k \in C. \]

If we can find an invariant \( C \) such that \( X_0 \subseteq C \subseteq S_{\text{safe}} \), then the safety property holds. Much of formal verification for robotics can be interpreted as computing or approximating such invariant sets.

To make formal guarantees concrete, consider a 1D robot moving along a line (e.g. a prismatic joint or a shuttle on a rail). We model the discrete-time dynamics as:

\[ x_{k+1} = x_k + T u_k, \quad u_k \in \mathbb{U} = [-u_{\max}, u_{\max}], \]

where \( x_k \in \mathbb{R} \) is position at step \( k \), \( T > 0 \) is the sampling time, and \( u_k \) is the commanded velocity (clipped by actuators).

The safety constraint is that the robot must always stay inside a corridor \( [-L, L] \), that is, \( S_{\text{safe}} = \{ x \in \mathbb{R} \mid |x| \leq L \} \).

Suppose we want a horizon guarantee: for the next \( N \) steps, the robot must remain safe under any control sequence satisfying the bound \( |u_k| \leq u_{\max} \). We assume the initial state satisfies

\[ |x_0| \leq L - N T u_{\max}. \]

Claim. Under this condition, for all control sequences respecting \( |u_k| \leq u_{\max} \), the positions \( x_1, \dots, x_N \) all satisfy \( |x_k| \leq L \). Thus the safety property over horizon \( N \) holds.

Proof (by induction).

• Base case. For \( k = 0 \), the assumption gives \( |x_0| \leq L - N T u_{\max} \leq L \).
• Inductive step. Suppose \( |x_k| \leq L - (N-k) T u_{\max} \) for some \( 0 \leq k < N \). Then

\[ |x_{k+1}| = |x_k + T u_k| \leq |x_k| + T |u_k| \leq \left(L - (N-k) T u_{\max}\right) + T u_{\max} = L - (N-k-1) T u_{\max}. \]

Hence the inductive condition holds with \( k+1 \) in place of \( k \). By induction, for all \( k = 0, \dots, N \) we have \( |x_k| \leq L - (N-k) T u_{\max} \leq L \).

We have thus proved, mathematically, that the 1D robot remains within the corridor for \( N \) steps, regardless of the control sequence, as long as the initial state satisfies a stronger tightened bound. This is a simple but fully formal guarantee.

In practice, roboticists often validate controllers using simulation and physical experiments. Suppose we run \( N \) independent tests of a planner and observe no violation (no collision). Let \( p \) be the true (unknown) probability of failure under random test scenarios.

Using standard concentration inequalities (e.g. Hoeffding's inequality), if the empirical failure rate is zero, then with high probability \( p \) is small. For example,

\[ \mathbb{P} \bigl( p \geq \varepsilon \bigr) \leq \exp(-2N\varepsilon^2). \]

This gives a probabilistic statement: "with confidence at least \( 1 - \delta \), the true failure probability satisfies \( p \leq \varepsilon(\delta, N) \)." However, this is not an absolute guarantee and depends heavily on how test scenarios are sampled.

In contrast, formal verification explores all behaviors in an abstract model and attempts to prove that \( p = 0 \) in the model. If the model faithfully over-approximates reality, then the guarantee transfers to the real system.

flowchart TD
  TST["Simulation / experiments"] --> S1["Sample subset of scenarios"]
  S1 --> S2["Estimate failure probability"]
  S2 --> S3["High confidence but no absolute guarantee"]

  MOD["Abstract model of robot"] --> V1["Systematically explore all states / inputs"]
  V1 --> V2["Prove property or generate counterexample"]
  V2 --> V3["Formal guarantee within model"]
        

Both approaches are complementary: simulations find typical bugs quickly, while formal methods target worst-case behaviors and subtle corner cases that might never appear in random testing.

We implement a finite abstraction of the 1D corridor robot from Section 4. We discretize the state space and control set and then perform a breadth-first search (BFS) over the abstract transition graph to check whether any reachable state violates the corridor constraint. This is a very small-scale analogue of graph-based model checking.

import collections
import math

# Discrete abstraction parameters
T = 0.1              # sampling time
L = 1.0              # corridor half-width
u_max = 0.5          # max velocity magnitude
N_horizon = 10       # steps
dx = 0.1             # state grid spacing
du = 0.5             # discrete control spacing (values in {-u_max, 0, u_max})

# Discrete sets (representative of configuration space cells)
states = [round(x, 2) for x in 
          [i * dx for i in range(int(-L / dx), int(L / dx) + 1)]]
controls = [-u_max, 0.0, u_max]

def successors(x):
    """All next abstract states from x for all discrete controls."""
    succ = []
    for u in controls:
        x_next = x + T * u
        # snap to grid if inside corridor abstraction
        if -L <= x_next <= L:
            x_next_disc = round(dx * round(x_next / dx), 2)
            succ.append((x_next_disc, u))
        else:
            # If we leave the corridor, record an unsafe successor
            succ.append((None, u))
    return succ

def bfs_check(initial_states, max_depth):
    """
    BFS over abstract transition graph up to horizon max_depth.
    Returns True if safe, False if an unsafe state is reachable.
    """
    # state is (x, step)
    queue = collections.deque()
    visited = set()
    for x0 in initial_states:
        queue.append((x0, 0))
        visited.add((x0, 0))

    while queue:
        x, step = queue.popleft()
        if step == max_depth:
            continue
        for x_next, u in successors(x):
            if x_next is None:
                print(f"Unsafe successor from x={x} with u={u}")
                return False
            node = (x_next, step + 1)
            if node not in visited:
                visited.add(node)
                queue.append(node)
    return True

# Choose initial set strictly inside corridor
initial_states = [0.0]  # or a small neighborhood around 0
is_safe = bfs_check(initial_states, N_horizon)

print("Abstraction declared SAFE up to horizon", N_horizon, ":", is_safe)
      

In robotics practice, similar abstractions can be constructed around manipulators or mobile robots (e.g. discrete configuration lattices), and safety checks can be integrated with planning libraries such as OMPL (for sampling-based planning) or MoveIt for manipulators.

Below are minimal C++ and Java skeletons for the same discrete safety check. They can be integrated into robotics software stacks such as ROS (C++) or Java-based simulation environments.

7.1 C++ Example (Discrete Safety Checker)

#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
#include <unordered_set>
#include <tuple>

struct StateStepHash {
    std::size_t operator()(const std::pair<double,int>& s) const {
        // Simple hash: combine rounded x and step
        long xi = std::lround(s.first * 100); // 2 decimals
        return std::hash<long>{}(xi) ^ (std::hash<int>{}(s.second) << 1);
    }
};

int main() {
    const double T = 0.1;
    const double L = 1.0;
    const double u_max = 0.5;
    const int N_horizon = 10;
    const double dx = 0.1;

    std::vector<double> controls = {-u_max, 0.0, u_max};
    std::vector<double> initial_states = {0.0};

    std::queue<std::pair<double,int>> q;
    std::unordered_set<std::pair<double,int>, StateStepHash> visited;

    for (double x0 : initial_states) {
        q.push({x0, 0});
        visited.insert({x0, 0});
    }

    auto succ = [&](double x) {
        std::vector<std::pair<double,bool>> result;
        for (double u : controls) {
            double x_next = x + T * u;
            if (x_next <= L && x_next >= -L) {
                double x_disc = dx * std::round(x_next / dx);
                result.push_back({x_disc, true});
            } else {
                result.push_back({0.0, false}); // unsafe
            }
        }
        return result;
    };

    bool safe = true;
    while (!q.empty() && safe) {
        auto [x, step] = q.front();
        q.pop();
        if (step == N_horizon) continue;

        for (auto& pair : succ(x)) {
            double x_next = pair.first;
            bool inside = pair.second;
            if (!inside) {
                std::cout << "Unsafe successor from x=" << x << std::endl;
                safe = false;
                break;
            }
            std::pair<double,int> node{x_next, step + 1};
            if (visited.find(node) == visited.end()) {
                visited.insert(node);
                q.push(node);
            }
        }
    }

    std::cout << "Abstraction declared SAFE up to horizon "
              << N_horizon << ": " << std::boolalpha << safe << std::endl;

    return 0;
}
      

7.2 Java Example (Discrete Safety Checker)

import java.util.*;

public class CorridorSafetyChecker {
    static class StateStep {
        final double x;
        final int step;

        StateStep(double x, int step) {
            this.x = x;
            this.step = step;
        }

        @Override
        public boolean equals(Object o) {
            if (!(o instanceof StateStep)) return false;
            StateStep other = (StateStep) o;
            return Math.round(100 * x) == Math.round(100 * other.x)
                   && step == other.step;
        }

        @Override
        public int hashCode() {
            long xi = Math.round(100 * x);
            return Objects.hash(xi, step);
        }
    }

    public static void main(String[] args) {
        double T = 0.1;
        double L = 1.0;
        double u_max = 0.5;
        int N_horizon = 10;
        double dx = 0.1;

        double[] controls = new double[]{-u_max, 0.0, u_max};
        double[] initialStates = new double[]{0.0};

        Queue<StateStep> queue = new ArrayDeque<>();
        Set<StateStep> visited = new HashSet<>();

        for (double x0 : initialStates) {
            StateStep ss = new StateStep(x0, 0);
            queue.add(ss);
            visited.add(ss);
        }

        boolean safe = true;

        while (!queue.isEmpty() && safe) {
            StateStep node = queue.poll();
            double x = node.x;
            int step = node.step;

            if (step == N_horizon) continue;

            for (double u : controls) {
                double xNext = x + T * u;
                if (xNext < -L || xNext > L) {
                    System.out.println("Unsafe successor from x=" + x
                                       + " with u=" + u);
                    safe = false;
                    break;
                }
                double xDisc = dx * Math.round(xNext / dx);
                StateStep nextNode = new StateStep(xDisc, step + 1);
                if (!visited.contains(nextNode)) {
                    visited.add(nextNode);
                    queue.add(nextNode);
                }
            }
        }

        System.out.println("Abstraction declared SAFE up to horizon "
                           + N_horizon + ": " + safe);
    }
}
      

These snippets illustrate how low-level verification-like checks can be embedded into robotics code bases, complementing higher-level planners and controllers from previous chapters.

MATLAB and Simulink are widely used in control and robotics. The following MATLAB code performs the same grid-based safety check. In practice, this script can be linked to a Simulink discrete-time integrator block (for the 1D robot) and augmented with tools such as Simulink Design Verifier to perform formal analysis of more complex models.

8.1 MATLAB / Simulink-Oriented Script

T = 0.1;
L = 1.0;
u_max = 0.5;
N_horizon = 10;
dx = 0.1;

controls = [-u_max, 0.0, u_max];
states = -L:dx:L;
initial_states = 0.0;  % could also use a vector of initial states

% Represent visited as logical matrix indexed by (state_index, step)
n_states = numel(states);
visited = false(n_states, N_horizon + 1);

% Map from x to index
[~, idx0] = min(abs(states - initial_states));
queue = [idx0, 0];  % [state_index, step]
visited(idx0, 1) = true;

safe = true;

while ~isempty(queue) && safe
    x_idx = queue(1,1);
    step  = queue(1,2);
    queue(1,:) = [];

    x = states(x_idx);
    if step == N_horizon
        continue;
    end

    for u = controls
        x_next = x + T * u;
        if x_next < -L || x_next > L
            fprintf('Unsafe successor from x=%f with u=%f\n', x, u);
            safe = false;
            break;
        end
        % snap to grid
        [~, idx_next] = min(abs(states - x_next));
        if ~visited(idx_next, step + 2)
            visited(idx_next, step + 2) = true;
            queue(end+1,:) = [idx_next, step + 1]; %#ok<AGROW>
        end
    end
end

fprintf('Abstraction declared SAFE up to horizon %d: %d\n', ...
        N_horizon, safe);
      

In a Simulink workflow, the same parameters T, L, and u_max can be used in both the Simulink diagram and this script, so that numerical and formal analyses are consistent.

8.2 Wolfram Mathematica Example

Mathematica is well suited for symbolic reasoning and discrete explorations. The following code constructs the abstract transition graph and checks safety.

T = 0.1;
L = 1.0;
uMax = 0.5;
nHorizon = 10;
dx = 0.1;
controls = {-uMax, 0.0, uMax};

states = Range[-L, L, dx];
initialStates = {0.0};

successors[x_] := Module[{xs = {}},
  Do[
    With[{xnext = x + T*u},
      If[xnext < -L || xnext > L,
        AppendTo[xs, {None, u}], (* unsafe *)
        (* snap to grid *)
        AppendTo[xs, {Nearest[states, xnext][[1]], u}]
      ]
    ],
    {u, controls}
  ];
  xs
];

bfsCheck[init_, maxDepth_] := Module[
  {queue, visited, safe = True},
  queue = Table[{x0, 0}, {x0, init}];
  visited = Association[];
  Scan[(visited[#] = True) &, queue];
  While[Length[queue] > 0 && safe,
    {x, step} = First[queue];
    queue = Rest[queue];
    If[step < maxDepth,
      Do[
        {xnext, u} = succ;
        If[xnext === None,
          Print["Unsafe successor from x=", x, " with u=", u];
          safe = False; Break[],
          key = {xnext, step + 1};
          If[!KeyExistsQ[visited, key],
            visited[key] = True;
            AppendTo[queue, {xnext, step + 1}];
          ];
        ],
        {succ, successors[x]}
      ];
    ];
  ];
  safe
];

safeFlag = bfsCheck[initialStates, nHorizon];
Print["Abstraction declared SAFE up to horizon ", nHorizon, ": ", safeFlag];
      

Mathematica also allows symbolic manipulation of simpler systems to obtain exact invariants (for example, proving inequalities or computing exact reachable sets for low-dimensional linear systems) which is a powerful complement to numeric grid-based checks.

Problem 1 (Invariance in the 1D Corridor). Consider the 1D robot model \( x_{k+1} = x_k + T u_k \) with \( |u_k| \leq u_{\max} \), and the corridor \( S_{\text{safe}} = \{ x \in \mathbb{R} \mid |x| \leq L \} \). Show that the set

\[ C = \left\{ x \in \mathbb{R} \;\middle|\; |x| \leq L - N T u_{\max} \right\} \]

is an invariant over horizon \( N \) in the sense of Section 4: if \( x_0 \in C \), then \( x_k \in S_{\text{safe}} \) for all \( k = 0, \dots, N \).

Solution. The proof in Section 4 already establishes this: we proved by induction that starting with \( |x_0| \leq L - N T u_{\max} \) implies \( |x_k| \leq L - (N-k) T u_{\max} \) for each \( k \). In particular, \( |x_k| \leq L \) for all \( k = 0, \dots, N \), hence \( x_k \in S_{\text{safe}} \). Thus every \( x_0 \in C \) yields a safe run over the horizon, so \( C \) is an invariant set for the finite horizon dynamics.

Problem 2 (Why Simulation Cannot Prove Zero Failure Rate). Suppose you run \( N \) independent simulations of a manipulator motion planner and observe zero failures (no collisions). Assume each trial fails with probability \( p \) and succeeds with probability \( 1 - p \), independently. Show that for any finite \( N \) and any \( 0 < \delta < 1 \), there exists some \( p > 0 \) such that the event "no failures in \( N \) trials" occurs with probability at least \( 1 - \delta \).

Solution. The probability of observing zero failures in \( N \) trials is \( (1-p)^N \). For any fixed \( N \) and \( \delta \), consider \( p = \delta / (2N) \). Then

\[ (1-p)^N = \left(1 - \frac{\delta}{2N}\right)^N \approx \exp\!\left(-\frac{\delta}{2}\right) \geq 1 - \delta \]

for sufficiently small \( \delta \) (or by using the inequality \( 1 - x \geq \exp(-x/(1-x)) \) for \( x \in (0,1) \)). Thus there is a strictly positive failure probability \( p \) for which "no failures in \( N \) trials" is still highly likely. Therefore simulation alone cannot establish that \( p = 0 \); it can only bound \( p \) with some confidence.

Problem 3 (Reachable Set Inclusion and Safety). Let \( \mathcal{S} = (X, X_0, U, \longrightarrow) \) be a transition system and \( S_{\text{safe}} \subseteq X \) a safe set. Assume that for some controller \( \mu \) we have \( \operatorname{Reach}(X_0) \subseteq S_{\text{safe}} \). Prove that every closed-loop run starting from \( X_0 \) satisfies the safety specification \( \varphi_{\text{safe}} \).

Solution. Let \( \pi = x_0 x_1 x_2 \dots \) be any closed-loop run induced by \( \mu \) with \( x_0 \in X_0 \). By definition of reachable sets, \( x_k \in \operatorname{Reach}(X_0) \) for all \( k \in \mathbb{N} \). By the assumed inclusion, each \( x_k \) belongs to \( S_{\text{safe}} \). Therefore the run satisfies

\[ \forall k \in \mathbb{N},\; x_k \in S_{\text{safe}}, \]

and hence \( \pi \in \varphi_{\text{safe}} \). Because the choice of \( \pi \) was arbitrary, all closed-loop runs satisfy the safety specification.

Problem 4 (Lyapunov-Type Invariant for a Linear System). Consider the autonomous linear system \( x_{k+1} = A x_k \), with \( A \in \mathbb{R}^{n \times n} \). Let \( P \in \mathbb{R}^{n \times n} \) be symmetric positive definite, and consider the ellipsoidal set

\[ C = \left\{ x \in \mathbb{R}^n \;\middle|\; x^{\mathsf{T}} P x \leq 1 \right\}. \]

Assume that \( A^{\mathsf{T}} P A - P \preceq -Q \) for some positive definite \( Q \). Show that \( C \) is an invariant set for the system, i.e., if \( x_0 \in C \) then \( x_k \in C \) for all \( k \in \mathbb{N} \). (You may use the fact that \( v^{\mathsf{T}} Q v > 0 \) for all \( v \neq 0 \) when \( Q \) is positive definite.)

Solution. Define the Lyapunov-like function \( V(x) = x^{\mathsf{T}} P x \). Then

\[ V(x_{k+1}) - V(x_k) = x_k^{\mathsf{T}}(A^{\mathsf{T}} P A - P)x_k \leq - x_k^{\mathsf{T}} Q x_k < 0 \]

whenever \( x_k \neq 0 \), since \( Q \) is positive definite. Thus the sequence \( V(x_k) \) is strictly decreasing along nonzero trajectories. If \( x_0 \in C \), then \( V(x_0) \leq 1 \) and for all \( k \), \( V(x_k) \leq V(x_0) \leq 1 \), because \( V \) cannot increase. Therefore \( x_k \in C \) for all \( k \), and \( C \) is invariant. This type of argument forms the basis of Lyapunov-based safety certificates used in many control and robotics applications.

Problem 5 (Abstraction Granularity and Soundness). In the grid-based abstraction used in Sections 6–8, suppose we use a coarse grid spacing \( dx \). Explain why declaring the grid model "safe" may fail to imply safety of the underlying continuous system unless we construct an over-approximate abstraction. Outline the role of such over-approximation.

Solution. When we discretize the state space by snapping continuous states to grid points, small continuous motions might be "hidden": a continuous successor state slightly outside the corridor might still snap back to an interior grid point. If we declare the grid model safe without accounting for this, the abstraction is under-approximate and we might miss real unsafe behaviors. A sound approach uses over-approximation: whenever any continuous successor of a state under allowed controls can leave the safe set, the abstract model either marks the transition as unsafe or includes explicit abstract unsafe states. This ensures that if the abstraction is safe, then all concrete behaviors are also safe, at the possible cost of introducing spurious (non-realizable) abstract counterexamples.

In this lesson we formalized the idea that advanced robotic systems require guarantees rather than only empirical evidence of safety or correctness. We introduced transition-system models, safety specifications as sets of traces, reachable sets and invariant sets, and we proved a simple corridor safety property by induction. We contrasted simulation-based testing with formal verification, emphasizing worst-case reasoning and model-based guarantees. Finally, we implemented small discrete safety checkers in Python, C++, Java, MATLAB/Simulink-oriented code, and Wolfram Mathematica, illustrating how verification-style algorithms can be integrated with robotics software stacks. Subsequent lessons will build on these foundations with temporal logic specifications, automata-based planning, and reachability analysis for higher-dimensional robotic systems.