Reachability using Polytope/ConstrainedZonotope classes

This python notebook goes over how we can compute various reachability sets using the Polytope and ConstrainedZonotope classes in pycvxset.

License information: pycvxset code is released under AGPL-3.0-or-later license, as found in the LICENSE.md file. The documentation for pycvxset is released under CC-BY-4.0 license, as found in the LICENSES/CC-BY-4.0.md.

Throughout this notebook, we will consider a discrete-time linear system, \begin{align} x_{t+1} = A x_t + B u_t + F w_t,\tag{1} \end{align} with state $x_t\in\mathbb{R}^n$, input $u_t\in\mathcal{U}\subset\mathbb{R}^m$, and disturbance $w_t\in\mathcal{W}\subset\mathbb{R}^p$, where the input set $\mathcal{U}$ and disturbance set $\mathcal{W}$ are assumed to be convex and compact.

This notebook is organized as follows:

The links below are for the HTML page in the documentation website, and they will not work in the Jupyter notebook.

1. Physical example: Robot constrained to move in a line
2. Forward reachable set
   1. Computation via set recursion
   2. Application to the physical example
      1. Example questions
      2. Validation via optimal control
3. Backward robust reachable set
   1. Computation via set recursion
   2. Application to the physical example
      1. Approximate validation via set-based control
      2. Example questions
4. Conclusions

To help the reader appreciate the flexibility (and equivalence of constrained zonotopes and polytopes), we perform all set-based computations in this notebook using both Polytope and ConstrainedZonotope classes provided by pycvxset.

A key feature of pycvxset is that it treats Polytope and ConstrainedZonotope equivalently and allows end-user to (in most cases) ignore whether an object is really a Polytope or a ConstrainedZonotope using Python's duck typing. The two cases where a difference pops-up are:

1. the Pontryagin difference ($\ominus$) involving a ConstrainedZonotope minuend for which we compute an inner-approximation (while raising an appropriate warning), and
2. the plotting of constrained zonotopes, for which we need to specify whether we require polytopic outer- or inner- approximations. By default, pycvxset chooses to plot inner-approximations.

Physical example: Robot constrained to move in a line

Consider a robot constrained to move in a straight line. We can control the robot's acceleration, but it may be perturbed in some cases. Such a system can be modeled as a double integrator system.

The continuous-time dynamics double integrator system are given by \begin{align} \left[\begin{array}{c} \dot{p}\\ \dot{v} \end{array}\right] =\left[\begin{array}{cc} 0 & 1\\ 0 & 0\end{array}\right] \left[\begin{array}{c} p\\ v\end{array}\right] +\left[\begin{array}{c} 0\\ 1\end{array}\right](u+w),\tag{2} \end{align} with state as position with respect to an origin and velocity (speed along the line) $x=[p,v]\in\mathbb{R}^2$, scalar input to model the robot input acceleration $u\in\mathcal{U}$, and scalar disturbance to model perturbations in the acceleration $w\in\mathcal{W}$. We will model input set $\mathcal{U}$ and disturbance set $\mathcal{W}$ are intervals.

We use zero-order hold to discretize the continuous-time system (2) using a sampling time $T_S$ to obtain a model in the form of (1), \begin{align} \left[\begin{array}{c} p_{t+1}\\ v_{t+1}\end{array}\right] =\left[\begin{array}{cc} 1 & T_S\\ 0 & 1\end{array}\right] \left[\begin{array}{c} p_t\\ v_t\end{array}\right] +\left[\begin{array}{c} \frac{T_S^2}{2}\\ T_S\end{array}\right]u +\left[\begin{array}{c} \frac{T_S^2}{2}\\ T_S\end{array}\right]w.\tag{3} \end{align}

# Importing necessary packages
from pycvxset import (
    approximate_volume_from_grid,
    is_constrained_zonotope,
    is_polytope,
    ConstrainedZonotope,
    Polytope,
)
import numpy as np
import matplotlib.pyplot as plt
import cvxpy as cp
import time

plt.rcParams["figure.figsize"] = [5, 3]
plt.rcParams["figure.dpi"] = 150

# Dynamics
sampling_time = 0.1
system_A = np.array([[1, sampling_time], [0, 1]])
system_B = np.array([[(sampling_time**2) / 2], [sampling_time]])
system_F = system_B

Forward reachable set

Forward reachability seeks to answer the question --- Where can I reach if I start from a given set of initial states under dynamical constraints?

Formally, given an initial set $\mathcal{X}_0\subset\mathbb{R}^n$ and a discrete-time linear dynamics (1), the forward reachable set is the set of states that can be reached by the system at a time $k\in\mathbb{N},k>0$ in the future. In this section, we will consider disturbance set to be empty $\mathcal{W}=\emptyset$, \begin{align} \mathcal{X}_k=\left\{x_k \middle| \begin{array}{c} \exists x_0\in\mathcal{X}_0,\ \forall t\in\{0,...,k-1\},\\ \exists u_t\in\mathcal{U},\ x_{t+1}=Ax_t + Bu_t\end{array}\right\}.\tag{4} \end{align}

Computation via set recursion

