#include <Mathematics/PLDPSolver.hh>
Public Member Functions | |
PLDPSolver (unsigned int CardU, double *iPu, double *Px, double *Pu, double *iLQ) | |
Constructor. More... | |
~PLDPSolver () | |
Destructor. More... | |
int | SolveProblem (double *CstPartOfTheCostFunction, unsigned int NbOfConstraints, double *LinearPartOfConstraints, double *CstPartOfConstraints, double *ZMPRef, double *XkYk, double *X, std::vector< int > &SimilarConstraint, unsigned int NumberOfRemovedConstraints, bool StartingSequence) |
Solve the optimization problem. More... | |
Protected Member Functions | |
void | InitializeSolver () |
void | AllocateMemoryForSolver () |
double | ComputeAlpha (vector< unsigned int > &NewActivatedConstraints, vector< int > &SimilarConstraint) |
void | StoreCurrentZMPSolution (double *XkYk) |
void | WriteCurrentZMPSolution (string filename, double *XkYk) |
Initial solution methods related | |
int | ComputeInitialSolution (double *ZMPRef, double *XkYk, bool StartingSequence) |
int | PrecomputeiPuPx () |
Projected descent direction methods related | |
int | ComputeProjectedDescentDirection () |
Compute Projected descent direction. More... | |
int | ForwardSubstitution () |
Forward substitution. First Phase EE^t v2 = v1 <-> LL^t v2 = v1 Now solving L y = v1. More... | |
int | BackwardSubstitution () |
Compute v2 q (14b) in Dimitrov 2009. Second phase a Now solving LL^t v2 = v1 <-> L y = v1 with L^t v2 = y y solved with first phase. So now we are looking for v2. More... | |
This class implements a two stage strategy to solve the following optimal problem:
PLDPSolver::PLDPSolver | ( | unsigned int | CardU, |
double * | iPu, | ||
double * | Px, | ||
double * | Pu, | ||
double * | iLQ | ||
) |
Constructor.
References AllocateMemoryForSolver().
PLDPSolver::~PLDPSolver | ( | ) |
Destructor.
|
protected |
|
protected |
Compute v2 q (14b) in Dimitrov 2009. Second phase a Now solving LL^t v2 = v1 <-> L y = v1 with L^t v2 = y y solved with first phase. So now we are looking for v2.
Referenced by ComputeProjectedDescentDirection().
|
protected |
Detecting violated constraints
Referenced by SolveProblem().
|
protected |
Compute the initial solution
The initial solution of the problem is given by eq(14) Dimitar ICRA 2008 U0 = iPu * Px [Xkt Ykt]t + iPu * ZMPRef The only part which can not be precomputed is ZMPRef.
Referenced by SolveProblem().
|
protected |
Compute Projected descent direction.
References BackwardSubstitution(), and ForwardSubstitution().
Referenced by SolveProblem().
|
protected |
Forward substitution. First Phase EE^t v2 = v1 <-> LL^t v2 = v1 Now solving L y = v1.
Referenced by ComputeProjectedDescentDirection().
|
protected |
Initialize the internal variables of the class.
Referenced by SolveProblem().
|
protected |
int PLDPSolver::SolveProblem | ( | double * | CstPartOfTheCostFunction, |
unsigned int | NbOfConstraints, | ||
double * | LinearPartOfConstraints, | ||
double * | CstPartOfConstraints, | ||
double * | ZMPRef, | ||
double * | XkYk, | ||
double * | X, | ||
std::vector< int > & | SimilarConstraint, | ||
unsigned int | NumberOfRemovedConstraints, | ||
bool | StartingSequence | ||
) |
Solve the optimization problem.
Initialization de cholesky.
Step two: Compute the projected descent direction.
Step three : Compute alpha
Compute new solution.
References ComputeAlpha(), ComputeInitialSolution(), ComputeProjectedDescentDirection(), i, InitializeSolver(), j, StoreCurrentZMPSolution(), and WriteCurrentZMPSolution().
|
protected |
Store the current ZMP solution for hot start purposes.
Referenced by SolveProblem().
|
protected |
Write current ZMP ref trajectory associated with current value of m_Vk.
Referenced by SolveProblem().