Trajectory Optimization Using OSQP

For embedded applications we may want to use QP solvers with code generation such as OSQP. The particular form of the QP program it solves lends itself to LQ trajectory optimization problems with additional box constraints, e.g. $$\begin{array}{rl}\underset{x_{0:N},u_{0:N-1}}{\min }& q_N^{\top }{x}_N+\frac{1}{2}{x}_N^{\top }{Q}_N{x}_N+\sum _{k=0}^{N-1}{\left[\begin{array}{c}{q}_k\\ r_k\end{array}\right]}^{\top }\left[\begin{array}{c}{x}_k\\ u_k\end{array}\right]+\frac{1}{2}{\left[\begin{array}{c}{x}_k\\ u_k\end{array}\right]}^{\top }\left[\begin{array}{cc}{Q}_k& S_k^{\top }\\ S_k& R_k\end{array}\right]\left[\begin{array}{c}{x}_k\\ u_k\end{array}\right]\\ \text{s.t.}& x_{k+1}=A_k{x}_k+B_k{u}_k+c_k,k=0,\dots ,N-1\\ & x_0={\hat{x}}_0\\ & x_{\min }\le x_k\le x_{\max },\forall k\in [0,N]\\ & u_{\min }\le u_k\le u_{\max },\forall k\in [0,N-1]\end{array}\text{\hspace{1em}(2a-c)}$$

OSQP form

To coincide with the form of the QP program described in the documentation of OSQP we define a vector of decision variables $$\begin{array}{c}x={\left[\begin{array}{ccccccc}{u}_0^{\top }& \dots & u_{N-1}^{\top }& x_0^{\top }& \dots & x_{N-1}^{\top }& x_N^{\top }\end{array}\right]}^{\top }\end{array}$$ for which the objective function contains the terms $$\begin{array}{c}P=\left[\begin{array}{ccccccc}{R}_1& & & S_k& & & \\ & \ddots & & & \ddots & & \\ & & R_{N-1}& & & S_{N-1}& \\ S_1^{\top }& & & Q_k& & & \\ & \ddots & & & \ddots & & \\ & & S_{N-1}^{\top }& & & Q_{N-1}& \\ & & & & & & Q_N\end{array}\right],q=\left[\begin{array}{c}{r}_0\\ ⋮\\ r_{N-1}\\ q_0\\ ⋮\\ q_{N-1}\\ q_N\end{array}\right].\end{array}$$ The constrains are then incorporated using $$\begin{array}{c}l=\left[\begin{array}{c}-{\hat{x}}_0\\ -c_0\\ ⋮\\ -c_{N-1}\\ x_{\min }\\ ⋮\\ ⋮\\ x_{\min }\\ u_{\min }\\ ⋮\\ u_{\min }\end{array}\right],A=\left[\begin{array}{ccccccc}0& & & -I& & & \\ B_0& & & A_0& -I& & \\ & \ddots & & & \ddots & \ddots & \\ & & B_{N-1}& & & A_{N-1}& -I\\ & & & I& & & \\ & & & & \ddots & & \\ & & & & & \ddots & \\ & & & & & & I\\ I& & & & & & \\ & \ddots & & & & & \\ & & I& & & & \end{array}\right],u=\left[\begin{array}{c}-{\hat{x}}_0\\ -c_0\\ ⋮\\ -c_{N-1}\\ x_{\max }\\ ⋮\\ ⋮\\ x_{\max }\\ u_{\max }\\ ⋮\\ u_{\max }\end{array}\right].\end{array}$$