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