Implementes a ZMP controller as a force set point regulator. More...

#include <ZmpController.h>

Public Member Functions
	ZmpController (const int period, std::shared_ptr< ocra::Model > modelPtr, std::shared_ptr< ZmpControllerParams > parameters)
virtual	~ZmpController ()
bool	computeFootZMP (FOOT whichFoot, Eigen::VectorXd wrench, Eigen::Vector2d &footZMP, Eigen::VectorXd &wrenchInWorldRef, const double tolerance=1e-3)
bool	computeGlobalZMPFromSensors (Eigen::VectorXd rawLeftFootWrench, Eigen::VectorXd rawRightFootWrench, Eigen::Vector2d &globalZMP)
void	getFTSensorAdjointMatrix (FOOT whichFoot, Eigen::MatrixXd &T, Eigen::Vector3d &sensorPosition)
void	getLeftFootPosition (Eigen::Vector3d &leftFootPosition)
void	getRightFootPosition (Eigen::Vector3d &rightFootPosition)
bool	computehd (Eigen::Vector2d p, Eigen::Vector2d pd, Eigen::Vector2d &dhd)
void	computehdd (Eigen::Vector3d comPosition, Eigen::Vector2d globalZMP, Eigen::Vector2d &ddh)
void	computeh (Eigen::Vector2d prevComPosition, Eigen::Vector2d prevComVel, Eigen::Vector2d &intComPosition)
ocra::TaskState	createDesiredState (Eigen::Vector2d comRefPosition, Eigen::Vector2d comRefVelocity, Eigen::Vector2d comRefAcceleration)
Private Attributes
std::shared_ptr < ZmpControllerParams >	_params
std::shared_ptr< ocra::Model >	_model

Detailed Description

Implementes a ZMP controller as a force set point regulator.

Author:: Jorhabib Eljaik

krause2012stabilization

Warning:: Work in progress! This is still a barebone class in the process of being tested.

In krause2012stabilization a position-based ZMP controller was implemented by relating the desired ZMP \( \mathbf{p}_d \) to a desired force, given the relationship between CoM acceleration and the ZMP. When the dynamical effects of the vertical motion of the robot together with the change of angular momentum are neglected, the horizontal location of the ZMP \(\mathbf{p} = (p_x, p_y)\) can be computed as:

\begin{equation} \ddot{h} = \omega^2(\mathbf{h} - \mathbf{p}) \end{equation}

Where \(\omega = \sqrt{g/c_z}\) as done in kajita2003biped. Therefore the desired \(\mathbf{p}_d\) can be associated to a desired force on the CoM:

\[ F_d = m \omega^2 (\mathbf{h} - \mathbf{p}_d) \]

Where \(m\) is the total mass of the robot and a force set point regulator can be implemented as:

\[ \dot{\mathbf{h}}_d = k_f(F_d - F) \]

Where \(k_f > 0\) is a force control gain. Substituting simplifiedZMP we get:

\[ \dot{\mathbf{h}}_d = k_f m \omega^2(\mathbf{p} - \mathbf{p}_d) \]

See also:: computehd()

Constructor & Destructor Documentation

ZmpController::ZmpController	(	const int	period,
		std::shared_ptr< ocra::Model >	modelPtr,
		std::shared_ptr< ZmpControllerParams >	parameters
	)

Todo:: Deprecate this class

ZmpController::~ZmpController ( ) [virtual]

Member Function Documentation

bool ZmpController::computeFootZMP	(	FOOT	whichFoot,
		Eigen::VectorXd	wrench,
		Eigen::Vector2d &	footZMP,
		Eigen::VectorXd &	wrenchInWorldRef,
		const double	tolerance = `1e-3`
	)

Computes the ZMP for a single foot in world reference frame.

Assuming that \(\mathbf{p}\) is the position of the ZMP for a single foot, \(\mathbf{p}_s\) the position of a force torque (F/T) sensor at the foot, the ZMP position can be computed as:

