NLOCBackendMP.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 <iostream>
#include <memory>
#include <thread>
#include <mutex>
#include <condition_variable>

#include "NLOCBackendBase.hpp"
#include <ct/optcon/solver/NLOptConSettings.hpp>

namespace ct {
namespace optcon {


template <size_t STATE_DIM,
    size_t CONTROL_DIM,
    size_t P_DIM,
    size_t V_DIM,
    typename SCALAR = double,
    bool CONTINUOUS = true>
class NLOCBackendMP final : public NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS> Base;
    typedef typename Base::OptConProblem_t OptConProblem_t;

    NLOCBackendMP(const OptConProblem_t& optConProblem, const NLOptConSettings& settings);


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

    virtual ~NLOCBackendMP();

protected:
    virtual void computeLQApproximation(size_t firstIndex, size_t lastIndex) override;

    virtual void rolloutShots(size_t firstIndex, size_t lastIndex) override;

    SCALAR performLineSearch() override;

private:
    enum WORKER_STATE
    {
        IDLE,
        LINE_SEARCH,
        ROLLOUT_SHOTS,
        COMPUTE_LQ_PROBLEM,
        SHUTDOWN
    };

    void startupRoutine();

    void shutdownRoutine();


    void launchWorkerThreads();


    void threadWork(size_t threadId);


    void lineSearchWorker(size_t threadId);


    void computeLQProblemWorker(size_t threadId);

    void rolloutShotWorker(size_t threadId);

    size_t generateUniqueProcessID(const size_t& iterateNo, const int workerState, const size_t resetCount);

    void printString(const std::string& text);

    std::vector<std::thread> workerThreads_;
    std::atomic_bool workersActive_;
    std::atomic_int workerTask_;

    std::mutex workerWakeUpMutex_;
    std::condition_variable workerWakeUpCondition_;

    std::mutex kCompletedMutex_;
    std::condition_variable kCompletedCondition_;

    std::mutex kCompletedMutexCost_;
    std::condition_variable kCompletedConditionCost_;

    std::mutex lineSearchResultMutex_;
    std::mutex alphaBestFoundMutex_;
    std::condition_variable alphaBestFoundCondition_;

    std::atomic_size_t alphaTaken_;
    size_t alphaMax_;
    size_t alphaExpBest_;
    size_t alphaExpMax_;
    std::atomic_bool alphaBestFound_;
    std::vector<size_t> alphaProcessed_;

    std::atomic_size_t kTaken_;
    std::atomic_size_t kCompleted_;

    size_t KMax_;
    size_t KMin_;

    SCALAR lowestCostPrevious_;
};


}  // namespace optcon
}  // namespace ct