#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, Eigen::VectorXd &CurrentConfiguration, Eigen::VectorXd &CurrentVelocity, Eigen::VectorXd &CurrentAcceleration)

int	FirstStageOfControl (FootAbsolutePosition &LeftFootPosition, FootAbsolutePosition &RightFootPosition, COMState &afCOMState)

int	EvaluateMultiBodyZMP (int StartingIteration)

int	SecondStageOfControl (COMState &refandfinal)

int	EvaluateStartingState (Eigen::VectorXd &BodyAngles, Eigen::Vector3d &aStartingCOMState, Eigen::Vector3d &aStartingZMPPosition, Eigen::Matrix< double, 6, 1 > &aStartingWaistPose, FootAbsolutePosition &InitLeftFootPosition, FootAbsolutePosition &InitRightFootPosition)

int	EvaluateStartingCoM (Eigen::MatrixXd BodyAngles, Eigen::Vector3d &aStartingCOMState, Eigen::VectorXd &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, Eigen::VectorXd &CurrentConfiguration, Eigen::VectorXd &CurrentVelocity, Eigen::VectorXd &CurrentAcceleration, int localindex)

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

int	EvaluateStartingCoM (Eigen::VectorXd &BodyAnglesInit, Eigen::Vector3d &aStartingCOMState, Eigen::Matrix< double, 6, 1 > &aStartingWaistPosition, FootAbsolutePosition &InitLeftFootPosition, FootAbsolutePosition &InitRightFootPosition)

Eigen::Matrix4d	GetFinalDesiredCOMPose ()

Eigen::Matrix4d	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, Eigen::VectorXd &CurrentConfiguration, Eigen::VectorXd &CurrentVelocity, Eigen::VectorXd &CurrentAcceleration, unsigned long int IterationNumber, int StageOfTheAlgorithm)

void	SetPreviewControl (PreviewControl *aPC)

Setter and getter for the jrlHumanoidDynamicRobot object.
bool	setPinocchioRobot (PinocchioRobot *aPinocchioRobot)

PinocchioRobot *	getPinocchioRobot () 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 )

Constructor.

◆ ~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,
		Eigen::VectorXd &	CurrentConfiguration,
		Eigen::VectorXd &	CurrentVelocity,
		Eigen::VectorXd &	CurrentAcceleration,
		unsigned long 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]	aLeftFAP	Pose of the left foot (3D position + 2 euler angles)
[in]	aRightFAP	Pose of the right foot (3D position + 2 euler angles)
[out]	CurrentConfiguration	Returns the part of state vector corresponding to the position of the free floating, and the articular values.
[out]	CurrentVelocity	Returns the part of state vector corresponding to the velocity of the free floating and the articular values.
[out]	CurrentAcceleration	Returns the part of state vector corresponding to the acceleration of the free floating, and the articular values.
[in]	IterationNumber	Number of time slot realized so far.
[in]	StageOfTheAlgorithm	Indicates if this is the second stage of the preview control or the first one.

◆ 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]	ExtraCOMBuffer	Extra FIFO for the CoM positions.
[out]	ExtraZMPBuffer	Extra FIFO for the ZMP positions (for the stepping over first preview control).
[out]	ExtraZMPRefBuffer	Extra FIFO for the ZMP ref positions.

◆ 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.

◆ EvaluateStartingCoM() [1/2]

int PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::EvaluateStartingCoM	(	Eigen::MatrixXd	BodyAngles,
		Eigen::Vector3d &	aStartingCOMState,
		Eigen::VectorXd &	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

	BodyAngles	Vector of the joint values for the robot.
[out]	aStartingCOMState	Position of the CoM.
[out]	aWaistPosition	Position of the Waist.
[out]	InitLeftFootPosition	Position of the left foot in the waist coordinates frame.
[out]	InitRightFootPosition	Position of the right foot in the waist coordinates frame.

◆ EvaluateStartingCoM() [2/2]

int ZMPPreviewControlWithMultiBodyZMP::EvaluateStartingCoM	(	Eigen::VectorXd &	BodyAnglesInit,
		Eigen::Vector3d &	aStartingCOMState,
		Eigen::Matrix< double, 6, 1 > &	aStartingWaistPosition,
		FootAbsolutePosition &	InitLeftFootPosition,
		FootAbsolutePosition &	InitRightFootPosition
	)

Evaluate Starting CoM for a given position.

Parameters

[in]	BodyAnglesInit	The state vector used to compute the CoM.
[out]	aStartingCOMState	The CoM of the position specified.
[out]	InitLeftFootPosition	Position of the InitLeftFootPosition in the same reference frame than the waist.
[out]	InitRightFootPosition	Position of the InitRightFootPosition in the same reference frame than the waist

◆ EvaluateStartingState()

int ZMPPreviewControlWithMultiBodyZMP::EvaluateStartingState	(	Eigen::VectorXd &	BodyAngles,
		Eigen::Vector3d &	aStartingCOMState,
		Eigen::Vector3d &	aStartingZMPPosition,
		Eigen::Matrix< double, 6, 1 > &	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]	BodyAngles	4x4 matrix of the robot's root (most of the time, the waist) pose (position + orientation).
[out]	aStartingCOMState	Returns the 3D position of the CoM for the current position of the robot.
[out]	aStartingZMPPosition	Returns the 3D position of the ZMP for the current position of the robot.
[out]	InitLeftFootPosition	Returns the position of the left foot in the waist coordinates frame.
[out]	InitRightFootPosition	Returns the position of the right foot in the waist coordinates frame.

◆ 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]	LeftFootPosition	The position of the k+NL Left Foot position.
[in]	RightFootPosition	The position of the k+NL Right Foot position.
[in]	afCOMState	A 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.

◆ getComAndFootRealization()

ComAndFootRealization* PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::getComAndFootRealization ( )

inline

◆ GetCurrentPositionofWaistInCOMFrame()

Eigen::Matrix4d ZMPPreviewControlWithMultiBodyZMP::GetCurrentPositionofWaistInCOMFrame ( )

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.

◆ GetDifferenceBetweenComAndWaist()

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

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

◆ GetFinalDesiredCOMPose()

Eigen::Matrix4d ZMPPreviewControlWithMultiBodyZMP::GetFinalDesiredCOMPose ( )

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.

◆ GetLastCOMFromFirstStage()

COMState ZMPPreviewControlWithMultiBodyZMP::GetLastCOMFromFirstStage ( )

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

◆ getPinocchioRobot()

PinocchioRobot* PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::getPinocchioRobot ( ) const

inline

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

◆ 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.

◆ OneGlobalStepOfControl()

int ZMPPreviewControlWithMultiBodyZMP::OneGlobalStepOfControl	(	FootAbsolutePosition &	LeftFootPosition,
		FootAbsolutePosition &	RightFootPosition,
		ZMPPosition &	NewZMPRefPos,
		COMState &	finalCOMState,
		Eigen::VectorXd &	CurrentConfiguration,
		Eigen::VectorXd &	CurrentVelocity,
		Eigen::VectorXd &	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]	LeftFootPosition	The position of the k+NL Left Foot position.
[in]	RightFootPosition	The position of the k+NL Right Foot position.
[in]	NewZMPRefPos	The ZMP position at k + 2*NL.
[out]	finalCOMState	returns position, velocity and acceleration of the CoM after the second stage of control, i.e. the final value.
[out]	CurrentConfiguration	The results is a state vector containing the articular positions.
[out]	CurrentVelocity	The results is a state vector containing the speed.
[out]	CurrentAcceleration	The results is a state vector containing the acceleration.

◆ 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] refandfinal The final position of the CoM.

Returns: If an error occurs returns a negative integer, 0 otherwise.

◆ setComAndFootRealization()

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

inline

◆ setPinocchioRobot()

bool PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::setPinocchioRobot ( PinocchioRobot * aPinocchioRobot )

inline

Parameters

[in] aHumanoidDynamicRobot an object able to compute dynamic parameters of the robot.

◆ SetPreviewControl()

void ZMPPreviewControlWithMultiBodyZMP::SetPreviewControl ( PreviewControl * aPC )

Set the link to the preview control.

◆ 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] anAlgo The 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.

◆ 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]	ZMPRefPositions	FIFO of the ZMP reference values.
[out]	COMStates	FIFO of the COM reference positions (here only the height position is taken into account).
[in]	LeftFootPositions	FIFO of the left foot positions computed by ZMPDiscretization (the object creating the ZMP and foot reference trajectories).
[in]	RightFootPositions	idem than the previous one but for the right foot.

◆ 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.

Parameters

[in]	ZMPRefPositions	FIFO of the ZMP reference values.
[in]	COMStates	FIFO of the COM reference positions (here only the height position is taken into account).
[in]	LeftFootPositions	FIFO of the left foot positions computed by ZMPDiscretization (the object creating the ZMP and foot reference trajectories).
[in]	RightFootPositions	idem than the previous one but for the right foot.

◆ SetupIterativePhase()

int ZMPPreviewControlWithMultiBodyZMP::SetupIterativePhase	(	deque< ZMPPosition > &	ZMPRefPositions,
		deque< COMState > &	COMStates,
		deque< FootAbsolutePosition > &	LeftFootPositions,
		deque< FootAbsolutePosition > &	RightFootPositions,
		Eigen::VectorXd &	CurrentConfiguration,
		Eigen::VectorXd &	CurrentVelocity,
		Eigen::VectorXd &	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]	ZMPRefPositions	FIFO of the ZMP reference values.
[out]	COMStates	FIFO of the COM reference positions (here only the height position is taken into account).
[in]	LeftFootPositions	FIFO of the left foot positions computed by ZMPDiscretization (the object creating the ZMP and foot reference trajectories).
[in]	RightFootPositions	idem than the previous one but for the right foot.
[out]	CurrentConfiguration	The position part of the state vector realizing the current CoM and feet position instance.
[out]	CurrentVelocity	The velocity part of the state vector realizing the current CoM and feet position instance.
[out]	CurrentAcceleration	The acceleration part of the state vector realizing the current CoM and feet position instance.
[in]	localindex	Value of the index which goes from 0 to 2 * m_NL.

◆ 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.

◆ 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.

◆ ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY

const int PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY = 2

static

Constant to compute only the second stage

The documentation for this class was generated from the following files:

src/PreviewControl/ZMPPreviewControlWithMultiBodyZMP.hh
src/PreviewControl/ZMPPreviewControlWithMultiBodyZMP.cpp

Public Member Functions

Static Public Attributes

Detailed Description

Constructor & Destructor Documentation

◆ ZMPPreviewControlWithMultiBodyZMP()

◆ ~ZMPPreviewControlWithMultiBodyZMP()

Member Function Documentation

◆ CallMethod()

◆ CallToComAndFootRealization()

◆ CreateExtraCOMBuffer()

◆ EvaluateMultiBodyZMP()

◆ EvaluateStartingCoM() [1/2]

◆ EvaluateStartingCoM() [2/2]

◆ EvaluateStartingState()

◆ FirstStageOfControl()

◆ getComAndFootRealization()

◆ GetCurrentPositionofWaistInCOMFrame()

◆ GetDifferenceBetweenComAndWaist()

◆ GetFinalDesiredCOMPose()

◆ GetLastCOMFromFirstStage()

◆ getPinocchioRobot()

◆ GetStrategyForPCStages()

◆ GetStrategyForStageActivation()

◆ OneGlobalStepOfControl()

◆ SecondStageOfControl()

◆ setComAndFootRealization()

◆ setPinocchioRobot()

◆ SetPreviewControl()

◆ SetStrategyForPCStages()

◆ SetStrategyForStageActivation()

◆ Setup()

◆ SetupFirstPhase()

◆ SetupIterativePhase()

◆ UpdateTheZMPRefQueue()

Member Data Documentation

◆ ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY

◆ ZMPCOM_TRAJECTORY_FULL

◆ ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY