iCub-main
iCub::iKin::LMCtrl Class Reference

#include <iKinInv.h>

Inheritance diagram for iCub::iKin::LMCtrl:

## Public Member Functions

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 yarp::sig::Vector computeGPM ()
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection Method, i.e. 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::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 Member Functions

virtual double update_mu ()

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

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

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

## Protected Attributes

double Ts

ctrl::IntegratorI

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
iKinChainchain

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 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 iKinCtrl implementing the Levenberg-Marquardt algorithm:

r(k)=dist(k)/dist(k-1) r(k)<1 => mu(k)=mu(k-1)*mu_dec; r(k)>1 => mu(k)=mu(k-1)*mu_inc;

H=Jt*J is the approximation of Hessian matrix

Definition at line 576 of file iKinInv.h.

## ◆ LMCtrl()

 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.

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. _Ts is the controller sample time. _mu0 is the initial value for weighting factor mu. _mu_inc is the increasing factor. _mu_dec is the drecreasing factor. _mu_min is the minimum value for mu. _mu_max is the maximum value for mu. _sv_thres is the minimum singular value under which the mu is constantly kept equal to _mu_max.

Definition at line 466 of file iKinInv.cpp.

## ◆ ~LMCtrl()

 LMCtrl::~LMCtrl ( )
virtual

Destructor.

Definition at line 669 of file iKinInv.cpp.

## ◆ computeGPM()

 virtual yarp::sig::Vector iCub::iKin::LMCtrl::computeGPM ( )
inlinevirtual

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 in iCub::iKin::LMCtrl_GPM.

Definition at line 645 of file iKinInv.h.

inlineprotectedvirtual

Method called whenever the watchDog is triggered.

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

Implements iCub::iKin::iKinCtrl.

Definition at line 609 of file iKinInv.h.

## ◆ get_gpm()

 yarp::sig::Vector iCub::iKin::LMCtrl::get_gpm ( ) const
inline

Returns the actual value of Gradient Projected.

Returns
the actual value of Gradient Projected.

Definition at line 668 of file iKinInv.h.

## ◆ get_mu()

 double iCub::iKin::LMCtrl::get_mu ( ) const
inline

Returns the current weighting factor mu.

Returns
the current weighting factor mu.

Definition at line 674 of file iKinInv.h.

## ◆ get_qdot()

 yarp::sig::Vector iCub::iKin::LMCtrl::get_qdot ( ) const
inline

Returns the actual derivative of joint angles.

Returns
the actual derivative of joint angles.

Definition at line 662 of file iKinInv.h.

## ◆ getAlgoName()

 virtual std::string iCub::iKin::LMCtrl::getAlgoName ( )
inlinevirtual

Returns the algorithm's name.

Returns
algorithm name as string. Shall be implemented.

Implements iCub::iKin::iKinCtrl.

Definition at line 651 of file iKinInv.h.

## ◆ inTargetFcn()

 virtual void iCub::iKin::LMCtrl::inTargetFcn ( )
inlineprotectedvirtual

Method called whenever in target.

Shall be implemented.

Implements iCub::iKin::iKinCtrl.

Definition at line 608 of file iKinInv.h.

## ◆ iterate()

 Vector LMCtrl::iterate ( yarp::sig::Vector & xd, const unsigned int verbose = 0 )
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.

Implements iCub::iKin::iKinCtrl.

Definition at line 570 of file iKinInv.cpp.

## ◆ pinv()

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

Definition at line 507 of file iKinInv.cpp.

## ◆ printIter()

 void LMCtrl::printIter ( const unsigned int verbose )
protectedvirtual

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.

Implements iCub::iKin::iKinCtrl.

Definition at line 638 of file iKinInv.cpp.

## ◆ reset_mu()

 void LMCtrl::reset_mu ( )

Sets the weighting factor mu equal to the initial value.

Definition at line 535 of file iKinInv.cpp.

## ◆ resetInt()

 void iCub::iKin::LMCtrl::resetInt ( )
inline

Resets integral status at the current joint angles.

Definition at line 656 of file iKinInv.h.

## ◆ restart()

 void LMCtrl::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 from iCub::iKin::iKinCtrl.

Definition at line 628 of file iKinInv.cpp.

## ◆ setChainConstraints()

 void LMCtrl::setChainConstraints ( bool _constrained )
virtual

Enables/Disables joint angles constraints.

Parameters
 _constrained if true then constraints are applied.

Reimplemented from iCub::iKin::iKinCtrl.

Definition at line 497 of file iKinInv.cpp.

## ◆ test_convergence()

 virtual bool iCub::iKin::LMCtrl::test_convergence ( const double tol_size )
inlinevirtual

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.

Implements iCub::iKin::iKinCtrl.

Definition at line 650 of file iKinInv.h.

## ◆ update_mu()

 double LMCtrl::update_mu ( )
protectedvirtual

Definition at line 543 of file iKinInv.cpp.

## ◆ constrained

 bool iCub::iKin::LMCtrl::constrained
protected

Definition at line 590 of file iKinInv.h.

## ◆ dist_old

 double iCub::iKin::LMCtrl::dist_old
protected

Definition at line 603 of file iKinInv.h.

## ◆ gpm

 yarp::sig::Vector iCub::iKin::LMCtrl::gpm
protected

Definition at line 593 of file iKinInv.h.

## ◆ I

 ctrl::Integrator* iCub::iKin::LMCtrl::I
protected

Definition at line 588 of file iKinInv.h.

## ◆ mu

 double iCub::iKin::LMCtrl::mu
protected

Definition at line 596 of file iKinInv.h.

## ◆ mu0

 double iCub::iKin::LMCtrl::mu0
protected

Definition at line 597 of file iKinInv.h.

## ◆ mu_dec

 double iCub::iKin::LMCtrl::mu_dec
protected

Definition at line 599 of file iKinInv.h.

## ◆ mu_inc

 double iCub::iKin::LMCtrl::mu_inc
protected

Definition at line 598 of file iKinInv.h.

## ◆ mu_max

 double iCub::iKin::LMCtrl::mu_max
protected

Definition at line 601 of file iKinInv.h.

## ◆ mu_min

 double iCub::iKin::LMCtrl::mu_min
protected

Definition at line 600 of file iKinInv.h.

## ◆ pinvLM

 yarp::sig::Matrix iCub::iKin::LMCtrl::pinvLM
protected

Definition at line 594 of file iKinInv.h.

## ◆ qdot

 yarp::sig::Vector iCub::iKin::LMCtrl::qdot
protected

Definition at line 592 of file iKinInv.h.

## ◆ svMin

 double iCub::iKin::LMCtrl::svMin
protected

Definition at line 604 of file iKinInv.h.

## ◆ svThres

 double iCub::iKin::LMCtrl::svThres
protected

Definition at line 605 of file iKinInv.h.

## ◆ Ts

 double iCub::iKin::LMCtrl::Ts
protected

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