REVISE

REVISE is a covariance-steering-based belief roadmapping algorithm for linear systems affected by a spatially varying and state-dependent disturbance represented by a Gaussian random field.

Graph Representation

class src.graph.Edge(start_node, end_node, mean, covariance, ff_ctrl, fb_ctrl)

Bases: object

Represents a directed edge in a belief roadmap between two Gaussian state distributions. Each edge is associated with a feedback control policy and a discrete-time Gaussian state trajectory between its start and end nodes.

Parameters:

• start_node (Node) – Start node of the edge

• end_node (Node) – End node of the edge

• mean (np.ndarray) – Array of means of intermediate Gaussian states

• covariance (np.ndarray) – Array of covariances of intermediate Gaussian states

• ff_ctrl (np.ndarray) – Open-loop control between start and end nodes

• fb_ctrl (np.ndarray) – Feedback control gain between start and end nodes

class src.graph.Graph(nodes, edges)

Bases: object

Represents a graph-structured belief roadmap, where the nodes in the roadmap are Gaussian distributions in the state space, and the directed edges in the roadmap are control policies steering between nodes.

Parameters:

• nodes (set) – Set of Node objects representing nodes

• edges (set) – Set of Edge objects representing edges

add_edge(edge)

Add edge to the graph, assuming the start and end nodes of the edge are already in the graph.

Parameters:

edge (Edge) – Edge to be added to the graph.

add_node(node)

Add node to the graph.

Parameters:

node (Node) – Node to be added to the graph.

deepcopy()

Deepcopy graph to a new Graph object.

Returns:

graph_copy – Deepcopy of self

Return type:

Graph

get_ancestors(node)

Get ancestors of a node in the graph, recursively, all the way back to the start node.

Parameters:

node (Node) – Node to look up ancestors

Returns:

ancestors – Ancestors of node

Return type:

set

get_children(node)

Get children of a node in the graph.

Parameters:

node (Node) – Node to look up children

Returns:

children – Set of child nodes of node (can be empty)

Return type:

set

get_descendants(node)

Get descendants of a node in the graph, recursively, all the way down to leaf nodes.

Parameters:

node (Node) – Node to find descendants of

Returns:

descendants – Set of descendants of node

Return type:

set

get_goal_node()

Get the goal node in the graph. Throws an error unless exactly one goal node is in the graph.

Returns:

goal_node – Goal node of the graph

Return type:

Node

get_parent(node)

Get the parent of a node in the graph. Throws an error if the node isn’t in the graph.

Parameters:

• node (Node) – Child node to look up parent

• parent (Node) – Parent of child node (None if child is start)

get_plan_to_goal()

Trace edges from start to goal, and extract planned control and state. Can probably be refactored to traverse over trimmed graph only.

Returns:

• u_traj (list) – List of planned open-loop control for each edge from start to goal

• K_traj (list) – List of planned feedback control for each edge

• x_traj (list) – List of mean state trajectory for each edge

• P_traj (list) – List of state covariance for each edge

get_start_node()

Get the start node in the graph. Throws an error unless exactly one start node is in the graph.

Returns:

start_node – Start node of the graph

Return type:

Node

look_up_by_mean(node_mean)

Look up a node in a graph by its mean. Throws an error unless exactly one node with the given mean is in the graph.

Parameters:

• node_mean (np.ndarray) – Mean of node to look up

• node (Node) – Node in graph with node_mean as its mean

trim()

Return minimal subgraph which connects the start node to the goal node. Assumes a path exists from the start to the goal.

Returns:

trimmed_graph – Minimal subgraph connecting start to goal

Return type:

Graph

class src.graph.Node(mean, covariance, is_start=False, is_goal=False)

Bases: object

Represents a multidimensional Gaussian state distribution in a graph-structured belief roadmap.

Parameters:

• mean (np.ndarray) – Mean of the Gaussian distribution

• covariance (np.ndarray) – Covariance of the Gaussian distribution, must be PSD

• is_start (bool) – True if start node, False otherwise

• is_goal (bool) – True if goal node, False otherwise

Math Utils

src.utils.math_utils.check_psd(A)

Check if matrix is positive semidefinite by attempting Cholesky decomposition.

Parameters:

A (np.ndarray) – Matrix to check for positive semidefiniteness

Return type:

True if A is PSD, False otherwise

src.utils.math_utils.ensure_psd(A, eps=1e-09)

Ensure matrix is positive semidefinite by adding I*eps.

Parameters:

A (np.ndarray) – Matrix which is within floating point error of having all positive eigenvalues

Returns:

A_psd – A + I*1e-9

Return type:

np.ndarray

Quadrotor

class src.problem.Problem

Bases: ABC

class src.quadrotor_problem.Quadrotor2DGRF(dt=0.1, obstacles='none')

Bases: Problem

get_wind_covariance_at_locations(x_i, x_j, wind_cov)

x_i and x_j are two different states.

Covariance Steering

class src.covariance_steering.EdgeController(value, names=<not given>, *values, module=None, qualname=None, type=None, start=1, boundary=None)

Bases: Enum

Enum representing choice of edge controller.

src.covariance_steering.baseline_covariance_steering(problem, A, B, G, mean_W, cov_W, x_0, cov_0, x_f)

Baseline covariance steering algorithm for steering in a Gaussian random field. Combines the coverage-maximizing objective from Aggarwal & How 2024 with the approach to covariance steering in a GRF from Ridderhof & Tsiotras 2022.

This algorithm corresponds to Problem III.1 in the paper. See section III of the paper for additional details.

Parameters:

• problem (Problem) – Problem class with dynamics and constraints

• A (np.ndarray) – State transition matrix, such that the open loop system dynamics in block-matrix notation are given by \(X = Ax_0 + BU + GW\)

• B (np.ndarray) – Control matrix in state-space dynamics, such that \(X = AX_0 + BU + GW\)

• G (np.ndarray) – Disturbance matrix in state-space dynamics, such that \(X + AX_0 + BU + GW\)

• mean_W (np.ndarray) – Mean of the Gaussian random field along x_nominal

• cov_W (np.ndarray) – Covariance of the Gaussian random field along x_nominal

• x_0 (np.ndarray) – Mean of the initial state distribution

• cov_0 (np.ndarray) – Covariance of the initial state distribution

• x_f (np.ndarray) – Desired mean of the final state distribution

Returns:

• success (bool) – True if MOSEK can solve the covariance steering problem, otherwise False

• X_traj (np.ndarray) – State trajectory solution to covariance steering problem (or none if no solution)

• U_traj (np.ndarray) – Nominal control trajectory solution to covariance steering problem (or None if no solution)

• P_traj (np.ndarray) – State covariance trajectory solution to covariance steering problem (or None if no solution)

• K (np.ndarray) – Control feedback gain solution to covariance steering problem (or None if no solution)

• cov_f (np.ndarray) – Final state covariance (or None if no solution to covariance steering problem)

src.covariance_steering.baseline_edge_controller(problem, x_0, P_0, x_f, n_states)

Get a belief roadmap edge from \(\mathcal{N}(\mathbf{x_0}, P_0)\) to a distribution with mean \(\mathbf{x_f}\), if such an edge exists, using baseline_edge_controller() as the edge controller.

Parameters:

• problem (Problem) – Problem class with associated dynamics and constraints

• x_0 (np.ndarray) – Initial state mean

• P_0 (np.ndarray) – Initial state covariance

• x_f (np.ndarray) – Final state mean

• n_states (int) – State trajectory length

Returns:

• success (bool) – True iff valid edge found

• x_traj (np.ndarray) – Mean state trajectory along edge, or None if no edge found

• u_traj (np.ndarray) – Nominal control trajectory along edge, or None if no edge found

• cov_traj (np.ndarray) – State covariance trajectory along edge, or None if no edge found

• K (np.ndarray) – State feedback gain along edge, or None if no edge found

• cov_f (np.ndarray) – Final state covariance, or None if no edge found

src.covariance_steering.get_E_k_u(k, n_controls, control_size)

Get \(E_k^u\) such that \(E_k^u \mathbf{U} = \mathbf{u_k}\). Wrapper around quadrotor_experiment.get_E_k_x()

src.covariance_steering.get_E_k_x(k, n_states, state_size)

