Abstract class for inverting chain's kinematics. More...

#include <iKinInv.h>

Inheritance diagram for iCub::iKin::iKinCtrl:

Public Member Functions
	iKinCtrl (iKinChain &c, unsigned int _ctrlPose)
	Constructor.

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

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

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.

virtual bool	test_convergence (const double tol_size)=0
	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.

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

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

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

virtual void	inTargetFcn ()=0
	Method called whenever in target.

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

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

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

Abstract class for inverting chain's kinematics.

Definition at line 64 of file iKinInv.h.

Constructor & Destructor Documentation

◆ iKinCtrl()

iKinCtrl::iKinCtrl	(	iKinChain &	c,
		unsigned int	_ctrlPose
	)

Constructor.

Parameters

c	is the Chain object on which the control operates. Do not change Chain DOF from this point onwards!!
_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.

Definition at line 34 of file iKinInv.cpp.

◆ ~iKinCtrl()

virtual iCub::iKin::iKinCtrl::~iKinCtrl ( )

inlinevirtual

Default destructor.

Definition at line 399 of file iKinInv.h.

Member Function Documentation

◆ calc_e()

Vector iKinCtrl::calc_e ( )

protectedvirtual

Computes the error according to the current controller settings (complete pose/translational/rotational part).

Note that x must be previously set.

Returns: the error.

Definition at line 88 of file iKinInv.cpp.

◆ checkVelocity()

Vector iKinCtrl::checkVelocity	(	const yarp::sig::Vector &	_qdot,
		double	_Ts
	)

protectedvirtual

Checks each joint velocity and sets it to zero if it steers the joint out of range.

Parameters

_qdot	is the joint velocities vector to be checked.
_Ts	is the joint velocities sample time.

Returns: the new velocity.

Definition at line 200 of file iKinInv.cpp.

◆ deadLockRecoveryFcn()

virtual void iCub::iKin::iKinCtrl::deadLockRecoveryFcn ( )

protectedpure virtual

Method called whenever the watchDog is triggered.

Put here the code to recover from deadLock. Shall be implemented.

Implemented in iCub::iKin::SteepCtrl, iCub::iKin::LMCtrl, and iCub::iKin::MultiRefMinJerkCtrl.

◆ dist()

virtual double iCub::iKin::iKinCtrl::dist ( ) const

inlinevirtual

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

Returns: actual distance from the target.

Definition at line 394 of file iKinInv.h.

◆ get_ctrlPose()

unsigned int iCub::iKin::iKinCtrl::get_ctrlPose ( ) const

inline

Returns the state of Pose control settings.

Returns: Pose control settings.

Definition at line 339 of file iKinInv.h.

◆ get_dim()

unsigned int iCub::iKin::iKinCtrl::get_dim ( ) const

inline

Returns the number of Chain DOF.

Returns: number of Chain DOF.

Definition at line 345 of file iKinInv.h.

◆ get_e()

virtual yarp::sig::Vector iCub::iKin::iKinCtrl::get_e ( ) const

inlinevirtual

Returns the actual cartesian position error.

Returns: actual cartesian position error.

Definition at line 363 of file iKinInv.h.

◆ get_grad()

virtual yarp::sig::Vector iCub::iKin::iKinCtrl::get_grad ( ) const

inlinevirtual

Returns the actual gradient.

Returns: actual gradient.

Definition at line 381 of file iKinInv.h.

◆ get_iter()

unsigned int iCub::iKin::iKinCtrl::get_iter ( ) const

inline

Returns the number of performed iterations.

Returns: number of performed iterations.

Definition at line 351 of file iKinInv.h.

◆ get_J()

virtual yarp::sig::Matrix iCub::iKin::iKinCtrl::get_J ( ) const

inlinevirtual

Returns the actual Jacobian used in computation.

Returns: actual Jacobian.

Definition at line 387 of file iKinInv.h.

◆ get_q()

virtual yarp::sig::Vector iCub::iKin::iKinCtrl::get_q ( ) const

inlinevirtual

Returns the actual joint angles values.

Returns: actual joint angles values.

Definition at line 375 of file iKinInv.h.

◆ get_state()

int iCub::iKin::iKinCtrl::get_state ( ) const

inline

Returns the algorithm's state.

