iKinInv.h

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
 * Copyright (C) 2006-2010 RobotCub Consortium
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD-3-Clause license. See the accompanying LICENSE file for
 * details.
*/
 
#ifndef __IKININV_H__
#define __IKININV_H__
 
#include <yarp/os/Property.h>
#include <iCub/ctrl/minJerkCtrl.h>
#include <iCub/ctrl/pids.h>
#include <iCub/iKin/iKinFwd.h>
 
#define IKINCTRL_STATE_RUNNING      0
#define IKINCTRL_STATE_INTARGET     1
#define IKINCTRL_STATE_DEADLOCK     2
 
#define IKINCTRL_POSE_FULL          0
#define IKINCTRL_POSE_XYZ           1
#define IKINCTRL_POSE_ANG           2
 
#define IKINCTRL_STEEP_JT           0
#define IKINCTRL_STEEP_PINV         1
 
#define IKINCTRL_RET_TOLX           0
#define IKINCTRL_RET_TOLSIZE        1
#define IKINCTRL_RET_TOLQ           2
#define IKINCTRL_RET_MAXITER        3
#define IKINCTRL_RET_EXHALT         4
                                    
#define IKINCTRL_DISABLED           -1
 
 
namespace iCub
{
 
namespace iKin
{
 
class iKinCtrl
{
private:
    // Default constructor: not implemented.
    iKinCtrl();
    // Copy constructor: not implemented.
    iKinCtrl(const iKinCtrl&);
    // Assignment operator: not implemented.
    iKinCtrl &operator=(const iKinCtrl&);
 
protected:
    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;
 
    virtual yarp::sig::Vector calc_e();
 
    virtual void update_state();
 
    virtual void watchDog();
 
    virtual yarp::sig::Vector checkVelocity(const yarp::sig::Vector &_qdot, double _Ts);
 
    virtual void inTargetFcn() = 0;
 
    virtual void deadLockRecoveryFcn() = 0;
 
    virtual void printIter(const unsigned int verbose=0) = 0;
 
    unsigned int printHandling(const unsigned int verbose=0);
 
public:
    iKinCtrl(iKinChain &c, unsigned int _ctrlPose);
 
    virtual void setChainConstraints(bool _constrained) { chain.setAllConstraints(_constrained); }
 
    virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, const unsigned int verbose=0) = 0;
 
    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);
 
    virtual bool test_convergence(const double tol_size) = 0;
 
    virtual void restart(const yarp::sig::Vector &q0);
 
    virtual std::string getAlgoName() = 0;
 
    void switchWatchDog(bool sw) { watchDogOn=sw; }
 
    virtual void setInTargetTol(double tol_x) { inTargetTol=tol_x; }
 
    virtual double getInTargetTol() const { return inTargetTol; }
 
    virtual void setWatchDogTol(double tol_q) { watchDogTol=tol_q; }
 
    virtual double getWatchDogTol() const { return watchDogTol; }
 
    virtual void setWatchDogMaxIter(int maxIter) { watchDogMaxIter=maxIter; }
 
    virtual int getWatchDogMaxIter() const { return watchDogMaxIter; }
 
    virtual bool isInTarget() { return dist()<inTargetTol; }
                                                                          
    int get_state() const { return state; }
 
    void set_ctrlPose(unsigned int _ctrlPose);
 
    unsigned int get_ctrlPose() const { return ctrlPose; }
 
    unsigned int get_dim() const { return dim; }
 
    unsigned int get_iter() const { return iter; }
 
    virtual yarp::sig::Vector get_x() const { return x; }
 
    virtual yarp::sig::Vector get_e() const { return e; }
 
    virtual void set_q(const yarp::sig::Vector &q0);
 
    virtual yarp::sig::Vector get_q() const { return q; }
 
    virtual yarp::sig::Vector get_grad() const { return grad; }
 
    virtual yarp::sig::Matrix get_J() const { return J; }
 
    virtual double dist() const { return yarp::math::norm(e); }
 
