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

#include <PreviewControl/ZMPPreviewControlWithMultiBodyZMP.hh>

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)
CjrlHumanoidDynamicRobot *	getHumanoidDynamicRobot () const
void	SetStrategyForPCStages (int Strategy)
int	GetStrategyForPCStages ()
void	CallMethod (std::string &Method, std::istringstream &astrm)
	Overloading method of SimplePlugin.
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 ( SimplePluginManager * lSPM )

Constructor.

References MAL_MATRIX_RESIZE, MAL_S4x4_MATRIX_ACCESS_I_J, PatternGeneratorJRL::OptimalControllerSolver::MODE_WITHOUT_INITIALPOS, and ZMPCOM_TRAJECTORY_FULL.

ZMPPreviewControlWithMultiBodyZMP::~ZMPPreviewControlWithMultiBodyZMP ( )

Destroctor.

Member Function Documentation

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

Overloading method of SimplePlugin.

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

Referenced by OneGlobalStepOfControl(), and SetupIterativePhase().

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.

References PatternGeneratorJRL::PreviewControl::OneIterationOfPreview().

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().

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:

	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.

Referenced by EvaluateStartingState().

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]	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

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

References EvaluateStartingCoM(), MAL_S3_VECTOR, and MAL_VECTOR.

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.

References 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().

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

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

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

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

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

COMState ZMPPreviewControlWithMultiBodyZMP::GetLastCOMFromFirstStage ( )

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

int ZMPPreviewControlWithMultiBodyZMP::GetStrategyForPCStages ( )

Get the strategy to handle the preview control stages.

int ZMPPreviewControlWithMultiBodyZMP::GetStrategyForStageActivation ( )

Get the strategy for the activation of the stage.

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.

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.

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

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.

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.

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

Referenced by OneGlobalStepOfControl().

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

bool PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::setHumanoidDynamicRobot ( CjrlHumanoidDynamicRobot * aHumanoidDynamicRobot ) [inline]

Parameters:

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

void ZMPPreviewControlWithMultiBodyZMP::SetPreviewControl ( PreviewControl * aPC )

Set the link to the preview control.

References PatternGeneratorJRL::PreviewControl::PreviewControlTime(), and PatternGeneratorJRL::PreviewControl::SamplingPeriod().

void ZMPPreviewControlWithMultiBodyZMP::SetStrategyForPCStages ( int Strategy )

Set the strategy to handle the preview control stages.

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.

References ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY, ZMPCOM_TRAJECTORY_FULL, and ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY.

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.

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

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.

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

Referenced by Setup().

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