Returns: algorithm's state: IKINCTRL_STATE_RUNNING IKINCTRL_STATE_INTARGET IKINCTRL_STATE_DEADLOCK

Definition at line 324 of file iKinInv.h.

◆ get_x()

virtual yarp::sig::Vector iCub::iKin::iKinCtrl::get_x ( ) const

inlinevirtual

Returns the actual cartesian position of the End-Effector.

Returns: actual cartesian position of the End-Effector.

Definition at line 357 of file iKinInv.h.

◆ getAlgoName()

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

pure virtual

Returns the algorithm's name.

Returns: algorithm name as string. Shall be implemented.

Implemented in iCub::iKin::SteepCtrl, iCub::iKin::VarKpSteepCtrl, iCub::iKin::LMCtrl, and iCub::iKin::MultiRefMinJerkCtrl.

◆ getInTargetTol()

virtual double iCub::iKin::iKinCtrl::getInTargetTol ( ) const

inlinevirtual

Returns tolerance for in-target check.

Returns: tolerance

Definition at line 284 of file iKinInv.h.

◆ getWatchDogMaxIter()

virtual int iCub::iKin::iKinCtrl::getWatchDogMaxIter ( ) const

inlinevirtual

Returns maximum number of iterations to trigger the watchDog.

Returns: iterations limit.

Definition at line 309 of file iKinInv.h.

◆ getWatchDogTol()

virtual double iCub::iKin::iKinCtrl::getWatchDogTol ( ) const

inlinevirtual

Returns tolerance for watchDog check.

Returns: tolerance

Definition at line 296 of file iKinInv.h.

◆ inTargetFcn()

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

protectedpure virtual

Method called whenever in target.

Shall be implemented.

Implemented in iCub::iKin::SteepCtrl, iCub::iKin::VarKpSteepCtrl, iCub::iKin::LMCtrl, and iCub::iKin::MultiRefMinJerkCtrl.

◆ isInTarget()

virtual bool iCub::iKin::iKinCtrl::isInTarget ( )

inlinevirtual

Checks if the End-Effector is in target.

Returns: true if in target.

Definition at line 315 of file iKinInv.h.

◆ iterate()

virtual yarp::sig::Vector iCub::iKin::iKinCtrl::iterate	(	yarp::sig::Vector &	xd,
		const unsigned int	verbose = `0`
	)

pure virtual

Executes one iteration of the control algorithm.

Parameters

xd is the End-Effector target Pose to be tracked.

