Task.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 "Task.h"
#include "../../util/CollisionCheckUtil.h"
 
#include <algorithm>
 
namespace armarx::astar
{
    Task::Task(const CSpaceBasePtr& cspace, const VectorXf& startCfg, const VectorXf& goalCfg, const std::string& taskName, float dcdStep,
               float gridStepSize, Ice::Long maximalPlanningTimeInSeconds)
    {
        this->cspace = cspace->clone();
        this->startCfg = startCfg;
        this->goalCfg = goalCfg;
        this->dcdStep = dcdStep;
        this->gridStepSize = gridStepSize;
        this->maximalPlanningTimeInSeconds = maximalPlanningTimeInSeconds;
        this->taskName = taskName;
 
        if (startCfg.size() != goalCfg.size() || startCfg.size() != static_cast<std::size_t>(this->cspace->getDimensionality()))
        {
            throw std::invalid_argument {"start and goal have to be the size of the cspace's dimensionality"};
        }
 
        if (gridStepSize < dcdStep)
        {
            throw std::invalid_argument {"the step size of the implicit grid has to be larger than the DCD step size"};
        }
    }
 
 
    Path Task::getPath(const Ice::Current&) const
    {
        std::lock_guard<std::mutex> lock {mtx};
        return solution;
    }
 
    void Task::run(const RemoteObjectNodePrxList&, const Ice::Current&)
    {
        const std::size_t n = cspace->getDimensionality();
        setTaskStatus(TaskStatus::ePlanning);
 
        //check trivial cases
        cspace->initCollisionTest();
 
        const auto startIsCollisionFree = cspace->isCollisionFree(std::pair<const Ice::Float*, const Ice::Float*> {startCfg.data(), startCfg.data() + startCfg.size()});
        const auto  goalIscollisionFree = cspace->isCollisionFree(std::pair<const Ice::Float*, const Ice::Float*> { goalCfg.data(), goalCfg.data() + goalCfg.size()});
        if (!startIsCollisionFree || !goalIscollisionFree)
        {
            setTaskStatus(TaskStatus::ePlanningFailed);
            return;
        }
 
        const auto distStartEnd = euclideanDistance(startCfg.begin(), startCfg.end(), goalCfg.begin());
 
        if (distStartEnd <= dcdStep)
        {
            setTaskStatus(TaskStatus::eDone);
            solution.nodes.emplace_back(startCfg);
            solution.nodes.emplace_back(goalCfg);
            return;
        }
 
        // //define grid
        using GridPointType = std::vector<long>;
 
        //grid bounds (the 0 gridpoint is the start cfg)
        LongRangeSeq gridBounds;
        gridBounds.reserve(n);
        const auto&& cspaceBounds = cspace->getDimensionsBounds();
        std::transform(
            cspaceBounds.begin(), cspaceBounds.end(), startCfg.begin(), std::back_inserter(gridBounds),
            [this](const FloatRange & dimBounds, float start)
        {
 
            const auto distLower = dimBounds.min - start;
            const auto gridPointsLower = std::trunc(distLower / gridStepSize);
 
            const auto distHigher = dimBounds.max - start;
            const auto gridPointsHigher = std::trunc(distHigher / gridStepSize);
 
            return LongRange {static_cast<Ice::Long>(gridPointsLower), static_cast<Ice::Long>(gridPointsHigher)};
 
        }
        );
 
 
        //calculates all neighbours in the grid (only along the cspace's coordinate axes)
        auto getNeighbours = [gridBounds, n](const GridPointType & p)
        {
            std::vector<GridPointType> neighbours;
            //allow diag neighbours
            neighbours.push_back(p);
            neighbours.reserve(std::pow(3, gridBounds.size()));
            for (std::size_t i = 0; i < n; ++i)
            {
                const auto n = neighbours.size();
                const auto generateLower = (gridBounds.at(i).min < p.at(i));
                const auto generateHigher = (gridBounds.at(i).max > p.at(i));
                neighbours.reserve(n * (1 + generateHigher + generateLower));//shold be already big enough
                if (generateLower)
                {
                    std::transform(
                        neighbours.begin(), neighbours.begin() + n, std::back_inserter(neighbours),
                        [i](typename decltype(neighbours)::value_type v)
                    {
                        --v.at(i);
                        return v;
                    }
                    );
                }
                if (generateHigher)
                {
                    std::transform(
                        neighbours.begin(), neighbours.begin() + n, std::back_inserter(neighbours),
                        [i](typename decltype(neighbours)::value_type v)
                    {
                        ++v.at(i);
                        return v;
                    }
                    );
                }
            }
            ARMARX_CHECK_EXPRESSION(p == neighbours.front());
            return neighbours;
        };
 
        //define grid conversion / distance meassure
        auto toConfig = [this](const GridPointType & gridPoint)
        {
            VectorXf cfg;
            cfg.reserve(gridPoint.size());
            std::transform(
                gridPoint.begin(), gridPoint.end(), startCfg.begin(), std::back_inserter(cfg),
                [this](float g, float s)
            {
                return s + g * gridStepSize;
            }
            );
            return cfg;
        };
 
        auto distance = [](const GridPointType & a, const GridPointType & b)
        {
            return euclideanDistance(a.begin(), a.end(), b.begin());
        };
 
        //project goal to grid
        GridPointType goal;
        goal.reserve(n);
        for (std::size_t i = 0; i < n; ++i)
        {
            auto dist = goalCfg.at(i) - startCfg.at(i);
            long gridPoint = std::round(dist / gridStepSize);
            goal.emplace_back(std::clamp(gridPoint, gridBounds.at(i).min, gridBounds.at(i).max));
        }
        if (!isPathCollisionFree(goalCfg, toConfig(goal)))
        {
            setTaskStatus(TaskStatus::ePlanningFailed);
            return;
        }
        setTaskStatus(TaskStatus::eDone);
 
        // //set up a*
        struct NodeData
        {
            GridPointType node;
            VectorXf cfg;
            float fScore = 0; //cost to goal
            float gScore = 0; //path cost
            std::size_t predecessor = 0;
        };
        //holds all created nodes
        std::deque<NodeData> nodes;
 
        std::set<std::size_t> openSet; //holds ids to nodes (these nodes are not processed yet)
        //add start to the open set
        nodes.emplace_back();
        nodes.back().cfg = startCfg;
        nodes.back().node.assign(n, 0);
        openSet.emplace(0);
 
        std::set<std::size_t> closedSet; //holds ids to nodes (these nodes are processed)
 
        auto assemblePath = [&](std::size_t toId)//returns the found path to a given node
        {
            std::deque<VectorXf> reversedPath;
            reversedPath.emplace_back(goalCfg);//maybe it was not on a grid point
            while (true)
            {
                auto& currentNode = nodes.at(toId);
                reversedPath.emplace_back(std::move(currentNode.cfg));
                if (toId == currentNode.predecessor)
                {
                    break;
                }
                toId = currentNode.predecessor;
            }
            VectorXfSeq path;
            path.reserve(reversedPath.size());
            std::move(reversedPath.rbegin(), reversedPath.rend(), std::back_inserter(path));
            return path;
        };
 
        auto expandNode = [&](std::size_t  idToExpand)
        {
            const auto& toExpand = nodes.at(idToExpand);
            const auto successors = getNeighbours(toExpand.node);//the first neighbour is the point itself
            for (std::size_t sucIdx = 1; sucIdx < successors.size(); ++sucIdx)
            {
                auto& successor = successors.at(sucIdx);
                //is the successor already in the open or closed set?
                auto successorIt = std::find_if(nodes.begin(), nodes.end(), [&](const NodeData & elem)
                {
                    return successor == elem.node;
                });
                auto successorId = std::distance(nodes.begin(), successorIt);
 
                if (closedSet.find(successorId) != closedSet.end())
                {
                    continue; //already in closed set
                }
 
                float tentativeGScore = toExpand.gScore + distance(successor, toExpand.node);
 
                bool successorIsInOpenSet = openSet.find(successorId) != openSet.end();
                if (successorIsInOpenSet && tentativeGScore >= successorIt->gScore)
                {
                    continue; // already a better path
                }
 
                //if successorIsInOpenSet the cfg will be moved back (2 moves should be faster than alloc + calc)
                auto successorCfg = successorIsInOpenSet ? std::move(successorIt->cfg) : toConfig(successor);
                //check path for collision
                if (!isPathCollisionFree(toExpand.cfg, successorCfg))
                {
                    continue;
                }
 
                const auto newFScore = tentativeGScore + distance(successor, goal);
                if (successorIsInOpenSet)
                {
                    //update
                    successorIt->cfg = std::move(successorCfg);
                    successorIt->fScore = newFScore;
                    successorIt->gScore = tentativeGScore;
                    successorIt->predecessor = idToExpand;
                }
                else
                {
                    //create entry
                    openSet.emplace(nodes.size());
                    nodes.emplace_back();
                    nodes.back().node = std::move(successor);
                    nodes.back().cfg = std::move(successorCfg);
                    nodes.back().fScore = newFScore;
                    nodes.back().gScore = tentativeGScore;
                    nodes.back().predecessor = idToExpand;
                }
            }
        };
 
        //wait for done or timeout
        const auto endTime = std::chrono::system_clock::now() + std::chrono::seconds {maximalPlanningTimeInSeconds};
 
        while (true)
        {
            //check for exit condition
            if (taskIsAborted)
            {
                setTaskStatus(TaskStatus::ePlanningAborted);
                return;
            }
            if (endTime < std::chrono::system_clock::now())
            {
                setTaskStatus(TaskStatus::ePlanningFailed);
                return;
            }
            if (openSet.empty())
            {
                //search done (but failed)!
                setTaskStatus(TaskStatus::ePlanningFailed);
                return;
            }
            auto currentIdIt = std::min_element(
                                   openSet.begin(), openSet.end(),
                                   [&](std::size_t osId1, std::size_t osId2)
            {
                return nodes.at(osId1).fScore < nodes.at(osId2).fScore;
            }
                               );
            auto currentId = *currentIdIt;
            openSet.erase(currentIdIt);
 
            if (nodes.at(currentId).node == goal)
            {
                solution.nodes = assemblePath(currentId);
                setTaskStatus(TaskStatus::eDone);
                return;
            }
 
            closedSet.emplace(currentId);
            expandNode(currentId);
        }
        ARMARX_CHECK_EXPRESSION(!"unreachable");
    }
 
 
    bool Task::isPathCollisionFree(const VectorXf& from, const VectorXf& to)
    {
        return dcdIsPathCollisionFree(
                   from, to, dcdStep,
                   [this](const VectorXf & cfg)
        {
            return cspace->isCollisionFree({cfg.data(), cfg.data() + cfg.size()});
        },
        false
               );
    }
}