We can set up the following set recursion to compute $\mathcal{X}_k$ (forward in time), \begin{align} \mathcal{X}_{t+1}&=A\mathcal{X}_{t}\oplus B\mathcal{U}.\tag{5} \end{align} for $t\in\{0,1,...,k-1\}$, initialized to the initial set $\mathcal{X}_0$.

Some notational details:

1. $M\mathcal{S}=\{M x|x\in\mathcal{S}\}$ for any set $\mathcal{S}$ and an appropriately dimensioned matrix $M$, and
2. $\mathcal{S}_1\oplus\mathcal{S}_2=\{s_1 + s_2|s_1\in\mathcal{S}_1,\ s_2\in\mathcal{S}_2\}$ for any two sets $\mathcal{S}_1,\mathcal{S}_2$.

def compute_forward_reachable_set(
    initial_set_recursion,
    time_for_forward_reachable_set,
    input_set_recursion,
):
    recursion_controlled_forward_reachable_sets = [None] * (
        time_for_forward_reachable_set + 1
    )
    recursion_controlled_forward_reachable_sets[0] = initial_set_recursion
    for t in range(time_for_forward_reachable_set):
        # Implement (5)
        recursion_controlled_forward_reachable_sets[t + 1] = (
            system_A @ recursion_controlled_forward_reachable_sets[t]
            + system_B @ input_set_recursion
        )
    return recursion_controlled_forward_reachable_sets

Application to the physical example

Forward reach set computations encode the the possible states the robot can be in at time $k$, given the dynamics (1), the initial set $\mathcal{X}_0$ and the input set $\mathcal{U}$. We set $k=5$ and choose $\mathcal{X}_0$ to be a hypercube centered at the origin, and unit side. Here, $\mathcal{X}_0$ is the set of all possible initial states (position and velocities) of the robot that we want to include in the analysis. We choose the input set $\mathcal{U}=[-1,1]$, and the disturbance set $\mathcal{W}=\emptyset$.

# Parameters
n_time_steps_for_forward_reach_computation = 5
FACECOLOR_LIST = [
    "black",
    "gray",
    "lightgray",
    "lightblue",
    "lightgreen",
    "white",
]

# Define the initial and input sets (Polytope)
initial_set_polytope = Polytope(c=[0, 0], h=0.5)
input_set_polytope = Polytope(lb=-1, ub=1)

# Define the initial and input sets (Constrained zonotopes)
initial_set_constrained_zonotope = ConstrainedZonotope(c=[0, 0], h=0.5)
input_set_constrained_zonotope = ConstrainedZonotope(lb=-1, ub=1)

We now perform the set recursion and plot the sets.

# Recursion (Polytope)
controlled_forward_reachable_sets_polytope = compute_forward_reachable_set(
    initial_set_polytope,
    n_time_steps_for_forward_reach_computation,
    input_set_polytope,
)

# Recursion (Constrained zonotope)
controlled_forward_reachable_sets_constrained_zonotope = (
    compute_forward_reachable_set(
        initial_set_constrained_zonotope,
        n_time_steps_for_forward_reach_computation,
        input_set_constrained_zonotope,
    )
)

# Plot the sets
plt.figure()
for index, [controlled_forward_reachable_sets, title] in enumerate(
    [
        [controlled_forward_reachable_sets_polytope, "Polytope"],
        [
            controlled_forward_reachable_sets_constrained_zonotope,
            "Constrained zonotope",
        ],
    ]
):
    ax = plt.subplot(120 + index + 1)
    for t, facecolor in zip(
        range(n_time_steps_for_forward_reach_computation, -1, -1),
        FACECOLOR_LIST,
    ):
        patch_args = {
            "facecolor": facecolor,
            "linewidth": 2,
            "alpha": 0.5,
            "label": f"t={t:d}",
        }
        if is_polytope(controlled_forward_reachable_sets[t]):
            controlled_forward_reachable_sets[t].plot(
                ax=ax, patch_args=patch_args
            )
        else:
            controlled_forward_reachable_sets[t].plot(
                method="outer", ax=ax, patch_args=patch_args
            )
    ax.set_xlabel("Position")
    ax.set_ylabel("Velocity")
    if index == 1:
        ax.legend(bbox_to_anchor=(1.05, 1))
    ax.grid()
    ax.set_aspect("equal")
    ax.set_title(title)
plt.subplots_adjust(left=0.1, wspace=0.5)
plt.suptitle("Forward reachable sets");

The above sets can be interpreted as follows. Every point in the set $t=5$ (black set) are states (position and velocity) of the robot that can be reached by some appropriate choice of control sequence $u_t\in\mathcal{U}$. Similarly, a point outside this set cannot be reached at time $k$.

Note that the same function compute_forward_reachable_set was used for polytopic as well as constrained zonotopic $\mathcal{X}_0$ and $\mathcal{U}$. We chose to plot the outer-approximation of the constrained zonotope, since forward reachable sets are typically outer-approximated.

Example questions

We can use forward reachable set to answer the following questions:

1. Determine how far (in positive position) can the robot by k and have any non-positive velocity at k?
2. Determine how far (in negative position) can the robot by k and have any non-positive velocity at k?
3. Determine how fast (in positive direction) can the robot go at position = -0.25 at k?
4. Determine how fast (in negative direction) can the robot go at position = 0 at k?
5. Can the robot reach $p=0.5$ and $v=-1$ at $k=5$ starting from $\mathcal{X}_0$?

