CollisionCheckUtil.h

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
 */
#pragma once
 
#include <optional>
 
#include <Eigen/Core>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
namespace armarx
{
    /**
     * @brief Tries to reach to from from using the given stepsize.
     * @param from The starting point.
     * @param to The target point.
     * @param dcdStepSize The used stepsize.
     * @param isCollisionFree The collision checker used to test configuration for collision.
     * @return Returns the point p that maximizes distance(from,p) while the line(from, p)
     * is collision free at all points with a distance of k * dcdStepSize from from.
     * If no such point is reachable exactly from is returned.
     * If to is reachable exactly to is returned.
     */
    template <class RealType, class CollisionChecker, class ConfigType>
    ConfigType
    dcdSteer(const ConfigType& from,
             const ConfigType& to,
             RealType dcdStepSize,
             CollisionChecker isCollisionFree)
    {
        using EigenVector = Eigen::Matrix<RealType, Eigen::Dynamic, 1>;
        ARMARX_CHECK_EXPRESSION(from.size() == to.size());
        const auto size = static_cast<long int>(from.size()); //eigen wants long int
        //get eigen views
        Eigen::Map<const EigenVector> eFrom{from.data(), size};
        Eigen::Map<const EigenVector> eTo{to.data(), size};
 
        EigenVector step = eTo - eFrom;
        const auto length = step.norm();
        step *= dcdStepSize / length; // if length == 0 this values does not get used
 
        //will truncate in most cases => we will need to check the destination step for collision
        ARMARX_CHECK_EXPRESSION(length / dcdStepSize < std::numeric_limits<RealType>::infinity());
        ARMARX_CHECK_EXPRESSION(length / dcdStepSize <
                                static_cast<RealType>(std::numeric_limits<std::size_t>::max()));
        const std::size_t stepsToTake = static_cast<std::size_t>(length / dcdStepSize);
 
        ConfigType stepToCheck(from);
        ARMARX_CHECK_EXPRESSION(stepToCheck.size() == from.size());
        Eigen::Map<EigenVector> eStepToCheck{stepToCheck.data(), size};
        std::size_t stepsTaken = 0;
 
        for (; stepsTaken < stepsToTake; ++stepsTaken)
        {
            eStepToCheck += step;
 
            if (!isCollisionFree(stepToCheck))
            {
                if (0 == stepsTaken)
                {
                    //could not move!
                    //return an exact copy
                    return from;
                }
 
                eStepToCheck -= step;
                return stepToCheck;
            }
        }
 
        ARMARX_CHECK_EXPRESSION(stepsTaken == stepsToTake);
 
        //check to
        if (isCollisionFree(to))
        {
            //return an exact copy
            return to;
        }
 
        //to has collision or was not reached
        return stepToCheck;
    }
 
    //startCfg is assumed to be collision free
    /**
     * @brief Returns whether the line startCfg to to is collision free.
     * @param from The config to startCfg from. (startCfg is assumed to be collision free.
     * @param to The config to reach
     * @param dcdStepSize The dcd step size
     * @param isCollisionFree The collision checker returning true, if a config is collision free.
     * @param toIsCollisionFree Whether to is collision free
     * @return Whether the line startCfg to to is collision free.
     */
    template <
        class RealType,
        class CollisionChecker,
        class ConfigType,
        class Distance = std::function<float(Eigen::VectorXf, Eigen::VectorXf)>,
        class Interpolate = std::function<Eigen::VectorXf(Eigen::VectorXf, Eigen::VectorXf, float)>>
    bool
    dcdIsPathCollisionFree(
        const ConfigType& from,
        const ConfigType& to,
        RealType dcdStepSize,
        CollisionChecker isCollisionFree,
        bool toIsCollisionFree = true,
        const std::optional<Distance>& distanceLambda = std::optional<Distance>(),
        const std::optional<Interpolate>& interpolationLambda = std::optional<Interpolate>(),
        std::vector<ConfigType>* resultPath = NULL)
    {
        if (!(toIsCollisionFree || isCollisionFree(to)))
        {
            return false;
        }
 
        using EigenVector = Eigen::Matrix<RealType, Eigen::Dynamic, 1>;
        ARMARX_CHECK_EXPRESSION(from.size() == to.size());
        const auto size = static_cast<long int>(from.size()); //eigen wants long int
        //get eigen views
        Eigen::Map<const EigenVector> eFrom{from.data(), size};
        Eigen::Map<const EigenVector> eTo{to.data(), size};
        EigenVector connecting = eTo - eFrom;
        std::vector<float> tmp(size);
        Eigen::Map<EigenVector> eTmp{tmp.data(), size};
        float distanceFromTo;
        if (distanceLambda)
        {
            distanceFromTo = (*distanceLambda)(eFrom, eTo);
        }
        else
        {
            distanceFromTo = connecting.norm();
        }
        ARMARX_INFO << deactivateSpam(1) << VAROUT(dcdStepSize) << VAROUT(distanceFromTo)
                    << VAROUT(eFrom) << VAROUT(eTo);
        if (distanceFromTo <= dcdStepSize)
        {
            if (resultPath)
            {
                resultPath->push_back(to);
            }
            //since to is collision free
            return true;
        }
 
        float dcdAsDistanceFactor = dcdStepSize / distanceFromTo;
 
        float checkedStepFactor;
        float betweenStepFactor;
        std::size_t stepsToCheck = 1;
 
        do
        {
            betweenStepFactor = 1.f / stepsToCheck;
            checkedStepFactor = betweenStepFactor / 2.f;
 
            float factor = checkedStepFactor;
 
            for (std::size_t step = 0; step < stepsToCheck; ++step, factor += betweenStepFactor)
            {
                if (interpolationLambda)
                {
                    eTmp = (*interpolationLambda)(eFrom, eTo, factor);
                }
                else
                {
                    eTmp = eFrom + factor * connecting;
                }
                if (!isCollisionFree(tmp))
                {
                    return false;
                }
            }
            if (resultPath)
            {
                resultPath->push_back(tmp);
            }
            //increment steps
            stepsToCheck = stepsToCheck << 1;
        } while (checkedStepFactor > dcdAsDistanceFactor);
 
        //checked everything and no collision
        return true;
    }
} // namespace armarx