#include <WalkingClient.h>

Collaboration diagram for WalkingClient:

Public Member Functions
	WalkingClient (std::shared_ptr< ocra::Model > modelPtr, const int loopPeriod)
virtual	~WalkingClient ()
bool	configure (yarp::os::ResourceFinder &rf)
void	printHelp ()
bool	queryMIQPSolution (const int miqpPeriod, const int miqpPreviewPeriod, const int clientPeriod, Eigen::VectorXd &preview, Eigen::VectorXd &tPreview)
bool	readFootWrench (FOOT whichFoot, Eigen::VectorXd &rawWrench)
std::vector< Eigen::Vector2d >	generateZMPTrajectoryTEST (const double tTrans, const double feetSeparation, const double timeStep, const int amplitudeFraction, const int N)
bool	getFeetSeparation (Eigen::Vector3d &sep)
bool	publish3dQuantity (yarp::os::BufferedPort< yarp::os::Bottle > &port, Eigen::Vector3d &value)
void	performZMPTest (ZmpTestType type)
void	performZMPPreviewTest (ZmpTestType type)
void	performZMPPreviewTestIntCoM (ZmpTestType type)
void	performMIQPTest ()
void	performSingleStepTest ()
void	steppingTest ()
std::string	composePortName (std::string portName)
void	findZMPPreviewControllerParams (yarp::os::ResourceFinder &rf)
void	findGeneralTestsParams (yarp::os::ResourceFinder &rf)
void	findCOMLinVelConstRefParams (yarp::os::ResourceFinder &rf)
void	findZMPConstRefParams (yarp::os::ResourceFinder &rf)
void	findSingleStepTestParams (yarp::os::ResourceFinder &rf)
void	findZMPVaryingReferenceParams (yarp::os::ResourceFinder &rf)
void	findMIQPParams (yarp::os::ResourceFinder &rf)
void	findSteppingTestParams (yarp::os::ResourceFinder &rf)
void	transformStdVectorToEigenVector (std::vector< Eigen::Vector2d > &fullTraj, int from, int Nc, VectorXd &output)
std::vector< Eigen::Vector2d >	generateZMPStepTrajectoryTEST (double feetSeparation, double period, double duration, double riseTime, double constantReferenceY)
std::vector< Eigen::Vector2d >	generateZMPSingleStepTrajectory (double period, double feetSeparation)
void	generateStepPattern ()
std::vector< Eigen::Vector2d >	generateZMPSteppingTrajectory ()
void	startSteppinMotherFucker (int &stepTrigger, double &error)
void	prepareAndsetDesiredCoMTaskState (Eigen::VectorXd comState, bool doSet)
Public Attributes
yarp::os::BufferedPort < yarp::sig::Vector >	portWrenchLeftFoot
yarp::os::BufferedPort < yarp::sig::Vector >	portWrenchRightFoot
Protected Member Functions
virtual bool	initialize ()
virtual void	release ()
virtual void	loop ()
Private Attributes
std::vector< FOOT >	_stepOrder
Eigen::MatrixXd	_stepTargets
Eigen::VectorXd	_stepTargetDurations
bool	_currentlyStepping
bool	_waitBeforeNextStep
double	_waitTimeStart
int	_currentStepIndex
std::vector< Eigen::Vector2d >	_steppingTrajectory
steppingTestParams	_steppingTestParams
std::shared_ptr< ZmpPreviewParams >	_zmpPreviewParams
std::shared_ptr < ZmpPreviewController >	_zmpPreviewController
std::shared_ptr < ocra_recipes::TaskConnection >	_comTask
std::shared_ptr< MIQPController >	_miqpController
std::shared_ptr< StepController >	_stepController
std::vector< Eigen::Vector2d >	_zmpTrajectory
std::vector< Eigen::Vector2d >	_singleStepTrajectory
ocra::TaskState	_desiredComState
MIQPParameters	_miqpParams
Eigen::VectorXd	_rawLeftFootWrench
Eigen::VectorXd	_rawRightFootWrench
Eigen::Vector2d	_globalZMP
Eigen::Vector2d	_previousCOM
Eigen::Vector2d	_previousCOMVel
std::string	_clientName
std::string	_robot
int	_period
bool	_isTestRun
std::string	_testType
ZmpTestType	_zmpTestType
std::string	_homeDataDir
double	_comYConstVel
double	_stopTimeConstComVel
double	_zmpYConstRef
double	_stopTimeConstZmp
double	_riseTimeConstZmp
double	_trajectoryDurationConstZmp
double	_constantReferenceY
singleStepTestParams	_singleStepTestParams
double	_tTrans
int	_numberOfTransitions
int	_amplitudeFraction
double	_stopTimeVaryingZmp
int	_k
yarp::os::BufferedPort < yarp::os::Bottle >	_zmpPort
yarp::os::BufferedPort < yarp::os::Bottle >	_dcomErrorPort
yarp::os::BufferedPort < yarp::os::Bottle >	_dComDesPort
yarp::os::BufferedPort < yarp::os::Bottle >	_dComCurPort
yarp::os::BufferedPort < yarp::os::Bottle >	_zmpDesPort
yarp::os::BufferedPort < yarp::os::Bottle >	_zmpCurPort
yarp::os::BufferedPort < yarp::os::Bottle >	_comCurrent
yarp::os::BufferedPort < yarp::os::Bottle >	_ddcomCurrent
yarp::os::BufferedPort < yarp::os::Bottle >	_ddcomFromZMP
Eigen::VectorXd	_hkkPrevious
bool	_firstLoop
Eigen::VectorXd	zmpRefInPreviewWindow
Eigen::VectorXd	comVelRefInPreviewWindow
Eigen::VectorXd	optimalU
Eigen::VectorXd	_X_kn
Eigen::VectorXd	_t_kn

