#include <MIQPState.h>

Public Member Functions
	MIQPState (ocra::Model::Ptr robotModel, std::string robot)
virtual	~MIQPState ()
bool	initialize ()
void	updateStateVector ()
void	updateBaseOfSupportDescriptors (Eigen::Vector2d &a, Eigen::Vector2d &b, Eigen::Vector2d &alpha, Eigen::Vector2d &beta, unsigned int &delta, unsigned int &gamma, double thresholdChange)
void	updateHorizontalCoMState (Eigen::VectorXd &hk)
bool	isRobotInSS (FOOT &footInSS)
bool	isFootInContact (FOOT whichFoot, double FzThreshold, double PzThreshold)
bool	readFootWrench (FOOT whichFoot, Eigen::VectorXd &rawWrench)
void	getFullState (Eigen::VectorXd &xi)
Eigen::Vector3d	getLeftFootPosition ()
Eigen::Vector3d	getRightFootPosition ()
Private Attributes
Eigen::VectorXd	_xi_k
Eigen::Vector2d	_a
Eigen::Vector2d	_b
Eigen::Vector2d	_alpha
Eigen::Vector2d	_beta
unsigned int	_delta
unsigned int	_gamma
Eigen::VectorXd	_hk
ocra::Model::Ptr	_robotModel
std::string	_robot
Eigen::Vector3d	_r_foot_coord
Eigen::Vector3d	_l_foot_coord
yarp::os::BufferedPort < yarp::sig::Vector >	_portWrenchLeftFoot
yarp::os::BufferedPort < yarp::sig::Vector >	_portWrenchRightFoot
double	_FzThreshold
double	_PzThreshold
Friends
std::ostream &	operator<< (std::ostream &out, const MIQPState &state)

Constructor & Destructor Documentation

MIQPState::MIQPState	(	ocra::Model::Ptr	robotModel,
		std::string	robot
	)

MIQPState::~MIQPState ( ) [virtual]

Member Function Documentation

void MIQPState::getFullState ( Eigen::VectorXd & xi )

Copies in #xi the full state _xi_k.

Parameters:

[out] xi Full state.

Eigen::Vector3d MIQPState::getLeftFootPosition ( )

Retrieves the translational part of the left foot position w.r.t world reference frame.

Returns:: Left foot 3D position

Eigen::Vector3d MIQPState::getRightFootPosition ( )

Retrieves the translational part of the right foot position w.r.t. world reference frame.

Returns:: Right foot 3D position.

bool MIQPState::initialize ( )

Open and connects ports to F/T sensors in the feet of the robot and sets some thresholds.

Returns:: True if everything ends successfully.

bool MIQPState::isFootInContact	(	FOOT	whichFoot,
		double	FzThreshold,
		double	PzThreshold
	)

Determins if #whichFoot is in contact with the ground by checking the normal F/T measurement of the corresponding foot in combination with the \(z\) coordinate.

Parameters:

whichFoot	Foot for which you want to know whether it's in contact or not.
FzThreshold	Z Force threshold to consider whether the foot is in contact or not.
PzThreshold	Z position threshold to consider whether the foot is in contact or not.

Returns:: True if #whichFoot is in contact. False otherwise.

bool MIQPState::isRobotInSS ( FOOT & footInSS )

This method uses the normal measurements of the F/T sensors in the feet of the robot along with the \(z\) coordinates of both feet to identify whether the robot is in DS or not.

Parameters:

[out] If the robot is in SS, this variable contains the foot in SS.

Returns:: True if robot is found in SS, false if it is in DS.

bool MIQPState::readFootWrench	(	FOOT	whichFoot,
		Eigen::VectorXd &	rawWrench
	)

Reads the raw wrench published for the corresponding analog force/torque sensors in iCub's feet.

Parameters:

whichFoot	For which foot is the measurement read.
rawWrench	Result of the reading.

Returns:: True if reading is successful, false otherwise.

void MIQPState::updateBaseOfSupportDescriptors	(	Eigen::Vector2d &	a,
		Eigen::Vector2d &	b,
		Eigen::Vector2d &	alpha,
		Eigen::Vector2d &	beta,
		unsigned int &	delta,
		unsigned int &	gamma,
		double	thresholdChange
	)

