Loading...
Searching...
No Matches
PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP Class Reference

Object to generate the angle positions every 5 ms from a set of absolute foot positions. More...

#include <PreviewControl/ZMPPreviewControlWithMultiBodyZMP.hh>

Inheritance diagram for PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP:
Collaboration diagram for PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP:

Public Member Functions

 ZMPPreviewControlWithMultiBodyZMP (SimplePluginManager *lSPM)
 
 ~ZMPPreviewControlWithMultiBodyZMP ()
 
void GetDifferenceBetweenComAndWaist (double lComAndWaist[3])
 
int OneGlobalStepOfControl (FootAbsolutePosition &LeftFootPosition, FootAbsolutePosition &RightFootPosition, ZMPPosition &NewZMPRefPos, COMState &finalCOMState, MAL_VECTOR_TYPE(double) &CurrentConfiguration, MAL_VECTOR_TYPE(double) &CurrentVelocity, MAL_VECTOR_TYPE(double) &CurrentAcceleration)
 
int FirstStageOfControl (FootAbsolutePosition &LeftFootPosition, FootAbsolutePosition &RightFootPosition, COMState &afCOMState)
 
int EvaluateMultiBodyZMP (int StartingIteration)
 
int SecondStageOfControl (COMState &refandfinal)
 
int EvaluateStartingState (MAL_VECTOR(&, double) BodyAngles, MAL_S3_VECTOR(&, double) aStartingCOMState, MAL_S3_VECTOR(&, double) aStartingZMPPosition, MAL_VECTOR(&, double) aStartingWaistPose, FootAbsolutePosition &InitLeftFootPosition, FootAbsolutePosition &InitRightFootPosition)
 
int EvaluateStartingCoM (MAL_MATRIX(&, double) BodyAngles, MAL_S3_VECTOR(&, double) aStartingCOMState, MAL_VECTOR(&, double) aWaistPose, FootAbsolutePosition &InitLeftFootPosition, FootAbsolutePosition &InitRightFootPosition)
 
Implementation of the GlobalStrategyManager interface.
void SetStrategyForStageActivation (int anAlgo)
 
int GetStrategyForStageActivation ()
 
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, MAL_VECTOR_TYPE(double) &CurrentConfiguration, MAL_VECTOR_TYPE(double) &CurrentVelocity, MAL_VECTOR_TYPE(double) &CurrentAcceleration, int localindex)
 
void CreateExtraCOMBuffer (deque< COMState > &ExtraCOMBuffer, deque< ZMPPosition > &ExtraZMPBuffer, deque< ZMPPosition > &ExtraZMPRefBuffer)
 
int EvaluateStartingCoM (MAL_VECTOR(&, double) BodyAnglesInit, MAL_S3_VECTOR(&, double) aStartingCOMState, MAL_VECTOR(&, double) aStartingWaistPosition, FootAbsolutePosition &InitLeftFootPosition, FootAbsolutePosition &InitRightFootPosition)
 
 MAL_S4x4_MATRIX_TYPE (double) GetFinalDesiredCOMPose()
 
 MAL_S4x4_MATRIX_TYPE (double) GetCurrentPositionofWaistInCOMFrame()
 
COMState GetLastCOMFromFirstStage ()
 
void UpdateTheZMPRefQueue (ZMPPosition NewZMPRefPos)
 
Setter and getter for the ComAndZMPTrajectoryGeneration.
bool setComAndFootRealization (ComAndFootRealization *aCFR)
 
ComAndFootRealization * getComAndFootRealization ()
 
void CallToComAndFootRealization (COMState &acomp, FootAbsolutePosition &aLeftFAP, FootAbsolutePosition &aRightFAP, MAL_VECTOR_TYPE(double) &CurrentConfiguration, MAL_VECTOR_TYPE(double) &CurrentVelocity, MAL_VECTOR_TYPE(double) &CurrentAcceleration, int IterationNumber, int StageOfTheAlgorithm)
 
void SetPreviewControl (PreviewControl *aPC)
 
Setter and getter for the jrlHumanoidDynamicRobot object.
bool setHumanoidDynamicRobot (CjrlHumanoidDynamicRobot *aHumanoidDynamicRobot)
 
CjrlHumanoidDynamicRobotgetHumanoidDynamicRobot () const
 
void SetStrategyForPCStages (int Strategy)
 
int GetStrategyForPCStages ()
 
