Component.cpp

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/>.
 *
 * @package    Navigation::ArmarXObjects::ControlMemory
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "Component.h"
 
// STD / STL
#include <filesystem>
#include <fstream>
#include <iostream>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <RobotAPI/libraries/armem/client/query.h>
#include <RobotAPI/libraries/armem/core/json_conversions.h>
#include <RobotAPI/libraries/armem/core/operations.h>
#include <RobotAPI/libraries/armem/server/ltm/Memory.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
 
#include <armarx/control/common/type.h>
#include <armarx/control/memory/constants.h>
// #include <armarx/navigation/algorithms/Costmap.h>
// #include <armarx/navigation/memory/constants.h>
// #include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
// #include <armarx/navigation/core/Graph.h>
// #include <armarx/navigation/core/aron/Graph.aron.generated.h>
// #include <armarx/navigation/core/aron/Location.aron.generated.h>
// #include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
// #include <armarx/navigation/core/aron/Twist.aron.generated.h>
// #include <armarx/navigation/graph/constants.h>
// #include <armarx/navigation/location/constants.h>
 
#include <armarx/control/common/control_law/aron/TaskspaceImpedanceControllerConfig.aron.generated.h>
 
// #include <armarx/control/njoint_qp_controller/aron_conversions.h>
 
// #include <simox/control/method/examples/json_conversions.h>
 
namespace armarx::control::components::control_memory
{
 
 
    armarx::PropertyDefinitionsPtr
    Component::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        // Publish to a topic (passing the TopicListenerPrx).
        // def->topic(myTopicListener);
 
        // Subscribe to a topic (passing the topic name).
        // def->topic<PlatformUnitListener>("MyTopic");
 
        // Use (and depend on) another component (passing the ComponentInterfacePrx).
        // def->component(myComponentProxy)
 
 
        workingMemory().name() = armarx::control::memory::constants::MemoryName;
        def->optional(properties.snapshotToLoad,
                      "p.snapshotToLoad",
                      "Memory snapshot to load at start up \n"
                      "(e.g. 'PriorKnowledgeData/navigation-graphs/snapshot').");
 
        def->optional(properties.locationGraph.visuFrequency,
                      "p.locationGraph.visuFrequency",
                      "Visualization frequeny of locations and graph edges [Hz].");
 
        // core segment max history sizes
        {
            def->optional(properties.coreSeg.defaultParameterization.maxHistorySize,
                          "p.coreSeg.defaultParameterization.maxHistorySize",
                          "Max history size of the " +
                              QUOTED(armarx::control::memory::constants::
                                         DefaultParameterizationCoreSegmentName) +
                              " core segment.");
 
            def->optional(
                properties.coreSeg.parameterization.maxHistorySize,
                "p.coreSeg.defaultParameterization.maxHistorySize",
                "Max history size of the " +
                    QUOTED(armarx::control::memory::constants::ParameterizationCoreSegmentName) +
                    " core segment.");
        }
 