    virtual ~iKinCtrl() { }
};
 
 
class SteepCtrl : public iKinCtrl
{
private:
    // Default constructor: not implemented.
    SteepCtrl();
    // Copy constructor: not implemented.
    SteepCtrl(const SteepCtrl&);
    // Assignment operator: not implemented.
    SteepCtrl &operator=(const SteepCtrl&);
 
protected:
    double Ts;
    ctrl::Integrator *I;
 
    unsigned int type;
    bool constrained;
 
    double Kp;
    yarp::sig::Vector qdot;
    yarp::sig::Vector gpm;
 
    virtual void inTargetFcn()         { }
    virtual void deadLockRecoveryFcn() { }
    virtual void printIter(const unsigned int verbose);
    virtual yarp::sig::Vector update_qdot();
 
public:
    SteepCtrl(iKinChain &c, unsigned int _type, unsigned int _ctrlPose,
              double _Ts, double _Kp);
 
    virtual yarp::sig::Vector computeGPM() { return yarp::sig::Vector(dim,0.0); }
 
    virtual void setChainConstraints(bool _constrained);
    virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, const unsigned int verbose=0);
    virtual void restart(const yarp::sig::Vector &q0);
    virtual bool test_convergence(const double tol_size) { return yarp::math::norm(grad)<tol_size; }
    virtual std::string getAlgoName()                    { return "steepest-descent";              }
 
    void resetInt() { I->reset(q); }
 
    yarp::sig::Vector get_qdot() const { return qdot; }
 
    yarp::sig::Vector get_gpm() const { return gpm; }
 
    double get_Kp() const { return Kp; }
 
    virtual ~SteepCtrl();
};
 
 
class VarKpSteepCtrl : public SteepCtrl
{
private:
    // Default constructor: not implemented.
    VarKpSteepCtrl();
    // Copy constructor: not implemented.
    VarKpSteepCtrl(const VarKpSteepCtrl&);
    // Assignment operator: not implemented.
    VarKpSteepCtrl &operator=(const VarKpSteepCtrl&);
 
protected:
    double Kp0;
    double Kp_inc;
    double Kp_dec;
    double Kp_max;
    double max_perf_inc;
 
    double dist_old;
 
    void         reset_Kp();
    virtual void inTargetFcn() { reset_Kp(); }
    virtual yarp::sig::Vector update_qdot();
 
public:
    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);
 
    virtual void restart(const yarp::sig::Vector &q0) { SteepCtrl::restart(q0); reset_Kp(); }
    virtual std::string getAlgoName() { return "variable-gain-steepest-descent"; }
};
 
 
class LMCtrl : public iKinCtrl
{
private:
    // Default constructor: not implemented.
    LMCtrl();
    // Copy constructor: not implemented.
    LMCtrl(const LMCtrl&);
    // Assignment operator: not implemented.
    LMCtrl &operator=(const LMCtrl&);
 
protected:
    double Ts;
    ctrl::Integrator *I;
 
    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;
    
    virtual double update_mu();
    virtual void   inTargetFcn()         { }
    virtual void   deadLockRecoveryFcn() { }
    virtual void   printIter(const unsigned int verbose);
 
    virtual yarp::sig::Matrix pinv(const yarp::sig::Matrix &A, const double tol=0.0);
 
public:
    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);
 
    virtual yarp::sig::Vector computeGPM() { return yarp::sig::Vector(dim,0.0); }
 
    virtual void setChainConstraints(bool _constrained);
    virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, const unsigned int verbose=0);
    virtual void restart(const yarp::sig::Vector &q0);
    virtual bool test_convergence(const double tol_size) { return yarp::math::norm(grad)<tol_size; }
    virtual std::string getAlgoName()                    { return "levenberg-marquardt";           }
 
    void resetInt() { I->reset(q); }
 
    yarp::sig::Vector get_qdot() const { return qdot; }
 
    yarp::sig::Vector get_gpm() const { return gpm; }
 
    double get_mu() const { return mu; }
 
    void reset_mu();
 
    virtual ~LMCtrl();
};
 
 
class LMCtrl_GPM : public LMCtrl
{
private:
    // Default constructor: not implemented.
    LMCtrl_GPM();
    // Copy constructor: not implemented.
    LMCtrl_GPM(const LMCtrl_GPM&);
    // Assignment operator: not implemented.
    LMCtrl_GPM &operator=(const LMCtrl_GPM&);
 
protected:
    double safeAreaRatio;
    double K;
 
