Loading...
Searching...
No Matches
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>
41
42using namespace ::std;
43
44namespace PatternGeneratorJRL {
69class ZMPPreviewControlWithMultiBodyZMP : public SimplePlugin {
70private:
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
178public:
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_ */
Definition: pinocchiorobot.hh:57
Class to implement the preview control.
Definition: PreviewControl.hh:48
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:69
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
void SetStrategyForPCStages(int Strategy)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:724
int EvaluateMultiBodyZMP(int StartingIteration)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:396
void GetDifferenceBetweenComAndWaist(double lComAndWaist[3])
bool setPinocchioRobot(PinocchioRobot *aPinocchioRobot)
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:507
PinocchioRobot * getPinocchioRobot() const
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:514
int EvaluateStartingCoM(Eigen::MatrixXd BodyAngles, Eigen::Vector3d &aStartingCOMState, Eigen::VectorXd &aWaistPose, FootAbsolutePosition &InitLeftFootPosition, FootAbsolutePosition &InitRightFootPosition)
ComAndFootRealization * getComAndFootRealization()
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:471
int Setup(deque< ZMPPosition > &ZMPRefPositions, deque< COMState > &COMStates, deque< FootAbsolutePosition > &LeftFootPositions, deque< FootAbsolutePosition > &RightFootPositions)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:433
int FirstStageOfControl(FootAbsolutePosition &LeftFootPosition, FootAbsolutePosition &RightFootPosition, COMState &afCOMState)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:334
int GetStrategyForStageActivation()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:672
static const int ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:187
COMState GetLastCOMFromFirstStage()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:275
void CallMethod(std::string &Method, std::istringstream &astrm)
Overloading method of SimplePlugin.
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:763
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
bool setComAndFootRealization(ComAndFootRealization *aCFR)
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:467
int SetupFirstPhase(deque< ZMPPosition > &ZMPRefPositions, deque< COMState > &COMStates, deque< FootAbsolutePosition > &LeftFootPositions, deque< FootAbsolutePosition > &RightFootPositions)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:456
int OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition, FootAbsolutePosition &RightFootPosition, ZMPPosition &NewZMPRefPos, COMState &finalCOMState, Eigen::VectorXd &CurrentConfiguration, Eigen::VectorXd &CurrentVelocity, Eigen::VectorXd &CurrentAcceleration)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:184
static const int ZMPCOM_TRAJECTORY_FULL
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:184
void UpdateTheZMPRefQueue(ZMPPosition NewZMPRefPos)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:649
int GetStrategyForPCStages()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:728
int SecondStageOfControl(COMState &refandfinal)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:281
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
Eigen::Matrix4d GetFinalDesiredCOMPose()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:689
void SetStrategyForStageActivation(int anAlgo)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:654
void SetPreviewControl(PreviewControl *aPC)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:98
~ZMPPreviewControlWithMultiBodyZMP()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:96
Eigen::Matrix4d GetCurrentPositionofWaistInCOMFrame()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:678
void CreateExtraCOMBuffer(deque< COMState > &ExtraCOMBuffer, deque< ZMPPosition > &ExtraZMPBuffer, deque< ZMPPosition > &ExtraZMPRefBuffer)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:577
static const int ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:190
\doc Simulate a rigid body
Definition: patterngeneratorinterface.hh:41
Structure to store the COM state computed by the preview control.
Definition: pgtypes.hh:76
Structure to store the absolute foot position.
Definition: pgtypes.hh:139
Definition: pgtypes.hh:119