Course project for ECE1505 (Winter 2025) on convex optimization

This repository contains all the relevant scripts and tools needed to replicate the findings presented in our final project for the ECE1505 (Winter 2025) course on convex optimization. The LaTeX files for the project report can be found here.

Results

GCS vs PRM: minimum-length trajectory

Notes:

• The best solutions obtained using GCS with 12 and 18 vertices are identical.
• Since both GCS (in these experiments) and PRM aim to find the shortest path, all resulting trajectories are traversed at a constant velocity.

GCS (6 vertices)	GCS (12/18 vertices)	PRM (no shortcutting)	PRM (shortcutting)

GCS: extension

Note: A graph with 6 vertices was used in these experiments.

Time optmization	Length optimization	The energy of the time derivative of the trajectory optimization

Installation

This project heavily depends on Drake and the Open Motion Planning Library (OMPL). Follow the steps below to set up and install all necessary dependencies.

Clone the Repository (including submodules):

git clone --recurse-submodules https://github.com/utiasSTARS/ece1505-w25-project
cd ece1505-w25-project

If you forgot --recurse-submodules, run these commands inside the repo:

git submodule init
git submodule update

Create and Activate a Virtual Environment:

It is highly recommended to use a virtual environment to isolate project dependencies:

python3.12 -m venv .venv
source .venv/bin/activate

Install Dependencies:

Main python dependencies:

pip install -r requirements.txt

Navigate to the ./external/gcs directory and install gcs dependencies by running:

pip install -r requirements.txt

Note: to successfully run our examples, you'll need to modify one line in the file ./external/gcs/gcs/linear.py. Specifically, remove the [1] indexing from:

edge_length = edge.AddCost(Binding[Cost](
    self.edge_cost, np.append(u.x(), v.x())))[1]

Setting up OMPL

This project depends on the latest 1.7.0 release of the Open Motion Planning Library (OMPL) and requires its Python bindings. As of this writing, OMPL does not have a PyPI package that can be installed directly via pip. Therefore, it must be installed manually. While there is no official installation method, the following is a recommended approach:

You can install the wheel file directly in your virtual environment. Download the appropriate wheel file from this link based on your system.

Once downloaded, install the wheel using pip:

pip install <wheel-file>.whl

Usage

Teleoperation of Manipulator Joints in Drake

Run the following command in the top-level directory of the repo:

python examples/joints_teleop.py --initial_conf <initial_conf>

<initial_conf> can be zeros, start or goal. After running the command, click on the provided link (likely http://localhost:7000). Once the page opens in your browser, click the "Open Controls" button in the top-right corner to open the joint control sliders.

Running Drake + OMPL demo

Run the following command in the top-level directory of the repository:

python examples/drake_ompl_planning.py

This should move the arms from an initial configuration to a goal configuration using the RRTConnect planner.

Benchmarking OMPL Planners

You can run the following script to benchmark the performance of OMPL planners:

python examples/drake_ompl_benchmarking.py

The script will generate .pkl files under the benchmarks folder. You can use them to replicate the results in the report using the provided notebook.

Adjust the following configurations as needed:

###############################
# Benchmarking Configurations #
###############################

PLANNERS = [
    "PRM",  # only PRM is supported at the moment
]

# this serves as a timeout value
# planning will stop as soon as it found an initial solution
RUNTIME_LIMIT = 3

RUN_COUNT = 1
SHORTCUTTING = True
PRECOMPUTE = False
PRECOMPUTATION_TIME = 10

# this is used when PRECOMPUTE is set to False

STORAGE_PATHS = {
    "PRM": os.path.join(
        CURRENT_DIR, "../data/PRM-600s.graph"
    ),  # 600s precomputed roadmap
}

SHORTCUTTING can be turned off to run the vanilla PRM; otherwise, it is enabled by default. We use the recent shortcutting approach proposed in RRT-Rope. We also rely on a precomputed PRM roadmap to ensure consistent planning results, which is provided through the STORAGE_PATHS parameter. If you want to regenerate the roadmap, set the PRECOMPUTE parameter to True. This will generate a new PRM graph in the benchmarks folder—make sure to provide the correct STORAGE_PATHS for subsequent runs.

Caveats

We have experienced issues when loading the roadmap back into PRM using the OMPL planner's storage functionality. OMPL internally relies on Boost to serialize and deserialize storage data, and we suspect the issue may be due to mismatched Boost versions. This benchmark script has been successfully tested on macOS 15 but not on Ubuntu 22.04.

Motion Planning with Graph of Convex Sets

We also formulate the problem of generating collision-free, kinodynamic motion plans as a mixed-integer convex program using the Graph of Convex Sets (GCS) approach. More information and runnable demos can be found here.