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 <random>
#include "Task.h"
#include "../../util/CollisionCheckUtil.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <MotionPlanning/CSpace/CSpaceSampled.h>
#include <VirtualRobot/RobotNodeSet.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);
    }
}