• Equality Constrained LQR
  • Benchmarks Implemented
  • Benchmarking Results
  • Directory Structure
  • Install Instructions
    • Matlab
    • C++

In this repo we present our factor graph-based approach to solving equality constrained LQR problems, as well as implementations of other versions as benchmarks. Please see our paper for additional details.

We compare three to solve equality constrained LQR problems:

1. Laine, Forrest, and Claire Tomlin. "Efficient computation of feedback control for equality-constrained lqr." 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019.

2. Sideris, Athanasios, and Luis A. Rodriguez. "A riccati approach to equality constrained linear quadratic optimal control." Proceedings of the 2010 American Control Conference. IEEE, 2010.

3. Our factor graph based approach

We also compare 2 additional trajectory optimization approaches (which do not generate feedback policies):

1. Matlab's QP solver quadprog (note: lsqlin could probably also be used)

2. KKT-based constrained least squares

TODO: update this section

We need to compare their

1. final cost
2. constraint violation
3. speed ( it is hard to compare for gtsam based method because it uses c++)

Benchmark: Intel i7-8809G 3.10GHz CPU

First a low-dimensional example to demonstrate linear runtime with trajectory length T. We use state and control dimensions n=m=3, and with m-1 dimensional local constraints at every time step:

Now we show a comparison for larger control dimensions by fixing T=100 and increasing n and m together:

TODO: fill in this section All functions start with "ecLQR_" are main solvers

C++ code is in src, tests, and scripts.

Dependencies:

• GTSAM with the matlab wrapper.

After installing GTSAM with the matlab wrapper, no additional installation is required.

Dependencies:

• GTSAM

From the root repo directory, run

mkdir build
cd build
cmake ..
make

To run the unit tests, run

make check

To run the scripts (e.g. laine_benchmarks.cpp), simply run

make laine_benchmarks.run