#include <Mathematics/PLDPSolver.hh>
Public Member Functions | |
| PLDPSolver (unsigned int CardU, double *iPu, double *Px, double *Pu, double *iLQ) | |
| Constructor. | |
| ~PLDPSolver () | |
| Destructor. | |
| 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. | |
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. | |
| int | ForwardSubstitution () |
| Forward substitution. First Phase EE^t v2 = v1 <-> LL^t v2 = v1 Now solving L y = v1. | |
| 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. | |
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().