Tree.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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    RobotComponents
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl.txt
 *             GNU General Public License
 */
 
#include <limits>
 
#include "../../util/Metrics.h"
 
#include "Tree.h"
#include "WorkerNode.h"
 
#include <ArmarXCore/core/util/StringHelpers.h>
 
namespace armarx::addirrtstar
{
    const NodeId Tree::ROOT_ID
    {
        0, 0
    };
 
    Tree::Tree(const VectorXf& startCfg)
    {
        init(
            FullIceTree
        {
            FullNodeDataListList{
                FullNodeDataList{
                    FullNodeData{
                        startCfg, //config
                        ROOT_ID, //parent
                        std::numeric_limits<Ice::Float>::infinity(), //radius;
                        0.f //fromParentCost
                    }
                }
            },
            Ice::LongSeq{ -1}
        },
        AdaptiveDynamicDomainParameters {1, 1, 1},
        -1 //since this tree does no updates the id is not important
        );
    }
 
    //init
    void Tree::init
    (
        const FullIceTree& iceTree,
        AdaptiveDynamicDomainParameters newaddParams,
        std::size_t newWorkerId
    )
    {
        addParams = newaddParams;
        workerId = newWorkerId;
 
        //goals
        goalNodes.clear();
 
        //updates
        localUpdates.clear();
        prepareNextUpdate();
        appliedUpdateIds.clear();
        appliedUpdateIds = iceTree.updateIds;
 
        //get storage (+ default objects)
        nodes.clear();
        nodeCount = 0;
 
        for (const auto& workerNodes : iceTree.nodes)
        {
            nodeCount += workerNodes.size();
            nodes.emplace_back();
            nodes.back().resize(workerNodes.size());
        }
 
        ARMARX_CHECK_EXPRESSION(nodes.size() == iceTree.nodes.size());
 
        //copy node data
        for (std::size_t wId = 0; wId < nodes.size(); ++wId)
        {
            for (std::size_t nId = 0; nId < nodes.at(wId).size(); ++nId)
            {
                NodeType& localNode = nodes.at(wId).at(nId);
                const FullNodeData& iceNode = iceTree.nodes.at(wId).at(nId);
                localNode.config = iceNode.config;
                localNode.parent = iceNode.parent;
                localNode.radius = iceNode.radius;
                localNode.fromParentCost = iceNode.fromParentCost;
                //register as child !!This adds root as child of root!!
                getNode(localNode.parent).children.insert(NodeId
                {
                    static_cast<Ice::Long>(wId),
                    static_cast<Ice::Long>(nId)
                });
            }
        }
 
        //remove root as child of root
        getNode(ROOT_ID).children.erase(ROOT_ID);
        //push all costs from root
        pushCosts(ROOT_ID);
    }
 
    void Tree::increaseWorkerCountTo(std::size_t count)
    {
        if (appliedUpdateIds.size() < count)
        {
            appliedUpdateIds.resize(count, -1);
        }
 
        if (nodes.size() < count)
        {
            nodes.resize(count);
        }
    }
 
    //updating
    bool Tree::canApplyUpdate(const Update& u)
    {
        ARMARX_CHECK_EXPRESSION(u.dependetOnUpdateIds.size() <= appliedUpdateIds.size());
 
        for (std::size_t i = 0; i < u.dependetOnUpdateIds.size(); ++i)
        {
            if (appliedUpdateIds.at(i) < u.dependetOnUpdateIds.at(i))
            {
                return false;
            }
        }
 
        return true;
    }
 
    void Tree::applyUpdate(const Update& u)
    {
        const auto workerId = getUpdatesWorkerId(u);
        ARMARX_CHECK_EXPRESSION(workerId != this->workerId);
        ARMARX_CHECK_EXPRESSION(workerId < appliedUpdateIds.size());
        ARMARX_CHECK_EXPRESSION(u.dependetOnUpdateIds.size() <= appliedUpdateIds.size());
        ARMARX_CHECK_EXPRESSION(canApplyUpdate(u));
 
        //do we need more worker?
        if (workerId > nodes.size())
        {
            nodes.resize(workerId);
        }
 
        //apply all nodeUpdates
        for (const auto& nodeCreationUpdate : u.nodes)
        {
            applyNodeCreationUpdate(nodeCreationUpdate, workerId);
        }
 
        //apply rewireUpdates
        for (const auto& rewireUpdate : u.rewires)
        {
            applyRewireUpdate(rewireUpdate);
        }
 
        //apply radiusUpdates
        for (const auto& radiusUpdate : u.radii)
        {
            applyRadiusUpdate(radiusUpdate);
        }
 
        //add new goal nodes
        applyGoalReachedUpdates(u.newGoalNodes);
        //increment status
        ++(appliedUpdateIds.at(workerId));
    }
 
    void Tree::sendCurrentUpdate(TreeUpdateInterfacePrx& prx)
    {
        ARMARX_CHECK_EXPRESSION(static_cast<Ice::Long>(getLocalUpdates().size()) - 1 == getCurrentUpdateId());
        getCurrentUpdateNonConst().dependetOnUpdateIds = appliedUpdateIds;
        prx->updateTree(getCurrentUpdate());
        //begin the next update
        prepareNextUpdate();
        //the update of this tree now counts as applied
        ++(appliedUpdateIds.at(workerId));
    }
 
    void Tree::applyRadiusUpdate(const RadiusUpdate& u)
    {
        if (u.increase)
        {
            doIncreaseRadius(u.node);
        }
        else
        {
            doDecreaseRadius(u.node);
        }
    }
 
    void Tree::applyRewireUpdate(const RewireUpdate& rewireUpdate)
    {
        //only rewire if it improves the cost
        const NodeId& child = rewireUpdate.child;
        const NodeId& newParent = rewireUpdate.newParent;
 
        const auto costOld = getNode(child).fromStartCost;
        const auto costNew = getNode(newParent).fromStartCost + rewireUpdate.fromNewParentCost;
 
        if (
            costOld < costNew ||
            ((costOld == costNew) && (rewireUpdate.newParent.workerId > getNode(child).parent.workerId)) //corner case
        )
        {
            //skip this update
            return;
        }
 
        doSetNodeParent(child, newParent, rewireUpdate.fromNewParentCost);
    }
 
    void Tree::prepareNextUpdate()
    {
        localUpdates.emplace_back();
        getCurrentUpdateNonConst().workerId = workerId;
        //dependent update ids gets set when the update is send
    }
 
    //changing the tree from the rrt algorithm
    NodeId Tree::addNode(ConfigType cfg, const NodeId& parent, float fromParentCost)
    {
        //add to current tree update
        createNewNodeCreationUpdate(cfg, parent, fromParentCost);
        //update parent
        increaseRadius(parent);
        //apply update
        return doAddNode(cfg, parent, fromParentCost, workerId);
    }
 
    //change the tree
    void  Tree::pushCosts(const NodeId& root)
    {
        for (const auto& child : getNode(root).children)
        {
            getNode(child).fromStartCost = getNode(child).fromParentCost + getNode(root).fromStartCost;
            pushCosts(child);
        }
    }
 
    NodeId Tree::doAddNode(ConfigType cfg, const NodeId& parent, float fromParentCost, std::size_t workerId)
    {
        NodeId id = getNextNodeIdFor(workerId);
        auto& parentNode = getNode(parent);
        ++nodeCount;
        nodes.at(workerId).emplace_back(NodeType
        {
            cfg,
            parent,
            std::numeric_limits<float>::infinity(),//radius
            fromParentCost,
            parentNode.fromStartCost + fromParentCost,
            std::set<NodeId>{} //children
        });
 
        parentNode.children.insert(id);
 
        return id;
    }
 
    void Tree::doSetNodeParent(const NodeId& child, const NodeId& newParent, float fromParentCost, bool updateFromStartCost)
    {
        const auto oldParent = getNode(child).parent;
        getNode(child).parent = newParent;
        //update forward edges
        getNode(oldParent).children.erase(child);
        getNode(newParent).children.insert(child);
        //update costs
        getNode(child).fromParentCost = fromParentCost;
 
        if (updateFromStartCost)
        {
            getNode(child).fromStartCost = fromParentCost + getNode(newParent).fromStartCost;
            pushCosts(child);
        }
    }
 
    void Tree::doDecreaseRadius(const NodeId& id)
    {
        if (getNode(id).radius < std::numeric_limits<float>::infinity())
        {
            getNode(id).radius = std::max(getNode(id).radius * (1.f - addParams.alpha), addParams.minimalRadius);
        }
        else
        {
            getNode(id).radius =  addParams.initialBorderRadius;
        }
    }
 
    //querry the tree
    std::vector<std::pair<NodeId, float>> Tree::getKNearestNeighboursAndDistances(const ConfigType& cfg, std::size_t k)
    {
        //change k to fit size
        //k = std::min(k, size());
        //calc all distances and use nth element + resize + shrik to fit to get the best k
        std::vector<std::pair<NodeId, float>> idDistanceVector {};
        idDistanceVector.reserve(size());
 
        for (std::size_t workerLevelIndex = 0; workerLevelIndex < nodes.size(); ++workerLevelIndex)
        {
            for (std::size_t nodeLevelIndex = 0; nodeLevelIndex < nodes.at(workerLevelIndex).size(); ++nodeLevelIndex)
            {
                const NodeId id
                {
                    static_cast<Ice::Long>(workerLevelIndex), static_cast<Ice::Long>(nodeLevelIndex)
                };
                const auto dist = WorkerNode::distance(nodes.at(workerLevelIndex).at(nodeLevelIndex).config, cfg);
                idDistanceVector.emplace_back(id, dist);
            }
        }
 
        ARMARX_CHECK_EXPRESSION(idDistanceVector.size() == size());
 
        //if we have <= k elements we dont need to search the smalles elements.
        if (k < idDistanceVector.size())
        {
            std::nth_element(
                idDistanceVector.begin(),
                idDistanceVector.begin() + (k - 1),
                idDistanceVector.end(),
                [](const std::pair<NodeId, float>& lhs, const std::pair<NodeId, float>& rhs)
            {
                return lhs.second < rhs.second;
            }
            );
            idDistanceVector.resize(k);
            //we could be using a large amount of memory => try to free it
            idDistanceVector.shrink_to_fit();
        }
 
        return idDistanceVector;
    }
 
    Tree::NodeType& Tree::getNode(const NodeId& id)
    {
        const auto worker = static_cast<std::size_t>(id.workerId);
        const auto node = static_cast<std::size_t>(id.numberOfNode);
        ARMARX_CHECK_EXPRESSION(worker < nodes.size());
        ARMARX_CHECK_EXPRESSION(node < nodes.at(worker).size());
        return nodes.at(worker).at(node);
    }
 
    const Tree::NodeType& Tree::getNode(const NodeId& id) const
    {
        const auto worker = static_cast<std::size_t>(id.workerId);
        const auto node = static_cast<std::size_t>(id.numberOfNode);
        ARMARX_CHECK_EXPRESSION(worker < nodes.size());
        ARMARX_CHECK_EXPRESSION(node < nodes.at(worker).size());
        return nodes.at(worker).at(node);
    }
 
    NodeId Tree::getIdOfIndex(std::size_t index) const
    {
        if (index >= size())
        {
            throw std::out_of_range {"index >= size"};
        }
 
        Ice::Long worker = 0;
 
        for (const auto& workerNodes : nodes)
        {
            if (index < workerNodes.size())
            {
                return {worker, static_cast<Ice::Long>(index)};
            }
 
            //adjust
            worker++;
            index -= workerNodes.size();
        }
 
        //since this tree should never decrease its size this line of code should never be called!
        //it could be called if a thread is concurrently changing the size of nodes,
        //causing iterators to be invalidated (thus causing ub while iterating in for(...))
        throw std::logic_error {"Tree size decreased during id calculation!"};
    }
 
    float Tree::getBestCost() const
    {
        auto min_elem = getBestCostIt();
        return ((min_elem == goalNodes.end()) ? std::numeric_limits<float>::infinity() : getNode(min_elem->node).fromStartCost + min_elem->costToGoToGoal);
    }
 
    //querry for paths
    PathWithCost Tree::getBestPath() const
    {
        auto min_elem = getBestCostIt();
        ARMARX_CHECK_EXPRESSION(min_elem != goalNodes.end());
        const auto id = min_elem->node;
        return PathWithCost {getPathTo(id), getNode(id).fromStartCost + min_elem->costToGoToGoal, "bestPath"};
    }
    PathWithCost Tree::getNthPathWithCost(std::size_t n) const
    {
        ARMARX_CHECK_EXPRESSION(n < goalNodes.size());
        //for client side request the index should be in bounds
        return PathWithCost {getPathTo(goalNodes.at(n).node), getNthPathCost(n), "Path_" + to_string(n)};
    }
 
    NodeIdList Tree::getNthPathIds(std::size_t n) const
    {
        ARMARX_CHECK_EXPRESSION(n < goalNodes.size());
        NodeIdList pathIds;
        auto id = goalNodes.at(n).node;
 
        //build reversed path
        while (id != ROOT_ID)
        {
            const auto& node = getNode(id);
            pathIds.emplace_back(id);
            id = node.parent;
        }
 
        //add the start node
        pathIds.emplace_back(ROOT_ID);
        std::reverse(pathIds.begin(), pathIds.end());
        return pathIds;
    }
 
    VectorXfSeq Tree::getPathTo(NodeId id) const
    {
        VectorXfSeq path;
 
        //build reversed path
        while (id != ROOT_ID)
        {
            const auto& node = getNode(id);
            path.emplace_back(node.config);
            id = node.parent;
        }
 
        //add the start node
        path.emplace_back(getNode(ROOT_ID).config);
        std::reverse(path.begin(), path.end());
        return path;
    }
 
    FullIceTree Tree::getIceTree() const
    {
        FullNodeDataListList allNodes {};
        allNodes.resize(nodes.size());
 
        for (std::size_t workerLevelIndex = 0; workerLevelIndex < nodes.size(); ++workerLevelIndex)
        {
            FullNodeDataList& iceWorkerNodes = allNodes.at(workerLevelIndex);
            const auto& workerNodes = nodes.at(workerLevelIndex);
            iceWorkerNodes.resize(workerNodes.size());
 
            for (std::size_t nodeLevelIndex = 0; nodeLevelIndex < iceWorkerNodes.size(); ++nodeLevelIndex)
            {
                const auto& localNode = workerNodes.at(nodeLevelIndex);
                FullNodeData& iceNode = iceWorkerNodes.at(nodeLevelIndex);
                iceNode.config = localNode.config;
                iceNode.parent = localNode.parent;
                iceNode.radius = localNode.radius;
                iceNode.fromParentCost = localNode.fromParentCost;
            }
        }
 
        return FullIceTree {allNodes, appliedUpdateIds};
    }
 
    void Tree::addPendingUpdate(const Update& u)
    {
        //fill up last spaces with no update (-1)
        //    increaseWorkerCountTo(u.dependetOnUpdateIds.size());//can't be called here because it invalidates iterators on nodes
        const auto nodeId = getUpdatesWorkerId(u);
        const auto updateSubId = getUpdatesSubId(u);
 
        if (hasPendingUpdate(nodeId, updateSubId))
        {
            return;
        }
 
        pendingUpdatesLookupTable[std::make_pair(nodeId, updateSubId)] = pendingUpdates.size();
        pendingUpdates.emplace_back(u);
    }
}