Task.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2017, 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    ArmarX
 * @author     Mirko Waechter( mirko.waechter at kit dot edu)
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "Task.h"
 
#include "../../util/CollisionCheckUtil.h"
#include "../../util/Metrics.h"
 
#include <MotionPlanning/Planner/BiRrt.h>
#include <MotionPlanning/CSpace/CSpacePath.h>
#include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
#include <ArmarXCore/core/util/OnScopeExit.h>
#include <MotionPlanning/PostProcessing/ShortcutProcessor.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
namespace armarx::birrt
{
 
    Task::Task(const CSpaceBasePtr& cspace, const VectorXf& startCfg, const VectorXf& goalCfg, const std::string& taskName, Ice::Long maximalPlanningTimeInSeconds, float dcdStep)
    {
        this->cspace = cspace->clone();
        this->startCfg = startCfg;
        this->goalCfg = goalCfg;
        this->dcdStep = dcdStep;
        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"};
        }
    }
 
    void Task::abortTask(const::Ice::Current&)
    {
    }
 
    Path Task::getPath(const::Ice::Current&) const
    {
        std::lock_guard<std::mutex> lock {mtx};
        return solution;
    }
 
    void Task::run(const RemoteObjectNodePrxList&, const::Ice::Current&)
    {
 
        auto failureOutput = [this]
        {
            SimoxCSpacePtr origCSpace = getOriginalCSpace();
            ARMARX_CHECK_EXPRESSION(origCSpace);
 
            std::stringstream ss;
            for (auto set : origCSpace->getCD().getSceneObjectSets())
            {
                if (origCSpace->getCD().isInCollision(set))
                {
                    std::string name = set->getName();
                    Ice::StringSeq names;
                    if (name.empty())
                    {
                        ARMARX_CHECK_EXPRESSION(set);
                        for (auto e : set->getSceneObjects())
                        {
                            names.push_back(e->getName());
                        }
                    }
                    else
                    {
                        names.push_back(name);
                    }
 
                    ss << "scene object set consisting of " << simox::alg::join(names, ", ") << " is in collision\n";
                }
            }
            return ss.str();
        };
 
 
        setTaskStatus(TaskStatus::ePlanning);
        ARMARX_CHECK_EXPRESSION(cspace);
        //check trivial cases
        cspace->initCollisionTest();
 
        const auto startIsCollisionFree = cspace->isCollisionFree(std::pair<const Ice::Float*, const Ice::Float*> {startCfg.data(), startCfg.data() + startCfg.size()});
        if (!startIsCollisionFree)
        {
            ARMARX_WARNING << "BiRRT failed trivially: start config is in collision: " << startCfg << " Collisions:\n" << failureOutput();
 
            setTaskStatus(TaskStatus::ePlanningFailed);
            return;
        }
        const auto  goalIscollisionFree = cspace->isCollisionFree(std::pair<const Ice::Float*, const Ice::Float*> { goalCfg.data(), goalCfg.data() + goalCfg.size()});
        if (!goalIscollisionFree)
        {
            ARMARX_WARNING << "BiRRT failed trivially: goal config is in collision" << goalCfg << " Collisions:\n" << failureOutput();
            setTaskStatus(TaskStatus::ePlanningFailed);
            return;
        }
 
        SimoxCSpacePtr origCSpace = getOriginalCSpace();
        ARMARX_CHECK_EXPRESSION(origCSpace);
        ARMARX_INFO << "RobotConfig: " << origCSpace->getAgentSceneObj()->getConfig()->getRobotNodeJointValueMap();
        Saba::CSpaceSampledPtr sampledcSpace = getOriginalCSpace()->createSimoxCSpace();
        ARMARX_CHECK_EXPRESSION(sampledcSpace);
        IceUtil::Time start = IceUtil::Time::now();
        ARMARX_ON_SCOPE_EXIT
        {
            ARMARX_INFO << "BiRRT took : " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms";
        };
 
        ScaledCSpacePtr scaledCSpace = ScaledCSpacePtr::dynamicCast(cspace);
        if (scaledCSpace)
        {
            ARMARX_VERBOSE << "unscaling configs " << VAROUT(startCfg) << VAROUT(goalCfg);
            scaledCSpace->unscaleConfig(startCfg);
            scaledCSpace->unscaleConfig(goalCfg);
            sampledcSpace->setMetricWeights(Eigen::Map<Eigen::VectorXf>(scaledCSpace->getScalingFactors().data(), scaledCSpace->getScalingFactors().size()));
            ARMARX_VERBOSE << "unscaled configs " << VAROUT(startCfg) << VAROUT(goalCfg);
        }
 
        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;
        }
 
 
 
        Saba::Rrt::RrtMethod mode;
        mode = Saba::Rrt::eExtend;
        ARMARX_CHECK_EXPRESSION(cspace);
        ARMARX_INFO << "CSpace type : " << cspace->ice_id();
 
 
        ARMARX_INFO << "Created simox cspace";
        //            float samplingSize = 0.04f;
        //            sampledcSpace->setSamplingSize(samplingSize);
        sampledcSpace->setSamplingSizeDCD(dcdStep);
        ARMARX_INFO << "SamplingSizeDCD: " << sampledcSpace->getSamplingSizeDCD();
        Saba::BiRrtPtr rrt(new Saba::BiRrt(sampledcSpace,
                                           mode/*, mode2, samplingSize*/));
 
 
        rrt->setStart(Eigen::Map<Eigen::ArrayXf>(startCfg.data(), startCfg.size()));
        rrt->setGoal(Eigen::Map<Eigen::ArrayXf>(goalCfg.data(), goalCfg.size()));
 
        bool planningSucceeded = rrt->plan();
        if (planningSucceeded)
        {
            ARMARX_VERBOSE << "BiRRT succeeded! ";
            Saba::CSpacePathPtr tmpSolution = rrt->getSolution();
            ARMARX_INFO << "waypoints: " << tmpSolution->getPoints().size();
 
 
            TIMING_START(ShortCutter);
            ARMARX_INFO << "Shortcutting!";
            Saba::ShortcutProcessor shortcutter(tmpSolution, sampledcSpace);
            tmpSolution = shortcutter.shortenSolutionRandom(100, 200);
            ARMARX_INFO << "Shortcutting resulted into " << tmpSolution->getPoints().size();
            TIMING_END(ShortCutter);
            for (auto& vec : tmpSolution->getPoints())
            {
                this->solution.nodes.push_back(VectorXf {vec.data(), vec.data() + vec.rows()});
            }
            ARMARX_VERBOSE << VAROUT(solution.nodes);
            if (scaledCSpace)
            {
                scaledCSpace->scalePath(solution);
            }
            //                auto from = *solution.nodes.begin();
            //                int i = 1;
            //                for (auto& to : solution.nodes)
            //                {
            //                    bool collisionFree = dcdIsPathCollisionFree(
            //                                             from, to, dcdStep,
            //                                             [this](const VectorXf & cfg)
            //                    {
            //                        return cspace->isCollisionFree({cfg.data(), cfg.data() + cfg.size()});
            //                    },
            //                    false
            //                                         );
            //                    if (!collisionFree)
            //                    {
            //                        ARMARX_ERROR << "Collision in path at node: " << i << " Collisions:\n" << failureOutput();
            //                        setTaskStatus(TaskStatus::ePlanningFailed);
            //                        return;
            //                    }
            //                    from = to;
            //                    i++;
            //                }
 
            setTaskStatus(TaskStatus::eDone);
            return;
        }
 
        else
        {
            setTaskStatus(TaskStatus::ePlanningFailed);
            return;
        }
    }
 
    SimoxCSpacePtr Task::getOriginalCSpace() const
    {
        SimoxCSpacePtr origCSpace;
        if (cspace->ice_isA(CSpaceAdaptorBase::ice_staticId()))
        {
            origCSpace = SimoxCSpacePtr::dynamicCast(CSpaceAdaptorBasePtr::dynamicCast(cspace)->getOriginalCSpace());
 
            //                sampledcSpace = ScaledCSpacePtr::dynamicCast(cspace)->getOriginal()->createSimoxCSpace();
        }
        else
        {
            origCSpace = SimoxCSpacePtr::dynamicCast(cspace);
        }
        return origCSpace;
    }
 
 
}