gcsopt 0.1.0__tar.gz

gcsopt-0.1.0/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2022 Tobia Marcucci
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

gcsopt-0.1.0/PKG-INFO

@@ -0,0 +1,17 @@
+Metadata-Version: 2.4
+Name: gcsopt
+Version: 0.1.0
+Summary: Library for solving optimization problems over Graphs of Convex Sets (GCS).
+Author: Tobia Marcucci
+Author-email: marcucci@ucsb.edu
+License: MIT
+License-File: LICENSE
+Requires-Dist: numpy
+Requires-Dist: cvxpy>=1.5
+Requires-Dist: pytest
+Dynamic: author
+Dynamic: author-email
+Dynamic: license
+Dynamic: license-file
+Dynamic: requires-dist
+Dynamic: summary

gcsopt-0.1.0/README.md

@@ -0,0 +1,7 @@
+# GCSOPT
+
+Library based on [CVXPY](https://www.cvxpy.org) to solve optimization problems in Graphs of Convex Sets (GCS).
+For a detailed description of the algorithms implemented implemented in this library see the PhD thesis [Graphs of Convex Sets with Applications to Optimal Control and Motion Planning
+](https://groups.csail.mit.edu/robotics-center/public_papers/Marcucci24a.pdf).
+
+**Warning:** The library is still under development and not thoroughly tested yet.

gcsopt-0.1.0/examples/inspection.py

@@ -0,0 +1,186 @@
+import cvxpy as cp
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+from gcsopt import GraphOfConvexSets
+
+# all the rooms in the floor plant
+# each room is described by a triplet (index, lower corner, upper corner)
+rooms = [
+    (0, [0, 5], [2, 8]), 
+    (1, [0, 3], [4, 5]),
+    (2, [0, 0], [2, 3]),
+    (3, [2, 5], [4, 8]),
+    (4, [2, 0], [4, 3]),
+    (5, [4, 0], [6, 8]),
+    (6, [6, 7], [8, 8]),
+    (7, [6, 1], [8, 7]),
+    (8, [6, 0], [8, 1]),
+    (9, [8, 0], [10, 10]),
+    (10, [10, 5], [12, 10]),
+    (11, [10, 4], [12, 5]),
+    (12, [10, 0], [12, 4]),
+    (13, [12, 0], [16, 2]),
+    (14, [12, 2], [14, 10]),
+    (15, [14, 7], [16, 10]),
+    (17, [0, 8], [4, 10]),
+    (18, [4, 8], [8, 10]),
+    (19, [14, 2], [16, 7]),
+]
+
+# all the doors in the floor plant
+# each door is described by
+# (first room index, second room index, door lower corner, door upper corner)
+doors = [
+    (0, 3, [2, 7], [2, 8]),
+    (1, 2, [1, 3], [2, 3]),
+    (1, 5, [4, 4], [4, 5]),
+    (2, 4, [2, 2], [2, 3]),
+    (3, 5, [4, 5], [4, 8]),
+    (3, 17, [3, 8], [4, 8]),
+    (4, 5, [4, 0], [4, 3]),
+    (5, 6, [6, 7], [6, 8]),
+    (5, 7, [6, 1], [6, 2]),
+    (5, 8, [6, 0], [6, 1]),
+    (6, 9, [8, 7], [8, 8]),
+    (7, 9, [8, 3], [8, 4]),
+    (8, 9, [8, 0], [8, 1]),
+    (9, 10, [10, 7], [10, 8]),
+    (9, 11, [10, 4], [10, 5]),
+    (9, 12, [10, 3], [10, 4]),
+    (9, 18, [8, 9], [8, 10]),
+    (10, 14, [12, 5], [12, 6]),
+    (11, 14, [12, 4], [12, 5]),
+    (12, 13, [12, 0], [12, 2]),
+    (14, 15, [14, 7], [14, 8]),
+    (14, 19, [14, 6], [14, 7]),
+]
+
+# rooms that must be visited by the minimum-length trajectory
+visit_rooms = [0, 2, 7, 10, 13, 15, 18]
+
+# helper class that allows to construct a floor
+class Floor(GraphOfConvexSets):
+
+    def __init__(self, rooms, doors, name):
+        super().__init__()
+        self.rooms = rooms
+        self.doors = doors
+        self.name = name
+        for room in rooms:
+            self.add_room(*room)
+        for door in doors:
+            self.add_door(*door)
+        
+    def add_room(self, n, l, u):
+        v = self.add_vertex(f"{self.name}_{n}")
+        x1 = v.add_variable(2)
+        x2 = v.add_variable(2)
+        v.add_constraints([x1 >= l, x1 <= u])
+        v.add_constraints([x2 >= l, x2 <= u])
+        v.add_cost(cp.norm2(x2 - x1))
+        return v
+    
+    def add_one_way_door(self, n, m, l, u):
+        tail = self.get_vertex(f"{self.name}_{n}")
+        head = self.get_vertex(f"{self.name}_{m}")
+        e = self.add_edge(tail, head)
+        e.add_constraint(tail.variables[1] == head.variables[0])
+        e.add_constraint(tail.variables[1] >= l)
+        e.add_constraint(tail.variables[1] <= u)
+        return e
+    
+    def add_door(self, n, m, l, u):
+        e1 = self.add_one_way_door(n, m, l, u)
+        e2 = self.add_one_way_door(m, n, l, u)
+        return e1, e2
+
+# initialize empty graph
+graph = GraphOfConvexSets()
+
+# adds one copy of the floor plant for each room that we must visit
+num_floors = len(visit_rooms)
+for floor in range(num_floors):
+    graph.add_disjoint_subgraph(Floor(rooms, doors, floor))
+
+# connects copies of the floors on a given room
+def connect_floors(floor1, floor2, room):
+    tail = graph.get_vertex(f"{floor1}_{room}")
+    head = graph.get_vertex(f"{floor2}_{room}")
+    edge = graph.add_edge(tail, head)
+    edge.add_constraint(tail.variables[1] == head.variables[0])
+
+# connect top floor to ground floor at first visit room
+first_room = visit_rooms[0]
+first_floor = 0
+last_floor = num_floors - 1
+connect_floors(last_floor, first_floor, first_room)
+
+# connect each floor to the floor above at the visit room
+for floor in range(last_floor):
+    for room in visit_rooms[1:]:
+        connect_floors(floor, floor + 1, room)
+
+# retrieve binary variables
+yv = graph.vertex_binaries()
+ye = graph.edge_binaries()
+
+# constraints of the integer programming formulation
+ilp_constraints = []
+for i, vertex in enumerate(graph.vertices):
+    inc_edges = graph.incoming_edge_indices(vertex)
+    out_edges = graph.outgoing_edge_indices(vertex)
+    ilp_constraints.append(yv[i] == sum(ye[inc_edges]))
+    ilp_constraints.append(yv[i] == sum(ye[out_edges]))
+    
+# returns the edge binary variable that connects two floors through a given room
+def get_binary_variable(floor1, floor2, room):
+    tail_name = f"{floor1}_{room}"
+    head_name = f"{floor2}_{room}"
+    edge = graph.get_edge(tail_name, head_name)
+    return ye[graph.edge_index(edge)]
+
+# add constraints that force the trajectory to move between floors
+ilp_constraints.append(get_binary_variable(last_floor, first_floor, first_room) == 1)
+for room in visit_rooms[1:]:
+    flow = sum(get_binary_variable(floor, floor + 1, room) for floor in range(last_floor))
+    ilp_constraints.append(flow == 1)
+
+# solve problem (this will take a very long time)
+prob = graph.solve_from_ilp(ilp_constraints)
+print('Problem status:', prob.status)
+print('Optimal value:', prob.value)
+
+# plot solution
+plt.figure()
+plt.axis("equal")
+
+# helper function that plots one room
+def plot_room(n, l, u):
+    l = np.array(l)
+    u = np.array(u)
+    d = u - l
+    fc = "mistyrose" if n in visit_rooms else "mintcream"
+    rect = patches.Rectangle(l, *d, fc=fc, ec="k")
+    plt.gca().add_patch(rect)
+        
+# helper function that plots one door
+def plot_door(l, u):
+    endpoints =  np.array([l, u]).T
+    plt.plot(*endpoints, color="mintcream", solid_capstyle="butt")
+    plt.plot(*endpoints, color="grey", linestyle=":")
+        
+# plot all rooms and doors
+for room in rooms:
+    plot_room(*room)
+for door in doors:
+    plot_door(door[2], door[3])
+
+# plot optimal trajectory
+for vertex in graph.vertices:
+    if np.isclose(vertex.binary_variable.value, 1):
+        x1, x2 = vertex.variables
+        values = np.array([x1.value, x2.value]).T
+        plt.plot(*values, c="b", linestyle="--")
+    
+plt.show()

gcsopt-0.1.0/gcsopt/__init__.py

@@ -0,0 +1,4 @@
+__version__ = "0.1.0"
+__author__ = "Tobia Marcucci"
+
+from gcsopt.graphs import GraphOfConicSets, GraphOfConvexSets

gcsopt-0.1.0/gcsopt/edges.py

@@ -0,0 +1,98 @@
+import numpy as np
+import cvxpy as cp
+from gcsopt.programs import ConicProgram, ConvexProgram
+
+class ConicEdge(ConicProgram):
+
+    def __init__(self, tail, head, size, id_to_range=None, binary_variable=None):
+
+        # Check inputs.
+        super().__init__(size, id_to_range, binary_variable)
+        self.slack_size = size - tail.size - head.size
+        if self.slack_size < 0:
+            raise ValueError(
+                f"Size mismatch: edge.size = {size}, tail.size = {tail.size}, "
+                f"head.size = {head.size}. Size of the edge must be larger "
+                "than the sum of tail and head sizes.")
+
+        # Store inputs.
+        self.tail = tail
+        self.head = head
+        self.name = (tail.name, head.name)
+
+    def _check_vector_sizes(self, xv, xw, xe):
+        sizes = (xv.size, xw.size, xe.size)
+        expected_sizes = (self.tail.size, self.head.size, self.slack_size)
+        if sizes != expected_sizes:
+            ValueError(
+                f"Size mismatch. Got vectors of size {sizes}. Expected vectors "
+                f"of size {expected_sizes}.")
+                
+    def _concatenate(self, xv, xw, xe):
+        if self.slack_size == 0:
+            return cp.hstack((xv, xw))
+        else:
+            return cp.hstack((xv, xw, xe))
+
+    def cost_homogenization(self, xv, xw, xe, y):
+        self._check_vector_sizes(xv, xw, xe)
+        x = self._concatenate(xv, xw, xe)
+        return super().cost_homogenization(x, y)
+        
+    def constraint_homogenization(self, xv, xw, xe, y):
+        self._check_vector_sizes(xv, xw, xe)
+        x = self._concatenate(xv, xw, xe)
+        return super().constraint_homogenization(x, y)
+    
+class ConvexEdge(ConvexProgram):
+
+    def __init__(self, tail, head):
+        super().__init__()
+        self.tail = tail
+        self.head = head
+        self.name = (self.tail.name, self.head.name)
+
+    def to_conic(self, conic_tail, conic_head):
+
+        # Include tail and head variables in id_to_range. Variable order is
+        # x_tail, x_head, a dn then x_edge. Start with copy of tail dictionary.
+        id_to_range = conic_tail.id_to_range.copy()
+
+        # Add copy of the head dictionary shifted by the size of the tail.
+        offset = conic_tail.size
+        for id, r in conic_head.id_to_range.items():
+            id_to_range[id] = range(r.start + offset, r.stop + offset)
+
+        # Add to dictionary variables that are associated with this edge if they
+        # are not in the tail or head dictionary yet.
+        conic_program = super().to_conic()
+        offset = conic_tail.size + conic_head.size
+        for id, r in conic_program.id_to_range.items():
+            if not id in id_to_range:
+                id_to_range[id] = range(r.start + offset, r.stop + offset)
+
+        # Initialize empty edge program.
+        size = max([r.stop for r in id_to_range.values()])
+        conic_edge = ConicEdge(
+            conic_tail,
+            conic_head,
+            size,
+            id_to_range,
+            conic_program.binary_variable)
+
+        # Reorder matrices and extend them with zeros according to the new
+        # id_to_range dictionary.
+        c = np.zeros(size)
+        A = np.zeros((conic_program.A.shape[0], size))
+        for id, r in conic_program.id_to_range.items():
+            c[id_to_range[id]] = conic_program.c[r]
+            A[:, id_to_range[id]] = conic_program.A[:, r]
+
+        # Assemble conic edge.
+        conic_edge.add_cost(c, conic_program.d)
+        conic_edge.add_constraints(A, conic_program.b, conic_program.K)
+        return conic_edge
+
+    def _check_variables_are_defined(self, variables):
+        defined_variables = self.variables + self.tail.variables + self.head.variables
+        super()._check_variables_are_defined(variables, defined_variables)

gcsopt-0.1.0/gcsopt/graph_problems/facility_location.py

@@ -0,0 +1,44 @@
+import cvxpy as cp
+import numpy as np
+from gcsopt.graph_problems.utils import define_variables, enforce_edge_programs, get_solution
+
+def facility_location(conic_graph, binary, tol, **kwargs):
+
+    # define variables
+    yv, zv, ye, ze, ze_tail, ze_head = define_variables(conic_graph, binary)
+
+    # edge costs and constraints
+    cost, constraints = enforce_edge_programs(conic_graph, ye, ze, ze_tail, ze_head)
+
+    # constraints on the vertices
+    for i, vertex in enumerate(conic_graph.vertices):
+        inc = conic_graph.incoming_edge_indices(vertex)
+        out = conic_graph.outgoing_edge_indices(vertex)
+
+        # check that graph topology is correct
+        if len(inc) > 0 and len(out) > 0:
+            raise ValueError("Graph is not bipartite.")
+
+        # user vertex
+        if len(inc) > 0:
+            cost += vertex.cost_homogenization(zv[i], 1)
+            constraints += [
+                yv[i] == 1,
+                sum(ye[inc]) == 1,
+                sum(ze_head[inc]) == zv[i]]
+        
+        # facility vertex
+        else:
+            cost += vertex.cost_homogenization(zv[i], yv[i])
+            constraints.append(yv[i] <= 1)
+
+    # constraints on the edges
+    for k, edge in enumerate(conic_graph.edges):
+        i = conic_graph.vertex_index(edge.tail)
+        constraints += edge.tail.constraint_homogenization(zv[i] - ze_tail[k], yv[i] - ye[k])
+
+    # solve problem
+    prob = cp.Problem(cp.Minimize(cost), constraints)
+    prob.solve(**kwargs)
+
+    return get_solution(conic_graph, prob, ye, ze, yv, zv, tol)

gcsopt-0.1.0/gcsopt/graph_problems/from_ilp.py

@@ -0,0 +1,136 @@
+import cvxpy as cp
+import numpy as np
+from gcsopt.programs import ConvexProgram
+from gcsopt.graph_problems.utils import define_variables, get_solution
+
+def from_ilp(conic_graph, ilp_constraints, binary, tol, **kwargs):
+
+    # Put given constraints in conic form. Next lines are not nice but I cannot
+    # use convex_ilp.add_variables.
+    convex_ilp = ConvexProgram()
+    vertex_binaries = conic_graph.vertex_binaries()
+    edge_binaries = conic_graph.edge_binaries()
+    convex_ilp.variables.extend(vertex_binaries)
+    convex_ilp.variables.extend(edge_binaries)
+    convex_ilp.add_constraints(ilp_constraints)
+    conic_ilp = convex_ilp.to_conic()
+
+    # Indices of vertex and edge binaries in conic program. Uses the fact that
+    # binaries are scalars.
+    vertex_indices = [conic_ilp.id_to_range[y.id].start for y in vertex_binaries]
+    edge_indices = [conic_ilp.id_to_range[y.id].start for y in edge_binaries]
+    Av = conic_ilp.A[:, vertex_indices]
+    Ae = conic_ilp.A[:, edge_indices]
+
+    # Variables of MICP. Note that xv and xe can always be omitted.
+    yv, zv, ye, ze, ze_tail, ze_head = define_variables(conic_graph, binary)
+
+    # Vertex costs and constraints.
+    cost = 0
+    constraints = [yv <= 1]
+    for i, vertex in enumerate(conic_graph.vertices):
+        cost += vertex.cost_homogenization(zv[i], yv[i])
+
+    # Edge costs and constraints.
+    for k, edge in enumerate(conic_graph.edges):
+        cost += edge.cost_homogenization(ze_tail[k], ze_head[k], ze[k], ye[k])
+        constraints += edge.constraint_homogenization(ze_tail[k], ze_head[k], ze[k], ye[k])
+
+        # Enforce constraint implied by the subgraph polytope: 0 <= ye <= yv.
+        # Letting the user decide when to enforce these is error prone.
+        constraints += edge.tail.constraint_homogenization(ze_tail[k], ye[k])
+        constraints += edge.head.constraint_homogenization(ze_head[k], ye[k])
+        i = conic_graph.vertex_index(edge.tail)
+        j = conic_graph.vertex_index(edge.head)
+        constraints += edge.tail.constraint_homogenization(zv[i] - ze_tail[k], yv[i] - ye[k])
+        constraints += edge.head.constraint_homogenization(zv[j] - ze_head[k], yv[j] - ye[k])
+
+    # Check each line of each conic constraint.
+    start = 0
+    for K, size in conic_ilp.K:
+        stop = start + size
+        for j in range(start, stop):
+
+            # Evaluate affine constraint using the problem binaries.
+            av = Av[j]
+            ae = Ae[j]
+            bj = conic_ilp.b[j]
+
+            # If there are no shared vertices, just enforce scalar constraint.
+            shared_vertices = find_shared_vertices(conic_graph, av, ae)
+            if not shared_vertices:
+                constraints.append(K(av @ yv + ae @ ye + bj))
+
+            # If there are shared vertices, apply constraint-generation lemma to
+            # each affine constraint.
+            for vertex in shared_vertices:
+
+                # Assemble implied constraint.
+                i = conic_graph.vertex_index(vertex)
+                inc = conic_graph.incoming_edge_indices(vertex)
+                out = conic_graph.outgoing_edge_indices(vertex)
+                lhs = (bj + av[i]) * yv[i] + ae @ ye
+                vector_lhs = (bj + av[i]) * zv[i]
+                vector_lhs += sum(ae[inc] * ze_head[inc])
+                vector_lhs += sum(ae[out] * ze_tail[out])
+
+                # Enforce implied constraints.
+                if K == cp.Zero:
+                    constraints += [lhs == 0, vector_lhs == 0]
+                    if not np.isclose(bj, 0):
+                        constraints.append(yv[i] == 1)
+                elif K == cp.NonNeg:
+                    constraints += vertex.constraint_homogenization(vector_lhs, lhs)
+                    if bj < 0:
+                        constraints.append(yv[i] == 1)
+                elif K == cp.NonPos:
+                    constraints += vertex.constraint_homogenization(-vector_lhs, -lhs)
+                    if bj > 0:
+                        constraints.append(yv[i] == 1)
+                else:
+                    raise ValueError(
+                        "All the constraints of ILP must be affine. Got cone "
+                        f"of type {type(K)}.")
+                
+        # Shift row indices.
+        start = stop
+                
+    # Solve problem.
+    prob = cp.Problem(cp.Minimize(cost), constraints)
+    prob.solve(**kwargs)
+
+    return get_solution(conic_graph, prob, ye, ze, yv, zv, tol)
+    
+def find_shared_vertices(conic_graph, av, ae):
+    """
+    Checks if the linear function
+    av^T y_v + ae^T y_e
+    is amenable to the lemma that is used to generate spatial constraints
+    (see Lemma 5.1 from thesis). To this end, it should be possible to rewrite
+    the linear function above as
+    b y_v_i + sum_{k in edges_incident_i} c_k y_e_k
+    where i is the index of a vertex and k is the index of an edge incident with
+    the ith vertex. This function returns all the values of i that allow such
+    a decomposition.
+    """
+
+    # Extract nonzero vertices and edges.
+    nonzero_vertices = [conic_graph.vertices[i] for i in np.nonzero(av)[0]]
+    nonzero_edges = [conic_graph.edges[k] for k in np.nonzero(ae)[0]]
+
+    # Compute shared vertices among all edges.
+    tails_and_heads = [{edge.tail, edge.head} for edge in nonzero_edges]
+    shared_vertices = set.intersection(*tails_and_heads) if tails_and_heads else set()
+
+    # If av is zero, return all the vertices shared by the edges.
+    if not nonzero_vertices:
+        return list(shared_vertices)
+    
+    # If av has one nonzero entry, there is only one candidate vertex, and
+    # no other shared vertex can be different from it.
+    elif len(nonzero_vertices) == 1 and shared_vertices <= set(nonzero_vertices):
+        return nonzero_vertices
+
+    # In all other cases, the decomposition is not possible.
+    else:
+        return []

gcsopt-0.1.0/gcsopt/graph_problems/minimum_spanning_tree.py

@@ -0,0 +1,109 @@
+import cvxpy as cp
+import numpy as np
+from itertools import combinations
+from gcsopt.graph_problems.utils import (define_variables, enforce_edge_programs,
+    get_solution, subtour_elimination_constraints)
+
+def undirected_minimum_spanning_tree(conic_graph, subtour_elimination, binary, tol, **kwargs):
+    """
+    Here we use the subtour-elimination formulation, which is also perfect.
+    """
+
+    # Check that graph is undirected.
+    if conic_graph.directed:
+        raise ValueError("Called MSTP for undirected graphs on a directed graph.")
+
+    # Define variables.
+    yv, zv, ye, ze, ze_tail, ze_head = define_variables(conic_graph, binary)
+
+    # Edge costs and constraints.
+    cost, constraints = enforce_edge_programs(conic_graph, ye, ze, ze_tail, ze_head)
+
+    # Number of edges in the tree.
+    constraints.append(sum(ye) == conic_graph.num_vertices() - 1)
+
+    # Vertex costs.
+    for i, vertex in enumerate(conic_graph.vertices):
+        cost += vertex.cost_homogenization(zv[i], 1)
+
+        # Cutset constraints for one vertex only.
+        incident = conic_graph.incident_edge_indices(vertex)
+        inc = [k for k in incident if conic_graph.edges[k].head == vertex]
+        out = [k for k in incident if conic_graph.edges[k].tail == vertex]
+        constraints += vertex.constraint_homogenization(
+            sum(ze_head[inc]) + sum(ze_tail[out]) - zv[i],
+            sum(ye[incident]) - 1)
+        
+        # Constraints implied by ye <= 1.
+        for k in inc:
+            constraints += vertex.constraint_homogenization(zv[i] - ze_head[k], 1 - ye[k])
+        for k in out:
+            constraints += vertex.constraint_homogenization(zv[i] - ze_tail[k], 1 - ye[k])
+
+    # Exponentially many subtour elimination constraints.
+    if subtour_elimination:
+        constraints += subtour_elimination_constraints(conic_graph, ye)
+
+    # Solve problem.
+    prob = cp.Problem(cp.Minimize(cost), constraints)
+    prob.solve(**kwargs)
+
+    # Set value of vertex binaries.
+    if prob.status == "optimal":
+        yv.value = np.ones(conic_graph.num_vertices())
+
+    return get_solution(conic_graph, prob, ye, ze, yv, zv, tol)
+
+def directed_minimum_spanning_tree(conic_graph, conic_root, subtour_elimination, binary, tol, **kwargs):
+    """
+    This is the cutset formulation of the directed MSTP. Unlike the undirected
+    case, this formulation is perfect for a directed graph, see
+    https://www.cs.cmu.edu/afs/cs.cmu.edu/academic/class/15850-f20/www/notes/lec2.pdf.
+    """
+
+    # Check that graph is directed.
+    if not conic_graph.directed:
+        raise ValueError("Called MSTP for directed graphs on an undirected graph.")
+
+    # Define variables.
+    yv, zv, ye, ze, ze_tail, ze_head = define_variables(conic_graph, binary)
+
+    # Edge basic costs and constraints.
+    cost, constraints = enforce_edge_programs(conic_graph, ye, ze, ze_tail, ze_head)
+
+    # Cost and constraints on the vertices.
+    for i, vertex in enumerate(conic_graph.vertices):
+        cost += vertex.cost_homogenization(zv[i], 1)
+
+        # Constraints on incoming edges.
+        inc = conic_graph.incoming_edge_indices(vertex)
+        if vertex == conic_root:
+            constraints += vertex.constraint_homogenization(zv[i], 1)
+            constraints += [ye[k] == 0 for k in inc]
+            constraints += [ze_head[k] == 0 for k in inc]
+        else:
+            constraints += [sum(ye[inc]) == 1, sum(ze_head[inc]) == zv[i]]
+
+        # Constraints on outgoing edges.
+        for k in conic_graph.outgoing_edge_indices(vertex):
+            constraints += vertex.constraint_homogenization(zv[i] - ze_tail[k], 1 - ye[k])
+
+    # Cutset constraints for all subsets of vertices with cardinality between
+    # 2 and num_vertices - 1.
+    if subtour_elimination:
+        i = conic_graph.vertex_index(conic_root)
+        subvertices = conic_graph.vertices[:i] + conic_graph.vertices[i+1:]
+        for subtour_size in range(2, conic_graph.num_vertices()):
+            for vertices in combinations(subvertices, subtour_size):
+                inc = conic_graph.incoming_edge_indices(vertices)
+                constraints.append(sum(ye[inc]) >= 1)
+
+    # Solve problem.
+    prob = cp.Problem(cp.Minimize(cost), constraints)
+    prob.solve(**kwargs)
+
+    # Set value of vertex binaries.
+    if prob.status == "optimal":
+        yv.value = np.ones(conic_graph.num_vertices())
+
+    return get_solution(conic_graph, prob, ye, ze, yv, zv, tol)