#include <Mathematics/PLDPSolverHerdt.hh>
Public Member Functions | |
PLDPSolverHerdt (unsigned int CardU, double *iPu, double *Px, double *Pu, double *iLQ) | |
Constructor. More... | |
~PLDPSolverHerdt () | |
Destructor. More... | |
int | SolveProblem (std::deque< PatternGeneratorJRL::LinearConstraintInequalityFreeFeet_t > &QueueOfLConstraintInequalitiesFreeFeet, std::deque< PatternGeneratorJRL::SupportFeet_t > &QueueOfSupportFeet, double *CstPartOfTheCostFunction, unsigned int NbOfConstraints, double *LinearPartOfConstraints, double *CstPartOfConstraints, double *XkYk, double *X, unsigned int NumberOfRemovedConstraints, unsigned int NbRemovedFootCstr, bool StartingSequence, unsigned int NumberSteps, bool CurrentStateChanged, double time) |
Solve the optimization problem. More... | |
Protected Member Functions | |
void | InitializeSolver (unsigned int NumberSteps) |
void | AllocateMemoryForSolver (unsigned int NumberSteps) |
double | ComputeAlpha (vector< unsigned int > &NewActivatedConstraints, unsigned int NumberSteps) |
void | WriteCurrentZMPSolution (string filename, double *XkYk) |
Initial solution methods related | |
int | ComputeInitialSolution (std::deque< PatternGeneratorJRL::LinearConstraintInequalityFreeFeet_t > &QueueOfLConstraintInequalitiesFreeFeet, std::deque< PatternGeneratorJRL::SupportFeet_t > &QueueOfSupportFeet, unsigned int NumberSteps, double *XkYk) |
int | PrecomputeiPuPx (unsigned int NumberSteps) |
Projected descent direction methods related | |
int | ComputeProjectedDescentDirection (unsigned int NumberSteps) |
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:
PLDPSolverHerdt::PLDPSolverHerdt | ( | unsigned int | CardU, |
double * | iPu, | ||
double * | Px, | ||
double * | Pu, | ||
double * | iLQ | ||
) |
Constructor.
PLDPSolverHerdt::~PLDPSolverHerdt | ( | ) |
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 identical zmp and feet positions that are lying inside the feasible zones defined by the constraints on the feet placements. U0 = -iPu * Px [Xkt Ykt]t + iPu * InitialZMP
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 PLDPSolverHerdt::SolveProblem | ( | std::deque< PatternGeneratorJRL::LinearConstraintInequalityFreeFeet_t > & | QueueOfLConstraintInequalitiesFreeFeet, |
std::deque< PatternGeneratorJRL::SupportFeet_t > & | QueueOfSupportFeet, | ||
double * | CstPartOfTheCostFunction, | ||
unsigned int | NbOfConstraints, | ||
double * | LinearPartOfConstraints, | ||
double * | CstPartOfConstraints, | ||
double * | XkYk, | ||
double * | X, | ||
unsigned int | NumberOfRemovedConstraints, | ||
unsigned int | NbRemovedFootCstr, | ||
bool | StartingSequence, | ||
unsigned int | NumberSteps, | ||
bool | CurrentStateChanged, | ||
double | time | ||
) |
Solve the optimization problem.
Initialization de cholesky.
Step two: Compute the projected descent direction.
Step three : Compute alpha
Compute new solution.
References AllocateMemoryForSolver(), ComputeAlpha(), ComputeInitialSolution(), ComputeProjectedDescentDirection(), i, InitializeSolver(), j, and WriteCurrentZMPSolution().
|
protected |
Write current ZMP ref trajectory associated with current value of m_Vk.
Referenced by SolveProblem().