optimalControl.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 __OPTIMALCONTROL_H__
#define __OPTIMALCONTROL_H__
 
#include <yarp/sig/Vector.h>
#include <yarp/sig/Matrix.h>
#include <iCub/ctrl/math.h>
 
 
namespace iCub
{
 
namespace ctrl
{
 
class Riccati
{
protected:
    yarp::sig::Matrix A, At;
    yarp::sig::Matrix B, Bt;
    yarp::sig::Matrix V, VN;
    yarp::sig::Matrix P;
 
    yarp::sig::Matrix TN, lastT;
    yarp::sig::Matrix *Ti;
    yarp::sig::Matrix *Li;
    
    yarp::sig::Vector x;
 
    size_t n;
    size_t m;
    int N;
 
    bool verbose;
 
public:
     Riccati(const yarp::sig::Matrix &_A, const yarp::sig::Matrix &_B,
             const yarp::sig::Matrix &_V, const yarp::sig::Matrix &_P,
             const yarp::sig::Matrix &_VN, bool verb=false);
 
     yarp::sig::Matrix L(int step);
 
     yarp::sig::Matrix T(int step);
 
     void setProblemData(const yarp::sig::Matrix &_A, const yarp::sig::Matrix &_B,
                         const yarp::sig::Matrix &_V, const yarp::sig::Matrix &_P,
                         const yarp::sig::Matrix &_VN);
 
     void solveRiccati(int steps);
 
     yarp::sig::Vector doLQcontrol(int step, const yarp::sig::Vector &x);
 
     void doLQcontrol(int step, const yarp::sig::Vector &x, yarp::sig::Vector &ret);
 
     void setVerbose(bool verb=true);
};
 
}
 
}
 
 
#endif