verbose is a integer whose 32 bits are intended as follows. The lowest word (16 bits) progressively enables different levels of warning messages or status dump: the larger this value the more detailed is the output (0x####0000=>off by default). The highest word indicates how many successive calls to the dump shall be skipped in order to reduce the amount of information on the screen (ex: 0x0000####=>print all iterations, 0x0001####=>print one iteration and skip the next one, 0x0002####=> print one iteration and skip the next two).

Returns: current estimation of joints configuration. Shall be implemented.

Implemented in iCub::iKin::MultiRefMinJerkCtrl, iCub::iKin::SteepCtrl, and iCub::iKin::LMCtrl.

◆ printHandling()

unsigned int iKinCtrl::printHandling ( const unsigned int verbose = 0 )

protected

Method to be called within the printIter routine inherited by children in order to handle the highest word of verbose integer.

Definition at line 159 of file iKinInv.cpp.

◆ printIter()

virtual void iCub::iKin::iKinCtrl::printIter ( const unsigned int verbose = 0 )

protectedpure virtual

Dumps warning or status messages.

Parameters

verbose is a integer whose 32 bits are intended as follows. The lowest word (16 bits) progressively enables different levels of warning messages or status dump: the larger this value the more detailed is the output (0x####0000=>off by default). The highest word indicates how many successive calls to the dump shall be skipped in order to reduce the amount of information on the screen (ex: 0x0000####=>print all iterations, 0x0001####=>print one iteration and skip the next one, 0x0002####=> print one iteration and skip the next two).

Note: Angles are dumperd as degrees. Shall be implemented.

Implemented in iCub::iKin::SteepCtrl, iCub::iKin::LMCtrl, and iCub::iKin::MultiRefMinJerkCtrl.

◆ restart()

void iKinCtrl::restart ( const yarp::sig::Vector & q0 )

virtual

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

Parameters

q0	is the new starting point.

Reimplemented in iCub::iKin::SteepCtrl, iCub::iKin::VarKpSteepCtrl, iCub::iKin::LMCtrl, and iCub::iKin::MultiRefMinJerkCtrl.

Definition at line 191 of file iKinInv.cpp.

◆ set_ctrlPose()

void iKinCtrl::set_ctrlPose ( unsigned int _ctrlPose )

Sets the state of Pose control settings.

Parameters

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

Definition at line 64 of file iKinInv.cpp.

◆ set_q()

void iKinCtrl::set_q ( const yarp::sig::Vector & q0 )

virtual

Sets the joint angles values.

Parameters

q0	is the joint angles vector.

Reimplemented in iCub::iKin::MultiRefMinJerkCtrl.

Definition at line 74 of file iKinInv.cpp.

◆ setChainConstraints()

virtual void iCub::iKin::iKinCtrl::setChainConstraints ( bool _constrained )

inlinevirtual

Enables/Disables joint angles constraints.

Parameters

_constrained if true then constraints are applied.

Reimplemented in iCub::iKin::SteepCtrl, and iCub::iKin::LMCtrl.

Definition at line 183 of file iKinInv.h.

◆ setInTargetTol()

virtual void iCub::iKin::iKinCtrl::setInTargetTol ( double tol_x )

inlinevirtual

Sets tolerance for in-target check (5e-3 by default).

Parameters

tol_x is the tolerance

Definition at line 278 of file iKinInv.h.

◆ setWatchDogMaxIter()

virtual void iCub::iKin::iKinCtrl::setWatchDogMaxIter ( int maxIter )

inlinevirtual

Sets maximum number of iterations to trigger the watchDog (200 by default).

Parameters

maxIter is the iterations limit.

Definition at line 303 of file iKinInv.h.

◆ setWatchDogTol()

virtual void iCub::iKin::iKinCtrl::setWatchDogTol ( double tol_q )

inlinevirtual

Sets tolerance for watchDog check (1e-4 by default).

Parameters

tol_q is the tolerance

Definition at line 290 of file iKinInv.h.

◆ solve()

Vector iKinCtrl::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`
	)

virtual

Iterates the control algorithm trying to converge on the target.

Parameters

xd	is the End-Effector target Pose to be tracked.
tol_size	exits if test_convergence(tol_size) is true (tol_size<0 disables this check, default).
max_iter	exits if iter>=max_iter (max_iter<0 disables this check, default).
verbose	is a integer whose 32 bits are intended as follows. The lowest word (16 bits) progressively enables different levels of warning messages or status dump: the larger this value the more detailed is the output (0x####0000=>off by default). The highest word indicates how many successive calls to the dump shall be skipped in order to reduce the amount of information on the screen (ex: 0x0000####=>print all iterations, 0x0001####=>print one iteration and skip the next one, 0x0002####=> print one iteration and skip the next two).
exit_code	stores the exit code (NULL by default). Test for one of this: IKINCTRL_RET_TOLX IKINCTRL_RET_TOLSIZE IKINCTRL_RET_TOLQ IKINCTRL_RET_MAXITER IKINCTRL_RET_EXHALT
exhalt	checks for an external request to exit (NULL by default).

See also: setInTargetTol; getInTargetTol

Reimplemented in iCub::iKin::MultiRefMinJerkCtrl.

Definition at line 214 of file iKinInv.cpp.

◆ switchWatchDog()

void iCub::iKin::iKinCtrl::switchWatchDog ( bool sw )

inline

Switch on/off the watchDog mechanism to trigger deadLocks.

A deadLock is triggered whenerver norm(q(k)-q(k-1))<tol_q for a specified number of iterations.

Parameters

sw	control the watchDog activation.

Definition at line 272 of file iKinInv.h.

◆ test_convergence()

virtual bool iCub::iKin::iKinCtrl::test_convergence ( const double tol_size )

pure virtual

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.

Parameters

tol_size is tolerance to compare to. Shall be implemented.

Implemented in iCub::iKin::SteepCtrl, iCub::iKin::LMCtrl, and iCub::iKin::MultiRefMinJerkCtrl.

◆ update_state()

void iKinCtrl::update_state ( )

protectedvirtual

Updates the control state.

Definition at line 126 of file iKinInv.cpp.

◆ watchDog()

void iKinCtrl::watchDog ( )

protectedvirtual

Handles the watchDog.

Definition at line 172 of file iKinInv.cpp.

Member Data Documentation

◆ chain

iKinChain& iCub::iKin::iKinCtrl::chain

protected

Definition at line 75 of file iKinInv.h.

◆ ctrlPose

unsigned int iCub::iKin::iKinCtrl::ctrlPose

protected

Definition at line 76 of file iKinInv.h.

◆ dim

unsigned int iCub::iKin::iKinCtrl::dim

protected

Definition at line 92 of file iKinInv.h.

◆ e

yarp::sig::Vector iCub::iKin::iKinCtrl::e

protected

Definition at line 80 of file iKinInv.h.

◆ grad

yarp::sig::Vector iCub::iKin::iKinCtrl::grad

protected

Definition at line 85 of file iKinInv.h.

◆ inTargetTol

double iCub::iKin::iKinCtrl::inTargetTol

protected

Definition at line 89 of file iKinInv.h.

◆ iter

unsigned int iCub::iKin::iKinCtrl::iter

protected

Definition at line 93 of file iKinInv.h.

◆ J

yarp::sig::Matrix iCub::iKin::iKinCtrl::J

protected

Definition at line 82 of file iKinInv.h.

◆ Jt

yarp::sig::Matrix iCub::iKin::iKinCtrl::Jt

protected

Definition at line 83 of file iKinInv.h.

◆ pinvJ

yarp::sig::Matrix iCub::iKin::iKinCtrl::pinvJ

protected

Definition at line 84 of file iKinInv.h.

◆ q

yarp::sig::Vector iCub::iKin::iKinCtrl::q

protected

Definition at line 81 of file iKinInv.h.

◆ q_old

yarp::sig::Vector iCub::iKin::iKinCtrl::q_old

protected

Definition at line 87 of file iKinInv.h.

◆ state

int iCub::iKin::iKinCtrl::state

protected

Definition at line 95 of file iKinInv.h.

◆ watchDogCnt

int iCub::iKin::iKinCtrl::watchDogCnt

protected

Definition at line 98 of file iKinInv.h.

◆ watchDogMaxIter

int iCub::iKin::iKinCtrl::watchDogMaxIter

protected

Definition at line 99 of file iKinInv.h.

◆ watchDogOn

bool iCub::iKin::iKinCtrl::watchDogOn

protected

Definition at line 97 of file iKinInv.h.

◆ watchDogTol

double iCub::iKin::iKinCtrl::watchDogTol

protected

Definition at line 90 of file iKinInv.h.

◆ x

yarp::sig::Vector iCub::iKin::iKinCtrl::x

protected

Definition at line 79 of file iKinInv.h.

◆ x_set

yarp::sig::Vector iCub::iKin::iKinCtrl::x_set

protected

Definition at line 78 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

◆ iKinCtrl()

◆ ~iKinCtrl()

Member Function Documentation

◆ calc_e()

◆ checkVelocity()

◆ deadLockRecoveryFcn()

◆ dist()

◆ get_ctrlPose()

◆ get_dim()

◆ get_e()

◆ get_grad()

◆ get_iter()

◆ get_J()

◆ get_q()

◆ get_state()

◆ get_x()

◆ getAlgoName()

◆ getInTargetTol()

◆ getWatchDogMaxIter()

◆ getWatchDogTol()

◆ inTargetFcn()

◆ isInTarget()

◆ iterate()

◆ printHandling()

◆ printIter()

◆ restart()

◆ set_ctrlPose()

◆ set_q()

◆ setChainConstraints()

◆ setInTargetTol()

◆ setWatchDogMaxIter()

◆ setWatchDogTol()

◆ solve()

◆ switchWatchDog()

◆ test_convergence()

◆ update_state()

◆ watchDog()

Member Data Documentation

◆ chain

◆ ctrlPose

◆ dim

◆ e

◆ grad

◆ inTargetTol

◆ iter

◆ J

◆ Jt

◆ pinvJ

◆ q

◆ q_old

◆ state

◆ watchDogCnt

◆ watchDogMaxIter

◆ watchDogOn

◆ watchDogTol

◆ x

◆ x_set