Get \(E_k^x\) such that \(E_k^x \mathbf{X} = \mathbf{x_k}\).

Parameters:

• k (int) – Timestep chosen so that \(E_k^x \mathbf{X} = \mathbf{x_k}\)

• n_states (int) – Number of states in mathbf{X}

• state_size (int) – Size of state vector

Returns:

E_k

Return type:

\(E_k^x\) such that \(E_k^x \mathbf{X} = \mathbf{x_k}\)

src.covariance_steering.get_lower_triangular_L(n_states, n_controls, state_size, control_size)

Get lower triangular matrix of cvxpy variables, of size (n_controls * control_size) x (n_states * state_size).

Parameters:

• n_states (int) – Number of states in trajectory

• n_controls (int) – Number of control variables in trajectory. Should be equal to n_states - 1

• state_size (int) – Size of state vector

• control_size (int) – Size of control vector

src.covariance_steering.mean_steering(problem, x_0, x_f, n_states)

Initialize a feasible mean state trajectory, subject to dynamics constraints but not to state or control chance constraints. Assumes disturbance is a linear function of the state. Minimizes control usage.

Parameters:

• problem (Problem) – Problem class with associated dynamics to compute mean trajectory for

• x_0 (np.ndarray) – Initial state

• x_f (np.ndarray) – Final state

• n_states (int) – Number of states in trajectory

Returns:

• success (bool) – True if SCS can find a valid trajectory, False otherwise

• u_traj (np.ndarray) – Control trajectory used to steer from x_0 to x_f (None if not success)

src.covariance_steering.nominal_rollout(problem, x_0, u_traj, n_states)

Roll out mean state trajectory of a system given an initial state and nominal control trajectory.

Parameters:

• problem (Problem) – Problem class with associated dynamics to roll out

• x_0 (np.ndarray) – Initial mean state

• u_traj (np.ndarray) – Nominal control trajectory

• n_states (int) – Number of states to roll out

Returns:

x_traj – Mean state trajectory

Return type:

np.ndarray

src.covariance_steering.robust_sigma_point_covariance_steering(problem, x_0, A, B, G, mean_W, cov_W, As, Bs, Gs, mean_Ws, cov_Ws, sigma_points, cov_0, x_f)

Robust sigma point steering algorithm for steering in a Gaussian random field.

This algorithm corresponds to Problem IV.1 in the paper. See section IV of the paper for additional details.

Parameters:

• problem (Problem) – Problem class with dynamics and constraints

• A (np.ndarray) – State transition matrix, such that the open loop system dynamics in block-matrix notation are given by \(\mathbf{X} = A\mathbf{x_0} + B\mathbf{U} + G\mathbf{W}\)

• B (np.ndarray) – Control matrix in state-space dynamics, such that the open loop system dynamics in block-matrix notation are given by \(\mathbf{X} = A\mathbf{x_0} + B\mathbf{U} + G\mathbf{W}\)

• G (np.ndarray) – Disturbance matrix in state-space dynamics, such that the open loop system dynamics in block-matrix notation are given by \(\mathbf{X} = A\mathbf{x_0} + B\mathbf{U} + G\mathbf{W}\)

• mean_W (np.ndarray) – Mean of the Gaussian random field along a nominal trajectory

• cov_W (np.ndarray) – Covariance of the Gaussian random field along a nominal trajectory

• As (list) – List of state-transition (\(A\)) matrices for each sigma point

• Bs (list) – List of control (\(B\)) matrices for each sigma point

• Gs (list) – List of disturbance (\(G\)) matrices for each sigma point

• mean_Ws (list) – List of mean disturbance trajectories starting at each sigma point, when initial guess control is applied

• cov_Ws (list) – List of covariances of disturbance trajectories starting at each sigma point, when initial guess control is applied

• sigma_points (list) – List of initial state for each sigma point

• cov_0 (np.ndarray) – Covariance of the initial state distribution

• x_f (np.ndarray) – Desired mean of the final state distribution

Returns:

• success (bool) – True if MOSEK can solve the covariance steering problem, otherwise False

• X_traj (np.ndarray) – State trajectory solution to covariance steering problem (or none if no solution)

• U_traj (np.ndarray) – Nominal control trajectory solution to covariance steering problem (or None if no solution)

