tuning.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 __TUNING_H__
#define __TUNING_H__

#include <mutex>
#include <condition_variable>

#include <yarp/os/all.h>
#include <yarp/dev/all.h>
#include <yarp/sig/all.h>
#include <iCub/ctrl/math.h>
#include <iCub/ctrl/pids.h>
#include <iCub/ctrl/kalman.h>
#include <iCub/ctrl/filters.h>
#include <iCub/ctrl/minJerkCtrl.h>
#include <iCub/ctrl/adaptWinPolyEstimator.h>


namespace iCub
{

namespace ctrl
{

class OnlineDCMotorEstimator
{
protected:
    yarp::sig::Matrix A;
    yarp::sig::Matrix F;
    yarp::sig::Vector B;
    yarp::sig::Matrix C;
    yarp::sig::Matrix Ct;
    yarp::sig::Matrix Q;
    yarp::sig::Matrix P;
    yarp::sig::Vector x;
    yarp::sig::Vector _x;
    double uOld;
    double Ts;
    double R;

public:
    OnlineDCMotorEstimator();

    bool init(const double Ts, const double Q, const double R,
              const double P0, const yarp::sig::Vector &x0);

    bool init(const double P0, const yarp::sig::Vector &x0);

    yarp::sig::Vector estimate(const double u, const double y);

    yarp::sig::Vector get_x() const { return _x; }

    yarp::sig::Matrix get_P() const { return P; }

    yarp::sig::Vector get_parameters() const { return _x.subVector(2,3); }
};


class OnlineStictionEstimator : public yarp::os::PeriodicThread
{
protected:
    yarp::dev::IControlMode    *imod;
    yarp::dev::IControlLimits  *ilim;
    yarp::dev::IEncoders       *ienc;
    yarp::dev::IPidControl     *ipid;
    yarp::dev::IPWMControl     *ipwm;
    yarp::dev::ICurrentControl *icur;

    std::mutex              mtx;
    std::mutex              mtx_doneEvent;
    std::condition_variable cv_doneEvent;
    yarp::sig::Vector       gamma;
    yarp::sig::Vector       stiction;
    yarp::os::Property      info;
    yarp::sig::Vector       done;

    AWLinEstimator   velEst;
    AWQuadEstimator  accEst;
    parallelPID     *pid;
    Integrator       intErr;
    minJerkTrajGen   trajGen;

    int    joint;
    double dpos_dV;
    double t0,T;
    double x_min,x_max;
    double x_pos,x_vel,x_acc;
    double Kp,Ki,Kd;
    double vel_thres,e_thres;
    double tg,xd_pos;
    double stiction_limit;
    bool   adapt,adaptOld;
    bool   configured;

    enum
    {
        rising,
        falling
    } state;

    void applyStictionLimit();
    bool threadInit();
    void run();
    void threadRelease();

    // prevent user from calling them directly
    bool start();
    void stop();

public:
    OnlineStictionEstimator();

    virtual bool configure(yarp::dev::PolyDriver &driver, const yarp::os::Property &options);

    virtual bool reconfigure(const yarp::os::Property &options);

    virtual bool isConfigured() const { return configured; }

    virtual bool startEstimation() { return PeriodicThread::start(); }

    virtual bool isDone();

    virtual bool waitUntilDone();

    virtual void stopEstimation() { PeriodicThread::stop(); }

    virtual bool getResults(yarp::sig::Vector &results);

    virtual bool getInfo(yarp::os::Property &info);

    virtual ~OnlineStictionEstimator() { }
};


class OnlineCompensatorDesign : public yarp::os::PeriodicThread
{
protected:
    OnlineDCMotorEstimator  plant;
    OnlineStictionEstimator stiction;
    Kalman                  predictor;

    yarp::dev::IControlMode     *imod;
    yarp::dev::IControlLimits   *ilim;
    yarp::dev::IEncoders        *ienc;
    yarp::dev::IPositionControl *ipos;
    yarp::dev::IPositionDirect  *idir;
    yarp::dev::IPidControl      *ipid;
    yarp::dev::IPWMControl      *ipwm;
    yarp::dev::ICurrentControl  *icur;
    yarp::dev::Pid              *pidCur;
    yarp::dev::Pid               pidOld;
    yarp::dev::Pid               pidNew;

    std::mutex mtx;
    std::mutex mtx_doneEvent;
    std::condition_variable cv_doneEvent;
    yarp::os::BufferedPort<yarp::sig::Vector> port;

    yarp::sig::Vector x0;
    yarp::sig::Vector meanParams;
    int               meanCnt;
    double            P0;

    int    joint;
    double t0,t1;
    double x_min,x_max,x_tg;
    double max_time,max_pwm,dpos_dV;
    double switch_timeout;
    int    measure_update_ticks;
    int    measure_update_cnt;    
    bool   controller_validation_ref_square;
    double controller_validation_ref_period;
    double controller_validation_ref_sustain_time;
    int    controller_validation_cycles_to_switch;
    int    controller_validation_num_cycles;
    bool   controller_validation_stiction_yarp;
    double controller_validation_stiction_up;
    double controller_validation_stiction_down;
    bool   pwm_pos;
    bool   configured;

    enum
    {
        plant_estimation,
        plant_validation,
        stiction_estimation,
        controller_validation
    } mode;

    void commandJoint(double &enc, double &u);
    bool threadInit();    
    void run();
    void threadRelease();

    // prevent user from calling them directly
    bool start();
    void stop();

public:
    OnlineCompensatorDesign();

    virtual bool configure(yarp::dev::PolyDriver &driver, const yarp::os::Property &options);

    virtual bool isConfigured() const { return configured; }

    virtual bool tuneController(const yarp::os::Property &options, yarp::os::Property &results);    

    virtual bool startPlantEstimation(const yarp::os::Property &options);

    virtual bool startPlantValidation(const yarp::os::Property &options);

    virtual bool startStictionEstimation(const yarp::os::Property &options);

    virtual bool startControllerValidation(const yarp::os::Property &options);

    virtual bool isDone();

    virtual bool waitUntilDone();

    virtual void stopOperation() { PeriodicThread::stop(); }

    virtual bool getResults(yarp::os::Property &results);

    virtual ~OnlineCompensatorDesign();
};

}

}

#endif