ControllerBuilder.h

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX 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 <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <filesystem>
#include <fstream>
#include <optional>
#include <type_traits>
 
#include <SimoxUtility/json/json.hpp>
 
#include "ArmarXCore/core/PackagePath.h"
#include "ArmarXCore/core/exceptions/LocalException.h"
#include "ArmarXCore/core/time/Clock.h"
 
#include "RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h"
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h>
 
#include <armarx/control/client/ControllerDescription.h>
#include <armarx/control/client/ControllerWrapper.h>
#include <armarx/control/common/type.h>
#include <armarx/control/core/Controller.h>
#include <armarx/control/interface/ConfigurableNJointControllerInterface.h>
#include <armarx/control/memory/config/Reader.h>
#include <armarx/control/memory/config/Writer.h>
#include <armarx/control/memory/constants.h>
 
namespace armarx::control::client
{
 
    template <auto T>
    class ControllerBuilder //: public AbstractControllerBuilder
    {
    public:
        using ControllerDescriptionType = ControllerDescription<T>;
 
        // Use ProxyType_t to conditionally use Proxy or the default fallback
        using CtrlProxy = detail::proxy::ProxyType_t<ControllerDescriptionType>;
 
        // Use CtrlConfigClassName_v to conditionally use ctrlConfigClassName or fallback to 'name'
        static constexpr const char* ctrlConfigClassName =
            detail::ctrl_class_name::CtrlConfigClassName_v<ControllerDescriptionType>;
 
        // static constexpr auto ControllerType = ct;
        using AronDTO = typename ControllerDescriptionType::AronDTO;
        // using AronType = AronDTO;
 
        static_assert(::armarx::aron::cpp::isAronGeneratedClass<AronDTO>,
                      "The AronConfigT must be an AronGeneratedClass!");
 
        ControllerBuilder(const armarx::RobotUnitInterfacePrx& controllerCreator,
                          const memory::config::Reader& configProvider,
                          const memory::config::Writer& configWriter,
                          const std::string& controllerNamePrefix) :
            controllerCreator(controllerCreator),
            configReader(configProvider),
            configWriter(configWriter),
            controllerNamePrefix(controllerNamePrefix)
        {
            ARMARX_CHECK_NOT_EMPTY(controllerNamePrefix)
                << "By convention, the controller name prefix must exist. This ensures that "
                   "in between components, no naming clashes occur.";
        }
 
        ControllerBuilder&
        withDefaultConfig(const std::string& name = "default")
        {
            if (auto cfg = configReader.getDefaultConfig(ControllerDescriptionType::name, name))
            {
                dto = AronDTO::FromAron(cfg);
            }
            else
            {
                ARMARX_WARNING << "Failed to obtain default config";
            }
 
            return *this;
        }
 
        ControllerBuilder&
        withDefaultConfigFile(const std::string& name = "default",
                              const std::string& robotNameSuffix = "",
                              const std::string& appendix = "")
        {
            const std::string configClassName = ctrlConfigClassName;
 
            const armarx::PackagePath configPath("armarx_control",
                                                 "controller_config/" + configClassName + "/" +
                                                     name + robotNameSuffix + appendix + ".json");
            ARMARX_INFO << "using default config: " << configPath.toSystemPath();
            return withConfig(configPath.toSystemPath());
        }
 
        ControllerBuilder&
        withConfig(const armarx::armem::MemoryID& memoryId)
        {
            auto cfg = configReader.getConfig(memoryId);
            dto = AronDTO::FromAron(cfg);
 
            return *this;
        }
 
        ControllerBuilder&
        withConfig(const ::armarx::aron::data::dto::DictPtr& configDict)
        {
            dto = AronDTO::FromAron(configDict);
            return *this;
        }
 