• P_traj (np.ndarray) – State covariance trajectory solution to covariance steering problem (or None if no solution). Computed by approximating the mixture of Gaussians given by the mixture of sigma points as a Gaussian at each timestep.

• K (np.ndarray) – Control feedback gain solution to covariance steering problem (or None if no solution)

• cov_f (np.ndarray) – Final state covariance (or None if no solution to covariance steering problem). Computed as an over-approximation of the state covariance, equivalent to the worst-case contribution to the state covariance over all sigma points. See Section IV in the paper for further details.

src.covariance_steering.robust_sigma_point_edge_controller(problem, x_0, P_0, x_f, n_states)

Get a belief roadmap edge from \(\mathcal{N}(\mathbf{x_0}, P_0)\) to a distribution with mean \(\mathbf{x_f}\), if such an edge exists, using robust_sigma_point_covariance_steering() as the edge controller.

Parameters:

• problem (Problem) – Problem class with associated dynamics and constraints

• x_0 (np.ndarray) – Initial state mean

• P_0 (np.ndarray) – Initial state covariance

• x_f (np.ndarray) – Final state mean

• n_states (int) – State trajectory length

Returns:

• success (bool) – True iff valid edge found

• x_traj (np.ndarray) – Mean state trajectory along edge, or None if no edge found

• u_traj (np.ndarray) – Nominal control trajectory along edge, or None if no edge found

• cov_traj (np.ndarray) – State covariance trajectory along edge, or None if no edge found

• K (np.ndarray) – State feedback gain along edge, or None if no edge found

• cov_f (np.ndarray) – Final state covariance, or None if no edge found

src.covariance_steering.select_sigma_points(x_0, P_0)

Select 2 * state_size symmetrically distributed sigma points on the \(\sqrt{state\_size}\) th covariance contour of a Gaussian state distribution.

See Julier & Uhlmann 2004 for further details.

Parameters:

• x_0 (np.ndarray) – State mean

• P_0 (np.ndarray) – State covariance

Returns:

sigma_points – List of sigma points

Return type:

list

src.covariance_steering.zero_control_rollout(problem, x_0, n_states)

Roll out mean state trajectory assuming zero control. Wrapper around quadrotor_experiment.nominal_rollout()

Belief Roadmap Generation

src.roadmap_generation.add_node_and_rewire_roadmap(problem, rewired_graph, graph, node_mean, parent, n_states, edge_controller, near_cutoff, max_nearby, node_is_goal)

Add a new node to rewired_graph if a valid edge can be found from parent to node_mean, and rewire edges.

Corresponds to lines 6-30 in Algorithm II in the paper.

Parameters:

• problem (Problem) – Problem class with associated dynamics and constraints

• rewired_graph (Graph) – Belief roadmap to update

• graph (Graph) – Belief roadmap with the same node means as rewired_graph, but with different node covariances, because graph is constructed without rewiring.

• node_mean (np.ndarray) – Candidate node mean to add to the roadmap

• parent (Node) – Existing node in the roadmap, candidate parent node

• n_states (int) – Trajectory length for edge

• edge_controller (EdgeController) – Edge controller to solve for edge

• near_cutoff (float) – Cutoff for proximity for rewiring (only look to rewire with nodes which are closer than near_cutoff)

• max_nearby (int) – Maximum number of nodes to consider for rewiring

• node_is_goal (bool) – True if new node is added as a goal node

Returns:

• rewired_graph (Graph) – Updated version of rewired_graph

• graph (Graph) – Updated version of graph

• success (bool) – True if node was successfully added to rewired_graph

• new_node (Node) – New node which was added to rewired_graph

src.roadmap_generation.add_node_to_roadmap(problem, graph, node_mean, parent, n_states, edge_controller, node_is_goal)

Add a new node to a belief roadmap if a valid edge can be found from parent to node_mean.

Corresponds to lines 5-9 in Algorithm I in the paper.

Parameters:

• problem (Problem) – Problem class with associated dynamics and constraints

• graph (Graph) – Belief roadmap to update

• node_mean (np.ndarray) – Candidate node mean to add to the roadmap

• parent (Node) – Existing node in the roadmap, candidate parent node

