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       2016
 * @copyright  http://www.gnu.org/licenses/gpl.txt
 *             GNU General Public License
 */
 
#include "Task.h"
 
#include <random>
 
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include "../../util/CollisionCheckUtil.h"
#include <MotionPlanning/CSpace/CSpaceSampled.h>
 
namespace armarx::rngshortcut
{
    Task::Task(MotionPlanningTaskBasePtr previousStep,
               const std::string& taskName,
               Ice::Long maxTimeForPostprocessingInSeconds,
               Ice::Float dcdStep,
               Ice::Long maxTries,
               Ice::Float minShortcutImprovementRatio,
               Ice::Float minPathImprovementRatio) :
        MotionPlanningTaskBase(taskName),
        PostprocessingMotionPlanningTaskBase(taskName, previousStep),
        PostprocessingMotionPlanningTask(previousStep, taskName),
        TaskBase(taskName,
                 previousStep,
                 dcdStep,
                 maxTries,
                 maxTimeForPostprocessingInSeconds,
                 minShortcutImprovementRatio,
                 minPathImprovementRatio)
    {
        if (!this->previousStep)
        {
            throw std::invalid_argument{"previousStep is NULL"};
        }
        if (this->dcdStep <= 0)
        {
            throw std::invalid_argument{"dcdStep <= 0"};
        }
        if (this->maxTimeForPostprocessingInSeconds <= 0)
        {
            throw std::invalid_argument{"maxTimeForPostprocessingInSeconds <= 0"};
        }
        if (this->minShortcutImprovementRatio >= 1)
        {
            throw std::invalid_argument{"minShortcutImprovementRatio >=1"};
        }
        if (this->minShortcutImprovementRatio < 0.01)
        {
            ARMARX_VERBOSE_S << "Changed minShortcutImprovementRatio from "
                             << this->minShortcutImprovementRatio << " to " << 0.01;
            this->minShortcutImprovementRatio = 0.01;
        }
        if (this->minPathImprovementRatio >= 1)
        {
            throw std::invalid_argument{"minPathImprovementRatio >=1"};
        }
        if (this->minPathImprovementRatio < 0.001)
        {
            ARMARX_VERBOSE_S << "Changed minShortcutImprovementRatio from "
                             << this->minShortcutImprovementRatio << " to " << 0.001;
            this->minPathImprovementRatio = 0.001;
        }
    }
 
    void
    Task::abortTask(const Ice::Current&)
    {
        if (!previousStep->finishedRunning())
        {
            previousStep->abortTask();
        }
        taskIsAborted = true;
    }
 
