Tree.h

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
 */
#pragma once
 
#include <deque>
#include <mutex>
#include <unordered_map>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/DataStructures.h>
#include <RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Task.h>
 
#include "util.h"
 
namespace armarx::addirrtstar
{
    /**
     * @brief A structure holding and managing all data connected to the tree used in the ADDIRRT* algorithm.
     *
     * Function overview:
     * [public   ] applyXYZUpdate(XYZUpdate u): applies an existing update u to the tree
     * [protected] createNewXYZUpdate(...)        : creates a new update and adds it to the tree's current update
     * [public   ] addNode/{in,de}creaseRadius/setNodeParent/addGoalReached: modifies the tree (through do functions)
     *      and adds the corresponding update to the local tree
     * [protected] doXYZ: performs the actual modification of the tree
     */
    class Tree
    {
        //types
    public:
        /**
         * @brief The type of configurations.
         */
        using ConfigType = VectorXf;
 
        /**
         * @brief Represents a node of thr rrt.
         */
        struct NodeType
        {
            /**
             * @brief The node's configuration.
             */
            ConfigType config;
            /**
             * @brief The node's parent.
             */
            NodeId parent;
            /**
             * @brief The node's radius. (adaptive dynamic domain)
             */
            float radius;
            /**
             * @brief Cost of the edge (parent node, this node)
             */
            float fromParentCost;
            /**
             * @brief Cost of the path (sttart node, this node)
             */
            float fromStartCost;
            /**
             * @brief Link to all children nodes
             */
            std::set<NodeId> children;
        };
 
        /**
         * @brief Lookuptable for pending updates.
         */
        using PendingUpdateLookuptableType =
            std::unordered_map<std::pair<std::size_t, std::size_t>, std::size_t>;
        /**
         * @brief Iterator for the PendingUpdateLookuptable.
         */
        using PendingUpdateLookuptableIterator = typename PendingUpdateLookuptableType::iterator;
        /**
         * @brief Const iterator for the PendingUpdateLookuptable.
         */
        using PendingUpdateLookuptableConstIterator =
            typename PendingUpdateLookuptableType::const_iterator;
 
        /**
         * @brief Default ctor.
         */
        Tree() = default;
 
        /**
         * @brief Creates a tree that can be used to analyze and apply updates.
         * Other operations are undefined.
         * @param startCfg The start config.
         */
        Tree(const VectorXf& startCfg);
 
        //init
        /**
         * @brief Initailizes the tree from the given tree.
         * @param iceTree The tree to use as initial state.
         * @param newaddParams The new adaptive dynamic domain parameters.
         * @param newWorkerId The owner's worker id.
         */
        void init(const FullIceTree& iceTree,
                  AdaptiveDynamicDomainParameters newaddParams,
                  std::size_t newWorkerId);
 
        /**
         * @brief Increases the storage of all data structures to be appropriate for count workers.
         * @param count The number of workers.
         */
        void increaseWorkerCountTo(std::size_t count);
 
        //updating
    public:
        /**
         * @param u The update.
         * @return Whether the given update can be applied.
         */
        bool canApplyUpdate(const Update& u);
        /**
         * @brief Adds a new update to the list of pending updates.
         * @param u The update.
         */
        void addPendingUpdate(const Update& u);
        /**
         * @brief Applies the given update.
         * @param u The update.
         */
        void applyUpdate(const Update& u);
        /**
         * @brief Sends the current update to the given proxy. A new update is prepared afterwards.
         * @param prx The proxy to send the update to.
         */
        void sendCurrentUpdate(TreeUpdateInterfacePrx& prx);
 
        /**
         * @return Returns all updates send by this tree.
         */
        const std::deque<Update>&
        getLocalUpdates() const
        {
            return localUpdates;
        }
 
        /**
         * @param workerId The update's worker id.
         * @param updateId The update's sub id.
         * @return Whether the given update was applied.
         */
        bool
        hasAppliedUpdate(std::size_t workerId, Ice::Long updateId)
        {
            ARMARX_CHECK_EXPRESSION(workerId < appliedUpdateIds.size());
            return updateId <= appliedUpdateIds.at(workerId);
        }
 
        /**
         * @param workerId The update's worker id.
         * @param updateId The update's sub id.
         * @return Whether the given update is in the list of pending updates.
         */
        bool
        hasPendingUpdate(std::size_t workerId, std::size_t updateId) const
        {
            return findPendingUpdate(workerId, updateId) != pendingUpdatesLookupTable.end();
        }
 
        /**
         * @param workerId The update's worker id.
         * @param updateId The update's sub id.
         * @return Returns the requested update from the list of pending updates.
         */
        const Update&
        getPendingUpdate(std::size_t workerId, std::size_t updateId) const
        {
            ARMARX_CHECK_EXPRESSION(hasPendingUpdate(workerId, updateId));
            return pendingUpdates.at(findPendingUpdate(workerId, updateId)->second);
        }
 
        /**
         * @brief Applies all pending updates. Updates are not consumes.
         * If a required update is missing this methode will fail. (no update getter is providet).
         * This function is not thread save! Use it only in a serial/protected environment.
         */
        void
        applyPendingUpdates()
        {
            std::mutex m;
            std::unique_lock<std::mutex> lock{m};
            applyPendingUpdates(
                lock,
                [](std::size_t w, Ice::Long u)
                {
                    ARMARX_ERROR_S
                        << "no remote update getter set but missing an update (w/s): " << w << "/"
                        << u;
                    throw std::logic_error{"no remote update getter set but missing an update!"};
                    return Update();
                });
        }
 
        /**
         * @brief The same as applyPendingUpdates(LockType, RemoteUpdateGetter, UpdateConsumer) but uses an noop as updateConsumer
         * This overload is needed since the defaultparameter does not work.
         * @param getRemoteUpdate Function used to request a missing update.
         * @see Tree::applyPendingUpdates
         */
        template <class LockType, class RemoteUpdateGetter>
        void
        applyPendingUpdates(LockType&& treeLock, RemoteUpdateGetter getRemoteUpdate)
        {
            applyPendingUpdates(treeLock, getRemoteUpdate, [](Update&&) {});
        }
 
        /**
         * @brief Applies all pending updates.
         * @param treeLock The lock used to protect the update structures.
         * It will be unlocked when getting a remote update.
         * @param getRemoteUpdate Function used to request a missing update.
         * @param updateConsumer Function used to consume updates after they were added. (will be moved to the consumer)
         */
        template <class LockType, class RemoteUpdateGetter, class UpdateConsumer>
        void
        applyPendingUpdates(
            LockType&& treeLock,
            RemoteUpdateGetter getRemoteUpdate,
            UpdateConsumer updateConsumer = [](Update&&) {} //no op
        )
        {
            ARMARX_CHECK_EXPRESSION(treeLock);
            ARMARX_CHECK_EXPRESSION(pendingUpdates.size() == pendingUpdatesLookupTable.size());
 
            //we want i for an assert after the loop
            std::size_t i = 0;
 
            for (; i < pendingUpdates.size(); ++i)
            {
                //apply this pending update
                applyPendingUpdate(i, treeLock, getRemoteUpdate, updateConsumer);
                ARMARX_CHECK_EXPRESSION(treeLock);
            }
 
            ARMARX_CHECK_EXPRESSION(treeLock);
            ARMARX_CHECK_EXPRESSION(i == pendingUpdates.size());
            //clear updates
            pendingUpdates.clear();
            pendingUpdatesLookupTable.clear();
        }
 
        /**
         * @return The update sub id of the current update. (The one currently constructed, not the last send)
         */
        Ice::Long
        getCurrentUpdateId() const
        {
            const auto id = static_cast<Ice::Long>(getLocalUpdates().size()) - 1;
            ARMARX_CHECK_EXPRESSION(id == appliedUpdateIds.at(workerId) + 1);
            return id;
        }
 
        /**
         * @return The update sub id of the last send update.
         */
        Ice::Long
        getPreviousUpdateId() const
        {
            return appliedUpdateIds.at(workerId);
        }
 
        /**
         * @return The currently constructed update.
         */
        const Update&
        getCurrentUpdate() const
        {
            return localUpdates.back();
        }
 
    protected:
        /**
         * @return The currently constructed update. (non const version for internal use)
         */
        Update&
        getCurrentUpdateNonConst()
        {
            return localUpdates.back();
        }
 
        /**
         * @brief Applies a radius update to the tree.
         * @param u The update.
         */
        void applyRadiusUpdate(const RadiusUpdate& u);
 
        /**
         * @brief Applies a node creation update to the tree.
         * @param u The update.
         * @param workerId The updates source worker.
         */
        void
        applyNodeCreationUpdate(const NodeCreationUpdate& u, std::size_t workerId)
        {
            doAddNode(u.config, u.parent, u.fromParentCost, workerId);
        }
 
