simple_optimization_problem.h

Go to the documentation of this file.

/*********************************************************************
 *
 *  Software License Agreement
 *
 *  Copyright (c) 2020,
 *  TU Dortmund - Institute of Control Theory and Systems Engineering.
 *  All rights reserved.
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <https://www.gnu.org/licenses/>.
 *
 *  Authors: Christoph Rösmann
 *********************************************************************/

#ifndef SRC_OPTIMIZATION_INCLUDE_CORBO_OPTIMIZATION_SIMPLE_OPTIMIZATION_PROBLEM_H_
#define SRC_OPTIMIZATION_INCLUDE_CORBO_OPTIMIZATION_SIMPLE_OPTIMIZATION_PROBLEM_H_

#include <corbo-optimization/optimization_problem_interface.h>

#include <functional>

namespace corbo {

class SimpleOptimizationProblem : public OptimizationProblemInterface
{
 public:
    SimpleOptimizationProblem() { _warn_if_not_specialized = false; }
    explicit SimpleOptimizationProblem(int parameter_dim)
    {
        _warn_if_not_specialized = false;
        resizeParameterVector(parameter_dim);
    }


    int getNonLsqObjectiveDimension() override = 0;
    int getLsqObjectiveDimension() override    = 0;
    int getObjectiveDimension() override       = 0;
    int getEqualityDimension() override = 0;
    int getInequalityDimension() override = 0;



    // implements interface method
    void computeValuesLsqObjective(Eigen::Ref<Eigen::VectorXd> values) override = 0;

    // implements interface method
    double computeValueNonLsqObjective() override = 0;

    // implements interface method
    void computeValuesEquality(Eigen::Ref<Eigen::VectorXd> values) override = 0;

    // implements interface method
    void computeValuesInequality(Eigen::Ref<Eigen::VectorXd> values) override = 0;



    void resizeParameterVector(int parameter_dim);



    void setX(const Eigen::Ref<const Eigen::VectorXd>& x) { _x = x; }
    Eigen::VectorXd& getXRef() { return _x; }
    const Eigen::VectorXd& getX() const { return _x; }

    void setLowerBounds(const Eigen::Ref<const Eigen::VectorXd>& lb) { _lb = lb; }
    Eigen::VectorXd& getLowerBoundsRef() { return _lb; }
    const Eigen::VectorXd& getLowerBounds() const { return _lb; }

    void setUpperBounds(const Eigen::Ref<const Eigen::VectorXd>& ub) { _ub = ub; }
    Eigen::VectorXd& getUpperBoundsRef() { return _ub; }
    const Eigen::VectorXd& getUpperBounds() const { return _ub; }

    // implements interface method
    double getParameterValue(int idx) override { return _x[idx]; }
    // implements interface method
    void setParameterValue(int idx, double x) override { _x[idx] = x; }

    // implements interface method
    void getBounds(Eigen::Ref<Eigen::VectorXd> lb, Eigen::Ref<Eigen::VectorXd> ub) override;
    // implements interface method
    void setBounds(const Eigen::Ref<const Eigen::VectorXd>& lb, const Eigen::Ref<const Eigen::VectorXd>& ub) override;
    // implements interface method
    double getLowerBound(int idx) override { return _lb[idx]; }
    // implements interface method
    double getUpperBound(int idx) override { return _ub[idx]; }
    // implements interface method
    void setLowerBound(int idx, double lb) override { _lb[idx] = lb; }
    // implements interface method
    void setUpperBound(int idx, double ub) override { _ub[idx] = ub; }

    void getParameterVector(Eigen::Ref<Eigen::VectorXd> x) override { x = _x; }
    void setParameterVector(const Eigen::Ref<const Eigen::VectorXd>& x) override { setX(x); }



    // implements interface method
    int getParameterDimension() override { return _x.size(); }

    // implements interface method
    void applyIncrement(const Eigen::Ref<const Eigen::VectorXd>& increment) override { _x += increment; }
    // implements interface method
    void applyIncrement(int idx, double increment) override { _x[idx] += increment; }

    // implements interface method
    void backupParameters() override { _x_backup.push_back(_x); }
    // implements interface method
    void restoreBackupParameters(bool keep_backup) override;
    // implements interface method
    void discardBackupParameters(bool all = false) override
    {
        _x_backup.pop_back();
        if (all)
        {
            while (!_x_backup.empty()) _x_backup.pop_back();
        }
    }



    // implements interface method
    bool isLeastSquaresProblem() const override = 0;


 private:
    Eigen::VectorXd _x;
    Eigen::VectorXd _lb;
    Eigen::VectorXd _ub;

    std::vector<Eigen::VectorXd> _x_backup;
};

class SimpleOptimizationProblemWithCallbacks : public SimpleOptimizationProblem
{
 public:
    SimpleOptimizationProblemWithCallbacks() {}
    explicit SimpleOptimizationProblemWithCallbacks(int param_dim) : SimpleOptimizationProblem(param_dim) {}


    void setObjectiveFunction(std::function<void(const Eigen::VectorXd&, Eigen::Ref<Eigen::VectorXd>)> obj_fun, int obj_dim, bool lsq_form = false);

    void setEqualityConstraint(std::function<void(const Eigen::VectorXd&, Eigen::Ref<Eigen::VectorXd>)> eq_fun, int eq_dim);

    void setInequalityConstraint(std::function<void(const Eigen::VectorXd&, Eigen::Ref<Eigen::VectorXd>)> ineq_fun, int ineq_dim);


    // implements interface method
    int getNonLsqObjectiveDimension() override { return (!_lsq_form && _obj_dim > 0) ? 1 : 0; }
    // implements interface method
    int getLsqObjectiveDimension() override { return _lsq_form ? _obj_dim : 0; }
    // implements interface method
    int getObjectiveDimension() override { return _obj_dim > 0 ? 1 : 0; }
    // implements interface method
    int getEqualityDimension() override { return _eq_dim; }
    // implements interface method
    int getInequalityDimension() override { return _ineq_dim; }

    // implements interface method
    bool isLeastSquaresProblem() const override { return _lsq_form; }

    // implmements interface method
    double computeValueNonLsqObjective() override
    {
        if (!_obj_fun || _lsq_form) return 0;
        Eigen::VectorXd values(getNonLsqObjectiveDimension());
        _obj_fun(getX(), values);
        return values.sum();
    }

    // implements interface method
    void computeValuesLsqObjective(Eigen::Ref<Eigen::VectorXd> values) override
    {
        if (_obj_fun) _obj_fun(getX(), values);
    }

    // implements interface method
    void computeValuesEquality(Eigen::Ref<Eigen::VectorXd> values) override
    {
        if (_eq_fun) _eq_fun(getX(), values);
    }

    // implements interface method
    void computeValuesInequality(Eigen::Ref<Eigen::VectorXd> values) override
    {
        if (_ineq_fun) _ineq_fun(getX(), values);
    }

 private:
    int _obj_dim   = 0;
    int _eq_dim    = 0;
    int _ineq_dim  = 0;
    bool _lsq_form = false;

    std::function<void(const Eigen::VectorXd&, Eigen::Ref<Eigen::VectorXd>)> _obj_fun;
    std::function<void(const Eigen::VectorXd&, Eigen::Ref<Eigen::VectorXd>)> _eq_fun;
    std::function<void(const Eigen::VectorXd&, Eigen::Ref<Eigen::VectorXd>)> _ineq_fun;
};

}  // namespace corbo

#endif  // SRC_OPTIMIZATION_INCLUDE_CORBO_OPTIMIZATION_SIMPLE_OPTIMIZATION_PROBLEM_H_