void CallMethod (std::string &Method, std::istringstream &astrm)
 Overloading method of SimplePlugin. More...
 

Static Public Attributes

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
 

Detailed Description

Object to generate the angle positions every 5 ms from a set of absolute foot positions.

This object handles one iteration.

This algorithm use the preview control proposed by Kajita-San in Kajita2003 with the two stages archictecture.

You therefore have to use first the Setup method to fill all the queues. Then every 5 ms just use OneGlobalStepOfControl to compute the Waist Computation position, speed, acceleration and the angular values for the left and right legs.

This class can also be used with Pierre-Brice Wieber algorithm's Wieber2006 where only the second stage is used.

Finally in the case that the strategy adopted do not involve to compute the second stage and the first stage you can use the dummy mode. The architecture is kept the same but no computation are performed.

Constructor & Destructor Documentation

◆ ZMPPreviewControlWithMultiBodyZMP()

ZMPPreviewControlWithMultiBodyZMP::ZMPPreviewControlWithMultiBodyZMP ( SimplePluginManager *  lSPM)

◆ ~ZMPPreviewControlWithMultiBodyZMP()

ZMPPreviewControlWithMultiBodyZMP::~ZMPPreviewControlWithMultiBodyZMP ( )

Destroctor.

Member Function Documentation

◆ CallMethod()

void ZMPPreviewControlWithMultiBodyZMP::CallMethod ( std::string &  Method,
std::istringstream &  astrm 
)

Overloading method of SimplePlugin.

◆ CallToComAndFootRealization()

void ZMPPreviewControlWithMultiBodyZMP::CallToComAndFootRealization ( COMState acomp,
FootAbsolutePosition aLeftFAP,
FootAbsolutePosition aRightFAP,
MAL_VECTOR_TYPE(double) &  CurrentConfiguration,
MAL_VECTOR_TYPE(double) &  CurrentVelocity,
MAL_VECTOR_TYPE(double) &  CurrentAcceleration,
int  IterationNumber,
int  StageOfTheAlgorithm 
)

Call To CoM And Foot Realization object, the last values in the stack for the CoM and the feet positions will be used.

Parameters
[in]acomp: COM position,
[in]aLeftFAPPose of the left foot (3D position + 2 euler angles)
[in]aRightFAPPose of the right foot (3D position + 2 euler angles)
[out]CurrentConfigurationReturns the part of state vector corresponding to the position of the free floating, and the articular values.
[out]CurrentVelocityReturns the part of state vector corresponding to the velocity of the free floating and the articular values.
[out]CurrentAccelerationReturns the part of state vector corresponding to the acceleration of the free floating, and the articular values.
[in]IterationNumberNumber of time slot realized so far.
[in]StageOfTheAlgorithmIndicates if this is the second stage of the preview control or the first one.

References CjrlDynamicRobot::currentAcceleration(), CjrlDynamicRobot::currentConfiguration(), CjrlDynamicRobot::currentVelocity(), MAL_VECTOR_DIM, MAL_VECTOR_TYPE, PatternGeneratorJRL::FootAbsolutePosition_t::omega, PatternGeneratorJRL::COMState_s::pitch, PatternGeneratorJRL::COMState_s::roll, PatternGeneratorJRL::FootAbsolutePosition_t::theta, PatternGeneratorJRL::COMState_s::x, PatternGeneratorJRL::FootAbsolutePosition_t::x, PatternGeneratorJRL::COMState_s::y, PatternGeneratorJRL::FootAbsolutePosition_t::y, PatternGeneratorJRL::COMState_s::yaw, PatternGeneratorJRL::COMState_s::z, and PatternGeneratorJRL::FootAbsolutePosition_t::z.

Referenced by OneGlobalStepOfControl(), and SetupIterativePhase().

◆ CreateExtraCOMBuffer()

void ZMPPreviewControlWithMultiBodyZMP::CreateExtraCOMBuffer ( deque< COMState > &  ExtraCOMBuffer,
deque< ZMPPosition > &  ExtraZMPBuffer,
deque< ZMPPosition > &  ExtraZMPRefBuffer 
)

Create an extra COM buffer with a first preview round to be used by the stepover planner.

Parameters
[out]ExtraCOMBufferExtra FIFO for the CoM positions.
[out]ExtraZMPBufferExtra FIFO for the ZMP positions (for the stepping over first preview control).
[out]ExtraZMPRefBufferExtra FIFO for the ZMP ref positions.

