Upgrade to use Drake's preprocessing and rounding on GraphOfConvexSet…

…s (#39)

* Use Drake's preprocessing on GraphOfConvexSets

* Use Drake's rounding on GraphOfConvexSets by default

* Update documentation to point to arxiv paper code

README.md

@@ -1,5 +1,7 @@
 # GCS - Motion Planning around Obstacles with Convex Optimization
 
+This code has been updated since the Arxiv paper was published. The code used to run the examples in that paper can be found under the [`arxiv_paper_version`](https://github.com/mpetersen94/gcs/releases/tag/arxiv_paper_version) tag. That version was tested to work with Drake version 1.3 (although version up to 1.8 should work with some deprecation warnings).
+
 ## Running via Deepnote
 Most of the examples and reproductions can be run on [Deepnote](https://deepnote.com/workspace/mark-petersen-2785519d-2c3e-430b-9a10-a1754f2de37d/project/GCS-Motion-Planning-around-Obstacles-with-Convex-Optimization-Duplicate-Duplicate-3afac8e3-cbc0-41d1-9afb-0d38dfbe9ffa).
 
@@ -45,7 +47,7 @@ from inside this repository.
 If you want to compare GCS to sampling based planners (such as PRM), you'll need to install a custom fork of drake that includes bindings for sampling based planners.  To do this run the following, including any of the proprietary solvers you have access to.
 
 ```
-git clone -b gcs git@github.com:mpetersen94/drake.git
+git clone -b gcs2 git@github.com:mpetersen94/drake.git
 mkdir drake-build
 cd drake-build
 cmake -DWITH_MOSEK=ON [-DWITH_GUROBI=ON -DWITH_ROBOTLOCOMOTION_SNOPT=ON] ../drake

examples/uav_planning.ipynb

@@ -20,7 +20,7 @@
     "from IPython.display import SVG\n",
     "\n",
     "from pydrake.examples.quadrotor import QuadrotorGeometry\n",
-    "from pydrake.geometry import MeshcatVisualizerCpp, Rgba, StartMeshcat\n",
+    "from pydrake.geometry import MeshcatVisualizer, Rgba, StartMeshcat\n",
     "from pydrake.geometry.optimization import HPolyhedron, VPolytope\n",
     "from pydrake.multibody.plant import AddMultibodyPlantSceneGraph\n",
     "from pydrake.multibody.parsing import Parser\n",
@@ -31,7 +31,6 @@
     "from pydrake.systems.framework import DiagramBuilder\n",
     "\n",
     "from gcs.bezier import BezierGCS\n",
-    "from gcs.rounding import *\n",
     "from reproduction.uav.helpers import FlatnessInverter\n",
     "from reproduction.uav.building_generation import *\n",
     "from reproduction.util import *\n",
@@ -101,7 +100,6 @@
     "\n",
     "gcs.setPaperSolverOptions()\n",
     "gcs.setSolver(MosekSolver())\n",
-    "gcs.setRoundingStrategy(randomForwardPathSearch, max_paths=10, max_trials=100, seed=0)\n",
     "\n",
     "# Solve GCS\n",
     "trajectory = gcs.SolvePath(True, verbose=False, preprocessing=True)[0]"
@@ -133,7 +131,7 @@
     "\n",
     "plant.Finalize()\n",
     "\n",
-    "meshcat_cpp = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat)\n",
+    "meshcat_cpp = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)\n",
     "\n",
     "\n",
     "animator = meshcat_cpp.StartRecording()\n",

gcs/base.py

@@ -3,6 +3,7 @@
 
 from pydrake.geometry.optimization import (
     GraphOfConvexSets,
+    GraphOfConvexSetsOptions,
     Point,
 )
 from pydrake.solvers.mathematicalprogram import (
@@ -13,11 +14,7 @@
 from pydrake.solvers.mosek import MosekSolver
 from pydrake.all import MathematicalProgram, le
 
-from gcs.preprocessing import removeRedundancies
-from gcs.rounding import (
-    MipPathExtraction,
-    randomForwardPathSearch,
-)
+from gcs.rounding import MipPathExtraction
 
 def polytopeDimension(A, b, tol=1e-4):
 
@@ -69,14 +66,13 @@ def __init__(self, regions):
             self.names = ["v" + str(ii) for ii in range(len(regions))]
         self.dimension = regions[0].ambient_dimension()
         self.regions = regions.copy()
-        self.solver = None
-        self.options = None
-        self.rounding_fn = [randomForwardPathSearch]
+        self.rounding_fn = []
         self.rounding_kwargs = {}
         for r in self.regions:
             assert r.ambient_dimension() == self.dimension
 
         self.gcs = GraphOfConvexSets()
+        self.options = GraphOfConvexSetsOptions()
         self.source = None
         self.target = None
 
@@ -146,20 +142,21 @@ def findStartGoalEdges(self, start, goal):
         return edges
 
     def setSolver(self, solver):
-        self.solver = solver
+        self.options.solver = solver
 
     def setSolverOptions(self, options):
-        self.options = options
+        self.options.solver_options = options
 
     def setPaperSolverOptions(self):
-        self.options = SolverOptions()
-        self.options.SetOption(CommonSolverOption.kPrintToConsole, 1)
-        self.options.SetOption(MosekSolver.id(), "MSK_DPAR_INTPNT_CO_TOL_REL_GAP", 1e-3)
-        self.options.SetOption(MosekSolver.id(), "MSK_IPAR_INTPNT_SOLVE_FORM", 1)
-        self.options.SetOption(MosekSolver.id(), "MSK_DPAR_MIO_TOL_REL_GAP", 1e-3)
-        self.options.SetOption(MosekSolver.id(), "MSK_DPAR_MIO_MAX_TIME", 3600.0)
-        self.options.SetOption(GurobiSolver.id(), "MIPGap", 1e-3)
-        self.options.SetOption(GurobiSolver.id(), "TimeLimit", 3600.0)
+        solver_options = SolverOptions()
+        solver_options.SetOption(CommonSolverOption.kPrintToConsole, 1)
+        solver_options.SetOption(MosekSolver.id(), "MSK_DPAR_INTPNT_CO_TOL_REL_GAP", 1e-3)
+        solver_options.SetOption(MosekSolver.id(), "MSK_IPAR_INTPNT_SOLVE_FORM", 1)
+        solver_options.SetOption(MosekSolver.id(), "MSK_DPAR_MIO_TOL_REL_GAP", 1e-3)
+        solver_options.SetOption(MosekSolver.id(), "MSK_DPAR_MIO_MAX_TIME", 3600.0)
+        solver_options.SetOption(GurobiSolver.id(), "MIPGap", 1e-3)
+        solver_options.SetOption(GurobiSolver.id(), "TimeLimit", 3600.0)
+        self.options.solver_options = solver_options
 
     def setRoundingStrategy(self, rounding_fn, **kwargs):
         self.rounding_kwargs = kwargs
@@ -198,12 +195,11 @@ def VisualizeGraph(self, file_type="svg"):
     def solveGCS(self, rounding, preprocessing, verbose):
 
         results_dict = {}
-        if preprocessing:
-            results_dict["preprocessing_stats"] = removeRedundancies(
-                self.gcs, self.source, self.target, verbose=verbose)
+        self.options.convex_relaxation = rounding
+        self.options.preprocessing = preprocessing
+        self.options.max_rounded_paths = 0
 
-        result = self.gcs.SolveShortestPath(
-            self.source, self.target, rounding, self.solver, self.options)
+        result = self.gcs.SolveShortestPath(self.source, self.target, self.options)
 
         if rounding:
             results_dict["relaxation_result"] = result
@@ -225,7 +221,7 @@ def solveGCS(self, rounding, preprocessing, verbose):
                   "Solver time:", result.get_solver_details().optimizer_time)
 
         # Solve with hard edge choices
-        if rounding:
+        if rounding and len(self.rounding_fn) > 0:
             # Extract path
             active_edges = []
             found_path = False
@@ -243,6 +239,7 @@ def solveGCS(self, rounding, preprocessing, verbose):
                 print("All rounding strategies failed to find a path.")
                 return None, None, results_dict
 
+            self.options.preprocessing = False
             rounded_results = []
             best_cost = np.inf
             best_path = None
@@ -259,7 +256,7 @@ def solveGCS(self, rounding, preprocessing, verbose):
                     else:
                         edge.AddPhiConstraint(False)
                 rounded_results.append(self.gcs.SolveShortestPath(
-                    self.source, self.target, rounding, self.solver, self.options))
+                    self.source, self.target, self.options))
                 solve_time = rounded_results[-1].get_solver_details().optimizer_time
                 max_rounded_solver_time = np.maximum(solve_time, max_rounded_solver_time)
                 total_rounded_solver_time += solve_time
@@ -290,6 +287,16 @@ def solveGCS(self, rounding, preprocessing, verbose):
             if best_path is None:
                 print("Second solve failed on all paths.")
                 return best_path, best_result, results_dict
+        elif rounding:
+            self.options.max_rounded_paths = 10
+
+            rounded_result = self.gcs.SolveShortestPath(self.source, self.target, self.options)
+            best_path = MipPathExtraction(self.gcs, rounded_result, self.source, self.target)[0]
+            best_result = rounded_result
+            results_dict["best_path"] = best_path
+            results_dict["best_result"] = best_result
+            results_dict["rounded_results"] = [rounded_result]
+            results_dict["rounded_cost"] = best_result.get_optimal_cost()
         else:
             best_path = MipPathExtraction(self.gcs, result, self.source, self.target)[0]
             best_result = result

reproduction/bimanual/bimanual_iiwa_example.ipynb

@@ -16,7 +16,7 @@
     "import multiprocessing as mp\n",
     "from IPython.display import display, Image, SVG\n",
     "\n",
-    "from pydrake.geometry import MeshcatVisualizerCpp, MeshcatVisualizerParams, Role, StartMeshcat\n",
+    "from pydrake.geometry import MeshcatVisualizer, MeshcatVisualizerParams, Role, StartMeshcat\n",
     "from pydrake.geometry.optimization import (\n",
     "    HPolyhedron,\n",
     "    Hyperellipsoid,\n",
@@ -71,7 +71,7 @@
     "meshcat_params.delete_on_initialization_event = False\n",
     "meshcat_params.role = Role.kIllustration\n",
     "# meshcat_params.role = Role.kProximity\n",
-    "meshcat_cpp = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat, meshcat_params)\n",
+    "meshcat_cpp = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat, meshcat_params)\n",
     "\n",
     "diagram = builder.Build()\n",
     "\n",
@@ -106,7 +106,6 @@
     "iris_options.iteration_limit = 5\n",
     "iris_options.termination_threshold = -1\n",
     "iris_options.relative_termination_threshold = 0.02\n",
-    "iris_options.enable_ibex = False\n",
     "CORE_CNT = int(mp.cpu_count()/2) # you may edit this\n",
     "\n",
     "filterCollsionGeometry(scene_graph, sg_context)\n",

reproduction/bimanual/helpers.py

@@ -7,7 +7,7 @@
 from pydrake.geometry import (
     CollisionFilterDeclaration,
     GeometrySet,
-    MeshcatVisualizerCpp,
+    MeshcatVisualizer,
     Rgba,
     Role,
     SceneGraph
@@ -311,7 +311,7 @@ def getLinearGcsPath(regions, sequence):
             print(f"Failed between {start_pt} and {goal_pt}")
             return None
         print(f"Planned segment in {np.round(time.time() - start_time, 4)}", flush=True)
-        run_time += results_dict["preprocessing_stats"]['linear_programs']
+        # run_time += results_dict["preprocessing_stats"]['linear_programs']
         run_time += results_dict["relaxation_solver_time"]
         run_time += results_dict["total_rounded_solver_time"]
 
@@ -342,7 +342,7 @@ def getBezierGcsPath(plant, regions, sequence, order, continuity, hdot_min = 1e-
             print(f"Failed between {start_pt} and {goal_pt}")
             return None
         print(f"Planned segment in {np.round(time.time() - start_time, 4)}", flush=True)
-        segment_run_time += results_dict["preprocessing_stats"]['linear_programs']
+        # segment_run_time += results_dict["preprocessing_stats"]['linear_programs']
         segment_run_time += results_dict["relaxation_solver_time"]
         segment_run_time += results_dict["total_rounded_solver_time"]
         trajectories.append(segment_traj)
@@ -401,7 +401,7 @@ def visualize_trajectory(traj, meshcat):
         end_time = traj.end_time()
     builder.Connect(traj_system.get_output_port(), to_pose.get_input_port())
 
-    meshcat_cpp = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat)
+    meshcat_viz = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)
     meshcat.Delete()
 
     vis_diagram = builder.Build()
@@ -432,9 +432,9 @@ def visualize_trajectory(traj, meshcat):
         meshcat.SetObject("paths/iiwa_2", iiwa2_pointcloud, 0.015,
                             rgba=Rgba(*rgb_color))
 
-    meshcat_cpp.StartRecording()
+    meshcat_viz.StartRecording()
     simulator.AdvanceTo(end_time)
-    meshcat_cpp.PublishRecording()
+    meshcat_viz.PublishRecording()
 
 def generate_segment_pics(traj, segment, meshcat):
     builder = DiagramBuilder()
@@ -472,7 +472,7 @@ def generate_segment_pics(traj, segment, meshcat):
 
     plant.Finalize()
 
-    meshcat_cpp = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat)
+    meshcat_viz = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)
     meshcat.Delete()
 
     diagram = builder.Build()

reproduction/min_length_vs_min_time.ipynb

@@ -18,7 +18,6 @@
     "\n",
     "from gcs.bezier import BezierGCS\n",
     "from gcs.linear import LinearGCS\n",
-    "from gcs.rounding import randomForwardPathSearch\n",
     "from models.env_2d import obstacles, vertices\n",
     "\n",
     "savefig = 0"
@@ -152,7 +151,6 @@
     "gcs = LinearGCS(regions)\n",
     "gcs.addSourceTarget(x_start, x_goal)\n",
     "gcs.setSolver(MosekSolver())\n",
-    "gcs.setRoundingStrategy(randomForwardPathSearch, max_paths=3, seed=0)\n",
     "waypoints = gcs.SolvePath(relaxation)[0]\n",
     "plot_trajectory(waypoints)\n",
     "if savefig:\n",
@@ -179,7 +177,6 @@
     "def solve_bezier(order, continuity, regularizer=None, hdot_min=1e-6, velocity=None):\n",
     "    \n",
     "    gcs = BezierGCS(regions, order, continuity, hdot_min=hdot_min)\n",
-    "    gcs.setRoundingStrategy(randomForwardPathSearch, max_paths=3, seed=0)\n",
     "\n",
     "    gcs.addTimeCost(1)\n",
     "    gcs.addVelocityLimits([qdot_min] * 2, [qdot_max] * 2)\n",

reproduction/prm_comparison/helpers.py

@@ -8,7 +8,7 @@
 from pydrake.common import FindResourceOrThrow
 from pydrake.geometry import (
     IllustrationProperties,
-    MeshcatVisualizerCpp,
+    MeshcatVisualizer,
     MeshcatVisualizerParams,
     Rgba,
     RoleAssign,
@@ -230,7 +230,7 @@ def visualize_trajectory(meshcat, traj_list, show_line = False, iiwa_ghosts = []
     meshcat_params.delete_on_initialization_event = False
     meshcat_params.role = Role.kIllustration
     # meshcat_params.role = Role.kProximity
-    meshcat_cpp = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat, meshcat_params)
+    meshcat_cpp = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat, meshcat_params)
     meshcat.Delete()
 
     diagram = builder.Build()

reproduction/prm_comparison/planning.py

@@ -8,6 +8,7 @@
     NNDistanceDirection,
     ShortcutSmoothPath,
     SimpleRRTPlannerState,
+    SimpleRRTPlannerTree,
     MakeKinematicLinearBiRRTNearestNeighborsFunction,
     MakeKinematicBiRRTExtendPropagationFunction,
     MakeKinematicBiRRTConnectPropagationFunction,
@@ -28,7 +29,6 @@ def __init__(self, plant, diagram, collision_step_size, seed):
         self.PositionUpperLimits = self.plant.GetPositionUpperLimits()
         self.PositionLowerLimits = self.plant.GetPositionLowerLimits()
 
-        self.RNG = RandomGenerator(seed)
         np.random.seed(seed)
 
     def make_context(self):
@@ -78,7 +78,7 @@ def shortcut(self, path, max_iter, max_failed_iter, max_backtracking_steps,max_s
                                               self.check_edge_validity_fn, 
                                               self.distance_fn,
                                               self.InterpolateWaypoint,
-                                              self.RNG)
+                                              np.random.rand)
         return shortcutted_path
 
 
@@ -126,7 +126,7 @@ def getPath(self, sequence, verbose = False, path_processing = lambda path: path
             prm_path = QueryPath([start_pt], [goal_pt], self.roadmap, self.distance_fn,
                     self.check_edge_validity_fn, self.K,
                     use_parallel=False,
-                    distance_is_symmetric=True,
+                    connection_is_symmetric=True,
                     add_duplicate_states=False,
                     limit_astar_pqueue_duplicates=True).Path()
 
@@ -265,15 +265,19 @@ def goal_bridge_fn(start_tree, start_idx, end_tree, end_idx, is_start_tree):
             return None
 
         connect_result = BiRRTPlanSinglePath(
-                start_tree=start_tree, goal_tree=end_tree,
+                start_tree=SimpleRRTPlannerTree(start_tree),
+                goal_tree=SimpleRRTPlannerTree(end_tree),
                 state_sampling_fn=birrt_sampling,
                 nearest_neighbor_fn=nearest_neighbor_fn, propagation_fn=propagate_fn,
                 state_added_callback_fn=None,
                 states_connected_fn=self.states_connected_fn,
                 goal_bridge_callback_fn=goal_bridge_fn,
                 tree_sampling_bias=0.5, p_switch_tree=0.25,
-                termination_check_fn=termination_fn, rng=self.RNG)
-        return connect_result, start_tree_extended, end_tree_extended
+                termination_check_fn=termination_fn,
+                uniform_unit_real_fn=np.random.rand)
+        return (connect_result,
+                start_tree_extended.GetNodesImmutable(),
+                end_tree_extended.GetNodesImmutable())
 
     def getPath(self, sequence, verbose = False, path_processing = lambda path: path):
         path = [sequence[0]]

reproduction/prm_comparison/prm_comparison.ipynb

@@ -16,7 +16,7 @@
     "import multiprocessing as mp\n",
     "from IPython.display import HTML, SVG\n",
     "\n",
-    "from pydrake.geometry import MeshcatVisualizerCpp, MeshcatVisualizerParams, Role, StartMeshcat\n",
+    "from pydrake.geometry import MeshcatVisualizer, MeshcatVisualizerParams, Role, StartMeshcat\n",
     "from pydrake.geometry.optimization import IrisInConfigurationSpace, IrisOptions\n",
     "from pydrake.multibody.parsing import LoadModelDirectives, Parser, ProcessModelDirectives\n",
     "from pydrake.multibody.plant import AddMultibodyPlantSceneGraph\n",
@@ -87,7 +87,7 @@
     "meshcat_params.delete_on_initialization_event = False\n",
     "meshcat_params.role = Role.kIllustration\n",
     "# meshcat_params.role = Role.kProximity\n",
-    "meshcat_cpp = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat, meshcat_params)\n",
+    "meshcat_cpp = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat, meshcat_params)\n",
     "\n",
     "diagram = builder.Build()\n",
     "\n",
@@ -140,7 +140,6 @@
     "iris_options.iteration_limit = 10\n",
     "iris_options.termination_threshold = -1\n",
     "iris_options.relative_termination_threshold = 0.01\n",
-    "iris_options.enable_ibex = False\n",
     "CORE_CNT = mp.cpu_count() # you may edit this"
    ]
   },
@@ -371,7 +370,7 @@
     "                print(f\"Failed between {start_pt} and {goal_pt}\")\n",
     "            return None\n",
     "\n",
-    "        run_time += results_dict[\"preprocessing_stats\"][\"linear_programs\"]\n",
+    "#         run_time += results_dict[\"preprocessing_stats\"][\"linear_programs\"]\n",
     "        run_time += results_dict[\"relaxation_solver_time\"]\n",
     "        run_time += results_dict[\"total_rounded_solver_time\"]\n",
     "        if verbose:\n",