A class derived from LMCtrl implementing the Gradient Projection Method according to the paper available here. More...

#include <iKinInv.h>

Inheritance diagram for iCub::iKin::LMCtrl_GPM:

Public Member Functions
	LMCtrl_GPM (iKinChain &c, unsigned int _ctrlPose, double _Ts, double _mu0, double _mu_inc, double _mu_dec, double _mu_min, double _mu_max, double _sv_thres=1e-6)
	Constructor. More...

virtual yarp::sig::Vector	computeGPM ()
	Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection Method, i.e. More...

void	set_K (const double _K)
	Sets the GPM gain (shall be positive). More...

double	get_K () const
	Returns the GPM gain (1.0 by default). More...

void	set_safeAreaRatio (const double _safeAreaRatio)
	Sets the safe area ratio [0-1], which is for each joint the ratio between the angle span within which the chain can be operated with 0-GPM and the overall angle span. More...

double	get_safeAreaRatio () const
	Returns the safe area ratio (0.9 by default). More...

Public Member Functions inherited from iCub::iKin::LMCtrl
	LMCtrl (iKinChain &c, unsigned int _ctrlPose, double _Ts, double _mu0, double _mu_inc, double _mu_dec, double _mu_min, double _mu_max, double _sv_thres=1e-6)
	Constructor. More...

virtual void	setChainConstraints (bool _constrained)
	Enables/Disables joint angles constraints. More...

virtual yarp::sig::Vector	iterate (yarp::sig::Vector &xd, const unsigned int verbose=0)
	Executes one iteration of the control algorithm. More...

virtual void	restart (const yarp::sig::Vector &q0)
	Reinitializes the algorithm's internal state and resets the starting point. More...

virtual bool	test_convergence (const double tol_size)
	Tests convergence by comparing the size of the algorithm internal structure (may be the gradient norm or the simplex size or whatever) to a certain tolerance. More...

virtual std::string	getAlgoName ()
	Returns the algorithm's name. More...

void	resetInt ()
	Resets integral status at the current joint angles. More...

yarp::sig::Vector	get_qdot () const
	Returns the actual derivative of joint angles. More...

yarp::sig::Vector	get_gpm () const
	Returns the actual value of Gradient Projected. More...

double	get_mu () const
	Returns the current weighting factor mu. More...

void	reset_mu ()
	Sets the weighting factor mu equal to the initial value. More...

virtual	~LMCtrl ()
	Destructor. More...

Public Member Functions inherited from iCub::iKin::iKinCtrl
	iKinCtrl (iKinChain &c, unsigned int _ctrlPose)
	Constructor. More...

virtual yarp::sig::Vector	solve (yarp::sig::Vector &xd, const double tol_size=IKINCTRL_DISABLED, const int max_iter=IKINCTRL_DISABLED, const unsigned int verbose=0, int exit_code=NULL, bool exhalt=NULL)
	Iterates the control algorithm trying to converge on the target. More...

void	switchWatchDog (bool sw)
	Switch on/off the watchDog mechanism to trigger deadLocks. More...

virtual void	setInTargetTol (double tol_x)
	Sets tolerance for in-target check (5e-3 by default). More...

virtual double	getInTargetTol () const
	Returns tolerance for in-target check. More...

virtual void	setWatchDogTol (double tol_q)
	Sets tolerance for watchDog check (1e-4 by default). More...

virtual double	getWatchDogTol () const
	Returns tolerance for watchDog check. More...

virtual void	setWatchDogMaxIter (int maxIter)
	Sets maximum number of iterations to trigger the watchDog (200 by default). More...

virtual int	getWatchDogMaxIter () const
	Returns maximum number of iterations to trigger the watchDog. More...

virtual bool	isInTarget ()
	Checks if the End-Effector is in target. More...

int	get_state () const
	Returns the algorithm's state. More...

void	set_ctrlPose (unsigned int _ctrlPose)
	Sets the state of Pose control settings. More...

unsigned int	get_ctrlPose () const
	Returns the state of Pose control settings. More...

unsigned int	get_dim () const
	Returns the number of Chain DOF. More...

unsigned int	get_iter () const
	Returns the number of performed iterations. More...

virtual yarp::sig::Vector	get_x () const
	Returns the actual cartesian position of the End-Effector. More...

virtual yarp::sig::Vector	get_e () const
	Returns the actual cartesian position error. More...

virtual void	set_q (const yarp::sig::Vector &q0)
	Sets the joint angles values. More...

virtual yarp::sig::Vector	get_q () const
	Returns the actual joint angles values. More...

virtual yarp::sig::Vector	get_grad () const
	Returns the actual gradient. More...

virtual yarp::sig::Matrix	get_J () const
	Returns the actual Jacobian used in computation. More...

virtual double	dist () const
	Returns the actual distance from the target in cartesian space (euclidean norm is used). More...

virtual	~iKinCtrl ()
	Default destructor. More...

Protected Attributes
double	safeAreaRatio

double	K

yarp::sig::Vector	span

yarp::sig::Vector	alpha_min

yarp::sig::Vector	alpha_max

yarp::sig::Matrix	Eye

Protected Attributes inherited from iCub::iKin::LMCtrl
double	Ts

ctrl::Integrator *	I

bool	constrained

yarp::sig::Vector	qdot

yarp::sig::Vector	gpm

yarp::sig::Matrix	pinvLM

double	mu

double	mu0

double	mu_inc

double	mu_dec

double	mu_min

double	mu_max

double	dist_old

double	svMin

double	svThres

