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 <MotionGeneration/ComAndFootRealization.hh>
37#include <SimplePlugin.hh>
38#include <deque>
40
41using namespace ::std;
42
43namespace PatternGeneratorJRL {
68class ZMPPreviewControlWithMultiBodyZMP : public SimplePlugin {
69 private:
73 Eigen::Vector3d m_StaticToTheLeftHip;
75 Eigen::Vector3d m_StaticToTheRightHip;
77 Eigen::Vector3d m_TranslationToTheLeftHip;
79 Eigen::Vector3d m_TranslationToTheRightHip;
80
82 Eigen::Vector3d m_Dt;
83
85 double m_PreviewControlTime;
86
88 unsigned int m_NL;
89
93 Eigen::MatrixXd Finalql;
95 Eigen::MatrixXd Finalqr;
97
99 deque<ZMPPosition> m_FIFOZMPRefPositions;
100
102 deque<ZMPPosition> m_FIFODeltaZMPPositions;
103
105 deque<COMState> m_FIFOCOMStates;
106
108 deque<FootAbsolutePosition> m_FIFOLeftFootPosition;
109
111 deque<FootAbsolutePosition> m_FIFORightFootPosition;
112
114 double m_sxzmp, m_syzmp;
115
117 double m_sxDeltazmp, m_syDeltazmp;
118
120 Eigen::MatrixXd m_PC1x;
121 Eigen::MatrixXd m_PC1y;
122
124 Eigen::MatrixXd m_Deltax;
125 Eigen::MatrixXd m_Deltay;
126
128 bool m_StartingNewSequence;
129
131 deque<ZMPPosition> m_FIFOTmpZMPPosition;
132
134 std::vector<COMState> m_ExtraCOMBuffer;
135
139 Eigen::Vector3d m_DiffBetweenComAndWaist;
140
142 Eigen::Vector3d m_StartingCOMState;
143
145 Eigen::Matrix4d m_FinalDesiredCOMPose;
146
148 double m_AnkleSoilDistance;
149
151 ComAndFootRealization *m_ComAndFootRealization;
152
154 PinocchioRobot *m_PinocchioRobot;
155
157 unsigned long int m_NumberOfIterations;
158
160 PreviewControl *m_PC;
161
163 int m_StageStrategy;
164
166 double m_SamplingPeriod;
167
169 void RegisterMethods();
170
172 void SetSamplingPeriod(double lSamplingPeriod);
173
175 void SetPreviewControlTime(double lPreviewControlTime);
176
177 public:
183 static const int ZMPCOM_TRAJECTORY_FULL = 1;
184
187
190
193 ZMPPreviewControlWithMultiBodyZMP(SimplePluginManager *lSPM);
194
197
223 void SetStrategyForStageActivation(int anAlgo);
224
228
232 void GetDifferenceBetweenComAndWaist(double lComAndWaist[3]);
233
257 int OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition,
258 FootAbsolutePosition &RightFootPosition,
259 ZMPPosition &NewZMPRefPos, COMState &finalCOMState,
260 Eigen::VectorXd &CurrentConfiguration,
261 Eigen::VectorXd &CurrentVelocity,
262 Eigen::VectorXd &CurrentAcceleration);
263
277 int FirstStageOfControl(FootAbsolutePosition &LeftFootPosition,
278 FootAbsolutePosition &RightFootPosition,
279 COMState &afCOMState);
280
290 int EvaluateMultiBodyZMP(int StartingIteration);
291
299 int SecondStageOfControl(COMState &refandfinal);
300
320 int EvaluateStartingState(Eigen::VectorXd &BodyAngles,
321 Eigen::Vector3d &aStartingCOMState,
322 Eigen::Vector3d &aStartingZMPPosition,
323 Eigen::Matrix<double, 6, 1> &aStartingWaistPose,
324 FootAbsolutePosition &InitLeftFootPosition,
325 FootAbsolutePosition &InitRightFootPosition);
326
340 int EvaluateStartingCoM(Eigen::MatrixXd BodyAngles,
341 Eigen::Vector3d &aStartingCOMState,
342 Eigen::VectorXd &aWaistPose,
343 FootAbsolutePosition &InitLeftFootPosition,
344 FootAbsolutePosition &InitRightFootPosition);
345
363 int Setup(deque<ZMPPosition> &ZMPRefPositions, deque<COMState> &COMStates,
364 deque<FootAbsolutePosition> &LeftFootPositions,
365 deque<FootAbsolutePosition> &RightFootPositions);
366
380 int SetupFirstPhase(deque<ZMPPosition> &ZMPRefPositions,
381 deque<COMState> &COMStates,
382 deque<FootAbsolutePosition> &LeftFootPositions,
383 deque<FootAbsolutePosition> &RightFootPositions);
384
409 int SetupIterativePhase(deque<ZMPPosition> &ZMPRefPositions,
410 deque<COMState> &COMStates,
411 deque<FootAbsolutePosition> &LeftFootPositions,
412 deque<FootAbsolutePosition> &RightFootPositions,
413 Eigen::VectorXd &CurrentConfiguration,
414 Eigen::VectorXd &CurrentVelocity,
415 Eigen::VectorXd &CurrentAcceleration, int localindex);
416
426 void CreateExtraCOMBuffer(deque<COMState> &ExtraCOMBuffer,
427 deque<ZMPPosition> &ExtraZMPBuffer,
428 deque<ZMPPosition> &ExtraZMPRefBuffer);
429
438 int EvaluateStartingCoM(Eigen::VectorXd &BodyAnglesInit,
439 Eigen::Vector3d &aStartingCOMState,
440 Eigen::Matrix<double, 6, 1> &aStartingWaistPosition,
441 FootAbsolutePosition &InitLeftFootPosition,
442 FootAbsolutePosition &InitRightFootPosition);
443
448 Eigen::Matrix4d GetFinalDesiredCOMPose();
449
456 Eigen::Matrix4d GetCurrentPositionofWaistInCOMFrame();
457
461
463 void UpdateTheZMPRefQueue(ZMPPosition NewZMPRefPos);
464
466 inline bool setComAndFootRealization(ComAndFootRealization *aCFR) {
467 m_ComAndFootRealization = aCFR;
468 return true;
469 };
470 inline ComAndFootRealization *getComAndFootRealization() {
471 return m_ComAndFootRealization;
472 };
473
495 COMState &acomp, FootAbsolutePosition &aLeftFAP,
496 FootAbsolutePosition &aRightFAP, Eigen::VectorXd &CurrentConfiguration,
497 Eigen::VectorXd &CurrentVelocity, Eigen::VectorXd &CurrentAcceleration,
498 unsigned long int IterationNumber, int StageOfTheAlgorithm);
499
502
506 inline bool setPinocchioRobot(PinocchioRobot *aPinocchioRobot) {
507 m_PinocchioRobot = aPinocchioRobot;
508 return true;
509 }
510
513 inline PinocchioRobot *getPinocchioRobot() const { return m_PinocchioRobot; }
514
516 void SetStrategyForPCStages(int Strategy);
517
520
522 void CallMethod(std::string &Method, std::istringstream &astrm);
523};
524} // namespace PatternGeneratorJRL
525#endif /* _ZMPREVIEWCONTROLWITHMULTIBODYZMP_H_ */
Definition: pinocchiorobot.hh:60
Class to implement the preview control.
Definition: PreviewControl.hh:48
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:68
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:514
void SetStrategyForPCStages(int Strategy)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:705
int EvaluateMultiBodyZMP(int StartingIteration)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:379
void GetDifferenceBetweenComAndWaist(double lComAndWaist[3])
bool setPinocchioRobot(PinocchioRobot *aPinocchioRobot)
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:506
PinocchioRobot * getPinocchioRobot() const
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:513
int EvaluateStartingCoM(Eigen::MatrixXd BodyAngles, Eigen::Vector3d &aStartingCOMState, Eigen::VectorXd &aWaistPose, FootAbsolutePosition &InitLeftFootPosition, FootAbsolutePosition &InitRightFootPosition)
ComAndFootRealization * getComAndFootRealization()
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:470
int Setup(deque< ZMPPosition > &ZMPRefPositions, deque< COMState > &COMStates, deque< FootAbsolutePosition > &LeftFootPositions, deque< FootAbsolutePosition > &RightFootPositions)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:416
int FirstStageOfControl(FootAbsolutePosition &LeftFootPosition, FootAbsolutePosition &RightFootPosition, COMState &afCOMState)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:328
int GetStrategyForStageActivation()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:653
static const int ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:186
COMState GetLastCOMFromFirstStage()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:270
void CallMethod(std::string &Method, std::istringstream &astrm)
Overloading method of SimplePlugin.
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:744
ZMPPreviewControlWithMultiBodyZMP(SimplePluginManager *lSPM)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:43
int EvaluateStartingState(Eigen::VectorXd &BodyAngles, Eigen::Vector3d &aStartingCOMState, Eigen::Vector3d &aStartingZMPPosition, Eigen::Matrix< double, 6, 1 > &aStartingWaistPose, FootAbsolutePosition &InitLeftFootPosition, FootAbsolutePosition &InitRightFootPosition)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:674
bool setComAndFootRealization(ComAndFootRealization *aCFR)
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:466
int SetupFirstPhase(deque< ZMPPosition > &ZMPRefPositions, deque< COMState > &COMStates, deque< FootAbsolutePosition > &LeftFootPositions, deque< FootAbsolutePosition > &RightFootPositions)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:439
int OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition, FootAbsolutePosition &RightFootPosition, ZMPPosition &NewZMPRefPos, COMState &finalCOMState, Eigen::VectorXd &CurrentConfiguration, Eigen::VectorXd &CurrentVelocity, Eigen::VectorXd &CurrentAcceleration)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:179
static const int ZMPCOM_TRAJECTORY_FULL
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:183
void UpdateTheZMPRefQueue(ZMPPosition NewZMPRefPos)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:630
int GetStrategyForPCStages()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:709
int SecondStageOfControl(COMState &refandfinal)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:276
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:101
Eigen::Matrix4d GetFinalDesiredCOMPose()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:670
void SetStrategyForStageActivation(int anAlgo)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:635
void SetPreviewControl(PreviewControl *aPC)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:94
~ZMPPreviewControlWithMultiBodyZMP()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:92
Eigen::Matrix4d GetCurrentPositionofWaistInCOMFrame()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:659
void CreateExtraCOMBuffer(deque< COMState > &ExtraCOMBuffer, deque< ZMPPosition > &ExtraZMPBuffer, deque< ZMPPosition > &ExtraZMPRefBuffer)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:559
static const int ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:189
\doc Simulate a rigid body
Definition: patterngeneratorinterface.hh:41
Structure to store the COM state computed by the preview control.
Definition: pgtypes.hh:75
Structure to store the absolute foot position.
Definition: pgtypes.hh:138
Definition: pgtypes.hh:118