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

#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. More...
 
virtual void restart (const yarp::sig::Vector &q0)
 Reinitializes the algorithm's internal state and resets the starting point. More...
 
virtual std::string getAlgoName ()
 Returns the algorithm's name. More...
 
- Public Member Functions inherited from iCub::iKin::SteepCtrl
 SteepCtrl (iKinChain &c, unsigned int _type, unsigned int _ctrlPose, double _Ts, double _Kp)
 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 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...
 
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_Kp () const
 Returns the gain. More...
 
virtual ~SteepCtrl ()
 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 Member Functions

void reset_Kp ()
 
virtual void inTargetFcn ()
 Method called whenever in target. More...
 
virtual yarp::sig::Vector update_qdot ()
 
- Protected Member Functions inherited from iCub::iKin::SteepCtrl
virtual void deadLockRecoveryFcn ()
 Method called whenever the watchDog is triggered. More...
 
virtual void printIter (const unsigned int verbose)
 Dumps warning or status messages. More...
 
- 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...
 

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::IntegratorI
 
unsigned int type
 
bool constrained
 
double Kp
 
yarp::sig::Vector qdot
 
yarp::sig::Vector gpm
 
- 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
 

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
cis the Chain object on which the control operates. Do not change Chain DOF from this point onwards!!
_typeone of the following: IKINCTRL_STEEP_JT => implements J transposed method. IKINCTRL_STEEP_PINV => implements J pseudo-inverse method.
_ctrlPoseone 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.
_Tsis the controller sample time.
_Kp0is the initial gain.
_Kp_incis the increasing factor.
_Kp_decis the drecreasing factor.
_Kp_maxis the maximum value for Kp.
_max_perf_incis 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
q0is 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: