iCub-main
Public Member Functions | Protected Attributes | List of all members
iCub::iKin::LMCtrl_GPM Class Reference

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::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 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
_KGPM 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
_safeAreaRatiothe 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: