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

#include <ct/optcon/nloc/NLOCBackendST.hpp>
#include <ct/optcon/nloc/NLOCBackendMP.hpp>

#include <ct/optcon/nloc/algorithms/SingleShooting.hpp>
#include <ct/optcon/nloc/algorithms/MultipleShooting.hpp>

namespace ct {
namespace optcon {


template <size_t STATE_DIM,
    size_t CONTROL_DIM,
    size_t P_DIM = STATE_DIM / 2,
    size_t V_DIM = STATE_DIM / 2,
    typename SCALAR = double,
    bool CONTINUOUS = true>
class NLOptConSolver final
    : public OptConSolver<NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>,
          typename NLOCAlgorithm<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::Policy_t,
          NLOptConSettings,
          STATE_DIM,
          CONTROL_DIM,
          SCALAR,
          CONTINUOUS>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    static const size_t STATE_D = STATE_DIM;
    static const size_t CONTROL_D = CONTROL_DIM;
    static const size_t POS_DIM = P_DIM;
    static const size_t VEL_DIM = V_DIM;

    typedef OptConSolver<NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>,
        typename NLOCAlgorithm<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::Policy_t,
        NLOptConSettings,
        STATE_DIM,
        CONTROL_DIM,
        SCALAR,
        CONTINUOUS>
        Base;


    typedef NLOptConSettings Settings_t;
    typedef SCALAR Scalar_t;

    typedef typename Base::OptConProblem_t OptConProblem_t;

    typedef NLOCAlgorithm<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS> NLOCAlgorithm_t;
    typedef typename NLOCAlgorithm_t::Policy_t Policy_t;
    typedef typename NLOCAlgorithm_t::Backend_t Backend_t;


    NLOptConSolver(const OptConProblem_t& optConProblem, const Settings_t& settings);

    NLOptConSolver(const OptConProblem_t& optConProblem,
        const std::string& settingsFile,
        bool verbose = true,
        const std::string& ns = "alg");

    virtual ~NLOptConSolver() = default;

    void initialize(const OptConProblem_t& optConProblem, const Settings_t& settings);

    void configure(const Settings_t& settings) override;


    virtual void prepareIteration();

    virtual bool finishIteration();


    virtual void prepareMPCIteration();

    virtual bool finishMPCIteration();

    virtual bool runIteration() override;

    void setInitialGuess(const Policy_t& initialGuess) override;

    virtual bool solve() override;

    virtual const Policy_t& getSolution() override;

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

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

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

    virtual SCALAR getTimeHorizon() const override;

    virtual void changeTimeHorizon(const SCALAR& tf) override;

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

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

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

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

    virtual SCALAR getCost() const override;

    const Settings_t& getSettings();

    const std::shared_ptr<Backend_t>& getBackend();

    std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() override;

    const std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() const override;

    std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() override;

    const std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() const override;

    std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() override;

    const std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() const override;

    std::vector<typename OptConProblem_t::ConstraintPtr_t>& getInputBoxConstraintsInstances() override;

    const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getInputBoxConstraintsInstances() const override;

    std::vector<typename OptConProblem_t::ConstraintPtr_t>& getStateBoxConstraintsInstances() override;

    const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getStateBoxConstraintsInstances() const override;

    std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances() override;

    const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances() const override;

    void logSummaryToMatlab(const std::string& fileName);

protected:
    std::shared_ptr<Backend_t> nlocBackend_;

    std::shared_ptr<NLOCAlgorithm_t> nlocAlgorithm_;

private:
    void setAlgorithm(const Settings_t& settings);
};


}  // namespace optcon
}  // namespace ct