        ControllerBuilder&
        withConfig(const std::filesystem::path& filename)
        {
            ARMARX_CHECK(std::filesystem::is_regular_file(filename)) << filename;
 
            ARMARX_INFO << "Loading config from file `" << filename << "`.";
            std::ifstream ifs{filename};
 
            nlohmann::json jsonConfig;
            ifs >> jsonConfig;
 
            ARMARX_VERBOSE << "Initializing config";
            if (not this->dto.has_value())
            {
                this->dto = AronDTO();
            }
 
            armarx::aron::data::reader::NlohmannJSONReaderWithoutTypeCheck reader;
 
            ARMARX_CHECK(this->dto.has_value());
 
            ARMARX_VERBOSE << "reading file.";
            this->dto->read(reader, jsonConfig);
 
            return *this;
        }
 
        ControllerBuilder&
        withConfig(const AronDTO& dto)
        {
            this->dto = dto;
            return *this;
        }
 
        AronDTO&
        config()
        {
            ensureInitialized();
 
            return dto.value();
        }
 
        ControllerBuilder&
        withNodeSet(const std::string& nodeSetName)
        {
            this->nodeSetName = nodeSetName;
            return *this;
        }
 
        ControllerBuilder&
        withNamePrefix(const std::string& namePrefix)
        {
            if (namePrefix.empty())
            {
                return *this;
            }
            this->controllerNamePrefix = this->controllerNamePrefix + "_" + namePrefix;
            return *this;
        }
 
        ControllerBuilder&
        daemonize()
        {
            this->daemonized = true;
            return *this;
        }
 
        /**
         * @brief Creates the controller with the default name
         * 
         * @return std::optional<ControllerWrapper<AronDTO, ct>> 
         */
        std::unique_ptr<ControllerWrapper<T>>
        create()
        {
            const std::string controllerClassName = ControllerDescriptionType::name;
            return create(controllerClassName);
        }
 