• n_states (int) – Trajectory length for edge

• edge_controller (EdgeController) – Edge controller to solve for edge

• node_is_goal (bool) – True if new node is added as a goal node

Returns:

• graph (Graph) – Belief roadmap, updated if edge was found

• success (bool) – True if new node and edge added to roadmap

src.roadmap_generation.construct_belief_roadmaps_and_rewire(save_dir, problem, x_0, P_0, graph_lims, n_states, n_nodes, edge_controller, near_cutoff, max_nearby)

Given an initial state distribution, build out two belief roadmaps with n_nodes, one with edge rewiring and one without, using the non-rewired roadmap to sample nodes for expansion, then adding each new node to both roadmaps, and rewiring the rewired roadmap along the way. Both roadmaps will always contain the same node means.

Equivalent to Algorithm II in the paper.

Parameters:

• save_dir (string) – Valid folder for saving data

• problem (Problem) – Problem class with associated dynamics, constraints, etc.

• x_0 (np.ndarray) – Initial state mean

• P_0 (np.ndarray) – Initial state covariance

• graph_lims (tuple) – 2-tuple of numpy arrays specifying minimum and maximum bounds of state space

• n_states (int) – Trajectory length for each edge in roadmap

• n_nodes (int) – Desired number of nodes in roadmap

• edge_controller (EdgeController) – Valid edge controller for constructing roadmap

• near_cutoff (float) – Proximity cutoff for neighboring nodes for edge rewiring

• max_nearby (int) – Maximum number of neighboring nodes to consider for rewiring

Returns:

• rewired_graph (Graph) – Belief roadmap constructed with edge rewiring

• graph (Graph) – Belief roadmap constructed without edge rewiring, with the same set of node means as rewired_graph

src.roadmap_generation.construct_belief_roadmaps_to_goal_and_rewire(save_dir, problem, x_0, P_0, x_f, P_f, graph_lims, n_states, n_nodes, edge_controller, near_cutoff, max_nearby)

Given an initial state distribution and a final goal distribution, build out two belief roadmaps with n_nodes, one with edge rewiring and one without, using the non-rewired roadmap to sample nodes for expansion, then adding each new node to both roadmaps, and rewiring the rewired roadmap along the way. Both roadmaps will always contain the same node means. The path to the goal will be rewired to maintain minimum cost in the rewired roadmap, but the first feasible path to the goal will be kept in the non-rewired roadmap without updating or rewiring.

Terminates when n_nodes have been added to both roadmaps, so goal may be reached by both roadmaps, or neither, or the rewired roadmap only.

Equivalent to Algorithm II in the paper, modified for single-query planning.

Parameters:

• save_dir (string) – Valid folder for saving data

• problem (Problem) – Problem class with associated dynamics, constraints, etc.

• x_0 (np.ndarray) – Initial state mean

• P_0 (np.ndarray) – Initial state covariance

• x_f (np.ndarray) – Goal state mean

• P_f (np.ndarray) – Goal state covariance

• graph_lims (tuple) – 2-tuple of numpy arrays specifying minimum and maximum bounds of state space

• n_states (int) – Trajectory length for each edge in roadmap

• n_nodes (int) – Desired number of nodes in roadmap

• edge_controller (EdgeController) – Valid edge controller for constructing roadmap

• near_cutoff (float) – Proximity cutoff for neighboring nodes for edge rewiring

• max_nearby (int) – Maximum number of neighboring nodes to consider for rewiring

Returns:

• rewired_graph (Graph) – Belief roadmap constructed with edge rewiring

• graph (Graph) – Belief roadmap constructed without edge rewiring, with the same set of node means as rewired_graph

src.roadmap_generation.get_nearest_nodes(problem, sample, nodes, near_cutoff, max_nearby)

Get closest node (regardless of distance) and up to max_nearby closest nodes which are all within near_cutoff of the sample. Uses weighted Euclidean distance to compute closeness.

Parameters:

• problem (Problem) – Problem class with proximity_weights property for computing weighted Euclidean distance

• sample (np.ndarray) – Random point in the state space

• nodes (set) – Set of Node objects in belief roadmap

• near_cutoff (float) – Cutoff for proximity for computing nearby nodes

