OptConSolver.h

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 <ct/optcon/problem/ContinuousOptConProblem.h>


namespace ct {
namespace optcon {


template <typename DERIVED,
    typename POLICY,
    typename SETTINGS,
    size_t STATE_DIM,
    size_t CONTROL_DIM,
    typename SCALAR = double,
    bool CONTINUOUS = true>
class OptConSolver
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef typename std::conditional<CONTINUOUS,
        ContinuousOptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>,
        DiscreteOptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>>::type OptConProblem_t;

    static const size_t STATE_D = STATE_DIM;
    static const size_t CONTROL_D = CONTROL_DIM;

    typedef POLICY Policy_t;
    typedef SETTINGS Settings_t;
    typedef DERIVED Derived;
    typedef SCALAR Scalar_t;

    OptConSolver() {}
    virtual ~OptConSolver() {}
    virtual void setProblem(const OptConProblem_t& optConProblem)
    {
        optConProblem.verify();

        changeTimeHorizon(optConProblem.getTimeHorizon());
        changeInitialState(optConProblem.getInitialState());
        changeCostFunction(optConProblem.getCostFunction());
        changeNonlinearSystem(optConProblem.getNonlinearSystem());
        changeLinearSystem(optConProblem.getLinearSystem());

        if (optConProblem.getInputBoxConstraints())
            changeInputBoxConstraints(optConProblem.getInputBoxConstraints());
        if (optConProblem.getStateBoxConstraints())
            changeStateBoxConstraints(optConProblem.getStateBoxConstraints());
        if (optConProblem.getGeneralConstraints())
            changeGeneralConstraints(optConProblem.getGeneralConstraints());
    }

    virtual void configure(const Settings_t& settings) = 0;

    void configureFromFile(const std::string& filename,
        bool verbose = true,
        const std::string& ns = DERIVED::SolverName)
    {
        Settings_t settings;
        settings.load(filename, verbose, ns);
        configure(settings);
    }

    virtual bool solve() = 0;

    virtual bool runIteration() { throw std::runtime_error("runIteration not supported by solver"); }
    virtual const Policy_t& getSolution() = 0;

    virtual const core::StateTrajectory<STATE_DIM, SCALAR> getStateTrajectory() const = 0;

    virtual const core::ControlTrajectory<CONTROL_DIM, SCALAR> getControlTrajectory() const = 0;

    virtual const core::tpl::TimeArray<SCALAR>& getTimeArray() const = 0;


    virtual void setInitialGuess(const Policy_t& initialGuess) = 0;


    virtual SCALAR getTimeHorizon() const = 0;


    virtual void changeTimeHorizon(const SCALAR& tf) = 0;

    virtual void changeInitialState(const core::StateVector<STATE_DIM, SCALAR>& x0) = 0;

    virtual void changeCostFunction(const typename OptConProblem_t::CostFunctionPtr_t& cf) = 0;

    virtual void changeNonlinearSystem(const typename OptConProblem_t::DynamicsPtr_t& dyn) = 0;

    virtual void changeLinearSystem(const typename OptConProblem_t::LinearPtr_t& lin) = 0;

    virtual void changeInputBoxConstraints(const typename OptConProblem_t::ConstraintPtr_t con)
    {
        throw std::runtime_error("The current solver does not support input box constraints!");
    }

    virtual void changeStateBoxConstraints(const typename OptConProblem_t::ConstraintPtr_t con)
    {
        throw std::runtime_error("The current solver does not support state box constraints!");
    }

    virtual void changeGeneralConstraints(const typename OptConProblem_t::ConstraintPtr_t con)
    {
        throw std::runtime_error("The current solver does not support general constraints!");
    }

    virtual SCALAR getCost() const { throw std::runtime_error("Get cost not implemented"); }
    virtual std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() = 0;

    virtual const std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() const = 0;

    virtual std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() = 0;

    virtual const std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() const = 0;

    virtual std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() = 0;

    virtual const std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() const = 0;

    virtual std::vector<typename OptConProblem_t::ConstraintPtr_t>& getInputBoxConstraintsInstances()
    {
        throw std::runtime_error("getInputBoxConstraintsInstances not supported.");
    }

    virtual const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getInputBoxConstraintsInstances() const
    {
        throw std::runtime_error("getInputBoxConstraintsInstances not supported.");
    }

    virtual std::vector<typename OptConProblem_t::ConstraintPtr_t>& getStateBoxConstraintsInstances()
    {
        throw std::runtime_error("getStateBoxConstraintsInstances not supported.");
    }

    virtual const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getStateBoxConstraintsInstances() const
    {
        throw std::runtime_error("getStateBoxConstraintsInstances not supported.");
    }


    virtual std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances()
    {
        throw std::runtime_error("getGeneralConstraintsInstances not supported.");
    }

    virtual const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances() const
    {
        throw std::runtime_error("getGeneralConstraintsInstances not supported.");
    }

#ifdef CPPADCG

    virtual void generateAndCompileCode(
        const ContinuousOptConProblem<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>& problemCG,
        const ct::core::DerivativesCppadSettings& settings)
    {
        throw std::runtime_error("Generate and compile code not implemented for this solver");
    }
#endif

    virtual void generateCode(const ct::core::DerivativesCppadSettings& settings)
    {
        throw std::runtime_error("Generate Code not implemented for this solver");
    }
};
}  // namespace optcon
}  // namespace ct