PLDPSolverHerdt.hh

Go to the documentation of this file.

/*
 * Copyright 2010,
 *
 * Medhi  Benallegue
 * Andrei Herdt
 *
 * JRL, CNRS/AIST
 *
 * This file is part of walkGenJrl.
 * walkGenJrl is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * walkGenJrl is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Lesser Public License for more details.
 * You should have received a copy of the GNU Lesser General Public License
 * along with walkGenJrl.  If not, see <http://www.gnu.org/licenses/>.
 *
 *  Research carried out within the scope of the
 *  Joint Japanese-French Robotics Laboratory (JRL)
 */
#ifndef _PLDP_SOLVER_H_HERDT
 
#include <Mathematics/OptCholesky.hh>
#include <deque>
#include <jrl/walkgen/pgtypes.hh>
#include <vector>
 
namespace Optimization {
namespace Solver {
class PLDPSolverHerdt {
 public:
  PLDPSolverHerdt(unsigned int CardU, double *iPu, double *Px, double *Pu,
                  double *iLQ);
 
  ~PLDPSolverHerdt();
 
  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);
 
 protected:
  int ComputeInitialSolution(
      std::deque<PatternGeneratorJRL::LinearConstraintInequalityFreeFeet_t>
          &QueueOfLConstraintInequalitiesFreeFeet,
      std::deque<PatternGeneratorJRL::SupportFeet_t> &QueueOfSupportFeet,
      unsigned int NumberSteps, double *XkYk);
 
  int PrecomputeiPuPx(unsigned int NumberSteps);
  void InitializeSolver(unsigned int NumberSteps);
 
  void AllocateMemoryForSolver(unsigned int NumberSteps);
 
  int ComputeProjectedDescentDirection(unsigned int NumberSteps);
 
  int ForwardSubstitution();
 
  int BackwardSubstitution();
 
  double ComputeAlpha(vector<unsigned int> &NewActivatedConstraints,
                      unsigned int NumberSteps);
 
  /*      void StoreCurrentZMPSolution(double *XkYk); */
 
  void WriteCurrentZMPSolution(string filename, double *XkYk);
 
 private:
  bool m_NoCholesky;
  double *m_Pu;
 
  double *m_iPu;
 
  double *m_Px;
 
  double *m_iPuPx;
 
  double *m_Vk;
 
  double *m_CstPartOfCostFunction;
 
  double *m_UnconstrainedDescentDirection;
 
  double *m_L;
 
  double *m_iL;
 
  double *m_iLQ;
 
  double *m_d;
 
  double *m_v1, *m_v2, *m_y;
 
  double *m_tmp1, *m_tmp2;
 
  double *m_A;
 
  double *m_b;
 
  bool *m_ConstraintsValueComputed;
 
  unsigned int m_NbMaxOfConstraints;
 
  double m_DistanceFeetCenters;
  unsigned int m_NbOfConstraints;
 
  unsigned int m_CardV;
 
  unsigned int *m_NbSteps;
 
  unsigned int m_DebugMode;
 
  int m_ItNb;
  PatternGeneratorJRL::OptCholesky *m_OptCholesky;
 
  vector<unsigned int> m_ActivatedConstraints;
 
  vector<unsigned int> m_PreviouslyActivatedConstraints;
 
  bool m_HotStart;
 
  double *m_InitialZMPSolution;
 
  double m_InternalTime;
 
  double m_tol;
 
  bool m_LimitedComputationTime;
 
  double m_AmountOfLimitedComputationTime;
 
};
}  // namespace Solver
}  // namespace Optimization
#endif /* _PLDPSOLVER_H_*/