def plot_forward_reachable_set_and_answer_example_questions(
    ax, initial_set, forward_reachable_set_at_k
):
    # Answer the questions irrespective of whether the set is a polytope or constrained zonotope
    terminal_state_positive_position_any_nonpositive_velocity = (
        forward_reachable_set_at_k.intersection_with_halfspaces(
            A=[0, 1], b=[0]
        ).extreme([1, 0])[0]
    )
    terminal_state_negative_position_any_nonpositive_velocity = (
        forward_reachable_set_at_k.intersection_with_halfspaces(
            A=[0, 1], b=[0]
        ).extreme([-1, 0])[0]
    )
    terminal_state_fixed_position_positive_velocity = (
        forward_reachable_set_at_k.slice(0, -0.25).extreme([0, 1])[0]
    )
    terminal_state_zero_position_negative_velocity = (
        forward_reachable_set_at_k.slice(0, 0).extreme([0, -1])[0]
    )
    terminal_state_to_test_for_inclusion = [0.5, -1]
    print(
        f"Answer to Q.5 from {title}: {forward_reachable_set_at_k.contains(terminal_state_to_test_for_inclusion)}"
    )

    # Plotting is different for polytope and constrained zonotope
    patch_args_for_forward_reachable_set_at_k = {
        "facecolor": FACECOLOR_LIST[0],
        "linewidth": 2,
        "alpha": 0.5,
        "label": f"Forward reach set at k={n_time_steps_for_forward_reach_computation:d}",
    }
    patch_args_for_initial_set = {
        "facecolor": FACECOLOR_LIST[-1],
        "linewidth": 2,
        "alpha": 0.5,
        "label": "Initial set",
    }
    if is_polytope(initial_set):
        forward_reachable_set_at_k.plot(
            ax=ax, patch_args=patch_args_for_forward_reachable_set_at_k
        )
        initial_set.plot(ax=ax, patch_args=patch_args_for_initial_set)
    else:
        forward_reachable_set_at_k.plot(
            method="outer",
            ax=ax,
            patch_args=patch_args_for_forward_reachable_set_at_k,
        )
        initial_set.plot(
            method="outer", ax=ax, patch_args=patch_args_for_initial_set
        )

    # Plot the results
    ax.scatter(
        *terminal_state_positive_position_any_nonpositive_velocity,
        100,
        color="m",
        marker="v",
        label="Most positive position, velocity <= 0",
    )
    ax.scatter(
        *terminal_state_negative_position_any_nonpositive_velocity,
        100,
        color="m",
        marker="v",
        label="Most negative position, velocity <= 0",
    )
    ax.scatter(
        *terminal_state_fixed_position_positive_velocity,
        100,
        color="r",
        marker="d",
        label="Most positive velocity, fixed position",
    )
    ax.scatter(
        *terminal_state_zero_position_negative_velocity,
        100,
        color="c",
        marker="^",
        label="Most negative velocity, zero position",
    )
    ax.scatter(
        *terminal_state_to_test_for_inclusion,
        100,
        marker="o",
        edgecolors="k",
        facecolor="none",
        label="Terminal state in Q.5",
    )
    ax.grid()
    ax.set_xlabel("Position")
    ax.set_ylabel("Velocity")
    ax.set_aspect("equal")
    if index == 1:
        ax.legend(bbox_to_anchor=(1.05, 1))
    terminal_state_list = [
        terminal_state_positive_position_any_nonpositive_velocity,
        terminal_state_negative_position_any_nonpositive_velocity,
        terminal_state_fixed_position_positive_velocity,
        terminal_state_zero_position_negative_velocity,
        terminal_state_to_test_for_inclusion,
    ]
    return terminal_state_list


# Plot the forward reachable sets and answer the questions
plt.figure()
list_of_terminal_state_list = [None] * 2
for index, [
    controlled_forward_reachable_sets,
    initial_set,
    title,
] in enumerate(
    [
        [
            controlled_forward_reachable_sets_polytope,
            initial_set_polytope,
            "Polytope",
        ],
        [
            controlled_forward_reachable_sets_constrained_zonotope,
            initial_set_constrained_zonotope,
            "Constrained zonotope",
        ],
    ]
):
    ax = plt.subplot(120 + index + 1)
    list_of_terminal_state_list[
        index
    ] = plot_forward_reachable_set_and_answer_example_questions(
        ax, initial_set, controlled_forward_reachable_sets[-1]
    )
    ax.set_title(title)
plt.subplots_adjust(left=0.1, wspace=0.5)
plt.suptitle("Forward reachable set");

Answer to Q.5 from Polytope: [False]
Answer to Q.5 from Constrained zonotope: False

Validation via optimal control

For each of the above terminal states, we use optimal control to identify an initial state within the initial set $\mathcal{X}_0$ and a minimum input energy, dynamically feasible trajectory to reach the terminal state.

generate_safe_trajectory differs in how the containment constraints are enforced internally, but thanks to Python's duck typing there is no difference in the code for the end-user.