References i, j, and PatternGeneratorJRL::PreviewControl::OneIterationOfPreview().

◆ EvaluateMultiBodyZMP()

int ZMPPreviewControlWithMultiBodyZMP::EvaluateMultiBodyZMP ( int  StartingIteration)

This methods is used only to update the queue of ZMP difference for the second stage of control. Also it does not return anything this method is crucial for the overall process.

Parameters
[in]StartingIteration-1 for the initialization, >=0 for a counter which gives the time.
Returns
If an error occurs returns a negative integer, 0 otherwise.

References CjrlDynamicRobot::computeForwardKinematics(), CjrlDynamicRobot::currentConfiguration(), CjrlDynamicRobot::getProperty(), MAL_VECTOR_TYPE, CjrlDynamicRobot::positionCenterOfMass(), PatternGeneratorJRL::ZMPPosition_s::px, PatternGeneratorJRL::ZMPPosition_s::py, PatternGeneratorJRL::ZMPPosition_s::pz, CjrlDynamicRobot::setProperty(), PatternGeneratorJRL::ZMPPosition_s::stepType, PatternGeneratorJRL::ZMPPosition_s::theta, PatternGeneratorJRL::ZMPPosition_s::time, and CjrlHumanoidDynamicRobot::zeroMomentumPoint().

Referenced by OneGlobalStepOfControl(), and SetupIterativePhase().

◆ EvaluateStartingCoM() [1/2]

int PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::EvaluateStartingCoM ( MAL_MATRIX(&, double)  BodyAngles,
MAL_S3_VECTOR(&, double)  aStartingCOMState,
MAL_VECTOR(&, double)  aWaistPose,
FootAbsolutePosition InitLeftFootPosition,
FootAbsolutePosition InitRightFootPosition 
)

Compute the COM of the robot with the Joint values given in BodyAngles, velocities set to zero, and returns the values of the COM in aStaringCOMState. Assuming that the waist is at (0,0,0) it returns the associate initial values for the left and right foot.

Parameters
BodyAnglesVector of the joint values for the robot.
[out]aStartingCOMStatePosition of the CoM.
[out]aWaistPositionPosition of the Waist.
[out]InitLeftFootPositionPosition of the left foot in the waist coordinates frame.
[out]InitRightFootPositionPosition of the right foot in the waist coordinates frame.

References MAL_VECTOR, and MAL_VECTOR_TYPE.

Referenced by EvaluateStartingState().

◆ EvaluateStartingCoM() [2/2]

int PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::EvaluateStartingCoM ( MAL_VECTOR(&, double)  BodyAnglesInit,
MAL_S3_VECTOR(&, double)  aStartingCOMState,
MAL_VECTOR(&, double)  aStartingWaistPosition,
FootAbsolutePosition InitLeftFootPosition,
FootAbsolutePosition InitRightFootPosition 
)

Evaluate Starting CoM for a given position.

Parameters
[in]BodyAnglesInitThe state vector used to compute the CoM.
[out]aStartingCOMStateThe CoM of the position specified.
[out]InitLeftFootPositionPosition of the InitLeftFootPosition in the same reference frame than the waist.
[out]InitRightFootPositionPosition of the InitRightFootPosition in the same reference frame than the waist

References MAL_VECTOR.

◆ EvaluateStartingState()

int ZMPPreviewControlWithMultiBodyZMP::EvaluateStartingState ( MAL_VECTOR(&, double)  BodyAngles,
MAL_S3_VECTOR(&, double)  aStartingCOMState,
MAL_S3_VECTOR(&, double)  aStartingZMPPosition,
MAL_VECTOR(&, double)  aStartingWaistPose,
FootAbsolutePosition InitLeftFootPosition,
FootAbsolutePosition InitRightFootPosition 
)

Compute the COM of the robot with the Joint values given in BodyAngles, velocities set to zero, and returns the values of the COM in aStaringCOMState. Assuming that the waist is at (0,0,0) it returns the associate initial values for the left and right foot.

Parameters
[in]BodyAngles4x4 matrix of the robot's root (most of the time, the waist) pose (position + orientation).
[out]aStartingCOMStateReturns the 3D position of the CoM for the current position of the robot.
[out]aStartingZMPPositionReturns the 3D position of the ZMP for the current position of the robot.
[out]InitLeftFootPositionReturns the position of the left foot in the waist coordinates frame.
[out]InitRightFootPositionReturns the position of the right foot in the waist coordinates frame.