Updates the base of support (BoS) descriptors, i.e. _a, _b, _alpha, _beta, _delta, _gamma.

Parameters:

[out]	a	Upper bounds variable.
[out]	b	Lower bounds variable.
[out]	alpha	Rising/falling edges of #a.
[out]	beta	Rising/falling edges of #b.
[out]	delta	Potential change from DS to SS.
[out]	gamma	SS/DS indicator.
	thresholdChange	Threshold used to detect a change in #a and #b.

void MIQPState::updateHorizontalCoMState ( Eigen::VectorXd & hk )

Updates the horizontal CoM state (horizonal position, velocity and acceleration).

Parameters:

[out] hk 6-dimensional CoM state vector.

void MIQPState::updateStateVector ( )

Calls the update of all the base of support descriptors and CoM state, and fills _xi_k with it. In order to retrieve, call MIQPState::getFullState().

Friends And Related Function Documentation

std::ostream& operator<<	(	std::ostream &	out,
		const MIQPState &	state
	)		`[friend]`

Operator to write the current state contents in a "pretty" way.

Member Data Documentation

Eigen::Vector2d MIQP::MIQPState::_a [private]

Upper bounds \(\mathbf{a} = (a_x, a_y)\). Its coordinates coincide with foot center coordinates.

Eigen::Vector2d MIQP::MIQPState::_alpha [private]

Rising/Falling edge indicator for \(\mathbf{a}\) (_a)

Eigen::Vector2d MIQP::MIQPState::_b [private]

Lower bounds \(\mathbf{b} = (b_x, b_y)\). It scoordinates coincide with foot center coordinates.

Eigen::Vector2d MIQP::MIQPState::_beta [private]

Rising/Falling edge indicator for \(\mathbf{b}\) (_b)

unsigned int MIQP::MIQPState::_delta [private]

Binary variable indicating potential changes in \(\mathbf{a}\) and \(\mathbf{b}\) from DS to SS.

If \(\alpha_x\) and \(\alpha_y\) change or \(\beta_x\) and \(\beta_y\) change, then \(\delta = 1\).

If \(\alpha_x\) and \(\beta_y\) or \(\beta_x\) and \(\alpha_y\) change, then \(\delta = 0\).

double MIQP::MIQPState::_FzThreshold [private]

unsigned int MIQP::MIQPState::_gamma [private]

Single/Double Support indicator. If the robot is in Double Support, then \(\gamma = 1\) If the robot is in Single Support, then \(\gamma = 0\)

Eigen::VectorXd MIQP::MIQPState::_hk [private]

Horizonal CoM state, i.e. \(\hat{\mathbf{h}}_k = [\mathbf{h}\;\dot{\mathbf{h}}\;\ddot{\mathbf{h}}]\).

Eigen::Vector3d MIQP::MIQPState::_l_foot_coord [private]

yarp::os::BufferedPort<yarp::sig::Vector> MIQP::MIQPState::_portWrenchLeftFoot [private]

yarp::os::BufferedPort<yarp::sig::Vector> MIQP::MIQPState::_portWrenchRightFoot [private]

double MIQP::MIQPState::_PzThreshold [private]

Eigen::Vector3d MIQP::MIQPState::_r_foot_coord [private]

std::string MIQP::MIQPState::_robot [private]

Robot prefix to be used (icubGazeboSim, icub)

ocra::Model::Ptr MIQP::MIQPState::_robotModel [private]

Robot model object necessary to update all the variables of the state vector. Robot state will be queried through this object.

Eigen::VectorXd MIQP::MIQPState::_xi_k [private]

Full state \(\mathbf{\xi}_k = (\mathbf{a} \; \mathbf{b} \; \mathbf{\alpha} \; \mathbf{\beta} \; \delta \; \gamma \; \mathbf{h} \; \mathbf{\dot{h}} \; \mathbf{\ddot{h}} )\)

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

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

Public Member Functions

Private Attributes

Friends

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation

Member Data Documentation