\[ \left[\begin{array}{c}p_x \\ p_y \end{array}\right] = \frac{1}{f_z} \left[\begin{array}{cccccc} -p_{s_z} & 0 & p_{s_x} & 0 & -1 & 0 \\ 0 & -p_{s_z} & p_{s_y} & 1 & 0 & 0 \end{array}\right] \left[\begin{array}{c} \mathbf{f}\\ \mathbf{\tau} \end{array}\right] \]

Parameters:

	whichFoot	LEFT_FOOT or RIGHT_FOOT.
	wrench	External wrench on the foot as read by the F/T sensors.
[out]	footZMP	Foot ZMP in world reference frame.
[out]	wrenchInWorldRef	Transformed wrench in world reference frame.
	tolerance	Tolerance value below which the ZMP is considered null. Kajita2014Intro

Returns:: True if all operations proceed successfully.

bool ZmpController::computeGlobalZMPFromSensors	(	Eigen::VectorXd	rawLeftFootWrench,
		Eigen::VectorXd	rawRightFootWrench,
		Eigen::Vector2d &	globalZMP
	)

Computes the global ZMP for two feet in contact.

After obtaining the ZMP position for both feet \(\mathbf{p}_R\) and \(\mathbf{p}_L\) independently and expressed in the world reference frame, in the case where both feet are in contact with the ground (or just one), the global expression of the ZMP \(\mathbf{p}\) expressed in the world reference frame is:

\[ \left[\begin{array}{c} p_x\\ p_y \end{array}\right] = \frac{1}{f_{R_z} + f_{L_z}} \left[\begin{array}{cc} \mathbf{p}_R & \mathbf{p}_L \end{array}\right] \left[\begin{array}{c} f_{R_z}\\ f_{L_z} \end{array}\right] \]

Parameters:

rawLeftFootWrench	Raw left foot wrench as read from the sensors [force \| torque]
rawRightFootWrench	Raw right foot wrench as read from the sensors.
globalZMP	Global zmp in world reference frame considering both feet. @ Kajita2014Intro

Returns:: True if all operations succeed, false otherwise.

void ZmpController::computeh	(	Eigen::Vector2d	prevComPosition,
		Eigen::Vector2d	prevComVel,
		Eigen::Vector2d &	intComPosition
	)

bool ZmpController::computehd	(	Eigen::Vector2d	p,
		Eigen::Vector2d	pd,
		Eigen::Vector2d &	dhd
	)

Computes the instantaneous desired horizontal COM velocity for the corresponding desired zmp position.

Parameters:

	p	Horizonal measured zmp position
	pd	Horizontal desired zmp position
[out]	dhd	Horizonal CoM velocity \(\dot{\mathbf{h}}_d\)

void ZmpController::computehdd	(	Eigen::Vector3d	comPosition,
		Eigen::Vector2d	globalZMP,
		Eigen::Vector2d &	ddh
	)

ocra::TaskState ZmpController::createDesiredState	(	Eigen::Vector2d	comRefPosition,
		Eigen::Vector2d	comRefVelocity,
		Eigen::Vector2d	comRefAcceleration
	)

void ZmpController::getFTSensorAdjointMatrix	(	FOOT	whichFoot,
		Eigen::MatrixXd &	T,
		Eigen::Vector3d &	sensorPosition
	)

Retrieves the FT sensor adjoint matrix expressed in the world reference frame which multiplied by the local measurement of the sensor gives you the measurement in the world reference.

Parameters:

	whichFoot	LEFT_FOOT or RIGHT_FOOT
[out]	T	Adjoint matrix.

void ZmpController::getLeftFootPosition ( Eigen::Vector3d & leftFootPosition )

void ZmpController::getRightFootPosition ( Eigen::Vector3d & rightFootPosition )

Member Data Documentation

std::shared_ptr<ocra::Model> ZmpController::_model [private]

std::shared_ptr<ZmpControllerParams> ZmpController::_params [private]

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

ocra-wbi-plugins/ocra-icub-clients/walking-client/include/walking-client/ZmpController.h
ocra-wbi-plugins/ocra-icub-clients/walking-client/src/ZmpController.cpp

Public Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation