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 <algorithm>
 
#include "../../util/CollisionCheckUtil.h"
 
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);
    }
} // namespace armarx::astar