References EvaluateStartingCoM(), MAL_S3_VECTOR, and MAL_VECTOR.

◆ FirstStageOfControl()

int ZMPPreviewControlWithMultiBodyZMP::FirstStageOfControl ( FootAbsolutePosition LeftFootPosition,
FootAbsolutePosition RightFootPosition,
COMState afCOMState 
)

First stage of the control, i.e.preview control on the CART model with delayed step parameters, Inverse Kinematics, and ZMP calculated with the multi body model. aCOMState will be updated with the new value of the COM computed by the card model.

Parameters
[in]LeftFootPositionThe position of the k+NL Left Foot position.
[in]RightFootPositionThe position of the k+NL Right Foot position.
[in]afCOMStateA COM position of reference, in this context, this will be the height of the waist.
Returns
If an error occurs returns a negative integer, 0 otherwise.

References j, PatternGeneratorJRL::PreviewControl::OneIterationOfPreview(), PatternGeneratorJRL::COMState_s::pitch, PatternGeneratorJRL::COMState_s::roll, PatternGeneratorJRL::COMState_s::x, PatternGeneratorJRL::COMState_s::y, PatternGeneratorJRL::COMState_s::yaw, PatternGeneratorJRL::COMState_s::z, ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY, ZMPCOM_TRAJECTORY_FULL, and ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY.

Referenced by OneGlobalStepOfControl(), and SetupIterativePhase().

◆ getComAndFootRealization()

ComAndFootRealization * PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::getComAndFootRealization ( )
inline

◆ GetDifferenceBetweenComAndWaist()

void PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::GetDifferenceBetweenComAndWaist ( double  lComAndWaist[3])

Returns the difference between the Waist and the CoM for a starting position.

References MAL_VECTOR, and MAL_VECTOR_TYPE.

◆ getHumanoidDynamicRobot()

CjrlHumanoidDynamicRobot * PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::getHumanoidDynamicRobot ( ) const
inline

Returns the object able to compute the dynamic parameters of the robot.

◆ GetLastCOMFromFirstStage()

COMState ZMPPreviewControlWithMultiBodyZMP::GetLastCOMFromFirstStage ( )

Returns the last element of the COM FIFO in the first stage of control

◆ GetStrategyForPCStages()

int ZMPPreviewControlWithMultiBodyZMP::GetStrategyForPCStages ( )

Get the strategy to handle the preview control stages.

◆ GetStrategyForStageActivation()

int ZMPPreviewControlWithMultiBodyZMP::GetStrategyForStageActivation ( )

Get the strategy for the activation of the stage.

◆ MAL_S4x4_MATRIX_TYPE() [1/2]

PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::MAL_S4x4_MATRIX_TYPE ( double  )

This method returns the current waist position in the COM reference frame. This can be used with the previous method to get the final Waist position.

Returns
A 4x4 matrix of double which includes the desired final Waist in the CoM phase position and orientation.

◆ MAL_S4x4_MATRIX_TYPE() [2/2]

PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::MAL_S4x4_MATRIX_TYPE ( double  )

This method returns the final COM pose matrix after the second stage of control.

Returns
A 4x4 matrix of double which includes the desired final CoM position and orientation.

◆ OneGlobalStepOfControl()

int ZMPPreviewControlWithMultiBodyZMP::OneGlobalStepOfControl ( FootAbsolutePosition LeftFootPosition,
FootAbsolutePosition RightFootPosition,
ZMPPosition NewZMPRefPos,
COMState finalCOMState,
MAL_VECTOR_TYPE(double) &  CurrentConfiguration,
MAL_VECTOR_TYPE(double) &  CurrentVelocity,
MAL_VECTOR_TYPE(double) &  CurrentAcceleration 
)

Perform a 5 ms step to generate the full set of angular positions. The main point of the preview control is to use the future to compute the current state needed for the robot. Therefore knowing that the future window needed is of size NL=SamplingPeriod * PreviewControlWindow, and that the algorithm is a two stages preview control, the foot position needs to be provided at k+NL, and the ZMP references at k+2*NL.

Parameters
[in]LeftFootPositionThe position of the k+NL Left Foot position.
[in]RightFootPositionThe position of the k+NL Right Foot position.
[in]NewZMPRefPosThe ZMP position at k + 2*NL.
[out]finalCOMStatereturns position, velocity and acceleration of the CoM after the second stage of control, i.e. the final value.
[out]CurrentConfigurationThe results is a state vector containing the articular positions.
[out]CurrentVelocityThe results is a state vector containing the speed.
[out]CurrentAccelerationThe results is a state vector containing the acceleration.

References c, CallToComAndFootRealization(), EvaluateMultiBodyZMP(), FirstStageOfControl(), MAL_S4x4_MATRIX_ACCESS_I_J, MAL_VECTOR_TYPE, SecondStageOfControl(), PatternGeneratorJRL::FootAbsolutePosition_t::stepType, PatternGeneratorJRL::COMState_s::x, PatternGeneratorJRL::FootAbsolutePosition_t::x, PatternGeneratorJRL::COMState_s::y, PatternGeneratorJRL::FootAbsolutePosition_t::y, PatternGeneratorJRL::COMState_s::z, PatternGeneratorJRL::FootAbsolutePosition_t::z, and ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY.

◆ SecondStageOfControl()

int ZMPPreviewControlWithMultiBodyZMP::SecondStageOfControl ( COMState refandfinal)

Second stage of the control, i.e. preview control on the Delta ZMP. COM correction, and computation of the final robot state (only the left and right legs).

Parameters
[out]refandfinalThe final position of the CoM.
Returns
If an error occurs returns a negative integer, 0 otherwise.

References i, PatternGeneratorJRL::PreviewControl::OneIterationOfPreview(), PatternGeneratorJRL::COMState_s::x, PatternGeneratorJRL::COMState_s::y, ZMPCOM_TRAJECTORY_FULL, and ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY.

Referenced by OneGlobalStepOfControl().

◆ setComAndFootRealization()

bool PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::setComAndFootRealization ( ComAndFootRealization *  aCFR)
inline

◆ setHumanoidDynamicRobot()

bool PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::setHumanoidDynamicRobot ( CjrlHumanoidDynamicRobot aHumanoidDynamicRobot)
inline
Parameters
[in]aHumanoidDynamicRobotan object able to compute dynamic parameters of the robot.

◆ SetPreviewControl()

void ZMPPreviewControlWithMultiBodyZMP::SetPreviewControl ( PreviewControl aPC)

◆ SetStrategyForPCStages()

void ZMPPreviewControlWithMultiBodyZMP::SetStrategyForPCStages ( int  Strategy)

Set the strategy to handle the preview control stages.

◆ SetStrategyForStageActivation()

void ZMPPreviewControlWithMultiBodyZMP::SetStrategyForStageActivation ( int  anAlgo)

Set the algorithm used for ZMP and CoM trajectory.

Parameters
[in]anAlgoThe algorithm to be used for ZMP and CoM trajectory generation. They are 3 possible values:
  • ZMPCOM_TRAJECTORY_FULL: Two preview control are computed. The first to generate a CoM trajectory based on the cart model. The second to correct this trajectory using the multibody ZMP.
  • ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY: Only the second stage is used. The first CoM trajectory is used by a different process. This allow to mix different algorithms (notable the quadratic problem with constraints).
  • ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY: Use only the first stage to generate the CoM trajectory. It is strongly adviced in this case, to not use the geometrical ZMP and CoM trajectory generation but an external CoM task.
Returns
Returns false if this is not possible.

References ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY, ZMPCOM_TRAJECTORY_FULL, and ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY.

◆ Setup()

int ZMPPreviewControlWithMultiBodyZMP::Setup ( deque< ZMPPosition > &  ZMPRefPositions,
deque< COMState > &  COMStates,
deque< FootAbsolutePosition > &  LeftFootPositions,
deque< FootAbsolutePosition > &  RightFootPositions 
)

Methods related to the preparation of the ZMP preview control with Multibody ZMP compensation.

Setup (Frontal Global), compute internally all the steps to get NL ZMP multibody values.

Parameters
[in]ZMPRefPositionsFIFO of the ZMP reference values.
[out]COMStatesFIFO of the COM reference positions (here only the height position is taken into account).
[in]LeftFootPositionsFIFO of the left foot positions computed by ZMPDiscretization (the object creating the ZMP and foot reference trajectories).
[in]RightFootPositionsidem than the previous one but for the right foot.