    void
    Task::run(const RemoteObjectNodePrxList& nodes, const Ice::Current&)
    {
        auto previousStep = MotionPlanningTaskPtr::dynamicCast(this->previousStep);
        ARMARX_CHECK_EXPRESSION(previousStep);
        previousStep->addTaskStatusCallback(
            [this](TaskStatus::Status s)
            {
                if (s != TaskStatus::eDone)
                {
                    setTaskStatus(s);
                }
            });
        auto endTime = std::chrono::high_resolution_clock::now() +
                       std::chrono::seconds{getMaximalPlanningTimeInSeconds()};
        previousStep->run(nodes);
        endTime = std::min(endTime,
                           std::chrono::high_resolution_clock::now() +
                               std::chrono::seconds{maxTimeForPostprocessingInSeconds});
        {
            std::lock_guard<std::mutex> lock{mtx};
            paths.reserve(2);
            auto p = previousStep->getPath();
            paths.emplace_back(p.nodes, 0, "smooth_" + p.pathName);
            paths.emplace_back(std::move(p.nodes), 0, std::move(p.pathName));
            //the path length is wrong!
            //we calculate it anyway
        }
 
        //setup the length vars.
        std::vector<float> lengths;
        auto updateLengths = [&, this]()
        {
            lengths.clear();
            lengths.reserve(paths.front().nodes.size() + 1);
            lengths.emplace_back(0);
            for (std::size_t i = 0; i + 1 < paths.front().nodes.size(); ++i)
            {
                auto& from = paths.front().nodes.at(i);
                auto& to = paths.front().nodes.at(i + 1);
                lengths.emplace_back(lengths.back() +
                                     euclideanDistance(from.begin(), from.end(), to.begin()));
            }
        };
        updateLengths();
 
        paths.at(0).cost = lengths.back();
        paths.at(1).cost = lengths.back();
        if (finishedRunning())
        {
            return;
        }
        ARMARX_CHECK_EXPRESSION(!paths.front().nodes.empty());
        if (getTaskStatus() != TaskStatus::eRefining)
        {
            setTaskStatus(TaskStatus::eRefining);
        }
        if (paths.front().nodes.size() == 2)
        {
            //this was a trivial path
            setTaskStatus(TaskStatus::eDone);
            return;
        }
 
        getCSpace()->initCollisionTest();
 
        std::mt19937 gen{std::random_device{}()};
        Ice::Long iteration = 0;
        for (; iteration < maxTries;)
        //don't increment the iteration count here.
        // we only want to count iterations that do collision checking.
        {
            ARMARX_CHECK_EXPRESSION(paths.front().nodes.size() == lengths.size());
            //stop?
            if (taskIsAborted)
            {
                setTaskStatus(TaskStatus::eRefinementAborted);
                return;
            }
            if (endTime <= std::chrono::high_resolution_clock::now())
            {
                break;
            }
 
            auto segmentOffsets = calcOffsets(lengths.back(), gen);
            float segmentLength = segmentOffsets.second - segmentOffsets.first;
 
            VectorXf from(getCSpace()->getDimensionality());
            VectorXf to(getCSpace()->getDimensionality());
 
            //nodes with idx < are kept
            std::size_t takeLastOPTE = 0;
            //nodes with idx >= are kept
            std::size_t takeAgainFirstIdx = 0;
            // select from and to bounding the segment to replace
            // calc the last node to keep before the segment starts and the first node to keep after tehe segment ends
            {
                std::size_t i = 0;
 
                //search node i with: segmentPoints.first <= pathLenght(start,node_i)
                for (; i < lengths.size() && segmentOffsets.first > lengths.at(i); ++i)
                    ;
                takeLastOPTE = i;
                if (takeLastOPTE + 1 == lengths.size())
                {
                    //already at the end
                    continue;
                }
                ARMARX_CHECK_EXPRESSION(takeLastOPTE + 1 < lengths.size());
                if (takeLastOPTE == 0)
                {
                    ARMARX_CHECK_EXPRESSION(segmentOffsets.first == 0);
                    //start the segment to replace at the start
                    from = paths.front().nodes.at(0);
                }
                else
                {
                    ARMARX_CHECK_EXPRESSION(takeLastOPTE);
                    const auto fromSubSegLen =
                        lengths.at(takeLastOPTE) - lengths.at(takeLastOPTE - 1);
                    const auto fromSubSegLenPart =
                        segmentOffsets.first - lengths.at(takeLastOPTE - 1);
                    if (fromSubSegLenPart < 1e-5)
                    {
                        //distance to prev node is low
                        //snap to node in path
                        from = paths.front().nodes.at(takeLastOPTE - 1);
                        --takeLastOPTE; //dont add the node a second time
                    }
                    else
                    {
                        const auto fromSubSegLenRatio = fromSubSegLenPart / fromSubSegLen;
 
                        ARMARX_CHECK_EXPRESSION(from.size() ==
                                                paths.front().nodes.at(takeLastOPTE - 1).size());
                        ARMARX_CHECK_EXPRESSION(from.size() ==
                                                paths.front().nodes.at(takeLastOPTE).size());
 
                        std::transform(paths.front().nodes.at(takeLastOPTE - 1).begin(),
                                       paths.front().nodes.at(takeLastOPTE - 1).end(),
                                       paths.front().nodes.at(takeLastOPTE).begin(),
                                       from.begin(),
                                       [fromSubSegLenRatio](float from, float to) {
                                           return (1.f - fromSubSegLenRatio) * from +
                                                  fromSubSegLenRatio * to;
                                       });
                    }
                }
 
                for (; i < lengths.size() && segmentOffsets.second > lengths.at(i); ++i)
                    ;
                takeAgainFirstIdx = i;
                ARMARX_CHECK_EXPRESSION(takeAgainFirstIdx);
                if (takeAgainFirstIdx >= lengths.size())
                {
                    //to the end
                    takeAgainFirstIdx = paths.front().nodes.size();
                    to = paths.front().nodes.back();
                }
                else
                {
                    ARMARX_CHECK_EXPRESSION(takeAgainFirstIdx < lengths.size());
                    //to either some node in the middle
                    const auto toSubSegLen =
                        lengths.at(takeAgainFirstIdx) - lengths.at(takeAgainFirstIdx - 1);
                    const auto toSubSegLenPart =
                        segmentOffsets.second - lengths.at(takeAgainFirstIdx - 1);
                    if (lengths.at(takeAgainFirstIdx) - toSubSegLenPart < 1e-5)
                    {
                        //snap to node in path
                        to = paths.front().nodes.at(takeAgainFirstIdx);
                        ++takeAgainFirstIdx; //dont add the node a second time
                    }
                    else
                    {
                        const auto toSubSegLenRatio = toSubSegLenPart / toSubSegLen;
                        ARMARX_CHECK_EXPRESSION(takeAgainFirstIdx > 0);
                        ARMARX_CHECK_EXPRESSION(
                            to.size() == paths.front().nodes.at(takeAgainFirstIdx - 1).size());
                        ARMARX_CHECK_EXPRESSION(to.size() ==
                                                paths.front().nodes.at(takeAgainFirstIdx).size());
                        std::transform(
                            paths.front().nodes.at(takeAgainFirstIdx - 1).begin(),
                            paths.front().nodes.at(takeAgainFirstIdx - 1).end(),
                            paths.front().nodes.at(takeAgainFirstIdx).begin(),
                            to.begin(),
                            [toSubSegLenRatio](float from, float to)
                            { return (1.f - toSubSegLenRatio) * from + toSubSegLenRatio * to; });
                    }
                }
            }
            if (takeLastOPTE == takeAgainFirstIdx)
            {
                continue;
            }
            float segmentShortcutLength = euclideanDistance(from.begin(), from.end(), to.begin());
 
            if (segmentShortcutLength > segmentLength * (1.f - minShortcutImprovementRatio))
            {
                // the possible improvement is not big enough
                continue;
            }
            //we only count iterations with collision checks
            ++iteration;
            if (!isPathCollisionFree(from, to))
            {
                continue;
            }
 
            ARMARX_CHECK_EXPRESSION(takeLastOPTE < takeAgainFirstIdx);
            VectorXfSeq newPostprocessedSolution;
            newPostprocessedSolution.reserve(paths.front().nodes.size() +
                                             1); //max one node longer (upper bound for new length
            std::copy(paths.front().nodes.begin(),
                      paths.front().nodes.begin() + takeLastOPTE,
                      std::back_inserter(newPostprocessedSolution));
            newPostprocessedSolution.emplace_back(std::move(from));
            newPostprocessedSolution.emplace_back(std::move(to));
            std::copy(paths.front().nodes.begin() + takeAgainFirstIdx,
                      paths.front().nodes.end(),
                      std::back_inserter(newPostprocessedSolution));
 
            {
                std::lock_guard<std::mutex> lock{mtx};
                paths.front().nodes = std::move(newPostprocessedSolution);
            }
            //the path changed
            updateLengths();
        }
        ARMARX_VERBOSE << "Smoothed with " << iteration << " tries";
        setTaskStatus(TaskStatus::eDone);
    }
 