• max_nearby (int) – Maximum number of nearby nodes to return

Returns:

• nearest_neighbor (Node) – Node in nodes which is closest to sample

• nearby_nodes (list) – List of closest nodes in nodes to sample, subject to the constraint that distance to sample is less than near_cutoff. Contains 0 to max_nearby nodes.

src.roadmap_generation.get_roadmap_edge(problem, x_0, P_0, x_f, n_states, edge_controller)

Get a belief roadmap edge from \(\mathcal{N}(\mathbf{x_0}, \lambda_{\max}(P_0)I)\) to a distribution with mean \(\mathbf{x_f}\), if such an edge exists.

Parameters:

• problem (Problem) – Problem class with associated dynamics and constraints

• x_0 (np.ndarray) – Initial state mean

• P_0 (np.ndarray) – Initial state covariance. Note that this function does not steer from \(\mathcal{N}(\mathbf{x_0}, P_0)\), but rather from \(\mathcal{N}(\mathbf{x_0}, P_{0, \max})\), where \(P_{0, \max} = \lambda_{\max}(P_0)I\).

• x_f (np.ndarray) – Final state mean

• n_states (int) – Trajectory length

• edge_controller (EdgeController) – Type of edge controller to use

Returns:

• success (bool) – True iff valid edge found

• x_traj (np.ndarray) – Mean state trajectory along edge, or None if no edge found

• u_traj (np.ndarray) – Nominal control trajectory along edge, or None if no edge found

• cov_traj (np.ndarray) – State covariance trajectory along edge, or None if no edge found

• K (np.ndarray) – State feedback gain along edge, or None if no edge found

• P_f (np.ndarray) – Final state covariance, or None if no edge found

Raises:

ValueError – If edge_controller is not a valid edge controller type. Currently supported edge controllers are EdgeController.BASELINE and EdgeController.ROBUST_SIGMA_POINT.

src.roadmap_generation.randomize_candidate_mean(problem, graph, graph_lims, sample_radius_lims)

Sample mean for a candidate node, biasing towards underexplored regions of the state space.

Parameters:

• problem (Problem) – Problem class with proximity_weights property, which specifies weights for computing Euclidean distance (e.g. weight position more than acceleration)

• graph (Graph) – Belief roadmap

• graph_lims (tuple) – 2-tuple of numpy arrays specifying minimum and maximum bounds of state space

• sample_radius_lims (tuple) – 2-tuple of numpy arrays specifying minimum and maximum bounds for sampling around an existing node in the graph

Returns:

• candiate_mean (np.ndarray) – Random point in state space, falling within sample_radius_lims of expansion_node

• expansion_node (Node) – Node in the graph that is a candidate for expansion towards candidate_mean

src.roadmap_generation.update_node_and_descendants(problem, graph, new_parent, node, x_traj, u_traj, cov_traj, K, P_f, n_states, edge_controller)

Update node in a belief roadmap, such that its parent is now new_parent. Recursively update state covariance at node and all of its descendants by setting the state covariance at node to P_f and then recursively recomputing edges from node to its descendants.

Rather than directly modifying graph, this function deepcopies graph, modifies the copy, and returns the modified copy.

Parameters:

• problem (Problem) – Problem class with associated dynamics, constraints, and parameters

• graph (Graph) – Belief roadmap to copy and update

• new_parent (Node) – Node in graph which is now the parent of node

• node (Node) – Node in graph whose parent is now new_parent

• x_traj (np.ndarray) – Mean state trajectory for edge from new_parent to node

• u_traj (np.ndarray) – Nominal control for edge from new_parent to node

• cov_traj (np.ndarray) – State covariance trajectory for edge from new_parent to node

• K (np.ndarray) – Feedback gain for edge from new_parent to node

• P_f (np.ndarray) – Final state covariance for edge from new_parent to node

• n_states (int) – Trajectory length for each edge

• edge_controller (EdgeController) – Edge controller to use for recomputing edges from node to its descendants

Returns:

new_graph – Updated belief roadmap, where new_parent is now the parent of node, and where the updated covariance of node is propagated to its descendants

Return type:

Graph

Indices and tables

• Index

• Module Index

• Search Page