def generate_safe_trajectory(initial_set, input_set, terminal_state):
    # Solve for the initial state from which an input sequence drives the system to the randomly chosen terminal state
    state_trajectory = cp.Variable(
        (2, n_time_steps_for_forward_reach_computation + 1)
    )
    input_trajectory = cp.Variable(
        (1, n_time_steps_for_forward_reach_computation)
    )
    initial_state = state_trajectory[:, 0]
    # x_k = terminal_state
    const = [
        state_trajectory[:, n_time_steps_for_forward_reach_computation]
        == terminal_state
    ]
    # x_0 \in \mathcal{X}_0 (containment constraints differ for polytope and constrained zonotope)
    const += initial_set.containment_constraints(initial_state)[0]
    for t in range(n_time_steps_for_forward_reach_computation):
        # x_{t+1} = A x_t + B u_t
        const += [
            state_trajectory[:, t + 1]
            == system_A @ state_trajectory[:, t]
            + system_B @ input_trajectory[:, t]
        ]
        # u_t \in \mathcal{U}
        const += input_set.containment_constraints(input_trajectory[:, t])[
            0
        ]
    # Solve the optimal control problem
    prob = cp.Problem(cp.Minimize(cp.norm(input_trajectory, p=2)), const)
    prob.solve(solver="CLARABEL")
    if prob.status in [cp.OPTIMAL, cp.OPTIMAL_INACCURATE]:
        return state_trajectory.value, prob.status
    else:
        return None, prob.status


plt.figure()
list_of_terminal_state_list = [None] * 2
for index, [
    controlled_forward_reachable_sets,
    input_set,
    initial_set,
    title,
] in enumerate(
    [
        [
            controlled_forward_reachable_sets_polytope,
            input_set_polytope,
            initial_set_polytope,
            "Polytope",
        ],
        [
            controlled_forward_reachable_sets_constrained_zonotope,
            input_set_constrained_zonotope,
            initial_set_constrained_zonotope,
            "Constrained zonotope",
        ],
    ]
):
    ax = plt.subplot(120 + index + 1)
    list_of_terminal_state_list[
        index
    ] = plot_forward_reachable_set_and_answer_example_questions(
        ax, initial_set, controlled_forward_reachable_sets[-1]
    )
    ax.set_title(title)
    for terminal_state in list_of_terminal_state_list[index]:
        # Use optimal control to obtain trajectories
        state_trajectory, prob_status = generate_safe_trajectory(
            initial_set, input_set, terminal_state
        )
        if state_trajectory is not None:
            # Plot the trajectory
            ax.plot(
                state_trajectory[0, :],
                state_trajectory[1, :],
                "kx:",
                label=(
                    f"Min. acceleration trajectory to terminal state "
                    f"({terminal_state[0]:.2f}, {terminal_state[1]:.2f})"
                ),
            )
        else:
            ax.scatter(
                *terminal_state,
                100,
                marker="o",
                edgecolors="k",
                facecolor="none",
                label="Terminal state in Q.5",
            )
            print(
                f"No trajectory to terminal state ({terminal_state[0]:.2f}, {terminal_state[1]:.2f}). "
                f"CVXPY status: {prob_status}"
            )
plt.subplots_adjust(left=0.1, wspace=0.5)
plt.suptitle("Validation using optimal control");

Answer to Q.5 from Polytope: [False]
No trajectory to terminal state (0.50, -1.00). CVXPY status: infeasible

Answer to Q.5 from Constrained zonotope: False

No trajectory to terminal state (0.50, -1.00). CVXPY status: infeasible

Backward robust reachable set

Backward reachability seeks to answer the question --- Where can I start so that I can reach a given set of terminal states under dynamical constraints?

Given a terminal set $\mathcal{X}_k\subset\mathbb{R}^n$ and a discrete-time linear dynamics (1), the backward reachable set is the set of initial states that can be driven to reach the terminal set $\mathcal{X}_k$ at time $k\in\mathbb{N},k>0$ in the future, despite the disturbance. Formally, \begin{align} \mathcal{X}_0=\left\{x_0 \middle| \begin{array}{c} \exists x_k\in\mathcal{X}_k,\ \forall t\in\{0,...,k-1\},\\ \exists u_t\in\mathcal{U},\ \forall w_t\in\mathcal{W},\\ x_{t+1}=Ax_t + Bu_t + Fw_t\end{array}\right\}.\tag{6} \end{align} The order of the quantifiers is important --- the controller $u_t$ uses the current state information $x_t$ but does not require the knowledge of the disturbance $w_t$ at each time step $t$.

Computation via set recursion

We can set up the following set recursion to compute $\mathcal{X}_k$, \begin{align} \mathcal{X}_t &=((\mathcal{X}_{t+1}\ominus F\mathcal{W})\oplus (-B\mathcal{U}))A\tag{7} \end{align} for $t\in\{k-1, k-2, \ldots, 0\}$, initialized to the terminal set $\mathcal{X}_k$.

Some notational details:

1. $M_1\mathcal{S}=\{M_1 x|x\in\mathcal{S}\}$ and $\mathcal{S}M_2=\{x|M_2 x\in\mathcal{S}\}$ for any set $\mathcal{S}$ and an appropriately dimensioned matrices $M_1,M_2$,
2. $\mathcal{S}_1\oplus\mathcal{S}_2=\{s_1 + s_2|s_1\in\mathcal{S}_1,\ s_2\in\mathcal{S}_2\}$ for any two sets $\mathcal{S}_1,\mathcal{S}_2$, and
3. $\mathcal{S}_1\ominus\mathcal{S}_2=\{s|\{s\}\oplus\mathcal{S}_2\subseteq\mathcal{S}_1\}$ for any two sets $\mathcal{S}_1,\mathcal{S}_2$.

