AnalyticalZMPCOGTrajectory.hh

Go to the documentation of this file.

/*
 * Copyright 2008, 2009, 2010,
 *
 * Alireza Nakhaei
 * Olivier Stasse
 *
 * 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 _ANALYTICAL_COG_H_
#define _ANALYTICAL_COG_H_
 
#include <Mathematics/Polynome.hh>
#include <iostream>
#include <vector>
 
namespace PatternGeneratorJRL {
 
class AnalyticalZMPCOGTrajectory {
 public:
  AnalyticalZMPCOGTrajectory(int lNbOfIntervals = 0);
 
  ~AnalyticalZMPCOGTrajectory();
 
  bool ComputeCOM(double t, double &r);
 
  bool ComputeCOMSpeed(double t, double &r);
 
  bool ComputeCOM(double t, double &r, int i);
 
  bool ComputeCOMSpeed(double t, double &r, int i);
 
  bool ComputeCOMAcceleration(double t, double &r, int j);
 
  bool ComputeZMP(double t, double &r);
 
  bool ComputeZMPSpeed(double t, double &r);
 
  bool ComputeZMP(double t, double &r, int i);
 
  void SetNumberOfIntervals(unsigned int lNbOfIntervals);
 
  void SetCoGHyperbolicCoefficients(std::vector<double> &lV,
                                    std::vector<double> &lW);
 
  void SetStartingTimeIntervalsAndHeightVariation(std::vector<double> &lDeltaTj,
                                                  std::vector<double> &lomegaj);
 
  void SetPolynomialDegrees(std::vector<unsigned int> &lPolynomialDegree);
 
  void GetPolynomialDegrees(std::vector<unsigned int> &lPolynomialDegree) const;
 
  void GetNumberOfIntervals(unsigned int &lNbOfIntervals) const;
 
  void GetHyperbolicCoefficients(std::vector<double> &lV,
                                 std::vector<double> &lW) const;
 
  void GetStartingPointAndHeightVariation(std::vector<double> &lTj,
                                          std::vector<double> &lomegaj);
 
  bool GetFromListOfCOGPolynomials(unsigned int j, Polynome *&aPoly) const;
 
  bool GetFromListOfZMPPolynomials(unsigned int j, Polynome *&aPoly) const;
 
  void TransfertCoefficientsFromCOGTrajectoryToZMPOne(
      std::vector<double> &lCOMZ, std::vector<double> &lZMPZ);
 
  void TransfertOneIntervalCoefficientsFromCOGTrajectoryToZMPOne(
      unsigned int IntervalIndex, double &lCOMZ, double &lZMPZ);
 
  void Building3rdOrderPolynomial(unsigned int anIntervalj, double pjTjm1,
                                  double pjTj);
 
  double FluctuationMaximal();
 
  friend std::ostream &operator<<(std::ostream &os,
                                  const AnalyticalZMPCOGTrajectory &obj);
 
  double GetAbsoluteTimeReference() const { return m_AbsoluteTimeReference; }
 
  void SetAbsoluteTimeReference(double anAbsoluteTimeReference) {
    m_AbsoluteTimeReference = anAbsoluteTimeReference;
  }
 
  bool GetIntervalIndexFromTime(double t, unsigned int &j);
 
  bool GetIntervalIndexFromTime(double t, unsigned int &j,
                                unsigned int &prev_j);
 
 protected:
  int m_NbOfIntervals;
 
  std::vector<double> m_V;
 
  std::vector<double> m_W;
 
  std::vector<double> m_DeltaTj;
 
  std::vector<double> m_omegaj;
 
  std::vector<double> m_RefTime;
 
  std::vector<unsigned int> m_PolynomialDegree;
 
  std::vector<Polynome *> m_ListOfCOGPolynomials;
 
  std::vector<Polynome *> m_ListOfZMPPolynomials;
 
  void FreePolynomes();
 
  double m_AbsoluteTimeReference;
 
  /* \brien Sensitivity to numerical noise. */
  double m_Sensitivity;
};
 
std::ostream &operator<<(std::ostream &os,
                         const AnalyticalZMPCOGTrajectory &obj);
}  // namespace PatternGeneratorJRL
#endif /* _ANALYTICAL_COG_H_ */