Solves banded conic programs arising in optimal control using condensing. More...
#include <condensing_based_cp_solver.hpp>
Solves banded conic programs arising in optimal control using condensing.
The class condensing based CP solver is a special solver for band structured conic programs that can be solved via a condensing technique.
Definition at line 59 of file condensing_based_cp_solver.hpp.
Default constructor.
Definition at line 46 of file condensing_based_cp_solver.cpp.
CondensingBasedCPsolver::CondensingBasedCPsolver | ( | UserInteraction * | _userInteraction, |
uint | nConstraints_, | ||
const DVector & | blockDims_ | ||
) |
Definition at line 58 of file condensing_based_cp_solver.cpp.
Copy constructor (deep copy).
Definition at line 73 of file condensing_based_cp_solver.cpp.
CondensingBasedCPsolver::~CondensingBasedCPsolver | ( | ) | [virtual] |
Destructor.
Definition at line 94 of file condensing_based_cp_solver.cpp.
BooleanType CondensingBasedCPsolver::areRealTimeParametersDefined | ( | ) | const [inline] |
BandedCPsolver * CondensingBasedCPsolver::clone | ( | ) | const [virtual] |
Assignment operator (deep copy).
Implements BandedCPsolver.
Definition at line 130 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::computeCondensingOperator | ( | BandedCP & | cp | ) | [protected] |
Definition at line 1375 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::condense | ( | BandedCP & | cp | ) | [protected] |
Performes the condensing of the dynamic system if necessary.
Definition at line 510 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::expand | ( | BandedCP & | cp | ) | [protected] |
Expands the KKT system if necessary.
Definition at line 1076 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::finalizeSolve | ( | BandedCP & | cp | ) | [virtual] |
Solves a given banded conic program
Reimplemented from BandedCPsolver.
Definition at line 248 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::freezeCondensing | ( | ) | [virtual] |
Reimplemented from BandedCPsolver.
Definition at line 329 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::generateBoundVectors | ( | ) | [protected] |
Definition at line 938 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::generateConstraintBlockLine | ( | uint | nn, |
uint | rowOffset, | ||
uint & | rowOffset1 | ||
) | [protected] |
Definition at line 762 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::generateConstraintVectors | ( | uint | nn, |
uint | rowOffset, | ||
uint & | rowOffset1 | ||
) | [protected] |
Definition at line 898 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::generateHessianBlockLine | ( | uint | nn, |
uint | rowOffset, | ||
uint & | rowOffset1 | ||
) | [protected] |
Definition at line 694 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::generateObjectiveGradient | ( | ) | [protected] |
Definition at line 1014 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::generateStateBoundBlockLine | ( | uint | nn, |
uint | rowOffset, | ||
uint & | rowOffset1 | ||
) | [protected] |
Definition at line 830 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::generateStateBoundVectors | ( | uint | nn, |
uint | rowOffset, | ||
uint & | rowOffset1 | ||
) | [protected] |
Definition at line 918 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::getFirstControl | ( | DVector & | u0_ | ) | const [virtual] |
Implements BandedCPsolver.
Definition at line 292 of file condensing_based_cp_solver.cpp.
uint CondensingBasedCPsolver::getNA | ( | ) | const [inline] |
uint CondensingBasedCPsolver::getNC | ( | ) | const [inline] |
uint CondensingBasedCPsolver::getNF | ( | ) | const [inline] |
uint CondensingBasedCPsolver::getNP | ( | ) | const [inline] |
uint CondensingBasedCPsolver::getNU | ( | ) | const [inline] |
uint CondensingBasedCPsolver::getNumPoints | ( | ) | const [inline] |
uint CondensingBasedCPsolver::getNW | ( | ) | const [inline] |
uint CondensingBasedCPsolver::getNX | ( | ) | const [inline] |
uint CondensingBasedCPsolver::getNXA | ( | ) | const [inline] |
returnValue CondensingBasedCPsolver::getParameters | ( | DVector & | p_ | ) | const [virtual] |
Implements BandedCPsolver.
Definition at line 278 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::getVarianceCovariance | ( | DMatrix & | var | ) | [virtual] |
Returns a variance-covariance estimate if possible or an error message otherwise.
Reimplemented from BandedCPsolver.
Definition at line 307 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::init | ( | const OCPiterate & | iter_ | ) | [virtual] |
initializes the banded conic solver
Implements BandedCPsolver.
Definition at line 137 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::initializeCondensingOperator | ( | ) | [protected] |
Definition at line 1345 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::initializeCPsolver | ( | InfeasibleQPhandling | infeasibleQPhandling | ) | [protected, virtual] |
Initializes QP objects.
Definition at line 1616 of file condensing_based_cp_solver.cpp.
CondensingBasedCPsolver & CondensingBasedCPsolver::operator= | ( | const CondensingBasedCPsolver & | rhs | ) |
Assignment operator (deep copy).
Definition at line 101 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::prepareSolve | ( | BandedCP & | cp | ) | [virtual] |
Solves a given banded conic program
Reimplemented from BandedCPsolver.
Definition at line 162 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::projectHessian | ( | DMatrix & | H_, |
double | dampingFactor | ||
) | [protected] |
Checks whether the Hessian is positive definite and projects
the Hessian based on a heuristic damping factor. If this
damping factor is smaller than 0, the routine does nothing.
Definition at line 354 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::setRealTimeParameters | ( | const DVector & | DeltaX, |
const DVector & | DeltaP = emptyConstVector |
||
) | [virtual] |
Reimplemented from BandedCPsolver.
Definition at line 317 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::setupRelaxedQPdata | ( | InfeasibleQPhandling | infeasibleQPhandling, |
DenseCP & | _denseCPrelaxed | ||
) | const [protected, virtual] |
Determines relaxed (constraints') bounds of an infeasible QP.
_denseCPrelaxed | OUTPUT: Relaxed QP data. |
Definition at line 1481 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::setupRelaxedQPdataL1 | ( | DenseCP & | _denseCPrelaxed | ) | const [protected, virtual] |
Determines relaxed (constraints') bounds of an infeasible QP.
_denseCPrelaxed | OUTPUT: Relaxed QP data. |
Definition at line 1499 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::setupRelaxedQPdataL2 | ( | DenseCP & | _denseCPrelaxed | ) | const [protected, virtual] |
Determines relaxed (constraints') bounds of an infeasible QP.
_denseCPrelaxed | OUTPUT: Relaxed QP data. |
Definition at line 1558 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::solve | ( | BandedCP & | cp | ) | [virtual] |
Solves a given banded conic program in feedback mode:
cp | the banded conic program to be solved |
DeltaX | difference between state estimate and previous prediction |
DeltaP | difference between current and previous parameter value |
Implements BandedCPsolver.
Definition at line 192 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::solveCPsubproblem | ( | ) | [protected, virtual] |
Definition at line 387 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::solveQP | ( | uint | maxIter, |
InfeasibleQPhandling | infeasibleQPhandling = IQH_UNDEFINED |
||
) | [protected, virtual] |
Solves current QP (or relaxation) using not more than the given number of iterations.
maxIter | Maximum number of iterations. |
Definition at line 1639 of file condensing_based_cp_solver.cpp.
returnValue CondensingBasedCPsolver::unfreezeCondensing | ( | ) | [virtual] |
Reimplemented from BandedCPsolver.
Definition at line 340 of file condensing_based_cp_solver.cpp.
BlockMatrix CondensingBasedCPsolver::ADense [protected] |
Constraint matrix
Definition at line 263 of file condensing_based_cp_solver.hpp.
DVector CondensingBasedCPsolver::blockDims [protected] |
Definition at line 243 of file condensing_based_cp_solver.hpp.
Definition at line 246 of file condensing_based_cp_solver.hpp.
DenseCPsolver* CondensingBasedCPsolver::cpSolver [protected] |
Definition at line 271 of file condensing_based_cp_solver.hpp.
DenseQPsolver* CondensingBasedCPsolver::cpSolverRelaxed [protected] |
Definition at line 272 of file condensing_based_cp_solver.hpp.
BlockMatrix CondensingBasedCPsolver::d [protected] |
the condensing offset
Definition at line 252 of file condensing_based_cp_solver.hpp.
DVector CondensingBasedCPsolver::deltaP [protected] |
Definition at line 277 of file condensing_based_cp_solver.hpp.
DVector CondensingBasedCPsolver::deltaX [protected] |
Definition at line 276 of file condensing_based_cp_solver.hpp.
DenseCP CondensingBasedCPsolver::denseCP [protected] |
Definition at line 274 of file condensing_based_cp_solver.hpp.
BlockMatrix CondensingBasedCPsolver::gDense [protected] |
Objective gradient after condensing
Definition at line 262 of file condensing_based_cp_solver.hpp.
BlockMatrix CondensingBasedCPsolver::HDense [protected] |
Hessian after condensing
Definition at line 261 of file condensing_based_cp_solver.hpp.
BlockMatrix CondensingBasedCPsolver::hT [protected] |
Definition at line 254 of file condensing_based_cp_solver.hpp.
OCPiterate CondensingBasedCPsolver::iter [protected] |
Definition at line 242 of file condensing_based_cp_solver.hpp.
BlockMatrix CondensingBasedCPsolver::lbADense [protected] |
Constraint lower bounds
Definition at line 264 of file condensing_based_cp_solver.hpp.
BlockMatrix CondensingBasedCPsolver::lbDense [protected] |
Simple lower bounds
Definition at line 266 of file condensing_based_cp_solver.hpp.
uint CondensingBasedCPsolver::nConstraints [protected] |
Definition at line 244 of file condensing_based_cp_solver.hpp.
BlockMatrix CondensingBasedCPsolver::T [protected] |
the condensing operator
Definition at line 251 of file condensing_based_cp_solver.hpp.
BlockMatrix CondensingBasedCPsolver::ubADense [protected] |
Constraint upper bounds
Definition at line 265 of file condensing_based_cp_solver.hpp.
BlockMatrix CondensingBasedCPsolver::ubDense [protected] |
Simple upper bounds
Definition at line 267 of file condensing_based_cp_solver.hpp.