AStarPathPlanner.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    RobotComponents::PathPlanner
* @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
* @date       2015 Humanoids Group, H2T, KIT
* @license    http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "AStarPathPlanner.h"
 
#include <cmath>
#include <map>
 
armarx::Vector3IBase
operator+(const armarx::Vector3IBase& lhs, const armarx::Vector3IBase& rhs)
{
    return armarx::Vector3IBase{lhs.x + rhs.x, lhs.y + rhs.y, lhs.z + rhs.z};
}
 
armarx::AStarPathPlanner::AStarPathPlanner() :
    AStarPathPlannerBase(250.f, //tStepSize
                         {0.f, 5500.f}, //xBoundingRange
                         {0.f, 11000.f}, //yBoundingRange
                         {
                             {+1, 0, 0}, //step in x
                             {-1, 0, 0}, //step in -x
                             {0, +1, 0}, //step in x
                             {0, -1, 0}, //step in -y
                             {0, 0, +1}, //rotate +
                             {0, 0, -1}, //rotate -
                             {+1, +1, 0}, //step in +x, +y
                             {+1, -1, 0}, //step in +x, -y
                             {-1, +1, 0}, //step in -x, +y
                             {-1, -1, 0} //step in -x, -y
                         }, //neighbourDeltas
                         100000000.f, //rotationFactorForHeuristic
                         1.f //distanceFactorForHeuristic
                         ),
    stepSizeDeg{5}
{
}
 
//slice impl from PathPlannerBase
void
armarx::AStarPathPlanner::setTranslationtStepSize(::Ice::Float step, const ::Ice::Current&)
{
    if (step < 0)
    {
        std::stringstream s;
        s << "Invalid translation step size: " << step << " < 0.0";
        ARMARX_ERROR_S << s.str();
        throw armarx::InvalidArgumentException{s.str()};
    }
 
    tStepSize = step;
}
 
void
armarx::AStarPathPlanner::setRotationStepSize(::Ice::Float step, const ::Ice::Current&)
{
    //step is in rad
    //we need values in [0, 360) but enforce this at a later point
    //(we add stepSizeDeg so we have to normalize the result to [0, 360) anyway.
    stepSizeDeg = static_cast<int>(step / M_PI * 180);
}
 
void
armarx::AStarPathPlanner::setBoundingXRange(const ::armarx::FloatRange& range,
                                            const ::Ice::Current&)
{
    if (range.max < range.min)
    {
        std::stringstream s;
        s << "Invalid bounding range for x dimension: min = " << range.min
          << ", max = " << range.max;
        ARMARX_ERROR_S << s.str();
        throw armarx::InvalidArgumentException{s.str()};
    }
 
    xBoundingRange = range;
}
 
void
armarx::AStarPathPlanner::setBoundingYRange(const ::armarx::FloatRange& range,
                                            const ::Ice::Current&)
{
    if (range.max < range.min)
    {
        std::stringstream s;
        s << "Invalid bounding range for y dimension: min = " << range.min
          << ", max = " << range.max;
        ARMARX_ERROR_S << s.str();
        throw armarx::InvalidArgumentException{s.str()};
    }
 
    yBoundingRange = range;
}
 
void
armarx::AStarPathPlanner::setNeighbourSteps(const ::armarx::Vector3IBaseList& neighbours,
                                            const ::Ice::Current&)
{
    neighbourDeltas = neighbours;
}
 
void
armarx::AStarPathPlanner::setRotationFactorForHeuristic(::Ice::Float factor, const ::Ice::Current&)
{
    rotationFactorForHeuristic = factor;
}
 
void
armarx::AStarPathPlanner::setDistanceFactorForHeuristic(::Ice::Float factor, const ::Ice::Current&)
{
    distanceFactorForHeuristic = factor;
}
 
float
armarx::AStarPathPlanner::heuristic(const armarx::Vector3& from, const armarx::Vector3& to) const
{
    const Eigen::Vector3f delta = to.toEigen() - from.toEigen();
    return distanceFactorForHeuristic * std::hypot(delta(0), delta(1)) +
           rotationFactorForHeuristic * std::abs(delta(2));
}
 
float
armarx::AStarPathPlanner::heuristic(const armarx::Vector3IBase& from,
                                    const armarx::Vector3IBase& to) const
{
    return distanceFactorForHeuristic * std::hypot(to.x - from.x, to.y - from.y) +
           rotationFactorForHeuristic * std::abs(to.z - from.z);
}
 
//slice impl from PathPlannerBase
::armarx::Vector3BaseList
armarx::AStarPathPlanner::getPath(const ::armarx::Vector3BasePtr& from,
                                  const ::armarx::Vector3BasePtr& to,
                                  const ::Ice::Current&) const
{
    /*
     * in this context the values of a Vector3IBase are interpreted as:
     * x => steps of size tStepSize in x direction from from.x
     * y => steps of size tStepSize in y direction from from.y
     * z => rotation in deg (in [0, 360)) with 0 <=> from->z
     * so (0, 0, 0) is the starting point from
     */
 
    if (!(isPositionValid(Vector3{from->x, from->y, from->z}) &&
          isPositionValid(Vector3{to->x, to->y, to->z})))
    {
        return Vector3BaseList{};
    }
 
    assert(from);
    assert(to);
    assert(agent);
 
    ARMARX_VERBOSE << "Planning path from (" << from->x << ", " << from->y << ", " << from->z
                   << ") to (" << to->x << ", " << to->y << ", " << to->z << ")";
 
    auto toGlobal = [&](const armarx::Vector3IBase& node)
    {
        return armarx::Vector3{from->x + node.x * tStepSize,
                               from->y + node.y * tStepSize,
                               from->z + /*deg to rad*/ static_cast<float>(node.z * M_PI / 180)};
    };
 
    auto getNeighbours = [&](const armarx::Vector3IBase& node)
    {
        armarx::Vector3IBaseList neighbours;
 
        for (const auto& delta : neighbourDeltas)
        {
            armarx::Vector3IBase neighbour{
                node.x + delta.x,
                node.y + delta.y,
                (((node.z + delta.z * stepSizeDeg) % 360) + 360) %
                    360 /*is rotation in deg (in [0, 360))*/
            };
            const armarx::Vector3 globalNeighbour = toGlobal(neighbour);
 
            if (globalNeighbour.x <= xBoundingRange.max &&
                globalNeighbour.x >= xBoundingRange.min &&
                globalNeighbour.y <= yBoundingRange.max &&
                globalNeighbour.y >= yBoundingRange.min && isPositionValid(globalNeighbour))
            {
                neighbours.push_back(neighbour);
            }
        }
 
        return neighbours;
    };
 
    //    auto drawDebugPoint = [&](const armarx::Vector3IBase & point, armarx::DrawColor color)
    //    {
    //        //visualize current
    //        std::stringstream s;
    //        s << "node" << point.x << "_" << point.y << "_" << point.z;
    //        armarx::Vector3BasePtr positionToDraw {new Vector3{toGlobal(point).toEigen()}};
    //        positionToDraw->z = point.z * 10;
    //        debugDrawer->setSphereDebugLayerVisu(s.str(), positionToDraw, color, 30.f);
    //    };
 
    const armarx::Vector3IBase start{0, 0, 0};
 
    armarx::Vector3 fromToDelta{to->x - from->x, to->y - from->y, to->z - from->z};
 
    armarx::Vector3IBase goal{static_cast<int>(std::lround(fromToDelta.x / tStepSize)),
                              static_cast<int>(std::lround(fromToDelta.y / tStepSize)),
                              ((static_cast<int>(fromToDelta.z / M_PI * 180) % 360) + 360) %
                                  360}; //we need values in [0, 360)
 
    //if goal.z == 10, start.z == 0 and stepSizeDeg == 20 we can't reach the goal.z value => we have to round goal.z
    int goalZMod = goal.z % stepSizeDeg;
    goal.z = goal.z - goalZMod; //floor
    goal.z += ((goalZMod >= stepSizeDeg / 2) ? stepSizeDeg : 0); //round up if needed
 
    const armarx::Vector3Ptr globalGoalPtr = armarx::Vector3Ptr::dynamicCast(to);
    assert(globalGoalPtr);
 
    ::armarx::Vector3BaseList path{};
    std::vector<armarx::Vector3IBase> openSet{};
    std::vector<armarx::Vector3IBase> closedSet{};
 
    openSet.push_back(start);
 
    std::map<armarx::Vector3IBase, float> fScore;
    std::map<armarx::Vector3IBase, float> gScore;
    gScore[start] = 0.f;
    fScore[start] = gScore.at(start) + heuristic(toGlobal(start), *globalGoalPtr);
 
    std::map<armarx::Vector3IBase, armarx::Vector3IBase> cameFrom;
    cameFrom[goal] = start; //in case start==goal
 
    ARMARX_VERBOSE << "Setup done. Star planning...";
 
    //    drawDebugPoint(start, armarx::DrawColor {0.f, 1.f, 0.f, 1.f});
    //    drawDebugPoint(goal, armarx::DrawColor {0.f, 0.f, 1.f, 1.f});
 
    while (!openSet.empty())
    {
        //find best
        auto currentIT =
            std::min_element(openSet.begin(),
                             openSet.end(),
                             [&](const armarx::Vector3IBase& a, const armarx::Vector3IBase& b)
                             { return fScore.at(a) < fScore.at(b); });
        assert(currentIT != openSet.end());
        const armarx::Vector3IBase current = *currentIT;
 
        //check if done
        if ((current.x == goal.x) && (current.y == goal.y) && (current.z == goal.z))
        {
            ARMARX_VERBOSE << "Assembling path";
 
            armarx::Vector3IBase cameFromNode = goal;
 
            while (cameFromNode != start)
            {
                armarx::Vector3BasePtr globalCameFromNodePtr =
                    armarx::Vector3BasePtr{new armarx::Vector3{toGlobal(cameFromNode)}};
                path.insert(path.begin(), globalCameFromNodePtr);
                cameFromNode = cameFrom.at(cameFromNode);
            }
 
            path.insert(path.begin(), from);
            break;
        }
 
        openSet.erase(currentIT);
        closedSet.push_back(current);
 
        //        drawDebugPoint(current, armarx::DrawColor {1.f, 0.f, 1.f, 1.f});
 
        //update
        for (const armarx::Vector3IBase& neighbour : getNeighbours(current))
        {
            if (std::find(closedSet.begin(), closedSet.end(), neighbour) != closedSet.end())
            {
                continue;
            }
 
            //const armarx::Vector3 globalCurrent = toGlobal(current);
            //const armarx::Vector3 globalNeighbour = toGlobal(neighbour);
 
            //float tentativeGScore = gScore.at(current) + heuristic(globalCurrent, globalNeighbour);
            float tentativeGScore = gScore.at(current) + heuristic(current, neighbour);
            bool notInOS = std::find(openSet.begin(), openSet.end(), neighbour) == openSet.end();
 
            if (notInOS || tentativeGScore < gScore.at(neighbour))
            {
                cameFrom[neighbour] = current;
                gScore[neighbour] = tentativeGScore;
                //fScore[neighbour] = tentativeGScore + heuristic(globalNeighbour, *globalGoalPtr);
                fScore[neighbour] = tentativeGScore + heuristic(neighbour, goal);
 
                if (notInOS)
                {
                    //                    drawDebugPoint(neighbour, armarx::DrawColor {1.f, 0.f, 0.f, 0.5f});
 
                    openSet.push_back(neighbour);
                }
            }
        }
    }
 
    return path;
}