ZMPPreviewControlWithMultiBodyZMP.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2006, 2007, 2008, 2009, 2010,
3  *
4  * Fumio Kanehiro
5  * Florent Lamiraux
6  * Alireza Nakhaei
7  * Mathieu Poirier
8  * Olivier Stasse
9  *
10  * JRL, CNRS/AIST
11  *
12  * This file is part of walkGenJrl.
13  * walkGenJrl is free software: you can redistribute it and/or modify
14  * it under the terms of the GNU Lesser General Public License as published by
15  * the Free Software Foundation, either version 3 of the License, or
16  * (at your option) any later version.
17  *
18  * walkGenJrl is distributed in the hope that it will be useful,
19  * but WITHOUT ANY WARRANTY; without even the implied warranty of
20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21  * GNU General Lesser Public License for more details.
22  * You should have received a copy of the GNU Lesser General Public License
23  * along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
24  *
25  * Research carried out within the scope of the
26  * Joint Japanese-French Robotics Laboratory (JRL)
27  */
32 #ifndef _ZMPREVIEWCONTROLWITHMULTIBODYZMP_H_
33 #define _ZMPREVIEWCONTROLWITHMULTIBODYZMP_H_
34 
35 #include <deque>
36 
37 #include <MotionGeneration/ComAndFootRealization.hh>
39 #include <SimplePlugin.hh>
40 #include <jrl/walkgen/pgtypes.hh>
41 
42 using namespace ::std;
43 
44 namespace PatternGeneratorJRL {
69 class ZMPPreviewControlWithMultiBodyZMP : public SimplePlugin {
70 private:
74  Eigen::Vector3d m_StaticToTheLeftHip;
76  Eigen::Vector3d m_StaticToTheRightHip;
78  Eigen::Vector3d m_TranslationToTheLeftHip;
80  Eigen::Vector3d m_TranslationToTheRightHip;
81 
83  Eigen::Vector3d m_Dt;
84 
86  double m_PreviewControlTime;
87 
89  unsigned int m_NL;
90 
94  Eigen::MatrixXd Finalql;
96  Eigen::MatrixXd Finalqr;
98 
100  deque<ZMPPosition> m_FIFOZMPRefPositions;
101 
103  deque<ZMPPosition> m_FIFODeltaZMPPositions;
104 
106  deque<COMState> m_FIFOCOMStates;
107 
109  deque<FootAbsolutePosition> m_FIFOLeftFootPosition;
110 
112  deque<FootAbsolutePosition> m_FIFORightFootPosition;
113 
115  double m_sxzmp, m_syzmp;
116 
118  double m_sxDeltazmp, m_syDeltazmp;
119 
121  Eigen::MatrixXd m_PC1x;
122  Eigen::MatrixXd m_PC1y;
123 
125  Eigen::MatrixXd m_Deltax;
126  Eigen::MatrixXd m_Deltay;
127 
129  bool m_StartingNewSequence;
130 
132  deque<ZMPPosition> m_FIFOTmpZMPPosition;
133 
135  std::vector<COMState> m_ExtraCOMBuffer;
136 
140  Eigen::Vector3d m_DiffBetweenComAndWaist;
141 
143  Eigen::Vector3d m_StartingCOMState;
144 
146  Eigen::Matrix4d m_FinalDesiredCOMPose;
147 
149  double m_AnkleSoilDistance;
150 
152  ComAndFootRealization *m_ComAndFootRealization;
153 
155  PinocchioRobot *m_PinocchioRobot;
156 
158  unsigned long int m_NumberOfIterations;
159 
161  PreviewControl *m_PC;
162 
164  int m_StageStrategy;
165 
167  double m_SamplingPeriod;
168 
170  void RegisterMethods();
171 
173  void SetSamplingPeriod(double lSamplingPeriod);
174 
176  void SetPreviewControlTime(double lPreviewControlTime);
177 
178 public:
184  static const int ZMPCOM_TRAJECTORY_FULL = 1;
185 
188 
191 
194  ZMPPreviewControlWithMultiBodyZMP(SimplePluginManager *lSPM);
195 
198 
224  void SetStrategyForStageActivation(int anAlgo);
225 
229 
233  void GetDifferenceBetweenComAndWaist(double lComAndWaist[3]);
234 
258  int OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition,
259  FootAbsolutePosition &RightFootPosition,
260  ZMPPosition &NewZMPRefPos, COMState &finalCOMState,
261  Eigen::VectorXd &CurrentConfiguration,
262  Eigen::VectorXd &CurrentVelocity,
263  Eigen::VectorXd &CurrentAcceleration);
264 
278  int FirstStageOfControl(FootAbsolutePosition &LeftFootPosition,
279  FootAbsolutePosition &RightFootPosition,
280  COMState &afCOMState);
281 
291  int EvaluateMultiBodyZMP(int StartingIteration);
292 
300  int SecondStageOfControl(COMState &refandfinal);
301 
321  int EvaluateStartingState(Eigen::VectorXd &BodyAngles,
322  Eigen::Vector3d &aStartingCOMState,
323  Eigen::Vector3d &aStartingZMPPosition,
324  Eigen::Matrix<double, 6, 1> &aStartingWaistPose,
325  FootAbsolutePosition &InitLeftFootPosition,
326  FootAbsolutePosition &InitRightFootPosition);
327 
341  int EvaluateStartingCoM(Eigen::MatrixXd BodyAngles,
342  Eigen::Vector3d &aStartingCOMState,
343  Eigen::VectorXd &aWaistPose,
344  FootAbsolutePosition &InitLeftFootPosition,
345  FootAbsolutePosition &InitRightFootPosition);
346 
364  int Setup(deque<ZMPPosition> &ZMPRefPositions, deque<COMState> &COMStates,
365  deque<FootAbsolutePosition> &LeftFootPositions,
366  deque<FootAbsolutePosition> &RightFootPositions);
367 
381  int SetupFirstPhase(deque<ZMPPosition> &ZMPRefPositions,
382  deque<COMState> &COMStates,
383  deque<FootAbsolutePosition> &LeftFootPositions,
384  deque<FootAbsolutePosition> &RightFootPositions);
385 
410  int SetupIterativePhase(deque<ZMPPosition> &ZMPRefPositions,
411  deque<COMState> &COMStates,
412  deque<FootAbsolutePosition> &LeftFootPositions,
413  deque<FootAbsolutePosition> &RightFootPositions,
414  Eigen::VectorXd &CurrentConfiguration,
415  Eigen::VectorXd &CurrentVelocity,
416  Eigen::VectorXd &CurrentAcceleration, int localindex);
417 
427  void CreateExtraCOMBuffer(deque<COMState> &ExtraCOMBuffer,
428  deque<ZMPPosition> &ExtraZMPBuffer,
429  deque<ZMPPosition> &ExtraZMPRefBuffer);
430 
439  int EvaluateStartingCoM(Eigen::VectorXd &BodyAnglesInit,
440  Eigen::Vector3d &aStartingCOMState,
441  Eigen::Matrix<double, 6, 1> &aStartingWaistPosition,
442  FootAbsolutePosition &InitLeftFootPosition,
443  FootAbsolutePosition &InitRightFootPosition);
444 
449  Eigen::Matrix4d GetFinalDesiredCOMPose();
450 
457  Eigen::Matrix4d GetCurrentPositionofWaistInCOMFrame();
458 
462 
464  void UpdateTheZMPRefQueue(ZMPPosition NewZMPRefPos);
465 
467  inline bool setComAndFootRealization(ComAndFootRealization *aCFR) {
468  m_ComAndFootRealization = aCFR;
469  return true;
470  };
471  inline ComAndFootRealization *getComAndFootRealization() {
472  return m_ComAndFootRealization;
473  };
474 
496  COMState &acomp, FootAbsolutePosition &aLeftFAP,
497  FootAbsolutePosition &aRightFAP, Eigen::VectorXd &CurrentConfiguration,
498  Eigen::VectorXd &CurrentVelocity, Eigen::VectorXd &CurrentAcceleration,
499  unsigned long int IterationNumber, int StageOfTheAlgorithm);
500 
503 
507  inline bool setPinocchioRobot(PinocchioRobot *aPinocchioRobot) {
508  m_PinocchioRobot = aPinocchioRobot;
509  return true;
510  }
511 
514  inline PinocchioRobot *getPinocchioRobot() const { return m_PinocchioRobot; }
515 
517  void SetStrategyForPCStages(int Strategy);
518 
521 
523  void CallMethod(std::string &Method, std::istringstream &astrm);
524 };
525 } // namespace PatternGeneratorJRL
526 #endif /* _ZMPREVIEWCONTROLWITHMULTIBODYZMP_H_ */
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::GetStrategyForPCStages
int GetStrategyForPCStages()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:728
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::EvaluateStartingState
int EvaluateStartingState(Eigen::VectorXd &BodyAngles, Eigen::Vector3d &aStartingCOMState, Eigen::Vector3d &aStartingZMPPosition, Eigen::Matrix< double, 6, 1 > &aStartingWaistPose, FootAbsolutePosition &InitLeftFootPosition, FootAbsolutePosition &InitRightFootPosition)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:693
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::GetDifferenceBetweenComAndWaist
void GetDifferenceBetweenComAndWaist(double lComAndWaist[3])
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::getPinocchioRobot
PinocchioRobot * getPinocchioRobot() const
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:514
PatternGeneratorJRL::PreviewControl
Class to implement the preview control.
Definition: PreviewControl.hh:48
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY
static const int ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:190
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::FirstStageOfControl
int FirstStageOfControl(FootAbsolutePosition &LeftFootPosition, FootAbsolutePosition &RightFootPosition, COMState &afCOMState)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:334
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::~ZMPPreviewControlWithMultiBodyZMP
~ZMPPreviewControlWithMultiBodyZMP()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:96
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::setComAndFootRealization
bool setComAndFootRealization(ComAndFootRealization *aCFR)
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:467
PreviewControl.hh
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::OneGlobalStepOfControl
int OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition, FootAbsolutePosition &RightFootPosition, ZMPPosition &NewZMPRefPos, COMState &finalCOMState, Eigen::VectorXd &CurrentConfiguration, Eigen::VectorXd &CurrentVelocity, Eigen::VectorXd &CurrentAcceleration)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:184
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::Setup
int Setup(deque< ZMPPosition > &ZMPRefPositions, deque< COMState > &COMStates, deque< FootAbsolutePosition > &LeftFootPositions, deque< FootAbsolutePosition > &RightFootPositions)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:433
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::ZMPPreviewControlWithMultiBodyZMP
ZMPPreviewControlWithMultiBodyZMP(SimplePluginManager *lSPM)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:45
PatternGeneratorJRL::ZMPPosition_s
Definition: pgtypes.hh:119
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::CallToComAndFootRealization
void CallToComAndFootRealization(COMState &acomp, FootAbsolutePosition &aLeftFAP, FootAbsolutePosition &aRightFAP, Eigen::VectorXd &CurrentConfiguration, Eigen::VectorXd &CurrentVelocity, Eigen::VectorXd &CurrentAcceleration, unsigned long int IterationNumber, int StageOfTheAlgorithm)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:105
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::SetStrategyForStageActivation
void SetStrategyForStageActivation(int anAlgo)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:654
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY
static const int ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:187
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::ZMPCOM_TRAJECTORY_FULL
static const int ZMPCOM_TRAJECTORY_FULL
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:184
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::GetStrategyForStageActivation
int GetStrategyForStageActivation()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:672
pgtypes.hh
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::SecondStageOfControl
int SecondStageOfControl(COMState &refandfinal)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:281
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::GetCurrentPositionofWaistInCOMFrame
Eigen::Matrix4d GetCurrentPositionofWaistInCOMFrame()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:678
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::SetupFirstPhase
int SetupFirstPhase(deque< ZMPPosition > &ZMPRefPositions, deque< COMState > &COMStates, deque< FootAbsolutePosition > &LeftFootPositions, deque< FootAbsolutePosition > &RightFootPositions)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:456
PatternGeneratorJRL::FootAbsolutePosition_t
Structure to store the absolute foot position.
Definition: pgtypes.hh:139
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::EvaluateMultiBodyZMP
int EvaluateMultiBodyZMP(int StartingIteration)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:396
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::CreateExtraCOMBuffer
void CreateExtraCOMBuffer(deque< COMState > &ExtraCOMBuffer, deque< ZMPPosition > &ExtraZMPBuffer, deque< ZMPPosition > &ExtraZMPRefBuffer)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:577
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::GetLastCOMFromFirstStage
COMState GetLastCOMFromFirstStage()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:275
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::UpdateTheZMPRefQueue
void UpdateTheZMPRefQueue(ZMPPosition NewZMPRefPos)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:649
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::SetPreviewControl
void SetPreviewControl(PreviewControl *aPC)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:98
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::setPinocchioRobot
bool setPinocchioRobot(PinocchioRobot *aPinocchioRobot)
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:507
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::SetStrategyForPCStages
void SetStrategyForPCStages(int Strategy)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:724
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::getComAndFootRealization
ComAndFootRealization * getComAndFootRealization()
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:471
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::CallMethod
void CallMethod(std::string &Method, std::istringstream &astrm)
Overloading method of SimplePlugin.
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:763
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::SetupIterativePhase
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)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:531
PatternGeneratorJRL::PinocchioRobot
Definition: pinocchiorobot.hh:57
PatternGeneratorJRL
\doc Simulate a rigid body
Definition: patterngeneratorinterface.hh:41
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::GetFinalDesiredCOMPose
Eigen::Matrix4d GetFinalDesiredCOMPose()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:689
PatternGeneratorJRL::COMState_s
Structure to store the COM state computed by the preview control.
Definition: pgtypes.hh:76
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::EvaluateStartingCoM
int EvaluateStartingCoM(Eigen::MatrixXd BodyAngles, Eigen::Vector3d &aStartingCOMState, Eigen::VectorXd &aWaistPose, FootAbsolutePosition &InitLeftFootPosition, FootAbsolutePosition &InitRightFootPosition)
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:69