        /**
         * @brief Applies a rewire update to the tree.
         * @param rewireUpdate The update.
         */
        void applyRewireUpdate(const RewireUpdate& rewireUpdate);
 
        /**
         * @brief Applies multiple goal reached updates to the tree.
         * @param newGoalNodes The new nodes that can reach the goal configuration.
         */
        void
        applyGoalReachedUpdates(const GoalInfoList newGoalNodes)
        {
            std::copy(newGoalNodes.begin(), newGoalNodes.end(), std::back_inserter(goalNodes));
        }
 
        /**
         * @brief Prepares the next update. (the update to be send)
         */
        void prepareNextUpdate();
 
        /**
         * @brief Adds a new radius update to the current update.
         * @param id The node's id.
         * @param increaseRadius Whether the node's radius increases.
         */
        void
        createNewRadiusUpdate(const NodeId& id, bool increaseRadius)
        {
            getCurrentUpdateNonConst().radii.emplace_back(RadiusUpdate{id, increaseRadius});
        }
 
        /**
         * @brief Adds a new node creation update to the current update.
         * @param cfg The new node's configuration.
         * @param parent The new node's parent node.
         * @param fromParentCost The cost of the edge (node, parent)
         */
        void
        createNewNodeCreationUpdate(const ConfigType& cfg,
                                    const NodeId& parent,
                                    float fromParentCost)
        {
            getCurrentUpdateNonConst().nodes.emplace_back(
                NodeCreationUpdate{cfg, parent, fromParentCost});
        }
 
        /**
         * @brief Adds a new node creation update to the current update.
         * @param child The rewired child node.
         * @param newParent The new parent node.
         * @param fromParentCost The cost of the edge (child, new parent)
         */
        void
        createNewRewireUpdate(const NodeId& child, const NodeId& newParent, float fromParentCost)
        {
            getCurrentUpdateNonConst().rewires.emplace_back(
                RewireUpdate{child, newParent, fromParentCost});
        }
 
        /**
         * @brief Adds a new goal reached update to the current update.
         * @param goal The node that can reach the goal configuration.
         * @param costToGoToGoal The cost to reach the goal configuration from the given node)
         */
        void
        createNewGoalReachedUpdate(const NodeId& goal, float costToGoToGoal)
        {
            getCurrentUpdateNonConst().newGoalNodes.emplace_back(GoalInfo{goal, costToGoToGoal});
        }
 
        /**
         * @brief Applies one pending update from the list of pending updates.
         * @param updateIndex The pending update's index.
         * @param treeLock The lock used to protect the update structures.
         * It will be unlocked when getting a remote update.
         * @param getRemoteUpdate Function used to request a missing update.
         * @param updateConsumer Function used to consume updates after they were added. (will be moved to the consumer)
         */
        template <class LockType, class RemoteUpdateGetter, class UpdateConsumer>
        void
        applyPendingUpdate(std::size_t updateIndex,
                           LockType&& treeLock,
                           RemoteUpdateGetter getRemoteUpdate,
                           UpdateConsumer updateConsumer)
        {
            ARMARX_CHECK_EXPRESSION(treeLock);
            ARMARX_CHECK_EXPRESSION(updateIndex < pendingUpdates.size());
 
            auto& u = pendingUpdates.at(updateIndex);
            increaseWorkerCountTo(u.dependetOnUpdateIds.size());
 
            const auto& dependetOnUpdateIds = u.dependetOnUpdateIds;
 
            ARMARX_CHECK_EXPRESSION(u.workerId >= 0);
            const auto updateWorkerId = static_cast<std::size_t>(u.workerId);
 
            if (dependetOnUpdateIds.empty())
            {
                //this update was moved from! => it was applied because it was queued out of order)
                return;
            }
 
            ARMARX_CHECK_EXPRESSION(updateWorkerId < dependetOnUpdateIds.size());
            const auto updateId = dependetOnUpdateIds.at(updateWorkerId) + 1;
 
            //is it an update from this node or was it already applied?
            if (updateWorkerId == workerId || //self update
                hasAppliedUpdate(updateWorkerId, updateId) //update was already applied
            )
            {
                return;
            }
 
            //this update is new => check if it requires prev updates (if we have then, apply them directly, else get them from remote)
            prepareUpdate(dependetOnUpdateIds, treeLock, getRemoteUpdate, updateConsumer);
            ARMARX_CHECK_EXPRESSION(treeLock);
 
            //now we have all required updates
            applyUpdate(u);
            //we dont need the update anymore. move it to the consumer
            updateConsumer(std::move(u));
        }
 
    public:
        /**
         * @brief Prepares an update by applying all dependet on updates.
         * @param dependetOnUpdateIds The ids of dependet on updates.
         * @param treeLock The lock used to protect the update structures.
         * It will be unlocked when getting a remote update.
         * @param getRemoteUpdate Function used to request a missing update.
         * @param updateConsumer Function used to consume updates after they were added. (will be moved to the consumer)
         */
        template <class LockType, class RemoteUpdateGetter, class UpdateConsumer>
        void
        prepareUpdate(Ice::LongSeq dependetOnUpdateIds,
                      LockType&& treeLock,
                      RemoteUpdateGetter getRemoteUpdate,
                      UpdateConsumer updateConsumer)
        {
            ARMARX_CHECK_EXPRESSION(treeLock);
 
            for (std::size_t workerNodeId = 0; workerNodeId < dependetOnUpdateIds.size();
                 ++workerNodeId)
            {
                const auto updateSubId = dependetOnUpdateIds.at(workerNodeId);
 
                if (hasAppliedUpdate(workerNodeId, updateSubId))
                {
                    //we have this update!
                    continue;
                }
 
                //the update was not applied
                //is this update local?
                auto it = findPendingUpdate(workerNodeId, updateSubId);
 
                if (it != pendingUpdatesLookupTable.end())
                {
                    auto missingIndex = (*it).second;
                    //recurse
                    applyPendingUpdate(missingIndex, treeLock, getRemoteUpdate, updateConsumer);
                }
                else
                {
                    ARMARX_CHECK_EXPRESSION(treeLock);
                    //get the update from remote. this may take a while => unlock
                    treeLock.unlock();
                    Update update = getRemoteUpdate(workerNodeId, updateSubId);
                    treeLock.lock();
                    //prepare this update and apply it
                    prepareUpdate(
                        update.dependetOnUpdateIds, treeLock, getRemoteUpdate, updateConsumer);
                    ARMARX_CHECK_EXPRESSION(treeLock);
                    applyUpdate(update);
                    //we dont need the update anymore. move it to the consumer
                    updateConsumer(std::move(update));
                }
            }
        }
 
    protected:
        /**
         * @param workerId The update's worker.
         * @param updateId The update's sub id.
         * @return An iterator tho the pending update. (If it does not exist to end.)
         */
        PendingUpdateLookuptableIterator
        findPendingUpdate(std::size_t workerId, std::size_t updateId)
        {
            return pendingUpdatesLookupTable.find(std::make_pair(workerId, updateId));
        }
 
        /**
         * @param workerId The update's worker.
         * @param updateId The update's sub id.
         * @return An iterator tho the pending update. (If it does not exist to end.) (const version)
         */
        PendingUpdateLookuptableConstIterator
        findPendingUpdate(std::size_t workerId, std::size_t updateId) const
        {
            return pendingUpdatesLookupTable.find(std::make_pair(workerId, updateId));
        }
 
        //changing the tree from the rrt algorithm
    public:
        /**
         * @brief Adds a node to the tree. (creates an appropriate update)
         * @param cfg The node's configuration.
         * @param parent The node's parent.
         * @param fromParentCost The cost of the edge (node, parent)
         * @return The new node's id.
         */
        NodeId addNode(ConfigType cfg, const NodeId& parent, float fromParentCost);
 
        /**
         * @brief Sets the parent node of child to parent. (costs are updated) (creates an appropriate update)
         * @param child The cild node.
         * @param newParent The new parent node.
         * @param fromParentCost The cost of the edge (child, new parent)
         */
        void
        setNodeParent(const NodeId& child, const NodeId& newParent, float fromParentCost)
        {
            doSetNodeParent(child, newParent, fromParentCost);
            createNewRewireUpdate(child, newParent, fromParentCost);
        }
 
        /**
         * @brief Decreases a node's radius.(creates an appropriate update)
         * @param id The node.
         */
        void
        decreaseRadius(const NodeId& id)
        {
            createNewRadiusUpdate(id, false);
            doDecreaseRadius(id);
        }
 
        /**
         * @brief Increases a node's radius.(creates an appropriate update)
         * @param id The node.
         */
        void
        increaseRadius(const NodeId& id)
        {
            createNewRadiusUpdate(id, true);
            doIncreaseRadius(id);
        }
 
        /**
         * @brief Adds a node to the list of nodes that can reach the goal configuration. (creates an appropriate update)
         * @param goal The node to add.
         * @param costToGoToGoal The cost to reach the gol configuration from the given node.
         */
        void
        addGoalReached(const NodeId& goal, float costToGoToGoal)
        {
            doAddGoalReached(goal, costToGoToGoal);
            createNewGoalReachedUpdate(goal, costToGoToGoal);
        }
 
    protected:
        //change the tree
        /**
         * @brief Updates fromStartCosts for all children from root.
         * @param root The root node for the cost update.
         */
        void pushCosts(const NodeId& root);
 
        /**
         * @brief Adds a node to the tree.
         * @param cfg The node's configuration.
         * @param parent The node's parent.
         * @param fromParentCost The cost of the edge (node, parent)
         * @param workerId The worker creating this node.
         * @return The new node's id.
         */
        NodeId
        doAddNode(ConfigType cfg, const NodeId& parent, float fromParentCost, std::size_t workerId);
        /**
         * @brief Sets the parent node of child to parent. (costs are updated)
         * @param child The cild node.
         * @param newParent The new parent node.
         * @param fromParentCost The cost of the edge (child, new parent)
         * @param updateFromStartCost Whether to update the costs of the subtree beneath cild.
         */
        void doSetNodeParent(const NodeId& child,
                             const NodeId& newParent,
                             float fromParentCost,
                             bool updateFromStartCost = true);
        /**
         * @brief Decreases a node's radius.
         * @param id The node.
         */
        void doDecreaseRadius(const NodeId& id);
 
        /**
         * @brief Increases a node's radius.
         * @param id The node.
         */
        void
        doIncreaseRadius(const NodeId& id)
        {
            getNode(id).radius *= (1.f + addParams.alpha); //inf will stay inf
        }
 
        /**
         * @brief Adds a node to the list of nodes that can reach the goal configuration.
         * @param goal The node to add.
         * @param costToGoToGoal The cost to reach the gol configuration from the given node.
         */
        void
        doAddGoalReached(const NodeId& goal, float costToGoToGoal)
        {
            goalNodes.emplace_back(GoalInfo{goal, costToGoToGoal});
        }
 
        //querry the tree
    public:
        /**
         * @param cfg The configuration.
         * @param k The number of neighbours.
         * @return The k nearest neighbours of and their distance to the given configuration.
         */
        std::vector<std::pair<NodeId, float>>
        getKNearestNeighboursAndDistances(const ConfigType& cfg, std::size_t k);
 
        /**
         * @param cfg The configuration.
         * @return The nearest neighbour of and its distance to the given configuration.
         */
        std::pair<NodeId, float>
        getNearestNeighbourAndDistance(const ConfigType& cfg)
        {
            return getKNearestNeighboursAndDistances(cfg, 1).front();
        }
 
        /**
         * @param id The node's id.
         * @return The requested node.
         */
        NodeType& getNode(const NodeId& id);
        /**
         * @param id The node's id.
         * @return The requested node. (const)
         */
        const NodeType& getNode(const NodeId& id) const;
 
        /**
         * @return All nodes.
         */
        const std::deque<std::deque<NodeType>>&
        getNodes() const
        {
            return nodes;
        }
 
        /**
         * @param index The index.
         * @return The id corresponding to the given node index.
         */
        NodeId getIdOfIndex(std::size_t index) const;
 
        /**
         * @return The number of nodes in the tree.
         */
        std::size_t
        size() const
        {
            return nodeCount;
        }
 
        /**
         * @param id The node's id.
         * @return The requested node.
         */
        NodeType&
        at(const NodeId& id)
        {
            return getNode(id);
        }
 
        /**
         * @param id The node's id.
         * @return The requested node. (const)
         */
        const NodeType&
        at(const NodeId& id) const
        {
            return getNode(id);
        }
 
        /**
         * @param index The node's index.
         * @return The requested node.
         */
        NodeType&
        at(std::size_t index)
        {
            return at(getIdOfIndex(index));
        }
 
        /**
         * @param index The node's index.
         * @return The requested node. (const)
         */
        const NodeType&
        at(std::size_t index) const
        {
            return at(getIdOfIndex(index));
        }
 