Constructor & Destructor Documentation

WalkingClient::WalkingClient	(	std::shared_ptr< ocra::Model >	modelPtr,
		const int	loopPeriod
	)

WalkingClient::~WalkingClient ( ) [virtual]

Member Function Documentation

std::string WalkingClient::composePortName ( std::string portName )

Composes a port name with the client name as the suffix. This client name is assumed to be passed through command line options or configuration file as per the policies of yarp's Resource Finder.

Parameters:

portName Name of the port without backslashes. e.g. "zmpError:o"

Returns:: A client-name-prepended port name, e.g. "/walkingClient/zmpError:o"

bool WalkingClient::configure ( yarp::os::ResourceFinder & rf )

Takes all the parameters used by this client from configuration file and parses them through yarp's Resource Finder.

Details on group [ZMP_CONTROLLER_PARAMS] in walking-client.ini Options are: 0 - ZMP_CONSTANT_REFERENCE 1 - ZMP_VARYING_REFERENCE 2 - COM_LIN_VEL_CONSTANT_REFERENCE Each of these tests are used to evaluate the correct gains to be used at each level of the control loops. These trajectorie will be used during the tests specified through the option 'test' which takes the values "zmpPreview" or "zmpController". When using this client for the first time on a robot, the gains of the comTask in its corresponding taskSet file must be tuned first as well and later those for the ZmpController class. Therefore we recommend executing this client first as a way of testing the "low" level ComTask control in order to find good kp and kd. Do this by setting 'type' to 2. Data will be saved at the location you specify through the option 'homeDataDir'. After having a good COM velocity tracking at the task level, you want to test the tracking of the zmp controller by setting 'type' to 0. A constant zmp reference is given and the controller gains kfx, kfy, kdx and kdy must be tuned accordingly. Finally, the tracking of varying zmp reference can be tested which takes the zmp from left to right, while the robot stands on both feet.

void WalkingClient::findCOMLinVelConstRefParams ( yarp::os::ResourceFinder & rf )

void WalkingClient::findGeneralTestsParams ( yarp::os::ResourceFinder & rf )

void WalkingClient::findMIQPParams ( yarp::os::ResourceFinder & rf )

void WalkingClient::findSingleStepTestParams ( yarp::os::ResourceFinder & rf )

void WalkingClient::findSteppingTestParams ( yarp::os::ResourceFinder & rf )

void WalkingClient::findZMPConstRefParams ( yarp::os::ResourceFinder & rf )

void WalkingClient::findZMPPreviewControllerParams ( yarp::os::ResourceFinder & rf )

void WalkingClient::findZMPVaryingReferenceParams ( yarp::os::ResourceFinder & rf )

void WalkingClient::generateStepPattern ( )

Generates a straight line step pattern with step targets, step order, and step durations. Used for testing.

std::vector< Eigen::Vector2d > WalkingClient::generateZMPSingleStepTrajectory	(	double	period,
		double	feetSeparation
	)

std::vector< Eigen::Vector2d > WalkingClient::generateZMPSteppingTrajectory ( )

Generates a ZMP trajectory for the STEPPING_TEST. Uses the step pattern generated by generateStepPattern.

std::vector< Eigen::Vector2d > WalkingClient::generateZMPStepTrajectoryTEST	(	double	feetSeparation,
		double	period,
		double	duration,
		double	riseTime,
		double	constantReferenceY
	)