        return def;
    }
 
    void
    Component::onInitComponent()
    {
        // Topics and properties defined above are automagically registered.
 
        // Keep debug observer data until calling `sendDebugObserverBatch()`.
        // (Requies the armarx::DebugObserverComponentPluginUser.)
        // setDebugObserverBatchModeEnabled(true);
 
        workingMemory()
            .addCoreSegment(
                armarx::control::memory::constants::DefaultParameterizationCoreSegmentName)
            .setMaxHistorySize(properties.coreSeg.defaultParameterization.maxHistorySize);
 
        workingMemory()
            .addCoreSegment(armarx::control::memory::constants::ParameterizationCoreSegmentName)
            .setMaxHistorySize(properties.coreSeg.parameterization.maxHistorySize);
 
 
        /*
        if (not properties.snapshotToLoad.empty())
        {
            std::filesystem::path snapshotToLoadPath(
                ArmarXDataPath::resolvePath(properties.snapshotToLoad));
            if (std::filesystem::is_directory(snapshotToLoadPath))
            {
                // This section loads the snapshot specified by the scenario parameters
                // resolve the paths for the locations and graphs
                const std::filesystem::path graph = snapshotToLoadPath / "Graph";
                const std::filesystem::path location = snapshotToLoadPath / "Location";
 
                // remove date from folder name (if present)
                // Sometimes, we use the date before the snapshotname and use a symlink to the real data (e.g. R003 and 2022-03-01_R003)
                auto splitted = simox::alg::split(snapshotToLoadPath.filename().string(), "_");
                ARMARX_CHECK_GREATER(splitted.size(), 0);
                const std::string providerName = splitted[splitted.size() - 1];
 
                // This if statement loads the location. Each location is a single file (without extension). The filename is the name of the location.
                // The file contains json with the globalRobotPose (4x4 matrix) and relativeToObject information
                if (std::filesystem::is_directory(location))
                {
                    armem::Commit c;
                    armem::MemoryID providerID = workingMemory().id();
                    providerID.coreSegmentName = "Location";
                    providerID.providerSegmentName = providerName;
                    for (const auto& subdir : std::filesystem::directory_iterator(
                             location)) // iterate over all files in folder (the locations)
                    {
                        const std::filesystem::path location = subdir.path();
                        if (std::filesystem::is_regular_file(
                                location)) // check if its a file (otherwise skip)
                        {
                            std::ifstream ifs(location);
                            const std::string content((std::istreambuf_iterator<char>(ifs)),
                                                      (std::istreambuf_iterator<char>()));
 
                            // parse location as json. All files in Location folder must be valid json objects!
                            nlohmann::json j = nlohmann::json::parse(content);
 
                            if (j.find("globalRobotPose") == j.end())
                            {
                                ARMARX_WARNING
                                    << "The file '" << location.string()
                                    << "' has no 'globalRobotPose' member. Skipping this file.";
                                continue;
                            }
 
                            if (j.find("relativeToObject") == j.end())
                            {
                                ARMARX_WARNING
                                    << "The file '" << location.string()
                                    << "' has no 'relativeToObject' member. Skipping this file.";
                                continue;
                            }
 
                            std::vector<float> p = j.at("globalRobotPose");
                            ARMARX_CHECK_EQUAL(p.size(), 16);
 
                            navigation::location::arondto::Location loc;
                            loc.globalRobotPose << p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7],
                                p[8], p[9], p[10], p[11], p[12], p[13], p[14],
                                p[15]; // load the 4x4 matrix
 
                            // TODO: All location I have seen were null.
                            // I don't know how this member should look like (von @Fabian Peller to @Fabian Reister)
                            loc.relativeToObject = std::nullopt;
 
                            // send commit to memory
                            auto& up = c.add();
                            up.confidence = 1.0;
                            up.referencedTime = armem::Time::Now();
                            up.timeSent = armem::Time::Now();
                            up.timeArrived = armem::Time::Now();
                            up.entityID = providerID.withEntityName(location.filename().string());
                            up.instancesData = {loc.toAron()};
                        }
                    }
 
                    // commit all locations at once
                    iceAdapter().commit(c);
                }
 
                // Next we load the graphs. The graph folder may contain several graphs, represented by different folders.
                // Each of those graphs contains a list of files representing the vertices. The filename is the vertice id (ideally starting at 0).
                // The file contains a json with the corresponding location name (as path) and the adjacent vertives (representing the directed outgoing edges).
                if (std::filesystem::is_directory(graph))
                {
                    armem::Commit c;
                    armem::MemoryID providerID = workingMemory().id();
                    providerID.coreSegmentName = memory::constants::GraphCoreSegmentName;
                    providerID.providerSegmentName = providerName;
 
                    for (const auto& graphdir : std::filesystem::directory_iterator(
                             graph)) // iterate over the different graphs (subfolders)
                    {
                        const std::filesystem::path singleGraph = graphdir.path();
                        if (std::filesystem::is_directory(
                                singleGraph)) // assure that its a folder. otherwise skip
                        {
                            navigation::core::arondto::Graph g;
 
                            for (const auto& subdir : std::filesystem::directory_iterator(
                                     singleGraph)) // iterate over all files in the graph
                            {
                                const std::filesystem::path vertice = subdir.path();
                                if (std::filesystem::is_regular_file(
                                        vertice)) // assure its a file. otherwise skip
                                {
                                    std::ifstream ifs(vertice);
                                    const std::string content((std::istreambuf_iterator<char>(ifs)),
                                                              (std::istreambuf_iterator<char>()));
 
                                    // parse vertice. Each vertice must be a valid json object
                                    nlohmann::json j = nlohmann::json::parse(content);
                                    if (j.find("location") == j.end())
                                    {
                                        ARMARX_WARNING
                                            << "The file '" << vertice.string()
                                            << "' has no 'location' member. Skipping this file.";
                                        continue;
                                    }
 
                                    if (j.find("outgoingEdges") == j.end())
                                    {
                                        ARMARX_WARNING << "The file '" << vertice.string()
                                                       << "' has no 'outgoingEdges' member. "
                                                          "Skipping this file.";
                                        continue;
                                    }
 
                                    std::string location = j.at("location");
                                    int id = std::stoi(vertice.filename());
 
                                    auto split = simox::alg::split(location, "/");
                                    ARMARX_CHECK_EQUAL(
                                        split.size(),
                                        4); // the location is always a path like Navigation/Location/XXX/YYY
 
                                    armarx::control::core::arondto::Vertex v;
                                    v.vertexID = id;
                                    v.locationID.memoryName = split[0];
                                    v.locationID.coreSegmentName = split[1];
                                    v.locationID.providerSegmentName = split[2];
                                    v.locationID.entityName = split[3];
                                    toAron(v.locationID.timestamp, armem::Time::Invalid());
                                    v.locationID.instanceIndex = 0;
 
                                    g.vertices.push_back(v);
 
                                    // add edges of this vertice to graph
                                    std::vector<float> edges = j.at("outgoingEdges");
 
                                    for (const auto edge_id : edges)
                                    {
                                        armarx::control::core::arondto::Edge e;
                                        e.sourceVertexID = id;
                                        e.targetVertexID = edge_id;
                                        g.edges.push_back(e);
                                    }
                                }
                            }
 
                            auto& up = c.add();
                            up.confidence = 1.0;
                            up.referencedTime = armem::Time::Now();
                            up.timeSent = armem::Time::Now();
                            up.timeArrived = armem::Time::Now();
                            up.entityID =
                                providerID.withEntityName(singleGraph.filename().string());
                            up.instancesData = {g.toAron()};
                        }
                    }
 
                    // send graph to memory
                    iceAdapter().commit(c);
                }
 
                // LEGACY CODE (Not working anymore since the wm json export changed due to ltm updates and aron updates)
                // armem::wm::Memory memory = armem::server::ltm::disk::load(path.value());
                //armem::server::ltm::disk::Memory ltm(path.value());
                //armem::wm::Memory memory;
                //ltm.loadAll(memory);
                //workingMemory().update(armem::toCommit(memory));
                ARMARX_INFO << "Loaded ControlMemory '" << properties.snapshotToLoad << "'";
            }
            else
            {
                ARMARX_WARNING << "Could not load ControlMemory '" << properties.snapshotToLoad
                               << "'. Continue with empty memory.";
            }
        }
        */
 
        loadDefaultConfigs();
    }
 
    void
    Component::loadDefaultConfigs()
    {
        // std::string filename;
        // ArmarXDataPath::getAbsolutePath("armarx_control/controller_config/default/"
        //                                 "njoint_controller/impedance_controller_config.json",
        //                                 filename);
 
        // std::ifstream ifs{filename};
 
        // nlohmann::json defaultConfig;
        // ifs >> defaultConfig;
 
        // {
 
        //     common::control_law::arondto::TaskSpaceImpedanceControllerConfig cfg;
        //     // TODO load
 
        //     armem::Commit c;
        //     armem::MemoryID providerID = workingMemory().id();
        //     providerID.coreSegmentName = memory::constants::DefaultParameterizationCoreSegmentName;
        //     providerID.providerSegmentName =
        //         common::ControllerTypeNames.to_name(common::ControllerType::TSImp);
 
 
        //     auto& up = c.add();
        //     up.confidence = 1.0;
        //     up.referencedTime = up.timeSent = up.timeArrived = armem::Time::Now();
 
        //     up.entityID = providerID.withEntityName("default");
        //     up.instancesData = {cfg.toAron()};
 
        //     iceAdapter().commit(c);
        // }
 
        // {
        //     njoint_qp_controller::impedance::arondto::WholeBodyImpedanceControllerConfig cfg;
 
        //     //  "kpImpedance":  [300, 300, 300, 3, 3, 3],
        //     //     "kdImpedance":    [ 50,  50,  50, 1, 1, 1],
        //     //     "kpNullspaceTorque":  [25.0, 15.0, 15.0, 5.0, 5.0, 5.0, 5.0, 5.0],
        //     //     "kdNullspaceTorque":    [4.0, 2.0, 2.0, 1.0, 1.0, 1.0, 1.0, 2.0],
        //     //     "desiredNullspaceJointAngles": [],
        //     //     "torqueLimit": 20,
        //     //     "qvelFilter": 0.9
 
 
        //     cfg.leftArmImpedanceConfig.kpImpedance << 300, 300, 300, 3, 3, 3;
        //     cfg.leftArmImpedanceConfig.kdImpedance << 50, 50, 50, 1, 1, 1;
        //     cfg.leftArmImpedanceConfig.kpNullspaceTorque << 25.0, 15.0, 15.0, 5.0, 5.0, 5.0, 5.0, 5.0;
        //     cfg.leftArmImpedanceConfig.kdNullspaceTorque << 4.0, 2.0, 2.0, 1.0, 1.0, 1.0, 1.0, 2.0;
        //     // cfg.leftArmImpedanceConfig.desiredNullspaceJointAngles;
        //     cfg.leftArmImpedanceConfig.torqueLimit = 20;
        //     cfg.leftArmImpedanceConfig.qvelFilter = 0.9;
 
        //     cfg.rightArmImpedanceConfig = cfg.leftArmImpedanceConfig;
 
 
        //     // TODO load
 
        //     armem::Commit c;
        //     armem::MemoryID providerID = workingMemory().id();
        //     providerID.coreSegmentName = memory::constants::DefaultParameterizationCoreSegmentName;
        //     providerID.providerSegmentName =
        //         common::ControllerTypeNames.to_name(common::ControllerType::QPWholeBodyImpedance);
 
 
        //     auto& up = c.add();
        //     up.confidence = 1.0;
        //     up.referencedTime = up.timeSent = up.timeArrived = armem::Time::Now();
 
        //     up.entityID = providerID.withEntityName("default");
        //     up.instancesData = {cfg.toAron()};
 
        //     iceAdapter().commit(c);
        // }
 
        // CMakePackageFinder pkg_finder("armarx_control");
        // const std::filesystem::path pkgDataDir{pkg_finder.getDataDir()};
 
        // const std::filesystem::path configBasePath =
        //     pkgDataDir / "armarx_control/controller_config";
 
        // for (auto const& dir_entry : std::filesystem::directory_iterator{
        //          configBasePath /
        //          common::ControllerTypeNames.to_name(common::ControllerType::QPWholeBodyVelocity)})
        // {
        //     if (not dir_entry.is_regular_file())
        //     {
        //         continue;
        //     }
 
        //     const std::string filename = dir_entry.path();
 
        //     ARMARX_INFO << "Loading config from file `" << filename << "`";
        //     std::ifstream ifs{filename};
 
        //     nlohmann::json jsonConfig;
        //     ifs >> jsonConfig;
 
        //     njoint_qp_controller::velocity::WholeBodyVelocityControllerConfig cfg;
        //     cfg.params.weights = jsonConfig.at("weights");
        //     cfg.params.kinematicConstraints = jsonConfig.at("kinematicConstraints");
 
        //     njoint_qp_controller::velocity::arondto::WholeBodyVelocityControllerConfig aronCfg;
        //     toAron(aronCfg, cfg);
 
 
        //     armem::Commit c;
        //     armem::MemoryID providerID = workingMemory().id();
        //     providerID.coreSegmentName = memory::constants::DefaultParameterizationCoreSegmentName;
        //     providerID.providerSegmentName =
        //         common::ControllerTypeNames.to_name(common::ControllerType::QPWholeBodyVelocity);
 
 
        //     auto& up = c.add();
        //     up.confidence = 1.0;
        //     up.referencedTime = up.timeSent = up.timeArrived = armem::Time::Now();
 
        //     up.entityID = providerID.withEntityName("default");
        //     up.instancesData = {aronCfg.toAron()};
 
        //     iceAdapter().commit(c);
        // }
    }
 
    void
    Component::onConnectComponent()
    {
        // Do things after connecting to topics and components.
 
 
        /* (Requies the armarx::DebugObserverComponentPluginUser.)
        // Use the debug observer to log data over time.
        // The data can be viewed in the ObserverView and the LivePlotter.
        // (Before starting any threads, we don't need to lock mutexes.)
        {
            setDebugObserverDatafield("numBoxes", properties.numBoxes);
            setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
            sendDebugObserverBatch();
        }
        */
 
        // Setup the remote GUI.
        {
            createRemoteGuiTab();
            RemoteGui_startRunningTask();
        }
 
        tasks.visuTask = new SimpleRunningTask<>([this]() { this->visuRun(); }, "Visualization");
        tasks.visuTask->start();
    }
 
    void
    Component::onDisconnectComponent()
    {
    }
 
    void
    Component::onExitComponent()
    {
    }
 
    std::string
    Component::getDefaultName() const
    {
        return GetDefaultName();
    }
 
    std::string
    Component::GetDefaultName()
    {
        return "ControlMemory";
    }
 
    void
    Component::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        // Setup the widgets.
        tab.locationGraph.setup(*this);
 
 
        // Setup the layout.
        VBoxLayout root = {tab.locationGraph.group, VSpacer()};
        RemoteGui_createTab(getName(), root, &tab);
    }
 
    void
    Component::RemoteGui_update()
    {
        tab.locationGraph.update(*this);
    }
 
    void
    Component::RemoteGuiTab::LocationGraph::setup(Component& owner)
    {
        using namespace armarx::RemoteGui::Client;
        GridLayout grid;
        int row = 0;
        {
            visuLocations.setValue(owner.properties.locationGraph.visuLocations);
            visuGraphEdges.setValue(owner.properties.locationGraph.visuLocations);
 
            grid.add(Label("Visualize Locations"), {row, 0}).add(visuLocations, {row, 1});
            ++row;
 
            grid.add(Label("Visualize Graph Edges"), {row, 0}).add(visuGraphEdges, {row, 1});
            ++row;
        }
 
        group = GroupBox({grid});
    }
 
    void
    Component::RemoteGuiTab::LocationGraph::update(Component& owner)
    {
        if (visuLocations.hasValueChanged() or visuGraphEdges.hasValueChanged())
        {
            std::scoped_lock lock(owner.propertiesMutex);
            owner.properties.locationGraph.visuLocations = visuLocations.getValue();
            owner.properties.locationGraph.visuGraphEdges = visuGraphEdges.getValue();
        }
    }
 
    void
    Component::visuRun()
    {
        // memory::Visu visu{arviz,
        //                   workingMemory().getCoreSegment(navigation::location::coreSegmentID),
        //                   workingMemory().getCoreSegment(navigation::graph::coreSegmentID)};
 
        // Properties::LocationGraph p;
        // {
        //     std::scoped_lock lock(propertiesMutex);
        //     p = properties.locationGraph;
        // }
 
        // CycleUtil cycle(static_cast<int>(1000 / p.visuFrequency));
        // while (tasks.visuTask and not tasks.visuTask->isStopped())
        // {
        //     {
        //         std::scoped_lock lock(propertiesMutex);
        //         p = properties.locationGraph;
        //     }
 
        //     std::vector<viz::Layer> layers;
 
        //     // Locations
        //     visu.drawLocations(layers, p.visuLocations);
 
        //     // Graph Edges
        //     visu.drawGraphs(layers, p.visuGraphEdges);
 
        //     arviz.commit(layers);
 
        //     cycle.waitForCycleDuration();
        // }
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
 
 
} // namespace armarx::control::components::control_memory