        /**
         * @return The current RRT with all updates applied. (used to transmitt it through ice.)
         */
        FullIceTree getIceTree() const;
 
    protected:
        /**
         * @return An iterator to the node that can reach the goal configuration and results in the lowest path cost. (end if no path was found)
         */
        typename std::deque<GoalInfo>::const_iterator
        getBestCostIt() const
        {
            return std::min_element(
                goalNodes.begin(),
                goalNodes.end(),
                [this](const GoalInfo& fst, const GoalInfo& snd)
                {
                    return (getNode(fst.node).fromStartCost + fst.costToGoToGoal) <
                           (getNode(snd.node).fromStartCost + snd.costToGoToGoal);
                });
        }
 
        //querry for paths
    public:
        /**
         * @return The cost of the shortest solution.
         */
        float getBestCost() const;
 
        /**
         * @return The path with the lowest cost.
         */
        PathWithCost getBestPath() const;
 
        /**
         * @param nodeId The node to check.
         * @return Whether the given node is a node in the list of nodes able to reach the goal configuration.
         */
        bool
        hasGoalNode(const NodeId& nodeId) const
        {
            return std::any_of(goalNodes.begin(),
                               goalNodes.end(),
                               [&nodeId](const GoalInfo& info) { return info.node == nodeId; });
        }
 
        /**
         * @return The number of paths found.
         */
        std::size_t
        getPathCount() const
        {
            return goalNodes.size();
        }
 
        /**
         * @param n The paths index.
         * @return The cost of the given path.
         */
        float
        getNthPathCost(std::size_t n) const
        {
            ARMARX_CHECK_EXPRESSION(n < goalNodes.size());
            return getNode(goalNodes.at(n).node).fromStartCost + goalNodes.at(n).costToGoToGoal;
        }
 
        /**
         * @param n N
         * @return The nth path.
         */
        PathWithCost getNthPathWithCost(std::size_t n) const;
        /**
         * @param n The path's index.
         * @return The node ids of nodes contained by the given path.
         */
        NodeIdList getNthPathIds(std::size_t n) const;
 
    protected:
        /**
         * @param id The node's id.
         * @return Returns a path to the requested node.
         */
        VectorXfSeq getPathTo(NodeId id) const;
        //utility
    public:
        /**
         * @param workerId The worker's id.
         * @return The next node id for the given worker.
         */
        NodeId
        getNextNodeIdFor(std::size_t workerId)
        {
            ARMARX_CHECK_EXPRESSION(workerId < nodes.size());
            return {static_cast<Ice::Long>(workerId),
                    static_cast<Ice::Long>(nodes.at(workerId).size())};
        }
 
        /**
         * @brief The root node's id
         */
        static const NodeId ROOT_ID;
 
        /**
         * @return List of ids of applied updates.
         */
        const Ice::LongSeq&
        getAppliedUpdateIds() const
        {
            return appliedUpdateIds;
        }
 
    protected:
        /**
         * @brief List of ids of applied updates.
         */
        Ice::LongSeq appliedUpdateIds;
        //deques are used because they don't need to copy all already stored data
        //when the capacity runs out
 
        /**
         * @brief Updates created by this tree.
         */
        std::deque<Update> localUpdates;
 
        /**
         * @brief Holds all nodes.
         * The node nodes[i][j] is the jth node created by worker i.
         */
        std::deque<std::deque<NodeType>> nodes;
        /**
         * @brief List of nodes able to reach the goal configuration.
         */
        std::deque<GoalInfo> goalNodes;
 
        /**
         * @brief The parameters of adaptive dynamic domain)
         */
        AdaptiveDynamicDomainParameters addParams;
 
        /**
         * @brief The worker id of this trees owner. (only used for sending updates and skipping updates send by this tree)
         */
        std::size_t workerId;
        /**
         * @brief The number of nodes in the rrt.
         */
        std::size_t nodeCount;
 
        /**
         * @brief Updates to apply to the tree.
         */
        std::deque<Update> pendingUpdates; //only default init required
 
        /**
         * @brief Speeds up lookup for available updates. (O(log(n)) instead of O(n))
         *
         * maps from UpdateId to the corresponding index in the pendingUpdates vector
         */
        PendingUpdateLookuptableType pendingUpdatesLookupTable; //only default init required
    };
} // namespace armarx::addirrtstar