        std::unique_ptr<ControllerWrapper<T>>
        create(const std::string& name)
        {
            if (not ensureInitialized())
            {
                return nullptr;
            }
 
            ::armarx::control::ConfigurableNJointControllerConfigPtr config =
                new ::armarx::control::ConfigurableNJointControllerConfig;
 
            auto start = armarx::DateTime::now();
            config->config = dto.value().toAronDTO();
            ARMARX_DEBUG << "--> Aron conversion took "
                         << (armarx::DateTime::now() - start).toSecondsDouble() << " sec.";
 
            //            if (not nodeSetName.has_value())
            //            {
            //                ARMARX_WARNING << "You forgot to set the node set name!";
            //                return std::nullopt;
            //            }
            //            config->nodeSetName = nodeSetName.value();
 
            //            controllerName = controllerNamePrefix + "_" + nodeSetName.value() + "_" + name;
            controllerName = controllerNamePrefix + "_" + name;
            const std::string controllerClassName = ControllerDescriptionType::name;
 
            // reuse or create controller
 
            start = armarx::DateTime::now();
            armarx::NJointControllerInterfacePrx controller =
                controllerCreator->getNJointController(controllerName);
            ARMARX_DEBUG << "--> get controller by name from robot unit took "
                         << (armarx::DateTime::now() - start).toSecondsDouble() << " sec.";
 
            this->reusing_ = [&]() -> bool
            {
                if (not controller)
                {
                    ARMARX_INFO << "Controller does not exist yet.";
                    return false;
                }
 
                if (not allowReuse_)
                {
                    ARMARX_INFO << "Not allowed to reuse controller.";
                    return false;
                }
 
                // check if controller has correct type
                if (controller->getClassName() != controllerClassName)
                {
                    ARMARX_INFO << "Controller class does not match.";
                    return false;
                }
 
                // TODO: check if kinematic chains match
                // if (controller->getNodeSetName() != nodeSetName)
                // {
                //     return false;
                // }
 
                return true;
            }();
 
            // if((not allowReuse_) and controller)
            // {
            //     ARMARX_TRACE;
            //
            //     ARMARX_INFO << "Deleting existing controller `" << controllerName << "`";
            //     try
            //     {
            //         ARMARX_TRACE;
            //         controllerCreator->deactivateAndDeleteNJointController(controllerName);
            //     }
            //     catch(...)
            //     {
            //         ARMARX_WARNING << GetHandledExceptionString();
            //     }
            //
            //     ARMARX_INFO << "Done deleting";
            //
            //     Clock::WaitFor(Duration::MilliSeconds(150));
            // }
 
            if ((not allowReuse_) and controller)
            {
                ARMARX_TRACE;
 
                ARMARX_INFO << "--- Deleting existing controller `" << controllerName << "`";
                try
                {
                    ARMARX_TRACE;
                    ARMARX_INFO << "--- Wait until controller is in-active";
                    //                    controllerCreator->deactivateAndDeleteNJointController(controllerName);
                    controller->deactivateController();
 
                    while (controller->isControllerActive())
                    {
                        armarx::Clock::WaitFor(armarx::Duration::SecondsDouble(0.1));
                    }
 
                    ARMARX_INFO << "--- Controller is not active anymore, deleting controller";
                    controller->deleteController();
                }
                catch (...)
                {
                    ARMARX_WARNING << GetHandledExceptionString();
                }
 
                ARMARX_INFO << "--- Done deleting";
 
                Clock::WaitFor(Duration::MilliSeconds(150));
            }
            else
            {
                ARMARX_INFO << "Reuse of controller would be possible.";
            }
 
            // create controller if necessary
            if (not this->reusing_)
            {
                ARMARX_TRACE;
 
                ARMARX_INFO << "Creating controller `" << controllerName << "`";
                start = armarx::DateTime::now();
                controller = controllerCreator->createNJointController(
                    controllerClassName, controllerName, config);
                ARMARX_DEBUG << "--> creating new controller took "
                             << (armarx::DateTime::now() - start).toSecondsDouble() << " sec.";
                ARMARX_CHECK_NOT_NULL(controller);
                ARMARX_INFO << "Controller `" << controllerName << "` created.";
            }
            else
            {
                ARMARX_IMPORTANT << "Reusing the controller: " << controllerName;
            }
 
            if (not controller)
            {
                ARMARX_WARNING << "Failed to create controller of type `" << controllerClassName
                               << "`";
                return nullptr;
            }
 
            auto ctrl =
                armarx::control::ConfigurableNJointControllerInterfacePrx::checkedCast(controller);
 
            std::unique_ptr<ControllerWrapper<T>> ctrlWrapper =
                std::make_unique<ControllerWrapper<T>>(
                    ctrl, controllerName, configWriter, dto.value());
            ctrlWrapper->daemonize(daemonized);
 
            // Sends the data to the memory and updates the controller config.
            // This is especially important, if the controller is reused.
            start = armarx::DateTime::now();
            ctrlWrapper->updateConfig();
            ARMARX_DEBUG << "--> ctrlWrapper->updateConfig() took "
                         << (armarx::DateTime::now() - start).toSecondsDouble() << " sec.";
            return ctrlWrapper;
        }
 
        std::unique_ptr<ControllerWrapper<T>>
        createTSComplianceCtrl(const std::string& namePrefix = "",
                               const ::armarx::aron::data::dto::DictPtr& configDict = nullptr,
                               const std::string& configFilename = "",
                               bool flagActivate = true,
                               bool flagAllowReuse = true,
                               bool flagFromMemory = false)
        {
            ARMARX_CHECK(not(configDict.get() == nullptr and configFilename.empty()));
            /// load predefined config from file or use the configFilename provided by the user
            auto start = armarx::DateTime::now();
            if (configDict)
            {
                withConfig(configDict);
            }
            else
            {
                if (configFilename.empty())
                {
                    if (flagFromMemory)
                        withDefaultConfig();
                    else
                        withDefaultConfigFile();
                }
                else
                {
                    ARMARX_CHECK(std::filesystem::exists(configFilename))
                        << "configuration file does not exist\n"
                        << configFilename
                        << "Other tips: \n"
                           "1) if you need to configure a controller by passing configuration "
                           "file "
                           "to "
                           "controllerBuilder (me), you need to make sure the "
                           "controllerCreator "
                           "component is running "
                           "on the same PC as your application, otherwise\n"
                           "2) use the createControllerFromConfig() interface, which does not "
                           "require "
                           "where controller"
                           "creator is running";
                    ARMARX_IMPORTANT << "Using user defined configuration file:\n"
                                     << configFilename;
                    withConfig(configFilename);
                }
            }
            ARMARX_DEBUG << "--> Loading config took "
                         << (armarx::DateTime::now() - start).toSecondsDouble() << " sec.";
 
            allowReuse(flagAllowReuse);
            daemonize();
 
            std::ostringstream oss;
            for (const auto& pair : config().limbs)
            {
                oss << pair.first;
                if (&pair != &(*config().limbs.rbegin()))
                {
                    oss << ",";
                }
            }
            std::string robotNodeSets = oss.str();
 
            if (namePrefix.empty())
            {
                withNamePrefix(robotNodeSets);
            }
            else
            {
                allowReuse(true);
                withNamePrefix(namePrefix + "_" + robotNodeSets);
            }
 
            ARMARX_TRACE;
            ARMARX_INFO << "creating controller";
 
            auto ctrlWrapper = create();
 
            ARMARX_TRACE;
 
            ARMARX_INFO << "using controller name " << controllerName;
            ARMARX_TRACE;
 
            if (getReusing())
            {
                ARMARX_INFO
                    << "setting nullspace target: if the default is null use the cfg from the "
                       "existing controller";
                /// get up-to-date controller configs and update the copy of it in wrapper
                /// with the null space target from the controller
                auto ctrlConfig = ctrlWrapper->getConfig().limbs;
                for (const auto& pair : config().limbs)
                {
                    if (not pair.second.desiredNullspaceJointAngles.has_value())
                    {
                        ctrlWrapper->config.limbs.at(pair.first).desiredNullspaceJointAngles =
                            ctrlConfig.at(pair.first).desiredNullspaceJointAngles;
                    }
                    //                    ctrlWrapper->config.limbs.at(pair.first).desiredPose =
                    //                        ctrlConfig.at(pair.first).desiredPose;
                    //                    ARMARX_INFO << "Activating controller ..."
                    //                                << ctrlWrapper->config.limbs.at(pair.first)
                    //                                       .desiredNullspaceJointAngles.has_value();
                }
            }
 
            ARMARX_TRACE;
            // ctrlWrapper->updateConfig();
            if (flagActivate)
            {
                ctrlWrapper->activate();
                ARMARX_TRACE;
                while (not ctrlWrapper->isActive())
                {
                    Clock::WaitFor(Duration::MilliSeconds(10));
                }
                ARMARX_INFO << "Controller " << controllerName << " is active";
            }
            else
            {
                ARMARX_INFO << "User have to activate " << controllerName
                            << " controller in your application";
            }
            return ctrlWrapper;
        }
 
        CtrlProxy
        createGetTSComplianceCtrl(const std::string& namePrefix = "",
                                  const ::armarx::aron::data::dto::DictPtr& configDict = nullptr,
                                  const std::string& configFilename = "",
                                  bool flagActivate = true,
                                  bool flagAllowReuse = true,
                                  bool flagFromMemory = false)
        {
            auto ctrlWrapper = createTSComplianceCtrl(namePrefix,
                                                      configDict,
                                                      configFilename,
                                                      flagActivate,
                                                      flagAllowReuse,
                                                      flagFromMemory);
            auto ctrl = CtrlProxy::checkedCast(ctrlWrapper->ctrl());
            return ctrl;
        }
 
        std::unique_ptr<ControllerWrapper<T>>
        createTSComplianceMPCtrl(const std::string& namePrefix,
                                 const ::armarx::aron::data::dto::DictPtr& mpConfigDict,
                                 const ::armarx::aron::data::dto::DictPtr& configDict = nullptr,
                                 const std::string& configFilename = "",
                                 bool flagActivate = true,
                                 bool flagAllowReuse = true,
                                 bool flagFromMemory = false,
                                 bool flagStart = true)
        {
            auto ctrlWrapper = createTSComplianceCtrl(namePrefix,
                                                      configDict,
                                                      configFilename,
                                                      flagActivate,
                                                      flagAllowReuse,
                                                      flagFromMemory);
            auto ctrl = CtrlProxy::checkedCast(ctrlWrapper->ctrl());
            ctrl->updateMPConfig(mpConfigDict);
            ctrl->trainMP();
 
            if (flagStart)
            {
                // start controller
                ARMARX_CHECK_NOT_NULL(ctrl);
                ctrl->startAll();
            }
            return ctrlWrapper;
        }
 
        CtrlProxy
        createGetTSComplianceMPCtrl(const std::string& namePrefix,
                                    const ::armarx::aron::data::dto::DictPtr& mpConfigDict,
                                    const ::armarx::aron::data::dto::DictPtr& configDict = nullptr,
                                    const std::string& configFilename = "",
                                    bool flagActivate = true,
                                    bool flagAllowReuse = true,
                                    bool flagFromMemory = false,
                                    bool flagStart = true)
        {
            auto ctrlWrapper = createTSComplianceCtrl(namePrefix,
                                                      configDict,
                                                      configFilename,
                                                      flagActivate,
                                                      flagAllowReuse,
                                                      flagFromMemory);
            auto ctrl = CtrlProxy::checkedCast(ctrlWrapper->ctrl());
            ctrl->stopAll();
            auto start = armarx::DateTime::Now();
            ctrl->updateMPConfig(mpConfigDict);
            ARMARX_DEBUG << "--> ctrl->updateMPConfig(mpConfigDict: DictPtr) took "
                         << (armarx::DateTime::Now() - start).toSecondsDouble() << " sec.";
 
            ARMARX_DEBUG << "--> calling train mp";
            start = armarx::DateTime::Now();
            ctrl->trainMP();
            ARMARX_DEBUG << "--> ctrl->trainMP() took "
                         << (armarx::DateTime::Now() - start).toSecondsDouble() << " sec.";
 
            if (flagStart)
            {
                // start controller
                ARMARX_CHECK_NOT_NULL(ctrl);
                ctrl->startAll();
            }
            return ctrl;
        }
 
        ControllerBuilder&
        allowReuse(const bool allowReuse)
        {
            this->allowReuse_ = allowReuse;
            return *this;
        }
 
        bool
        getReusing()
        {
            return this->reusing_;
        }
 
        std::string
        getControllerName()
        {
            return controllerName;
        }
 
    private:
        bool
        initDefaultConfig()
        {
            if (auto cfg = configReader.getDefaultConfig(ControllerDescriptionType::name))
            {
                dto = AronDTO::FromAron(cfg);
                return true;
            }
 
            ARMARX_WARNING << "Failed to obtain default config";
            return false;
        }
 
        bool
        ensureInitialized()
        {
            if (not dto.has_value())
            {
                return initDefaultConfig();
            }
 
            return true;
        }
 
        armarx::RobotUnitInterfacePrx controllerCreator;
 
        const memory::config::Reader configReader;
        const memory::config::Writer configWriter;
 
        std::optional<AronDTO> dto;
 
        std::optional<std::string> nodeSetName;
 
        std::string controllerNamePrefix;
        std::string controllerName;
 
        bool daemonized = false;
        bool allowReuse_ = false;
        bool reusing_ = false;
    };
} // namespace armarx::control::client