CostFunctionQuadratic.hpp

Go to the documentation of this file.

/**********************************************************************************************************************
This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
Licensed under the BSD-2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#pragma once

#include "CostFunction.hpp"
#include "term/TermBase.hpp"

namespace ct {
namespace optcon {


template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
class CostFunctionQuadratic : public CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
    typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
    typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;

    typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
    typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;

    typedef CostFunction<STATE_DIM, CONTROL_DIM, SCALAR> BASE;

    CostFunctionQuadratic();

    CostFunctionQuadratic(const CostFunctionQuadratic& arg);

    virtual CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const = 0;

    virtual ~CostFunctionQuadratic();

    virtual size_t addIntermediateTerm(std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> term,
        bool verbose = false);

    virtual size_t addFinalTerm(std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> term, bool verbose = false);


#ifdef CPPADCG
    virtual void addIntermediateADTerm(
        std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, ct::core::ADCGScalar>> term,
        bool verbose = false);

    virtual void addFinalADTerm(std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, ct::core::ADCGScalar>> term,
        bool verbose = false);
#endif

    virtual void loadFromConfigFile(const std::string& filename, bool verbose = false);

    virtual state_vector_t stateDerivativeIntermediate() = 0;

    virtual state_vector_t stateDerivativeTerminal() = 0;

    virtual state_matrix_t stateSecondDerivativeIntermediate() = 0;

    virtual state_matrix_t stateSecondDerivativeTerminal() = 0;

    virtual control_vector_t controlDerivativeIntermediate() = 0;

    virtual control_vector_t controlDerivativeTerminal();
    virtual control_matrix_t controlSecondDerivativeIntermediate() = 0;

    virtual control_matrix_t controlSecondDerivativeTerminal();

    virtual control_state_matrix_t stateControlDerivativeIntermediate() = 0;

    virtual control_state_matrix_t stateControlDerivativeTerminal();

    virtual void updateReferenceState(const state_vector_t& x_ref);

    virtual void updateFinalState(const state_vector_t& x_final);

    virtual void updateReferenceControl(const control_vector_t& u_ref);

    bool stateDerivativeIntermediateTest(bool verbose = false);

    bool controlDerivativeIntermediateTest(bool verbose = false);

    std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> getIntermediateTermById(const size_t id);

    std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> getFinalTermById(const size_t id);

    std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> getIntermediateTermByName(const std::string& name);

    std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> getFinalTermByName(const std::string& name);

    virtual void initialize();

protected:
    SCALAR evaluateIntermediateBase();

    SCALAR evaluateTerminalBase();

    state_vector_t stateDerivativeIntermediateBase();

    state_vector_t stateDerivativeTerminalBase();

    state_matrix_t stateSecondDerivativeIntermediateBase();

    state_matrix_t stateSecondDerivativeTerminalBase();

    control_vector_t controlDerivativeIntermediateBase();

    control_vector_t controlDerivativeTerminalBase();

    control_matrix_t controlSecondDerivativeIntermediateBase();

    control_matrix_t controlSecondDerivativeTerminalBase();

    control_state_matrix_t stateControlDerivativeIntermediateBase();

    control_state_matrix_t stateControlDerivativeTerminalBase();

    state_vector_t stateDerivativeIntermediateNumDiff();

    control_vector_t controlDerivativeIntermediateNumDiff();

    SCALAR eps_;

    bool doubleSidedDerivative_ = true;

    std::vector<std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>>> intermediateCostAnalytical_;

    std::vector<std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>>> finalCostAnalytical_;
};


}  // namespace optcon
}  // namespace ct