    Ice::Long
    Task::getPathCount(const Ice::Current&) const
    {
        std::lock_guard<std::mutex> lock{mtx};
        return paths.size();
    }
 
    PathWithCost
    Task::getBestPath(const Ice::Current&) const
    {
        return getNthPathWithCost(0);
    }
 
    PathWithCost
    Task::getNthPathWithCost(Ice::Long n, const Ice::Current&) const
    {
        std::lock_guard<std::mutex> lock{mtx};
        return paths.at(n);
    }
 
    PathWithCostSeq
    Task::getAllPathsWithCost(const Ice::Current&) const
    {
        std::lock_guard<std::mutex> lock{mtx};
        return paths;
    }
 
    bool
    Task::isPathCollisionFree(const VectorXf& from, const VectorXf& to)
    {
        // it is usually either a simox cspace or an adapter, if not use standard interpolation
 
 
        SimoxCSpacePtr simoxcspace = SimoxCSpacePtr::dynamicCast(getCSpace());
        if (!simoxcspace)
        {
            CSpaceAdaptorBasePtr simoxcspaceadapter =
                CSpaceAdaptorBasePtr::dynamicCast(getCSpace());
            if (simoxcspaceadapter)
            {
                simoxcspace = SimoxCSpacePtr::dynamicCast(simoxcspaceadapter->getOriginalCSpace());
            }
        }
        if (simoxcspace)
        {
            // do robot specific interpolation
            VirtualRobot::RobotNodeSetPtr rns = VirtualRobot::RobotNodeSet::createRobotNodeSet(
                simoxcspace->getAgentSceneObj(), "tmp", simoxcspace->getAgentJointNames());
            Saba::CSpaceSampled tmpCSpace(
                simoxcspace->getAgentSceneObj(),
                VirtualRobot::CDManagerPtr(new VirtualRobot::CDManager(simoxcspace->getCD())),
                rns);
            auto distance = [&, this](Eigen::VectorXf const& from, Eigen::VectorXf const& to)
            { return tmpCSpace.calcDist(from, to); };
 
            auto interpolation =
                [&, this](Eigen::VectorXf const& from, Eigen::VectorXf const& to, float step)
            { return tmpCSpace.interpolate(from, to, step); };
            return dcdIsPathCollisionFree(
                from,
                to,
                dcdStep,
                [this](const VectorXf& cfg) {
                    return getCSpace()->isCollisionFree({cfg.data(), cfg.data() + cfg.size()});
                },
                false,
                std::optional<decltype(distance)>(distance),
                std::optional<decltype(interpolation)>(interpolation));
        }
        else
        {
            return dcdIsPathCollisionFree(
                from,
                to,
                dcdStep,
                [this](const VectorXf& cfg) {
                    return getCSpace()->isCollisionFree({cfg.data(), cfg.data() + cfg.size()});
                },
                false);
        }
    }
 
    std::pair<float, float>
    Task::calcOffsets(float length, std::mt19937& gen)
    {
        std::uniform_real_distribution<float> distA{
            0, std::nextafter(length, std::numeric_limits<float>::max())};
        const auto a = distA(gen);
        const auto minDelta = minPathImprovementRatio * length;
        std::uniform_real_distribution<float> distB{
            0, std::nextafter(length - 2 * minDelta, std::numeric_limits<float>::max())};
        const auto bSample = distB(gen);
        const auto b = (bSample <= (a - minDelta)) ? bSample : bSample + 2 * minDelta;
 
        return std::minmax(a, b);
    }
} // namespace armarx::rngshortcut