rsLQR  0.1
RiccatiSolver Struct Reference

Solver that uses Riccati recursion to solve an LQR problem. More...

#include <riccati_solver.h>

Data Fields

LQRProblemprob
 Problem data.
 
int nhorizon
 length of the time horizon
 
int nstates
 size of state vector (n)
 
int ninputs
 number of control inputs (m)
 
int nvars
 total number of decision variables, including the dual variables
 
double * data
 pointer to the beginning of the single block of memory allocated by the solver
 
MatrixK
 N-1 feedback gain matrices of size (m,n)
 
Matrixd
 N-1 feedforward gains of size (m,)
 
MatrixP
 N cost-to-go Hessians of size (n,n)
 
Matrixp
 N cost-to-go gradients of size (n,)
 
MatrixX
 State trajectory. N vectors of size (n,)
 
MatrixU
 Control trajectory. N-1 vectors of size (m,)
 
MatrixY
 Lagrange multipliers. N vectors of size (n,)
 
MatrixQx
 Gradient of the action-value function with respect to the state. N vectors of size (n,).
 
MatrixQu
 Gradient of the action-value function with respect to the control. N vectors of size (m,).
 
MatrixQxx
 Hessian of the action-value function with respect to the state. N vectors of size (n,n).
 
MatrixQux
 Cross-term Hessian of the action-value function. N vectors of size (m,n).
 
MatrixQuu
 Hessian of the action-value function with respect to the control. N vectors of size (m,m).
 
double t_solve_ms
 Total solve time in milliseconds.
 
double t_backward_pass_ms
 Time spent in the backward pass in milliseconds.
 
double t_forward_pass_ms
 Time spent in the forward pass in milliseconds.
 

Detailed Description

Solver that uses Riccati recursion to solve an LQR problem.

Solves the generic LQR problem with affine terms using Riccati recursion and a forward simulation of the linear dynamics. Assumes problems are of the following form:

\begin{align*} \underset{x_{1:N}, u_{1:N-1}}{\text{minimize}} &&& \frac{1}{2} x_N^T Q_N + x_N + q_N^T x_N + \sum_{k-1}^{N-1} \frac{1}{2} x_k^T Q_k + x_k + q_k^T x_k + u_k^T R_k + u_k + r_k^T u_k \\ \text{subject to} &&& x_{k+1} = A_k x_k + B_k u_k + f_k \\ &&& x_1 = x_\text{init} \end{align*}

All the memory required by the solver is initialized upon the creation of the solver to avoid any dynamic memory allocations during the solve.

Construction and destruction

Use ndlqr_NewRiccatiSolver() to initialize a new solver, which much be paired with a single call to ndlqr_FreeRiccatiSolver() to free all of solver's memory.

Typical Usage

Standard usage will typically look like the following:

LQRProblem* lqrprob = ndlqr_ReadTestLQRProblem(); // your data here
double* soln = (double*) malloc(solver->nvars * sizeof(double));
free(soln);

Methods


The documentation for this struct was generated from the following file:
ndlqr_PrintRiccatiSummary
int ndlqr_PrintRiccatiSummary(RiccatiSolver *solver)
Prints a summary of the solve.
Definition: riccati_solver.c:155
ndlqr_FreeRiccatiSolver
int ndlqr_FreeRiccatiSolver(RiccatiSolver *solver)
Free the memory for a Riccati solver.
Definition: riccati_solver.c:139
ndlqr_CopyRiccatiSolution
int ndlqr_CopyRiccatiSolution(RiccatiSolver *solver, double *soln)
Copies the solution to a user-supplied array.
Definition: riccati_solver.c:181
RiccatiSolver
Solver that uses Riccati recursion to solve an LQR problem.
Definition: riccati_solver.h:62
RiccatiSolver::nvars
int nvars
total number of decision variables, including the dual variables
Definition: riccati_solver.h:68
ndlqr_FreeLQRProblem
int ndlqr_FreeLQRProblem(LQRProblem *lqrprob)
Free the data stored by and LQRProblem.
Definition: lqr_problem.c:38
ndlqr_SolveRiccati
int ndlqr_SolveRiccati(RiccatiSolver *solver)
Solve the LQR problem using Riccati recursion and a forward simulation of the linear dynamics.
Definition: riccati_solve.c:7
LQRProblem
Describes an LQR problem with affine terms.
Definition: lqr_problem.h:31
ndlqr_NewRiccatiSolver
RiccatiSolver * ndlqr_NewRiccatiSolver(LQRProblem *lqrprob)
Initialize a new Riccati solver.
Definition: riccati_solver.c:7