References PatternGeneratorJRL::PreviewControl::ComputeOptimalWeights(), CjrlDynamicRobot::currentAcceleration(), CjrlDynamicRobot::currentConfiguration(), CjrlDynamicRobot::currentVelocity(), i, MAL_VECTOR_TYPE, PatternGeneratorJRL::OptimalControllerSolver::MODE_WITHOUT_INITIALPOS, CjrlDynamicRobot::setProperty(), SetupFirstPhase(), and SetupIterativePhase().

◆ SetupFirstPhase()

int ZMPPreviewControlWithMultiBodyZMP::SetupFirstPhase ( deque< ZMPPosition > &  ZMPRefPositions,
deque< COMState > &  COMStates,
deque< FootAbsolutePosition > &  LeftFootPositions,
deque< FootAbsolutePosition > &  RightFootPositions 
)

Method to perform the First Phase. It initializes properly the internal fields of ZMPPreviewControlWithMultiBodyZMP for the setup phase.

  @param[in] ZMPRefPositions: FIFO of the ZMP reference values.
Parameters
[in]COMStatesFIFO of the COM reference positions (here only the height position is taken into account).
[in]LeftFootPositionsFIFO of the left foot positions computed by ZMPDiscretization (the object creating the ZMP and foot reference trajectories).
[in]RightFootPositionsidem than the previous one but for the right foot.

References CjrlDynamicRobot::currentAcceleration(), CjrlDynamicRobot::currentConfiguration(), CjrlDynamicRobot::currentVelocity(), i, MAL_VECTOR, and MAL_VECTOR_SIZE.

Referenced by Setup().

◆ SetupIterativePhase()

int ZMPPreviewControlWithMultiBodyZMP::SetupIterativePhase ( deque< ZMPPosition > &  ZMPRefPositions,
deque< COMState > &  COMStates,
deque< FootAbsolutePosition > &  LeftFootPositions,
deque< FootAbsolutePosition > &  RightFootPositions,
MAL_VECTOR_TYPE(double) &  CurrentConfiguration,
MAL_VECTOR_TYPE(double) &  CurrentVelocity,
MAL_VECTOR_TYPE(double) &  CurrentAcceleration,
int  localindex 
)

Method to call while feeding the 2 preview windows. It updates the first values of the Preview control This structure is needed if it is needed to modify BodyAngles according to the value of the COM.

Parameters
[in]ZMPRefPositionsFIFO of the ZMP reference values.
[out]COMStatesFIFO of the COM reference positions (here only the height position is taken into account).
[in]LeftFootPositionsFIFO of the left foot positions computed by ZMPDiscretization (the object creating the ZMP and foot reference trajectories).
[in]RightFootPositionsidem than the previous one but for the right foot.
[out]CurrentConfigurationThe position part of the state vector realizing the current CoM and feet position instance.
[out]CurrentVelocityThe velocity part of the state vector realizing the current CoM and feet position instance.
[out]CurrentAccelerationThe acceleration part of the state vector realizing the current CoM and feet position instance.
[in]localindexValue of the index which goes from 0 to 2 * m_NL.

References CallToComAndFootRealization(), EvaluateMultiBodyZMP(), FirstStageOfControl(), MAL_VECTOR_TYPE, PatternGeneratorJRL::COMState_s::x, PatternGeneratorJRL::FootAbsolutePosition_t::x, x, PatternGeneratorJRL::COMState_s::y, PatternGeneratorJRL::FootAbsolutePosition_t::y, PatternGeneratorJRL::COMState_s::z, and PatternGeneratorJRL::FootAbsolutePosition_t::z.

Referenced by Setup().

◆ UpdateTheZMPRefQueue()

void ZMPPreviewControlWithMultiBodyZMP::UpdateTheZMPRefQueue ( ZMPPosition  NewZMPRefPos)

Update the queue of ZMP reference value.

Member Data Documentation

◆ ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY

const int PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY =3
static

Constant to compute only the first stage.

Referenced by FirstStageOfControl(), OneGlobalStepOfControl(), and SetStrategyForStageActivation().

◆ ZMPCOM_TRAJECTORY_FULL

const int PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::ZMPCOM_TRAJECTORY_FULL =1
static

Constantes to define the strategy with the first and second stage.

Constant to compute the first and second stage.

Referenced by FirstStageOfControl(), SecondStageOfControl(), SetStrategyForStageActivation(), and ZMPPreviewControlWithMultiBodyZMP().

◆ ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY

const int PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY =2
static

Constant to compute only the second stage

Referenced by FirstStageOfControl(), SecondStageOfControl(), and SetStrategyForStageActivation().