Go to the documentation of this file.
32 #ifndef _ZMPREVIEWCONTROLWITHMULTIBODYZMP_H_
33 #define _ZMPREVIEWCONTROLWITHMULTIBODYZMP_H_
37 #include <MotionGeneration/ComAndFootRealization.hh>
39 #include <SimplePlugin.hh>
42 using namespace ::std;
74 Eigen::Vector3d m_StaticToTheLeftHip;
76 Eigen::Vector3d m_StaticToTheRightHip;
78 Eigen::Vector3d m_TranslationToTheLeftHip;
80 Eigen::Vector3d m_TranslationToTheRightHip;
86 double m_PreviewControlTime;
94 Eigen::MatrixXd Finalql;
96 Eigen::MatrixXd Finalqr;
100 deque<ZMPPosition> m_FIFOZMPRefPositions;
103 deque<ZMPPosition> m_FIFODeltaZMPPositions;
106 deque<COMState> m_FIFOCOMStates;
109 deque<FootAbsolutePosition> m_FIFOLeftFootPosition;
112 deque<FootAbsolutePosition> m_FIFORightFootPosition;
115 double m_sxzmp, m_syzmp;
118 double m_sxDeltazmp, m_syDeltazmp;
121 Eigen::MatrixXd m_PC1x;
122 Eigen::MatrixXd m_PC1y;
125 Eigen::MatrixXd m_Deltax;
126 Eigen::MatrixXd m_Deltay;
129 bool m_StartingNewSequence;
132 deque<ZMPPosition> m_FIFOTmpZMPPosition;
135 std::vector<COMState> m_ExtraCOMBuffer;
140 Eigen::Vector3d m_DiffBetweenComAndWaist;
143 Eigen::Vector3d m_StartingCOMState;
146 Eigen::Matrix4d m_FinalDesiredCOMPose;
149 double m_AnkleSoilDistance;
152 ComAndFootRealization *m_ComAndFootRealization;
158 unsigned long int m_NumberOfIterations;
167 double m_SamplingPeriod;
170 void RegisterMethods();
173 void SetSamplingPeriod(
double lSamplingPeriod);
176 void SetPreviewControlTime(
double lPreviewControlTime);
261 Eigen::VectorXd &CurrentConfiguration,
262 Eigen::VectorXd &CurrentVelocity,
263 Eigen::VectorXd &CurrentAcceleration);
322 Eigen::Vector3d &aStartingCOMState,
323 Eigen::Vector3d &aStartingZMPPosition,
324 Eigen::Matrix<double, 6, 1> &aStartingWaistPose,
342 Eigen::Vector3d &aStartingCOMState,
343 Eigen::VectorXd &aWaistPose,
364 int Setup(deque<ZMPPosition> &ZMPRefPositions, deque<COMState> &COMStates,
365 deque<FootAbsolutePosition> &LeftFootPositions,
366 deque<FootAbsolutePosition> &RightFootPositions);
382 deque<COMState> &COMStates,
383 deque<FootAbsolutePosition> &LeftFootPositions,
384 deque<FootAbsolutePosition> &RightFootPositions);
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);
428 deque<ZMPPosition> &ExtraZMPBuffer,
429 deque<ZMPPosition> &ExtraZMPRefBuffer);
440 Eigen::Vector3d &aStartingCOMState,
441 Eigen::Matrix<double, 6, 1> &aStartingWaistPosition,
468 m_ComAndFootRealization = aCFR;
472 return m_ComAndFootRealization;
498 Eigen::VectorXd &CurrentVelocity, Eigen::VectorXd &CurrentAcceleration,
499 unsigned long int IterationNumber,
int StageOfTheAlgorithm);
508 m_PinocchioRobot = aPinocchioRobot;
523 void CallMethod(std::string &Method, std::istringstream &astrm);
int GetStrategyForPCStages()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:728
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
void GetDifferenceBetweenComAndWaist(double lComAndWaist[3])
PinocchioRobot * getPinocchioRobot() const
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:514
Class to implement the preview control.
Definition: PreviewControl.hh:48
static const int ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:190
int FirstStageOfControl(FootAbsolutePosition &LeftFootPosition, FootAbsolutePosition &RightFootPosition, COMState &afCOMState)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:334
~ZMPPreviewControlWithMultiBodyZMP()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:96
bool setComAndFootRealization(ComAndFootRealization *aCFR)
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:467
int OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition, FootAbsolutePosition &RightFootPosition, ZMPPosition &NewZMPRefPos, COMState &finalCOMState, Eigen::VectorXd &CurrentConfiguration, Eigen::VectorXd &CurrentVelocity, Eigen::VectorXd &CurrentAcceleration)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:184
int Setup(deque< ZMPPosition > &ZMPRefPositions, deque< COMState > &COMStates, deque< FootAbsolutePosition > &LeftFootPositions, deque< FootAbsolutePosition > &RightFootPositions)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:433
ZMPPreviewControlWithMultiBodyZMP(SimplePluginManager *lSPM)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:45
Definition: pgtypes.hh:119
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
void SetStrategyForStageActivation(int anAlgo)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:654
static const int ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:187
static const int ZMPCOM_TRAJECTORY_FULL
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:184
int GetStrategyForStageActivation()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:672
int SecondStageOfControl(COMState &refandfinal)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:281
Eigen::Matrix4d GetCurrentPositionofWaistInCOMFrame()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:678
int SetupFirstPhase(deque< ZMPPosition > &ZMPRefPositions, deque< COMState > &COMStates, deque< FootAbsolutePosition > &LeftFootPositions, deque< FootAbsolutePosition > &RightFootPositions)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:456
int EvaluateMultiBodyZMP(int StartingIteration)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:396
void CreateExtraCOMBuffer(deque< COMState > &ExtraCOMBuffer, deque< ZMPPosition > &ExtraZMPBuffer, deque< ZMPPosition > &ExtraZMPRefBuffer)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:577
COMState GetLastCOMFromFirstStage()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:275
void UpdateTheZMPRefQueue(ZMPPosition NewZMPRefPos)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:649
void SetPreviewControl(PreviewControl *aPC)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:98
bool setPinocchioRobot(PinocchioRobot *aPinocchioRobot)
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:507
void SetStrategyForPCStages(int Strategy)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:724
ComAndFootRealization * getComAndFootRealization()
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:471
void CallMethod(std::string &Method, std::istringstream &astrm)
Overloading method of SimplePlugin.
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:763
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
Definition: pinocchiorobot.hh:57
\doc Simulate a rigid body
Definition: patterngeneratorinterface.hh:41
Eigen::Matrix4d GetFinalDesiredCOMPose()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:689
Structure to store the COM state computed by the preview control.
Definition: pgtypes.hh:76
int EvaluateStartingCoM(Eigen::MatrixXd BodyAngles, Eigen::Vector3d &aStartingCOMState, Eigen::VectorXd &aWaistPose, FootAbsolutePosition &InitLeftFootPosition, FootAbsolutePosition &InitRightFootPosition)
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:69