final_state_cost.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_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_FUNCTIONS_FINAL_STATE_COST_H_
#define SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_FUNCTIONS_FINAL_STATE_COST_H_

#include <corbo-optimal-control/functions/quadratic_state_cost.h>

#include <corbo-core/reference_trajectory.h>
#include <corbo-optimal-control/functions/stage_functions.h>
#include <corbo-systems/system_dynamics_interface.h>

#include <memory>

namespace corbo {

class BaseQuadraticFinalStateCost : public FinalStageCost
{
 public:
    using Ptr      = std::shared_ptr<BaseQuadraticFinalStateCost>;
    using ConstPtr = std::shared_ptr<const BaseQuadraticFinalStateCost>;

    virtual const Eigen::MatrixXd& getWeightQf() const = 0;
};

class QuadraticFinalStateCost : public BaseQuadraticFinalStateCost
{
 public:
    using Ptr = std::shared_ptr<QuadraticFinalStateCost>;

    QuadraticFinalStateCost() { _Qf_sqrt = Eigen::MatrixXd::Constant(1, 1, 1); }

    QuadraticFinalStateCost(const Eigen::Ref<const Eigen::MatrixXd>& Qf, bool lsq_form) : _lsq_form(lsq_form) { setWeightQf(Qf); }

    FinalStageCost::Ptr getInstance() const override { return std::make_shared<QuadraticFinalStateCost>(); }

    int getNonIntegralStateTermDimension(int k) const override { return _lsq_form ? _Qf.rows() : 1; }
    bool isLsqFormNonIntegralStateTerm(int k) const override { return _lsq_form; }

    bool setWeightQf(const Eigen::Ref<const Eigen::MatrixXd>& Qf);
    bool setWeightQf(const Eigen::DiagonalMatrix<double, -1>& Qf);

    const Eigen::MatrixXd& getWeightQf() const override { return _Qf; }

    void setLsqForm(bool lsq_form) { _lsq_form = lsq_form; }

    void computeNonIntegralStateTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, Eigen::Ref<Eigen::VectorXd> cost) const override;

    bool update(int n, double t, ReferenceTrajectoryInterface& xref, ReferenceTrajectoryInterface& uref, ReferenceTrajectoryInterface* sref,
                bool single_dt, const Eigen::VectorXd& x0, StagePreprocessor::Ptr stage_preprocessor, const std::vector<double>& dts,
                const DiscretizationGridInterface* grid) override
    {
        bool dimension_modified = BaseQuadraticFinalStateCost::update(n, t, xref, uref, sref, single_dt, x0, stage_preprocessor, dts, grid);

        _x_ref      = &xref;
        _zero_x_ref = _x_ref->isZero();

        return dimension_modified;
    }

    bool checkParameters(int state_dim, int control_dim, std::stringstream* issues) const override;

#ifdef MESSAGE_SUPPORT
    bool fromMessage(const messages::FinalStageCost& message, std::stringstream* issues) override;
    void toMessage(messages::FinalStageCost& message) const override;
#endif

 protected:
    Eigen::MatrixXd _Qf_sqrt;
    Eigen::MatrixXd _Qf;
    Eigen::DiagonalMatrix<double, -1> _Qf_diag_sqrt;
    Eigen::DiagonalMatrix<double, -1> _Qf_diag;
    bool _diagonal_mode               = false;
    bool _diagonal_mode_intentionally = false;
    bool _lsq_form                    = true;

    const ReferenceTrajectoryInterface* _x_ref = nullptr;
    bool _zero_x_ref                           = false;
};
FACTORY_REGISTER_FINAL_STAGE_COST(QuadraticFinalStateCost)

class QuadraticFinalStateCostRiccati : public BaseQuadraticFinalStateCost
{
 public:
    QuadraticFinalStateCostRiccati() { _Qf_sqrt = Eigen::MatrixXd::Constant(1, 1, 1); }

    QuadraticFinalStateCostRiccati(SystemDynamicsInterface::Ptr dynamics, const Eigen::Ref<const Eigen::MatrixXd>& Q,
                                   const Eigen::Ref<const Eigen::MatrixXd>& R)
    {
        setSystemDynamics(dynamics);
        setWeightQ(Q);
        setWeightR(R);
    }

    FinalStageCost::Ptr getInstance() const override { return std::make_shared<QuadraticFinalStateCostRiccati>(); }

    int getNonIntegralStateTermDimension(int k) const override { return 1; }

    void setSystemDynamics(SystemDynamicsInterface::Ptr dynamics)
    {
        _are_solved_before = false;
        _dynamics          = dynamics;
    }

    bool setWeightQ(const Eigen::Ref<const Eigen::MatrixXd>& Q);
    bool setWeightQ(const Eigen::DiagonalMatrix<double, -1>& Q);
    bool setWeightR(const Eigen::Ref<const Eigen::MatrixXd>& R);
    bool setWeightR(const Eigen::DiagonalMatrix<double, -1>& R);

    void setLsqForm(bool lsq_form) { _lsq_form = lsq_form; }

    const Eigen::MatrixXd& getWeightQf() const override { return _Qf; }

    void computeNonIntegralStateTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, Eigen::Ref<Eigen::VectorXd> cost) const override;

    bool update(int n, double t, ReferenceTrajectoryInterface& xref, ReferenceTrajectoryInterface& uref, ReferenceTrajectoryInterface* sref,
                bool single_dt, const Eigen::VectorXd& x0, StagePreprocessor::Ptr stage_preprocessor, const std::vector<double>& dts,
                const DiscretizationGridInterface* /*grid*/) override;

    bool checkParameters(int state_dim, int control_dim, std::stringstream* issues) const override;

#ifdef MESSAGE_SUPPORT
    bool fromMessage(const messages::FinalStageCost& message, std::stringstream* issues) override;
    void toMessage(messages::FinalStageCost& message) const override;
#endif

 protected:
    bool computeWeightQfSqrt();

    Eigen::MatrixXd _Q;
    Eigen::MatrixXd _R;
    bool _Q_diagonal_mode_intentionally = false;
    bool _R_diagonal_mode_intentionally = false;

    Eigen::MatrixXd _Qf_sqrt;
    Eigen::MatrixXd _Qf;
    SystemDynamicsInterface::Ptr _dynamics;
    Eigen::VectorXd _steady_state_x;
    Eigen::VectorXd _steady_state_u;
    bool _lsq_form          = true;
    bool _are_solved_before = false;  // algebraic riccati equation solved before (e.g., for linear systems solving once is sufficient)

    const ReferenceTrajectoryInterface* _x_ref = nullptr;
    bool _zero_x_ref                           = false;
};
FACTORY_REGISTER_FINAL_STAGE_COST(QuadraticFinalStateCostRiccati)

}  // namespace corbo

#endif  // SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_FUNCTIONS_FINAL_STATE_COST_H_