Repository containing code for a distributed implementation of Potential iLQR. Putting Potential iLQR into a nutshell, the ultimate goal is enable cooperative real-time multi-agent navigation by posing the coupled optimal control problem for each of the agents as a combined decoupled problem encapsulating the interactions between all agents simultaneously using the concept of a potential game. DP-iLQR is an extension of this algorithm that improves scalability by splitting up the centralized problem with all agents into smaller problems with subsets of agents based on their relative proximities.
The setup that we're most interested in is one in which multiple agents would like to navigate around each other in a shared space. Each agent starts at some position and would like to arrive at some goal position or state. Several applications in mobile robotics include:
- warehouse navigation
- robot/human crowd navigation
- space robotics
The above is one example of what this looks like using 5 unicycle models. Note that while the dynamics of this scenario are homogeneous, this library currently supports simulation of non-homogeneous models via zero-padding the states
-
After cloning the repo, one must first install all necessary dependencies (ideally into an environment). Then, one can run the following with the environment activated:
pip install -e .This will install an editable version of the package into the local environment. To ensure it worked, navigate to any directory and run:
python -c "import dpilqr; print(dpilqr.util.repopath)"This should print out the top level of the repository.
-
Additionally, you must compile the C++ extensions using Cython by running:
python setup.py build_ext --inplaceThis should create a
bbdynamicswrap.cppas well as a*.sofile thatdpilqrwill automatically include in the package namespace.
- dpilqr contains the project source code
- scripts/examples.py provides several examples that exercise many aspects of the library.
- scripts/analysis.py contains scripts that run monte-carlo simulations to compare Potential-iLQR from DP-iLQR across several parameters.
- Simulation and integration of state space models by defining a ordinary differential
equation and a linearization method in C++. These are the currently implemented models:
DoubleIntDynamics4DCarDynamics3DUnicycleDynamics4DBikeDynamics5DHumanDynamics6DQuadcopterDynamics6DQuadcopterDynamics12D
- Construction of various cost models including:
-
ReferenceCost- Penalizes deviations from some reference trajectory
$C(x, u) = (x - \bar{x})^\intercal Q (x - \bar{x}) + u^\intercal R u$
-
ProximityCost- Penalizes the distances
$d^{ij}$ between agents$i$ and$j$ below some threshold$d_{\text{prox}}$ - $C(d^{ij}) = \begin{cases} \beta( d^{ij} - d_{\text{prox}})^2 & d^{ij} < d_{\text{prox}} \ 0 & \text{otherwise} \end{cases}$
- Penalizes the distances
-
Cost- Any other cost implementing the appropriate methods for the particular problem.
-
- Potential iLQR solver based on this paper by Yuval Tassa that incorporates the above dynamical models and cost structures
- DP-iLQR solver that takes advantage of the sparsity of the state space to solve subproblems
individually. The GIF below visualizes what that might look like for 'agents' in Brownian motion
where the red is the proximity cost and the grey is the neighborhood that creates the subproblem.
We see as the agents move around, their interaction graphs are dynamically updated.
The following two repositories have been instrumental from both an algorithms and software architecture perspective in the development of this project: