ZMPPreviewControlWithMultiBodyZMP.hh

Go to the documentation of this file.

/*
 * Copyright 2006, 2007, 2008, 2009, 2010,
 *
 * Fumio   Kanehiro
 * Florent Lamiraux
 * Alireza Nakhaei
 * Mathieu Poirier
 * 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 _ZMPREVIEWCONTROLWITHMULTIBODYZMP_H_
#define _ZMPREVIEWCONTROLWITHMULTIBODYZMP_H_
 
#include <MotionGeneration/ComAndFootRealization.hh>
#include <PreviewControl/PreviewControl.hh>
#include <SimplePlugin.hh>
#include <deque>
#include <jrl/walkgen/pgtypes.hh>
 
using namespace ::std;
 
namespace PatternGeneratorJRL {
class ZMPPreviewControlWithMultiBodyZMP : public SimplePlugin {
 private:
  Eigen::Vector3d m_StaticToTheLeftHip;
  Eigen::Vector3d m_StaticToTheRightHip;
  Eigen::Vector3d m_TranslationToTheLeftHip;
  Eigen::Vector3d m_TranslationToTheRightHip;
 
  Eigen::Vector3d m_Dt;
 
  double m_PreviewControlTime;
 
  unsigned int m_NL;
 
  Eigen::MatrixXd Finalql;
  Eigen::MatrixXd Finalqr;
 
  deque<ZMPPosition> m_FIFOZMPRefPositions;
 
  deque<ZMPPosition> m_FIFODeltaZMPPositions;
 
  deque<COMState> m_FIFOCOMStates;
 
  deque<FootAbsolutePosition> m_FIFOLeftFootPosition;
 
  deque<FootAbsolutePosition> m_FIFORightFootPosition;
 
  double m_sxzmp, m_syzmp;
 
  double m_sxDeltazmp, m_syDeltazmp;
 
  Eigen::MatrixXd m_PC1x;
  Eigen::MatrixXd m_PC1y;
 
  Eigen::MatrixXd m_Deltax;
  Eigen::MatrixXd m_Deltay;
 
  bool m_StartingNewSequence;
 
  deque<ZMPPosition> m_FIFOTmpZMPPosition;
 
  std::vector<COMState> m_ExtraCOMBuffer;
 
  Eigen::Vector3d m_DiffBetweenComAndWaist;
 
  Eigen::Vector3d m_StartingCOMState;
 
  Eigen::Matrix4d m_FinalDesiredCOMPose;
 
  double m_AnkleSoilDistance;
 
  ComAndFootRealization *m_ComAndFootRealization;
 
  PinocchioRobot *m_PinocchioRobot;
 
  unsigned long int m_NumberOfIterations;
 
  PreviewControl *m_PC;
 
  int m_StageStrategy;
 
  double m_SamplingPeriod;
 
  void RegisterMethods();
 
  void SetSamplingPeriod(double lSamplingPeriod);
 
  void SetPreviewControlTime(double lPreviewControlTime);
 
 public:
  static const int ZMPCOM_TRAJECTORY_FULL = 1;
 
  static const int ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY = 2;
 
  static const int ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY = 3;
 
  ZMPPreviewControlWithMultiBodyZMP(SimplePluginManager *lSPM);
 
  ~ZMPPreviewControlWithMultiBodyZMP();
 
  void SetStrategyForStageActivation(int anAlgo);
 
  int GetStrategyForStageActivation();
 
  void GetDifferenceBetweenComAndWaist(double lComAndWaist[3]);
 
  int OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition,
                             FootAbsolutePosition &RightFootPosition,
                             ZMPPosition &NewZMPRefPos, COMState &finalCOMState,
                             Eigen::VectorXd &CurrentConfiguration,
                             Eigen::VectorXd &CurrentVelocity,
                             Eigen::VectorXd &CurrentAcceleration);
 
  int FirstStageOfControl(FootAbsolutePosition &LeftFootPosition,
                          FootAbsolutePosition &RightFootPosition,
                          COMState &afCOMState);
 
  int EvaluateMultiBodyZMP(int StartingIteration);
 
  int SecondStageOfControl(COMState &refandfinal);
 
  int EvaluateStartingState(Eigen::VectorXd &BodyAngles,
                            Eigen::Vector3d &aStartingCOMState,
                            Eigen::Vector3d &aStartingZMPPosition,
                            Eigen::Matrix<double, 6, 1> &aStartingWaistPose,
                            FootAbsolutePosition &InitLeftFootPosition,
                            FootAbsolutePosition &InitRightFootPosition);
 
  int EvaluateStartingCoM(Eigen::MatrixXd BodyAngles,
                          Eigen::Vector3d &aStartingCOMState,
                          Eigen::VectorXd &aWaistPose,
                          FootAbsolutePosition &InitLeftFootPosition,
                          FootAbsolutePosition &InitRightFootPosition);
 
  int Setup(deque<ZMPPosition> &ZMPRefPositions, deque<COMState> &COMStates,
            deque<FootAbsolutePosition> &LeftFootPositions,
            deque<FootAbsolutePosition> &RightFootPositions);
 
  int SetupFirstPhase(deque<ZMPPosition> &ZMPRefPositions,
                      deque<COMState> &COMStates,
                      deque<FootAbsolutePosition> &LeftFootPositions,
                      deque<FootAbsolutePosition> &RightFootPositions);
 
  int SetupIterativePhase(deque<ZMPPosition> &ZMPRefPositions,
                          deque<COMState> &COMStates,
                          deque<FootAbsolutePosition> &LeftFootPositions,
                          deque<FootAbsolutePosition> &RightFootPositions,
                          Eigen::VectorXd &CurrentConfiguration,
                          Eigen::VectorXd &CurrentVelocity,
                          Eigen::VectorXd &CurrentAcceleration, int localindex);
 
  void CreateExtraCOMBuffer(deque<COMState> &ExtraCOMBuffer,
                            deque<ZMPPosition> &ExtraZMPBuffer,
                            deque<ZMPPosition> &ExtraZMPRefBuffer);
 
  int EvaluateStartingCoM(Eigen::VectorXd &BodyAnglesInit,
                          Eigen::Vector3d &aStartingCOMState,
                          Eigen::Matrix<double, 6, 1> &aStartingWaistPosition,
                          FootAbsolutePosition &InitLeftFootPosition,
                          FootAbsolutePosition &InitRightFootPosition);
 
  Eigen::Matrix4d GetFinalDesiredCOMPose();
 
  Eigen::Matrix4d GetCurrentPositionofWaistInCOMFrame();
 
  COMState GetLastCOMFromFirstStage();
 
  void UpdateTheZMPRefQueue(ZMPPosition NewZMPRefPos);
 
  inline bool setComAndFootRealization(ComAndFootRealization *aCFR) {
    m_ComAndFootRealization = aCFR;
    return true;
  };
  inline ComAndFootRealization *getComAndFootRealization() {
    return m_ComAndFootRealization;
  };
 
  void CallToComAndFootRealization(
      COMState &acomp, FootAbsolutePosition &aLeftFAP,
      FootAbsolutePosition &aRightFAP, Eigen::VectorXd &CurrentConfiguration,
      Eigen::VectorXd &CurrentVelocity, Eigen::VectorXd &CurrentAcceleration,
      unsigned long int IterationNumber, int StageOfTheAlgorithm);
 
  void SetPreviewControl(PreviewControl *aPC);
 
  inline bool setPinocchioRobot(PinocchioRobot *aPinocchioRobot) {
    m_PinocchioRobot = aPinocchioRobot;
    return true;
  }
 
  inline PinocchioRobot *getPinocchioRobot() const { return m_PinocchioRobot; }
 
  void SetStrategyForPCStages(int Strategy);
 
  int GetStrategyForPCStages();
 
  void CallMethod(std::string &Method, std::istringstream &astrm);
};
}  // namespace PatternGeneratorJRL
#endif /* _ZMPREVIEWCONTROLWITHMULTIBODYZMP_H_ */