Protected Attributes inherited from iCub::iKin::iKinCtrl
iKinChain &	chain

unsigned int	ctrlPose

yarp::sig::Vector	x_set

yarp::sig::Vector	x

yarp::sig::Vector	e

yarp::sig::Vector	q

yarp::sig::Matrix	J

yarp::sig::Matrix	Jt

yarp::sig::Matrix	pinvJ

yarp::sig::Vector	grad

yarp::sig::Vector	q_old

double	inTargetTol

double	watchDogTol

unsigned int	dim

unsigned int	iter

int	state

bool	watchDogOn

int	watchDogCnt

int	watchDogMaxIter

Additional Inherited Members
Protected Member Functions inherited from iCub::iKin::LMCtrl
virtual double	update_mu ()

virtual void	inTargetFcn ()
	Method called whenever in target. More...

virtual void	deadLockRecoveryFcn ()
	Method called whenever the watchDog is triggered. More...

virtual void	printIter (const unsigned int verbose)
	Dumps warning or status messages. More...

virtual yarp::sig::Matrix	pinv (const yarp::sig::Matrix &A, const double tol=0.0)

Protected Member Functions inherited from iCub::iKin::iKinCtrl
virtual yarp::sig::Vector	calc_e ()
	Computes the error according to the current controller settings (complete pose/translational/rotational part). More...

virtual void	update_state ()
	Updates the control state. More...

virtual void	watchDog ()
	Handles the watchDog. More...

virtual yarp::sig::Vector	checkVelocity (const yarp::sig::Vector &_qdot, double _Ts)
	Checks each joint velocity and sets it to zero if it steers the joint out of range. More...

unsigned int	printHandling (const unsigned int verbose=0)
	Method to be called within the printIter routine inherited by children in order to handle the highest word of verbose integer. More...

Detailed Description

A class derived from LMCtrl implementing the Gradient Projection Method according to the paper available here.

Definition at line 695 of file iKinInv.h.

Constructor & Destructor Documentation

◆ LMCtrl_GPM()

LMCtrl_GPM::LMCtrl_GPM	(	iKinChain &	c,
		unsigned int	_ctrlPose,
		double	_Ts,
		double	_mu0,
		double	_mu_inc,
		double	_mu_dec,
		double	_mu_min,
		double	_mu_max,
		double	_sv_thres = `1e-6`
	)

Constructor.

Definition at line 676 of file iKinInv.cpp.

Member Function Documentation

◆ computeGPM()

Vector LMCtrl_GPM::computeGPM ( )

virtual

Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection Method, i.e.

qdot=pinvJ*xdot+(I-pinvJ*J)*w

Returns: shall return the quantity (I-pinvJ*J)*w.

Note: This method shall be inherited and handled accordingly (here a vector of 0s is returned). To do that, J and pinvJ are already computed when this method is called. The LM-inverse matrix pinvLM is also available.

Reimplemented from iCub::iKin::LMCtrl.

Definition at line 708 of file iKinInv.cpp.

◆ get_K()

double iCub::iKin::LMCtrl_GPM::get_K ( ) const

inline

Returns the GPM gain (1.0 by default).

Returns: the GPM gain.

Definition at line 733 of file iKinInv.h.

◆ get_safeAreaRatio()

double iCub::iKin::LMCtrl_GPM::get_safeAreaRatio ( ) const

inline

Returns the safe area ratio (0.9 by default).

Returns: the safe area ratio.

Definition at line 747 of file iKinInv.h.

◆ set_K()

void iCub::iKin::LMCtrl_GPM::set_K ( const double _K )

inline

Sets the GPM gain (shall be positive).

Parameters

_K GPM gain.

Definition at line 727 of file iKinInv.h.

◆ set_safeAreaRatio()

void LMCtrl_GPM::set_safeAreaRatio ( const double _safeAreaRatio )

Sets the safe area ratio [0-1], which is for each joint the ratio between the angle span within which the chain can be operated with 0-GPM and the overall angle span.

Parameters

_safeAreaRatio the safe area ratio.

Definition at line 692 of file iKinInv.cpp.

Member Data Documentation

◆ alpha_max

yarp::sig::Vector iCub::iKin::LMCtrl_GPM::alpha_max

protected

Definition at line 711 of file iKinInv.h.

◆ alpha_min

yarp::sig::Vector iCub::iKin::LMCtrl_GPM::alpha_min

protected

Definition at line 710 of file iKinInv.h.

◆ Eye

yarp::sig::Matrix iCub::iKin::LMCtrl_GPM::Eye

protected

Definition at line 712 of file iKinInv.h.

◆ K

double iCub::iKin::LMCtrl_GPM::K

protected

Definition at line 707 of file iKinInv.h.

◆ safeAreaRatio

double iCub::iKin::LMCtrl_GPM::safeAreaRatio

protected

Definition at line 706 of file iKinInv.h.

◆ span

yarp::sig::Vector iCub::iKin::LMCtrl_GPM::span

protected

Definition at line 709 of file iKinInv.h.

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

icub-main/src/libraries/iKin/include/iCub/iKin/iKinInv.h
icub-main/src/libraries/iKin/src/iKinInv.cpp

Public Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ LMCtrl_GPM()

Member Function Documentation

◆ computeGPM()

◆ get_K()

◆ get_safeAreaRatio()

◆ set_K()

◆ set_safeAreaRatio()

Member Data Documentation

◆ alpha_max

◆ alpha_min

◆ Eye

◆ K

◆ safeAreaRatio

◆ span