    yarp::sig::Vector span;
    yarp::sig::Vector alpha_min;
    yarp::sig::Vector alpha_max;
    yarp::sig::Matrix Eye;
 
public:
    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);
 
    virtual yarp::sig::Vector computeGPM();
 
    void set_K(const double _K) { K=_K>0 ? _K : -_K; }
 
    double get_K() const { return K; }
 
    void set_safeAreaRatio(const double _safeAreaRatio);
 
    double get_safeAreaRatio() const { return safeAreaRatio; }
};
 
 
class MultiRefMinJerkCtrl : public iKinCtrl
{
private:
    // Default constructor: not implemented.
    MultiRefMinJerkCtrl();
    // Copy constructor: not implemented.
    MultiRefMinJerkCtrl(const MultiRefMinJerkCtrl&);
    // Assignment operator: not implemented.
    MultiRefMinJerkCtrl &operator=(const MultiRefMinJerkCtrl&);
 
protected:
    ctrl::minJerkVelCtrl *mjCtrlJoint;
    ctrl::minJerkVelCtrl *mjCtrlTask;
    ctrl::Integrator     *I;
 
    yarp::sig::Vector q_set;
    yarp::sig::Vector qdot;
    yarp::sig::Vector xdot;
    yarp::sig::Matrix W;
    yarp::sig::Matrix Eye6;
 
    double Ts;
    double execTime;
    double gamma;
    double guardRatio;
 
    yarp::sig::Vector qGuard;
    yarp::sig::Vector qGuardMinInt, qGuardMinExt, qGuardMinCOG;
    yarp::sig::Vector qGuardMaxInt, qGuardMaxExt, qGuardMaxCOG;
 
    yarp::sig::Vector compensation;
 
    virtual void computeGuard();
    virtual void computeWeight();
    virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd,
                                      yarp::sig::Vector *xdot_set, const unsigned int verbose);
 
    virtual void inTargetFcn()         { }
    virtual void deadLockRecoveryFcn() { }
    virtual void printIter(const unsigned int verbose);
 
    // disable unused father's methods
    virtual bool test_convergence(const double)                                  { return false; }
    virtual yarp::sig::Vector iterate(yarp::sig::Vector&, const unsigned int)    { return yarp::sig::Vector(0); }
    virtual yarp::sig::Vector solve(yarp::sig::Vector&, const double,
                                    const int, const unsigned int, int*, bool *) { return yarp::sig::Vector(0); }
 
public:
    MultiRefMinJerkCtrl(iKinChain &c, unsigned int _ctrlPose, double _Ts, bool nonIdealPlant=false);
 
    virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd,
                                      const unsigned int verbose=0);
 
    virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd,
                                      yarp::sig::Vector &xdot_set, const unsigned int verbose=0);
 
    virtual void restart(const yarp::sig::Vector &q0);
 
    virtual std::string getAlgoName() { return "multi-referential-minimum-jerk-controllers"; }
 
    double get_guardRatio() const { return guardRatio; }
 
    double get_gamma() const { return gamma; }
 
    double get_execTime() const { return execTime; }
 
    yarp::sig::Vector get_qdot() const { return qdot; }
 
    yarp::sig::Vector get_xdot() const { return xdot; }
 
    void set_guardRatio(double _guardRatio);
 
    void set_gamma(double _gamma) { gamma=_gamma; }
 
    virtual void set_q(const yarp::sig::Vector &q0);
 
    double set_execTime(const double _execTime, const bool warn=false);
 
    void add_compensation(const yarp::sig::Vector &comp);
 
    void setPlantParameters(const yarp::os::Property &parameters,
                            const std::string &entryTag="dimension");
 
    virtual ~MultiRefMinJerkCtrl();
};
 
}
 
}
 
#endif