def compute_backward_robust_reachable_set(
    terminal_set_recursion,
    time_for_backward_robust_reachable_set,
    input_set_recursion,
    disturbance_set_recursion,
):
    recursion_backward_robust_reachable_sets = [None] * (
        time_for_backward_robust_reachable_set + 1
    )
    recursion_backward_robust_reachable_sets[
        time_for_backward_robust_reachable_set
    ] = terminal_set_recursion
    scaled_disturbance_set = system_F @ disturbance_set_recursion
    negative_scaled_input_set = (-system_B) @ input_set_recursion
    for t in range(
        time_for_backward_robust_reachable_set - 1, -1, -1
    ):  # for t in \{k-1, k-2, ..., 0\}
        # Implement (7)
        recursion_backward_robust_reachable_sets[t] = (
            (
                recursion_backward_robust_reachable_sets[t + 1]
                - scaled_disturbance_set
            )
            + negative_scaled_input_set
        ) @ system_A
    return recursion_backward_robust_reachable_sets

Application to the physical example

Backward robust reachable set computations encode the the possible initial states the robot can start at time $k=$ so that it can be steered to the given terminal set at time $k$, given the dynamics (1), the terminal set $\mathcal{X}_k$, the input set $\mathcal{U}$, and the disturbance set $\mathcal{W}$.

We set $k=20$ and choose the terminal set $\mathcal{X}_k=[-0.75,0.75]\times[-0.2, 0.2]$. Here, $\mathcal{X}_0$ is the set of all admissible terminal states (position and velocities) of the robot that we want to include in the analysis.

We choose the input set $\mathcal{U}=[-1,1]$, and the disturbance set $\mathcal{W}=s\mathcal{U}$ with $s\in\{0, 0.2, 0.4, 0.8\}$.

k_backward_robust_reachable_set = 20
DISTURBANCE_SCALING_LIST = [0, 0.2, 0.4, 0.8]

# Define the terminal set and input set (Polytope)
terminal_set_polytope = Polytope(c=[0, 0], h=[0.75, 0.25])
input_set_polytope = Polytope(lb=-1, ub=1)

# Define the terminal set and input set (Constrained zonotope)
terminal_set_constrained_zonotope = ConstrainedZonotope(
    c=[0, 0], h=[0.75, 0.25]
)
input_set_constrained_zonotope = ConstrainedZonotope(lb=-1, ub=1)

# Recursion
list_of_backward_robust_reachable_sets_dict = [None] * 2
elapsed_time = np.zeros((len(DISTURBANCE_SCALING_LIST), 2))
area_of_sets = np.zeros((len(DISTURBANCE_SCALING_LIST), 2))
for index, [input_set, terminal_set] in enumerate(
    [
        [input_set_polytope, terminal_set_polytope],
        [
            input_set_constrained_zonotope,
            terminal_set_constrained_zonotope,
        ],
    ]
):
    list_of_backward_robust_reachable_sets_dict[index] = {}
    for disturbance_index, disturbance_scaling in enumerate(
        DISTURBANCE_SCALING_LIST
    ):
        start_time = time.time()
        disturbance_set = disturbance_scaling * input_set
        list_of_backward_robust_reachable_sets_dict[index][
            disturbance_scaling
        ] = compute_backward_robust_reachable_set(
            terminal_set,
            k_backward_robust_reachable_set,
            input_set,
            disturbance_set,
        )
        elapsed_time[disturbance_index, index] = time.time() - start_time
        if index == 0:
            area_of_sets[
                disturbance_index, index
            ] = list_of_backward_robust_reachable_sets_dict[index][
                disturbance_scaling
            ][
                0
            ].volume()
        else:
            area_of_sets[
                disturbance_index, index
            ] = approximate_volume_from_grid(
                list_of_backward_robust_reachable_sets_dict[index][
                    disturbance_scaling
                ][0],
                0.5,
            )

# Show the compute times
disturbance_elapsed_time = np.hstack(
    (np.array([DISTURBANCE_SCALING_LIST]).T, elapsed_time, area_of_sets)
)

print("       | Time (s)      | Area of sets  |")
print(" Scale | Poly. | ConZ. | Poly. | ConZ. |")
for row in disturbance_elapsed_time:
    print(
        f"{np.array2string(row, formatter={'float_kind': lambda x: f'{x:1.3f}'}, separator=' | ')}"
    )

/home/vinod/workspace/git/PaperCodes/general/pycvxset/pycvxset/ConstrainedZonotope/operations_binary.py:644: UserWarning: This function returns an inner-approximation of the Pontryagin difference with a zonotope.
  warnings.warn(

       | Time (s)      | Area of sets  |
 Scale | Poly. | ConZ. | Poly. | ConZ. |
[0.000 | 3.925 | 0.164 | 14.070 | 13.750]
[0.200 | 4.183 | 0.150 | 10.296 | 7.815]
[0.400 | 4.872 | 0.152 | 7.069 | 4.984]
[0.800 | 4.290 | 0.156 | 2.257 | 1.293]

Observe that the backward robust reachable set computation using constrained zonotopes is about 2 orders of magnitude faster than the set computation using polytopes.

The speed-up is due to the fact that all set operations using constrained zonotopes have closed-form expressions, and can be implemented without invoking any optimization solvers. On the other hand, set computation using polytopes require vertex-halfspace enumeration.

Backward reachable set computation using constrained zonotope yields an inner-approximation since there is no exact approach to compute the Pontryagin difference. On the other hand, the polytope-based set computations are exact and compute the true backward reachable sets. However, as seen from the table, the inner-approximations provided by the constrained zonotope recover most of the true backward robust reachable set.

def plot_backward_reachable_sets(
    ax,
    terminal_set,
    backward_robust_reachable_sets_dict,
    disturbance_scaling_to_plot,
    title,
    plot_all,
):
    # Plot the sets
    if plot_all:
        for t, facecolor in zip(
            range(k_backward_robust_reachable_set, -1, -5), FACECOLOR_LIST
        ):
            patch_args = {
                "facecolor": facecolor,
                "linewidth": 2,
                "alpha": 0.5,
                "label": f"t={t:d}",
            }
            set_to_plot = backward_robust_reachable_sets_dict[
                disturbance_scaling_to_plot
            ][t]
            set_to_plot.plot(ax=ax, patch_args=patch_args)
    else:
        patch_args = {
            "facecolor": FACECOLOR_LIST[0],
            "linewidth": 2,
            "alpha": 0.5,
            "label": f"Backward robust reachable set",
        }
        set_to_plot = backward_robust_reachable_sets_dict[
            disturbance_scaling_to_plot
        ][0]
        set_to_plot.plot(ax=ax, patch_args=patch_args)
    patch_args = {"fill": False, "linestyle": ":", "label": "Terminal set"}
    terminal_set.plot(ax=ax, patch_args=patch_args)
    ax.set_xlabel("Position")
    ax.set_ylabel("Velocity")
    ax.set_xlim([-4, 4])
    ax.set_ylim([-3, 3])
    ax.grid()
    ax.set_aspect("equal")
    ax.set_title(title)
    if index == 1:
        ax.legend(bbox_to_anchor=(1.15, 1))


# Plot the sets
for disturbance_scaling in DISTURBANCE_SCALING_LIST:
    plt.figure()
    for index, [terminal_set, title] in enumerate(
        [
            [terminal_set_polytope, "Polytope"],
            [terminal_set_constrained_zonotope, "Constrained zonotope"],
        ]
    ):
        ax = plt.subplot(120 + index + 1)
        plot_backward_reachable_sets(
            ax,
            terminal_set,
            list_of_backward_robust_reachable_sets_dict[index],
            disturbance_scaling,
            title,
            plot_all=True,
        )
    plt.subplots_adjust(left=0.1, wspace=0.5)
    plt.suptitle(
        f"Backward robust reach set ($\mathcal{{W}}={disturbance_scaling:1.1f} \mathcal{{U}}$)"
    )

As expected and seen from the table above, the backward robust reachable sets (the set of admissible initial states) shrink as the scaling $s$ used to define the disturbance set increases.

Approximate validation via set-based control

It is hard to simulate backward robust reachable sets since the reachability problem tackles a two-player game --- input trying to drive the system to terminal set, while disturbance trying to drive the system away from the terminal set.

We resort to an approximation where we design trajectories based on the optimal control, but possibly suboptimal disturbance (less adversarial).

From (6), the optimal controller at the current state $x_t$ for $t\in\{0,1,\ldots,k-1\}$ must satisfy requirements: \begin{align} u_t&\in\mathcal{U},\tag{8a}\\\{Ax_t + Bu_t\}\oplus F\mathcal{W}\subseteq \mathcal{X}_{t+1} \Longleftrightarrow B u_t &\in (\mathcal{X}_{t+1}\ominus F\mathcal{W})\oplus \{-Ax_t\},\tag{8b} \end{align} where (8a) is the control constraint, and (8b) arises from the "forward roll-out" of the set recursion (7), i.e., $u_t$ should be such that irrespective of the disturbance $w_t$ $$x_{t+1}\in\mathcal{X}_{t+1}.$$

In order to alleviate numerical issues faced by solvers, we set up the following linear program to compute $u_t^\ast$ with a slack variable $s$, \begin{align} \begin{array}{rl} \underset{s, u\in\mathbb{R}}{\text{minimize}} &\quad s\\ \text{subject\ to} &\quad A_{\mathcal{U}} u \leq b_\mathcal{U} + s, \\ &\quad A_{\mathcal{X}_t^\dagger} B u \leq b_{\mathcal{X}_t^\dagger} + s, \\ &\quad s \geq 0, \end{array}\tag{(9)} \end{align} where $\mathcal{X}_t^\dagger = (\mathcal{X}_{t+1}\ominus F\mathcal{W})\oplus \{-Ax_t\}$. The controller designed using (9) is guaranteed to keep the system safe irrespective of the choice of disturbance (including the optimally adversarial disturbance).

As discussed before, the computation of the adversarial disturbance is a bit more complicated, and we rely on a heuristic. At each time step, for the disturbance, we choose $w_t\in\mathcal{W}$ that solves, \begin{align} w_t^\ast=\arg\max_{w_t\in\mathcal{W}}\ \ \mathrm{dist}(\mathcal{X}_k, Ax_t + Bu_t^\ast + Fw_t).\tag{10} \end{align} In other words, we pick the disturbance that pushes the system the furthest away from the terminal set. Since (10) is a convex maximization problem, the optimal occurs at the vertices of $\mathcal{W}$. An optimally adversarial disturbance may choose a more future-looking choice, instead of the myopic choice in (10).

def generate_approximate_worst_case_trajectory(
    backward_robust_reachable_sets_list,
    initial_state_to_simulate,
    input_set,
    terminal_set,
    disturbance_scaling_to_simulate,
):
    disturbance_set_to_simulate = (
        disturbance_scaling_to_simulate * input_set
    )
    scaled_disturbance_set = system_F @ disturbance_set_to_simulate
    if is_constrained_zonotope(scaled_disturbance_set):
        scaled_disturbance_set_outer_approx_polytope = (
            scaled_disturbance_set.polytopic_outer_approximation(
                n_halfspaces=10
            )
        )
        scaled_disturbance_set_outer_approx_polytope.minimize_V_rep()

    state_trajectory = np.zeros((2, k_backward_robust_reachable_set + 1))
    state_trajectory[:, 0] = initial_state_to_simulate
    if (
        initial_state_to_simulate
        not in backward_robust_reachable_sets_list[0]
    ):
        raise ValueError("Invalid initial state provided!")
    for t in range(k_backward_robust_reachable_set):
        temp_next_state_natural_dynamics = (
            system_A @ state_trajectory[:, t]
        )
        # Compute X_t^\dagger
        next_backward_robust_reachable_set = (
            backward_robust_reachable_sets_list[t + 1]
        )
        next_backward_robust_reachable_set_minus_dist = (
            next_backward_robust_reachable_set - scaled_disturbance_set
        ) - temp_next_state_natural_dynamics

        # Implement (9) to compute the optimal action
        input_to_apply = cp.Variable((input_set.dim,))
        if is_polytope(input_set):
            slack = cp.Variable((1,), nonneg=True)
            const = [
                input_set.A @ input_to_apply <= input_set.b + slack,
                next_backward_robust_reachable_set_minus_dist.A
                @ system_B
                @ input_to_apply
                <= next_backward_robust_reachable_set_minus_dist.b + slack,
            ]
            if input_set.n_equalities:
                const += [
                    cp.abs(input_set.Ae @ input_to_apply - input_set.be)
                    <= slack
                ]
            if next_backward_robust_reachable_set_minus_dist.n_equalities:
                const += [
                    cp.abs(
                        next_backward_robust_reachable_set_minus_dist.Ae
                        @ system_B
                        @ input_to_apply
                        - next_backward_robust_reachable_set_minus_dist.be
                    )
                    <= slack
                ]
            problem = cp.Problem(cp.Minimize(slack), const)
        else:
            const = input_set.containment_constraints(input_to_apply)[0]
            const += next_backward_robust_reachable_set_minus_dist.containment_constraints(
                system_B @ input_to_apply
            )[
                0
            ]
            problem = cp.Problem(cp.Minimize(cp.Constant(0)), const)
        problem.solve(**input_set.cvxpy_args_lp)
        if problem.status not in [cp.OPTIMAL, cp.OPTIMAL_INACCURATE]:
            raise RuntimeError("Could not find an optimal action!")
        if is_polytope(input_set) and slack.value > 1e-6:
            raise RuntimeError(
                "(next_backward_robust_reachable_set_minus_dist @ system_B) \cap input_set is empty!"
            )
        optimal_input_to_apply = input_to_apply.value

        # Apply the optimal_input_to_apply
        temp_next_state_without_disturbance = (
            temp_next_state_natural_dynamics
            + system_B @ optimal_input_to_apply
        )

        # Approximate the worst-case disturbance using (10)
        if is_polytope(scaled_disturbance_set):
            possible_scaled_W = scaled_disturbance_set.V
        else:
            possible_scaled_W = (
                scaled_disturbance_set_outer_approx_polytope.V
            )
        distance_list = []
        for scaled_W in possible_scaled_W:
            distance_list.append(
                terminal_set.distance(
                    temp_next_state_without_disturbance + scaled_W
                )
            )
        disturbance_vertex_index_to_apply = np.argmax(distance_list)
        scaled_disturbance_to_apply = possible_scaled_W[
            disturbance_vertex_index_to_apply
        ]

        # Get the next state
        state_trajectory[:, t + 1] = (
            temp_next_state_without_disturbance
            + scaled_disturbance_to_apply
        )
    return state_trajectory

In generate_approximate_worst_case_trajectory, ideally we would have liked to use the code used for ConstrainedZonotope to also work for the Polytope case as well. However, due to numerical issues possibly from the vertex-facet enumeration, we resort to a "soft" enforcement of the constraints and ensure that the slack variables evaluate to zero. On the other hand, the constrained zonotope-based approach since it has closed-form expressions for all the set computations and rely on standard linear algebraic operations are typically more numerically stable.

From (8) and (9), $x_{t+1}=Ax_t+Bu_t^\ast+Fw_t^\ast\in\mathcal{X}_{t+1}$ for every $t\in\{1,2,\ldots,k-1\}$. Thus, we can guarantee that the system to the terminal set $\mathcal{X}_k$ at $k$ under the dynamical constraints (1).

disturbance_scaling = 0.2
direction_list = [
    [1, 1],
    [1, -1],
    [-1, 1],
    [-1, -1],
    [-0.01, -1],
    [0.01, 1],
]

plt.figure()
for index, [input_set, terminal_set, title] in enumerate(
    [
        [input_set_polytope, terminal_set_polytope, "Polytope"],
        [
            input_set_constrained_zonotope,
            terminal_set_constrained_zonotope,
            "Constrained zonotope",
        ],
    ]
):
    ax = plt.subplot(120 + index + 1)
    plot_backward_reachable_sets(
        ax,
        terminal_set,
        list_of_backward_robust_reachable_sets_dict[index],
        disturbance_scaling,
        title,
        plot_all=False,
    )
    backward_robust_reachable_sets_list = (
        list_of_backward_robust_reachable_sets_dict[index][
            disturbance_scaling
        ]
    )
    for direction in direction_list:
        initial_state = backward_robust_reachable_sets_list[0].extreme(
            direction
        )[0]
        ax.scatter(
            *initial_state,
            50,
            label=f"Initial state ({initial_state[0]:1.2f}, {initial_state[1]:1.2f})",
        )
        state_trajectory = generate_approximate_worst_case_trajectory(
            backward_robust_reachable_sets_list,
            initial_state,
            input_set,
            terminal_set,
            disturbance_scaling,
        )
        ax.plot(
            state_trajectory[0, :],
            state_trajectory[1, :],
            "kx-",
            label=f"Trajectory from ({initial_state[0]:1.2f}, {initial_state[1]:1.2f})",
        )
plt.subplots_adjust(left=0.1, wspace=0.5)
plt.suptitle(
    f"Backward robust reach set ($\mathcal{{W}}={disturbance_scaling:1.1f} \mathcal{{U}}$)"
)
ax.legend(bbox_to_anchor=(1.25, 1.2), ncols=2);

Example questions

Some example questions we can answer using backward robust reachable set:

1. Determine how far (in positive position) can the robot start with any non-positive velocity?
2. Given we can start only at position = 0.25, what is the highest initial positive velocity we can have?
3. Can the robot starting at position = 0 with velocity = 0.75 be steered to the terminal set $\mathcal{X}_k$ at k=20?

s = 0.2
plt.figure()
patch_args = {
    "facecolor": FACECOLOR_LIST[-1],
    "linewidth": 2,
    "alpha": 0.5,
    "label": f"{k_backward_robust_reachable_set:d}-step backward robust reachable set for s={s:1.1f}",
}
for index, title in enumerate(["Polytope", "Constrained zonotope"]):
    ax = plt.subplot(120 + index + 1)
    backward_robust_reachable_set = (
        list_of_backward_robust_reachable_sets_dict[index][s][0]
    )
    initial_state_positive_position_any_nonnegative_velocity = (
        backward_robust_reachable_set.intersection_with_halfspaces(
            A=[0, -1], b=[-0.5]
        ).extreme([1, 0])[0]
    )
    initial_state_fixed_position_any_positive_velocity = (
        backward_robust_reachable_set.slice(0, -0.6).extreme([0, -1])[0]
    )

    # Plot the results
    backward_robust_reachable_set.plot(ax=ax, patch_args=patch_args)
    terminal_set_polytope.plot(
        ax=ax,
        patch_args={
            "fill": False,
            "linestyle": ":",
            "label": "Terminal set",
        },
    )
    ax.scatter(
        *initial_state_positive_position_any_nonnegative_velocity,
        100,
        color="r",
        marker="d",
        label="Most positive initial position where initial velocity >= 0.5",
    )
    ax.scatter(
        *initial_state_fixed_position_any_positive_velocity,
        100,
        color="m",
        marker="v",
        label="Most negative initial velocity where initial position = -0.6",
    )
    if index == 1:
        ax.legend(bbox_to_anchor=(1.15, 1))
    ax.set_xlabel("Position")
    ax.set_ylabel("Velocity")
    ax.set_xlim([-2, 2])
    ax.set_ylim([-1, 1])
    ax.grid()
    ax.set_aspect("equal")
    ax.set_title(title)
plt.subplots_adjust(left=0.1, wspace=0.5)
plt.suptitle(
    f"Backward robust reach set ($\mathcal{{W}}={disturbance_scaling:1.1f} \mathcal{{U}}$)"
);

Conclusions

• pycvxset permits the computation of (forward and backward) reachable sets using polytopes and constrained zonotopes.
• The classes for polytopes and constrained zonotopes are designed such that one can be substituted for another without any thought.
• Two key distinctions between polytopes and constrained zonotopes are:
  • Plotting: Constrained zonotopes require polytopic_inner/outer_approximations, while polytopes can be plotted exactly.
  • Pontryagin difference: When using ConstrainedZonotope as the minuend (first term of the Pontryagin difference), we have to settle with an inner-approximation. However, as seen from the examples of backward robust reachable set computation, the inner-approximations obtained are typically non-trivial.