minJerkCtrl.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 __MINJERKCTRL_H__
#define __MINJERKCTRL_H__
 
#include <string>
#include <deque>
 
#include <yarp/os/Property.h>
#include <yarp/sig/Vector.h>
#include <yarp/sig/Matrix.h>
#include <iCub/ctrl/filters.h>
 
 
namespace iCub
{
 
namespace ctrl
{
 
class minJerkVelCtrl
{
protected:
    virtual void computeCoeffs() = 0;
 
public:
    virtual yarp::sig::Vector computeCmd(const double _T, const yarp::sig::Vector &e) = 0;
 
    virtual void reset(const yarp::sig::Vector &u0) = 0;
 
    virtual ~minJerkVelCtrl() { }
};
 
 
class minJerkVelCtrlForIdealPlant : public minJerkVelCtrl
{
private:
    // Default constructor: not implemented.
    minJerkVelCtrlForIdealPlant();
 
protected:
    ctrl::Filter *F;
 
    double Ts;
    double T;
    int dim;
 
    virtual void computeCoeffs();
 
public:
    minJerkVelCtrlForIdealPlant(const double _Ts, const int _dim);
 
    virtual yarp::sig::Vector computeCmd(const double _T, const yarp::sig::Vector &e);
 
    virtual void reset(const yarp::sig::Vector &u0);
 
    virtual ~minJerkVelCtrlForIdealPlant();
};
 
 
class minJerkVelCtrlForNonIdealPlant : public minJerkVelCtrl
{
private:
    // Default constructor: not implemented.
    minJerkVelCtrlForNonIdealPlant();
 
protected:
    yarp::sig::Vector Kp;
    yarp::sig::Vector Tz;
    yarp::sig::Vector Tw;
    yarp::sig::Vector Zeta;
    std::deque<ctrl::Filter*> F;
 
    double Ts;
    double T;
    int dim;
 
    virtual void computeCoeffs();
 
public:
    minJerkVelCtrlForNonIdealPlant(const double _Ts, const int _dim);
 
    virtual yarp::sig::Vector computeCmd(const double _T, const yarp::sig::Vector &e);
 
    virtual void reset(const yarp::sig::Vector &u0);
 
    virtual void setPlantParameters(const yarp::os::Property &parameters,
                                    const std::string &entryTag="dimension",
                                    const yarp::os::Bottle &ordering=yarp::os::Bottle());
 
    virtual void getPlantParameters(yarp::os::Property &parameters,
                                    const std::string &entryTag="dimension");
 
    virtual ~minJerkVelCtrlForNonIdealPlant();
};
 
 
class minJerkBaseGen
{
protected:
    Filter* posFilter;              // filter used to compute the position
    Filter* velFilter;              // filter used to compute the velocity
    Filter* accFilter;              // filter used to compute the acceleration
 
    yarp::sig::Vector pos;          // current position
    yarp::sig::Vector vel;          // current velocity
    yarp::sig::Vector acc;          // current acceleration
    yarp::sig::Vector lastRef;      // last reference position
 
    double Ts;                      // sample time in seconds
    double T;                       // trajectory reference time in seconds
    unsigned int dim;               // dimension of the controlled variable
 
    virtual void computeCoeffs()=0; // compute the filter coefficients
 
public:
    minJerkBaseGen(const unsigned int _dim, const double _Ts, const double _T);
 
    minJerkBaseGen(const yarp::sig::Vector &y0, const double _Ts, const double _T);
 
    minJerkBaseGen(const minJerkBaseGen &z);
 
    minJerkBaseGen& operator=(const minJerkBaseGen &z);
 
    virtual ~minJerkBaseGen();
 
    virtual void init(const yarp::sig::Vector &y0);
 
    virtual void computeNextValues(const yarp::sig::Vector &yd)=0;
 
    const yarp::sig::Vector& getPos() const { return pos; }
    
    const yarp::sig::Vector& getVel() const { return vel; }
 
    const yarp::sig::Vector& getAcc() const { return acc; }
 
    double getT() const { return T; }
 
    double getTs() const { return Ts; }
 
    bool setT(const double _T);
 
    bool setTs(const double _Ts);
};
 
 
class minJerkTrajGen : public minJerkBaseGen
{
protected:
    virtual void computeCoeffs();
 
public:
    minJerkTrajGen(const unsigned int _dim, const double _Ts, const double _T);
 
    minJerkTrajGen(const yarp::sig::Vector &y0, const double _Ts, const double _T);
 
    minJerkTrajGen(const minJerkTrajGen &z);
 
    minJerkTrajGen& operator=(const minJerkTrajGen &z);
 
    void computeNextValues(const yarp::sig::Vector &yd);
};
 
 
class minJerkRefGen : public minJerkBaseGen
{
protected:
    void computeCoeffs();
 
public:
    minJerkRefGen(const unsigned int _dim, const double _Ts, const double _T);
 
    minJerkRefGen(const yarp::sig::Vector &y0, const double _Ts, const double _T);
 
    minJerkRefGen(const minJerkRefGen &z);
 
    minJerkRefGen& operator=(const minJerkRefGen &z);
 
    void computeNextValues(const yarp::sig::Vector &y);
 
    virtual void computeNextValues(const yarp::sig::Vector &y,
                                   const yarp::sig::Vector &yd);
};
 
}
 
}
 
#endif