A class derived from SteepCtrl implementing the variable gain algorithm. More...

#include <iKinInv.h>

Inheritance diagram for iCub::iKin::VarKpSteepCtrl:

Public Member Functions
	VarKpSteepCtrl (iKinChain &c, unsigned int _type, unsigned int _ctrlPose, double _Ts, double _Kp0, double _Kp_inc, double _Kp_dec, double _Kp_max, double _max_perf_inc)
	Constructor.

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

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

Public Member Functions inherited from iCub::iKin::SteepCtrl
	SteepCtrl (iKinChain &c, unsigned int _type, unsigned int _ctrlPose, double _Ts, double _Kp)
	Constructor.

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

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

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

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.

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

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

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

double	get_Kp () const
	Returns the gain.

virtual	~SteepCtrl ()
	Destructor.

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

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.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

virtual	~iKinCtrl ()
	Default destructor.

Protected Member Functions
void	reset_Kp ()

virtual void	inTargetFcn ()
	Method called whenever in target.

virtual yarp::sig::Vector	update_qdot ()

Protected Member Functions inherited from iCub::iKin::SteepCtrl
virtual void	deadLockRecoveryFcn ()
	Method called whenever the watchDog is triggered.

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

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

virtual void	update_state ()
	Updates the control state.

virtual void	watchDog ()
	Handles the watchDog.

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.

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.

Protected Attributes
double	Kp0

double	Kp_inc

double	Kp_dec

double	Kp_max

double	max_perf_inc

double	dist_old

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

ctrl::Integrator *	I

unsigned int	type

bool	constrained

double	Kp

yarp::sig::Vector	qdot

yarp::sig::Vector	gpm

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

Detailed Description

A class derived from SteepCtrl implementing the variable gain algorithm.

r(k)=dist(k)/dist(k-1) r(k)<1 => Kp(k)=Kp(k-1)*Kp_inc; r(k)>max_per_inc => Kp(k)=Kp(k-1)*Kp_dec;

Definition at line 512 of file iKinInv.h.

Constructor & Destructor Documentation

◆ VarKpSteepCtrl()

VarKpSteepCtrl::VarKpSteepCtrl	(	iKinChain &	c,
		unsigned int	_type,
		unsigned int	_ctrlPose,
		double	_Ts,
		double	_Kp0,
		double	_Kp_inc,
		double	_Kp_dec,
		double	_Kp_max,
		double	_max_perf_inc
	)

Constructor.

Parameters

c	is the Chain object on which the control operates. Do not change Chain DOF from this point onwards!!
_type	one of the following: IKINCTRL_STEEP_JT => implements J transposed method. IKINCTRL_STEEP_PINV => implements J pseudo-inverse method.
_ctrlPose	one of the following: IKINCTRL_POSE_FULL => complete pose control. IKINCTRL_POSE_XYZ => translational part of pose controlled. IKINCTRL_POSE_ANG => rotational part of pose controlled.
_Ts	is the controller sample time.
_Kp0	is the initial gain.
_Kp_inc	is the increasing factor.
_Kp_dec	is the drecreasing factor.
_Kp_max	is the maximum value for Kp.
_max_perf_inc	is the threshold value to decreas Kp.

Definition at line 413 of file iKinInv.cpp.

Member Function Documentation

◆ getAlgoName()

virtual std::string iCub::iKin::VarKpSteepCtrl::getAlgoName ( )

inlinevirtual

Returns the algorithm's name.

Returns: algorithm name as string. Shall be implemented.

Reimplemented from iCub::iKin::SteepCtrl.

Definition at line 558 of file iKinInv.h.

◆ inTargetFcn()

virtual void iCub::iKin::VarKpSteepCtrl::inTargetFcn ( )

inlineprotectedvirtual

Method called whenever in target.

Shall be implemented.

Reimplemented from iCub::iKin::SteepCtrl.

Definition at line 532 of file iKinInv.h.

◆ reset_Kp()

void VarKpSteepCtrl::reset_Kp ( )

protected

Definition at line 429 of file iKinInv.cpp.

◆ restart()

virtual void iCub::iKin::VarKpSteepCtrl::restart ( const yarp::sig::Vector & q0 )

inlinevirtual

Reinitializes the algorithm's internal state and resets the starting point.

Parameters

q0	is the new starting point.

Reimplemented from iCub::iKin::SteepCtrl.

Definition at line 557 of file iKinInv.h.

◆ update_qdot()

Vector VarKpSteepCtrl::update_qdot ( )

protectedvirtual

Reimplemented from iCub::iKin::SteepCtrl.

Definition at line 437 of file iKinInv.cpp.

Member Data Documentation

◆ dist_old

double iCub::iKin::VarKpSteepCtrl::dist_old

protected

Definition at line 529 of file iKinInv.h.

◆ Kp0

double iCub::iKin::VarKpSteepCtrl::Kp0

protected

Definition at line 523 of file iKinInv.h.

◆ Kp_dec

double iCub::iKin::VarKpSteepCtrl::Kp_dec

protected

Definition at line 525 of file iKinInv.h.

◆ Kp_inc

double iCub::iKin::VarKpSteepCtrl::Kp_inc

protected

Definition at line 524 of file iKinInv.h.

◆ Kp_max

double iCub::iKin::VarKpSteepCtrl::Kp_max

protected

Definition at line 526 of file iKinInv.h.

◆ max_perf_inc

double iCub::iKin::VarKpSteepCtrl::max_perf_inc

protected

Definition at line 527 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 Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ VarKpSteepCtrl()

Member Function Documentation

◆ getAlgoName()

◆ inTargetFcn()

◆ reset_Kp()

◆ restart()

◆ update_qdot()

Member Data Documentation

◆ dist_old

◆ Kp0

◆ Kp_dec

◆ Kp_inc

◆ Kp_max

◆ max_perf_inc