Loading...
Searching...
No Matches
PLDPSolverHerdt.hh
Go to the documentation of this file.
1/*
2 * Copyright 2010,
3 *
4 * Medhi Benallegue
5 * Andrei Herdt
6 *
7 * JRL, CNRS/AIST
8 *
9 * This file is part of walkGenJrl.
10 * walkGenJrl is free software: you can redistribute it and/or modify
11 * it under the terms of the GNU Lesser General Public License as published by
12 * the Free Software Foundation, either version 3 of the License, or
13 * (at your option) any later version.
14 *
15 * walkGenJrl is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU General Lesser Public License for more details.
19 * You should have received a copy of the GNU Lesser General Public License
20 * along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
21 *
22 * Research carried out within the scope of the
23 * Joint Japanese-French Robotics Laboratory (JRL)
24 */
32#ifndef _PLDP_SOLVER_H_HERDT
33
35#include <deque>
37#include <vector>
38
39namespace Optimization {
40namespace Solver {
45 public:
47 PLDPSolverHerdt(unsigned int CardU, double *iPu, double *Px, double *Pu,
48 double *iLQ);
49
52
55 int SolveProblem(
56 std::deque<PatternGeneratorJRL::LinearConstraintInequalityFreeFeet_t>
57 &QueueOfLConstraintInequalitiesFreeFeet,
58 std::deque<PatternGeneratorJRL::SupportFeet_t> &QueueOfSupportFeet,
59 double *CstPartOfTheCostFunction, unsigned int NbOfConstraints,
60 double *LinearPartOfConstraints, double *CstPartOfConstraints,
61 double *XkYk, double *X, unsigned int NumberOfRemovedConstraints,
62 unsigned int NbRemovedFootCstr, bool StartingSequence,
63 unsigned int NumberSteps, bool CurrentStateChanged, double time);
64
65 protected:
71 std::deque<PatternGeneratorJRL::LinearConstraintInequalityFreeFeet_t>
72 &QueueOfLConstraintInequalitiesFreeFeet,
73 std::deque<PatternGeneratorJRL::SupportFeet_t> &QueueOfSupportFeet,
74 unsigned int NumberSteps, double *XkYk);
75
77 int PrecomputeiPuPx(unsigned int NumberSteps);
82 void InitializeSolver(unsigned int NumberSteps);
83
85 void AllocateMemoryForSolver(unsigned int NumberSteps);
86
91 int ComputeProjectedDescentDirection(unsigned int NumberSteps);
92
100
109
113 double ComputeAlpha(vector<unsigned int> &NewActivatedConstraints,
114 unsigned int NumberSteps);
115
117 /* void StoreCurrentZMPSolution(double *XkYk); */
118
121 void WriteCurrentZMPSolution(string filename, double *XkYk);
122
126 private:
127 bool m_NoCholesky;
129 double *m_Pu;
130
132 double *m_iPu;
133
135 double *m_Px;
136
138 double *m_iPuPx;
139
141 double *m_Vk;
142
144 double *m_CstPartOfCostFunction;
145
147 double *m_UnconstrainedDescentDirection;
148
150 double *m_L;
151
153 double *m_iL;
154
156 double *m_iLQ;
157
159 double *m_d;
160
162 double *m_v1, *m_v2, *m_y;
163
165 double *m_tmp1, *m_tmp2;
166
168 double *m_A;
169
171 double *m_b;
172
174 bool *m_ConstraintsValueComputed;
175
179 unsigned int m_NbMaxOfConstraints;
180
182 double m_DistanceFeetCenters;
184 unsigned int m_NbOfConstraints;
185
187 unsigned int m_CardV;
188
190 unsigned int *m_NbSteps;
191
196 unsigned int m_DebugMode;
197
199 int m_ItNb;
204 PatternGeneratorJRL::OptCholesky *m_OptCholesky;
205
207 vector<unsigned int> m_ActivatedConstraints;
208
210 vector<unsigned int> m_PreviouslyActivatedConstraints;
211
213 bool m_HotStart;
214
216 double *m_InitialZMPSolution;
217
219 double m_InternalTime;
220
222 double m_tol;
223
228 bool m_LimitedComputationTime;
229
231 double m_AmountOfLimitedComputationTime;
232
234};
235} // namespace Solver
236} // namespace Optimization
237#endif /* _PLDPSOLVER_H_*/
Definition PLDPSolverHerdt.hh:44
double ComputeAlpha(vector< unsigned int > &NewActivatedConstraints, unsigned int NumberSteps)
Definition PLDPHerdt.cpp:512
void InitializeSolver(unsigned int NumberSteps)
Definition PLDPHerdt.cpp:132
int ComputeInitialSolution(std::deque< PatternGeneratorJRL::LinearConstraintInequalityFreeFeet_t > &QueueOfLConstraintInequalitiesFreeFeet, std::deque< PatternGeneratorJRL::SupportFeet_t > &QueueOfSupportFeet, unsigned int NumberSteps, double *XkYk)
Definition PLDPHerdt.cpp:251
void WriteCurrentZMPSolution(string filename, double *XkYk)
Definition PLDPHerdt.cpp:902
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...
Definition PLDPHerdt.cpp:369
int PrecomputeiPuPx(unsigned int NumberSteps)
Definition PLDPHerdt.cpp:185
void AllocateMemoryForSolver(unsigned int NumberSteps)
Definition PLDPHerdt.cpp:102
int ForwardSubstitution()
Forward substitution. First Phase EE^t v2 = v1 <-> LL^t v2 = v1 Now solving L y = v1.
Definition PLDPHerdt.cpp:346
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.
Definition PLDPHerdt.cpp:603
~PLDPSolverHerdt()
Destructor.
Definition PLDPHerdt.cpp:154
PLDPSolverHerdt(unsigned int CardU, double *iPu, double *Px, double *Pu, double *iLQ)
Constructor.
Definition PLDPHerdt.cpp:56
int ComputeProjectedDescentDirection(unsigned int NumberSteps)
Compute Projected descent direction.
Definition PLDPHerdt.cpp:398
Definition PLDPSolver.hh:38