Generates a step-like zmp trajectory for testing/assessment purposes. This assumes a world reference frame with positive x axis pointing forward and positive z axis pointing up.

Parameters:

feetSeparation	Separation between the feet.
period	Thread period in ms.
duration	Duration of the trajectory in seconds.
riseTime	Time in seconds at which the step should happen.
constantReferenceY	Value of the

Returns:: Vector of horizontal ZMP positions.

std::vector< Eigen::Vector2d > WalkingClient::generateZMPTrajectoryTEST	(	const double	tTrans,
		const double	feetSeparation,
		const double	timeStep,
		const int	amplitudeFraction,
		const int	N
	)

Generates a sinusoidal zmp trajectory on the \(y\) expressed in the world reference frame. This is intended for testing purposes only.

Parameters:

tTrans	Time in which you want the ZMP to go from one foot to the other.
feetSeparation	Separation between the feet in meters.
timeStep	Desired time step.
amplitudeFraction	Fraction of the initial feet separation to determine max amplitude of movement.
N	Number of transitions (left to right or right to left).

Returns:: Trajectory of 2D ZMP points.

bool WalkingClient::getFeetSeparation ( Eigen::Vector3d & sep )

Returns the current feet separation vector;

Parameters:

sep	Separation vector.

Returns:: True if all operations proceed well. False otherwise.

bool WalkingClient::initialize ( ) [protected, virtual]

Todo:: Dummy initial COM states reference!

void WalkingClient::loop ( ) [protected, virtual]

void WalkingClient::performMIQPTest ( )

void WalkingClient::performSingleStepTest ( )

void WalkingClient::performZMPPreviewTest ( ZmpTestType type )

Performs a zmpPreviewTest for assessing and tuning of its parameters. This test has been succesfully performed with the iCub platform on Gazebo using the following configuration set in walking-client.ini:

| Parameter | Value | | ----------: | :-----| | Nc | 200 | | nw | 0.0 | | nb | 1.0 | | nu | 1e-6 |

Parameters:

type	Trajectory type.

See also:: ZmpTestType.

void WalkingClient::performZMPPreviewTestIntCoM ( ZmpTestType type )

void WalkingClient::performZMPTest ( ZmpTestType type )

Performs a zmp test for the specified type of trajectory.

Parameters:

type	See ZmpTestType for possible options.

Note:: To be deprecated as the zmp controller became unnecessary.

void WalkingClient::prepareAndsetDesiredCoMTaskState	(	Eigen::VectorXd	comState,
		bool	doSet
	)

Prepares an object of type ocra::TaskState with the com state passed to this method and when doSet is true, applies the control to the robot.

Parameters:

comState	6-dim CoM state vector (horizontal dynamics).
doSet	True if the desired state is to be set. False otherwise.

Todo:: Change input to this method to just com acceleration.

void WalkingClient::printHelp ( )

Prints a list of options accepted by the client.

bool WalkingClient::publish3dQuantity	(	yarp::os::BufferedPort< yarp::os::Bottle > &	port,
		Eigen::Vector3d &	value
	)

Write the ZMP error (externally computed, thus, any zmp related measurement) to a port.

Parameters:

zmpError \(\mathbf{p} - \mathbf{p_d}\)

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

bool WalkingClient::queryMIQPSolution	(	const int	miqpPeriod,
		const int	miqpPreviewPeriod,
		const int	clientPeriod,
		Eigen::VectorXd &	preview,
		Eigen::VectorXd &	tPreview
	)

If miqpPeriod/clientPeriod samples have passed, this method asks for the MIQP solution.

Parameters:

	miqpPeriod	Period of the MIQP thread.
	miqpPreviewPeriod	Period used to discretize the MIQP
	clientPeriod	Period of the current client thread.
[out]	Preview	solution from the MIQP.
[out]	tPreview	time vector in preview used by the client

Returns:: True if the right amount of time has passed, false otherwise.

bool WalkingClient::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 WalkingClient::release ( ) [protected, virtual]

void WalkingClient::startSteppinMotherFucker	(	int &	stepTrigger,
		double &	error
	)

Manages the step switching for the STEPPING_TEST.

void WalkingClient::steppingTest ( )

A test where the robot steps forward in a straight line. Only uses ZMP controller.

void WalkingClient::transformStdVectorToEigenVector	(	std::vector< Eigen::Vector2d > &	fullTraj,
		int	from,
		int	Nc,
		VectorXd &	output
	)

Takes an std::Vector of ZMP trajectories at time \(k\) and outputs the ZMP samples from time \(k\) until \(k + N_c\)s, i.e. the ZMP preview window.

Parameters:

fullTraj	Vector of horizontal ZMP positions.
from	Index in the vector corresponding to the current time instant.
Nc	Length of the preview window.
output	ZMP samples in the preview window.

Member Data Documentation

int WalkingClient::_amplitudeFraction [private]

std::string WalkingClient::_clientName [private]

yarp::os::BufferedPort<yarp::os::Bottle> WalkingClient::_comCurrent [private]

std::shared_ptr<ocra_recipes::TaskConnection> WalkingClient::_comTask [private]

double WalkingClient::_comYConstVel [private]

double WalkingClient::_constantReferenceY [private]

bool WalkingClient::_currentlyStepping [private]

int WalkingClient::_currentStepIndex [private]

yarp::os::BufferedPort<yarp::os::Bottle> WalkingClient::_dComCurPort [private]

yarp::os::BufferedPort<yarp::os::Bottle> WalkingClient::_dComDesPort [private]

yarp::os::BufferedPort<yarp::os::Bottle> WalkingClient::_dcomErrorPort [private]

yarp::os::BufferedPort<yarp::os::Bottle> WalkingClient::_ddcomCurrent [private]

yarp::os::BufferedPort<yarp::os::Bottle> WalkingClient::_ddcomFromZMP [private]

ocra::TaskState WalkingClient::_desiredComState [private]

bool WalkingClient::_firstLoop [private]

Eigen::Vector2d WalkingClient::_globalZMP [private]

Eigen::VectorXd WalkingClient::_hkkPrevious [private]

std::string WalkingClient::_homeDataDir [private]

bool WalkingClient::_isTestRun [private]

int WalkingClient::_k [private]

std::shared_ptr<MIQPController> WalkingClient::_miqpController [private]

MIQPParameters WalkingClient::_miqpParams [private]

int WalkingClient::_numberOfTransitions [private]

int WalkingClient::_period [private]

Eigen::Vector2d WalkingClient::_previousCOM [private]

Eigen::Vector2d WalkingClient::_previousCOMVel [private]

Eigen::VectorXd WalkingClient::_rawLeftFootWrench [private]

Eigen::VectorXd WalkingClient::_rawRightFootWrench [private]

double WalkingClient::_riseTimeConstZmp [private]

std::string WalkingClient::_robot [private]

singleStepTestParams WalkingClient::_singleStepTestParams [private]

std::vector<Eigen::Vector2d> WalkingClient::_singleStepTrajectory [private]

std::shared_ptr<StepController> WalkingClient::_stepController [private]

std::vector<FOOT> WalkingClient::_stepOrder [private]

steppingTestParams WalkingClient::_steppingTestParams [private]

std::vector<Eigen::Vector2d> WalkingClient::_steppingTrajectory [private]

Eigen::VectorXd WalkingClient::_stepTargetDurations [private]

Eigen::MatrixXd WalkingClient::_stepTargets [private]

double WalkingClient::_stopTimeConstComVel [private]

double WalkingClient::_stopTimeConstZmp [private]

double WalkingClient::_stopTimeVaryingZmp [private]

Eigen::VectorXd WalkingClient::_t_kn [private]

std::string WalkingClient::_testType [private]

double WalkingClient::_trajectoryDurationConstZmp [private]

double WalkingClient::_tTrans [private]

bool WalkingClient::_waitBeforeNextStep [private]

double WalkingClient::_waitTimeStart [private]

Eigen::VectorXd WalkingClient::_X_kn [private]

yarp::os::BufferedPort<yarp::os::Bottle> WalkingClient::_zmpCurPort [private]

yarp::os::BufferedPort<yarp::os::Bottle> WalkingClient::_zmpDesPort [private]

yarp::os::BufferedPort<yarp::os::Bottle> WalkingClient::_zmpPort [private]

std::shared_ptr<ZmpPreviewController> WalkingClient::_zmpPreviewController [private]

std::shared_ptr<ZmpPreviewParams> WalkingClient::_zmpPreviewParams [private]

ZmpTestType WalkingClient::_zmpTestType [private]

std::vector<Eigen::Vector2d> WalkingClient::_zmpTrajectory [private]

double WalkingClient::_zmpYConstRef [private]

Eigen::VectorXd WalkingClient::comVelRefInPreviewWindow [private]

Eigen::VectorXd WalkingClient::optimalU [private]

yarp::os::BufferedPort<yarp::sig::Vector> WalkingClient::portWrenchLeftFoot

yarp::os::BufferedPort<yarp::sig::Vector> WalkingClient::portWrenchRightFoot

Eigen::VectorXd WalkingClient::zmpRefInPreviewWindow [private]

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

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

Public Member Functions

Public Attributes

Protected Member Functions

Private Attributes

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation