RobotUnitModuleSelfCollisionChecker.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    RobotAPI::ArmarXObjects::RobotUnit
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "RobotUnitModuleSelfCollisionChecker.h"
 
#include <algorithm>
#include <cstddef>
#include <string>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Obstacle.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/time/Metronome.h"
#include <ArmarXCore/core/util/OnScopeExit.h>
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
 
#include "RobotUnitModuleRobotData.h"
 
#define FLOOR_OBJ_STR "FLOOR"
 
namespace armarx::RobotUnitModule
{
    void
    SelfCollisionChecker::_preOnInitRobotUnit()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        //get the robot
        selfCollisionAvoidanceRobot = _module<RobotData>().cloneRobot(true);
        //get pairs to check
        {
            const std::string colModelsString =
                getProperty<std::string>("SelfCollisionCheck_ModelGroupsToCheck").getValue();
            std::vector<std::string> groups;
            if (!colModelsString.empty())
            {
                groups = armarx::Split(colModelsString, ";", true, true);
            }
            ARMARX_DEBUG << "Processing groups for self collision checking:";
            for (std::string& group : groups)
            {
                ARMARX_DEBUG << "---- group: " << group;
                // Removing parentheses
                simox::alg::trim_if(group, " \t{}");
                std::set<std::set<std::string>> setsOfNode;
                {
                    auto splittedRaw = armarx::Split(group, ",", true, true);
                    if (splittedRaw.size() < 2)
                    {
                        continue;
                    }
                    for (auto& subentry : splittedRaw)
                    {
                        simox::alg::trim_if(subentry, " \t{}");
                        if (selfCollisionAvoidanceRobot->hasRobotNodeSet(subentry))
                        {
                            std::set<std::string> nodes;
                            //the entry is a set
                            for (const auto& node :
                                 selfCollisionAvoidanceRobot->getRobotNodeSet(subentry)
                                     ->getAllRobotNodes())
                            {
                                if (!node->getCollisionModel())
                                {
 
                                    ARMARX_WARNING << "Self Collision Avoidance: No collision "
                                                      "model found for '"
                                                   << node->getName() << "'";
                                    continue;
                                }
                                nodes.emplace(node->getName());
                                ARMARX_DEBUG << "-------- from set: " << subentry
                                             << ",  node: " << node->getName();
                            }
                            setsOfNode.emplace(std::move(nodes));
                        }
                        else if (selfCollisionAvoidanceRobot->hasRobotNode(subentry))
                        {
                            //the entry is a node
                            if (!selfCollisionAvoidanceRobot->getRobotNode(subentry)
                                     ->getCollisionModel())
                            {
 
                                ARMARX_WARNING
                                    << "Self Collision Avoidance: No collision model found for '"
                                    << selfCollisionAvoidanceRobot->getRobotNode(subentry)
                                           ->getName()
                                    << "'";
                                continue;
                            }
                            setsOfNode.emplace(std::set<std::string>{subentry});
                            ARMARX_DEBUG << "-------- node: " << subentry;
                        }
                        else if (subentry == FLOOR_OBJ_STR)
                        {
                            //the entry is the floor
                            setsOfNode.emplace(std::set<std::string>{subentry});
                            ARMARX_DEBUG << "-------- floor: " << subentry;
                        }
                        else
                        {
                            ARMARX_ERROR << "No RobotNodeSet or RobotNode with name '" << subentry
                                         << "' defined in "
                                         << _module<RobotData>().getRobotFileName()
                                         << ". Skipping.";
                            continue;
                        }
                    }
                }
 
                auto addCombinationOfSetsToCollisionCheck =
                    [this](const std::set<std::string>& a, const std::set<std::string>& b)
                {
                    for (const auto& nodeA : a)
                    {
                        for (const auto& nodeB : b)
                        {
                            if (nodeA == nodeB)
                            {
                                continue;
                            }
                            if (nodeA < nodeB)
                            {
                                ARMARX_DEBUG << "------------ " << nodeA << "  " << nodeB;
                                namePairsToCheck.emplace(nodeA, nodeB);
                            }
                            else
                            {
                                ARMARX_DEBUG << "------------ " << nodeB << "  " << nodeA;
                                namePairsToCheck.emplace(nodeB, nodeA);
                            }
                        }
                    }
                };
 
                ARMARX_DEBUG << "-------- adding pairs to check:";
                for (auto setAIt = setsOfNode.begin(); setAIt != setsOfNode.end(); ++setAIt)
                {
                    auto setBIt = setAIt;
                    ++setBIt;
                    for (; setBIt != setsOfNode.end(); ++setBIt)
                    {
                        addCombinationOfSetsToCollisionCheck(*setAIt, *setBIt);
                    }
                }
 
 
                ARMARX_DEBUG << "-------- group: " << group << "...DONE!\n";
            }
            ARMARX_DEBUG << "Processing groups for self collision checking...DONE!";
        }
        setSelfCollisionAvoidanceFrequency(
            getProperty<float>("SelfCollisionCheck_Frequency").getValue());
        setSelfCollisionAvoidanceDistance(
            getProperty<float>("SelfCollisionCheck_MinSelfDistance").getValue());
    }
 
    void
    SelfCollisionChecker::setSelfCollisionAvoidanceDistance(Ice::Float distance,
                                                            const Ice::Current&)
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        std::lock_guard<std::mutex> guard{selfCollisionDataMutex};
        if (distance < 0)
        {
            throw InvalidArgumentException{std::string{__FILE__} + ": " + BOOST_CURRENT_FUNCTION +
                                           ": illegal distance:" + std::to_string(distance)};
        }
        if (distance == minSelfDistance && !nodePairsToCheck.empty())
        {
            return;
        }
        //reset data
        ARMARX_CHECK_GREATER(distance, 0);
        minSelfDistance = distance;
        selfCollisionAvoidanceRobot = _module<RobotData>().cloneRobot(true);
        selfCollisionAvoidanceRobotNodes = selfCollisionAvoidanceRobot->getRobotNodes();
        nodePairsToCheck.clear();
        //set floor
        {
            floor.reset(new VirtualRobot::SceneObjectSet(
                "FLOORSET", selfCollisionAvoidanceRobot->getCollisionChecker()));
            static constexpr float floorSize = 1e16f;
            VirtualRobot::ObstaclePtr boxOb = VirtualRobot::Obstacle::createBox(
                floorSize,
                floorSize,
                std::min(0.001f, minSelfDistance / 2),
                VirtualRobot::VisualizationFactory::Color::Red(),
                "",
                selfCollisionAvoidanceRobot->getCollisionChecker());
            boxOb->setGlobalPose(Eigen::Matrix4f::Identity());
            boxOb->setName(FLOOR_OBJ_STR);
            floor->addSceneObject(boxOb);
        }
        //inflate robot
        for (const auto& node : selfCollisionAvoidanceRobotNodes)
        {
            if (node->getCollisionModel())
            {
                node->getCollisionModel()->inflateModel(minSelfDistance / 2.f);
            }
        }
 
        // Remove / filter collision pairs according to robot model (XML: Physics/IgnoreCollision)
        {
            ARMARX_VERBOSE << "Removing ignored collision pairs";
            // introduce vector to remove elements "in-place" via remove-erase-if idiom (not possible for sets)
            std::vector<std::pair<std::string, std::string>> validNamePairsToCheck(
                namePairsToCheck.begin(), namePairsToCheck.end());
 
            const auto isCollisionIgnored = [this](const std::string& a,
                                                   const std::string& b) -> bool
            {
                if (a == FLOOR_OBJ_STR or b == FLOOR_OBJ_STR)
                {
                    return false;
                }
 
                const auto nodeA = selfCollisionAvoidanceRobot->getRobotNode(a);
                const auto nodeB = selfCollisionAvoidanceRobot->getRobotNode(b);
 
                if (nodeA == nullptr or nodeB == nullptr)
                {
                    return false;
                }
 
                const std::vector<std::string> nodesIgnoredByA = nodeA->getIgnoredCollisionModels();
                const std::vector<std::string> nodesIgnoredByB = nodeB->getIgnoredCollisionModels();
 
                if (std::find(nodesIgnoredByA.begin(), nodesIgnoredByA.end(), b) !=
                    nodesIgnoredByA.end())
                {
                    ARMARX_VERBOSE << "Ignoring collision between nodes: " << a << " -- " << b;
                    return true;
                }
 
                if (std::find(nodesIgnoredByB.begin(), nodesIgnoredByB.end(), a) !=
                    nodesIgnoredByB.end())
                {
                    ARMARX_VERBOSE << "Ignoring collision between nodes: " << b << " -- " << a;
                    return true;
                }
 
                return false;
            };
 
            validNamePairsToCheck.erase(std::remove_if(validNamePairsToCheck.begin(),
                                                       validNamePairsToCheck.end(),
                                                       [&isCollisionIgnored](const auto& p) -> bool
                                                       {
                                                           const auto& [a, b] = p;
                                                           return isCollisionIgnored(a, b);
                                                       }),
                                        validNamePairsToCheck.end());
 
            ARMARX_VERBOSE << "Removed " << (namePairsToCheck.size() - validNamePairsToCheck.size())
                           << " collision pairs.";
 
            // copy over name pairs which should not be ignored
            namePairsToCheck = std::set(validNamePairsToCheck.begin(), validNamePairsToCheck.end());
        }
 
        //collect pairs
        for (const auto& pair : namePairsToCheck)
        {
            VirtualRobot::SceneObjectPtr first =
                (pair.first == FLOOR_OBJ_STR)
                    ? floor->getSceneObject(0)
                    : selfCollisionAvoidanceRobot->getRobotNode(pair.first);
 
            ARMARX_CHECK_NOT_NULL(first) << pair.first;
 
            VirtualRobot::SceneObjectPtr second =
                (pair.second == FLOOR_OBJ_STR)
                    ? floor->getSceneObject(0)
                    : selfCollisionAvoidanceRobot->getRobotNode(pair.second);
 
            ARMARX_CHECK_NOT_NULL(second) << pair.second;
 
            nodePairsToCheck.emplace_back(first, second);
        }
        ARMARX_CHECK_EQUAL(nodePairsToCheck.size(), namePairsToCheck.size());
    }
 
    void
    SelfCollisionChecker::setSelfCollisionAvoidanceFrequency(Ice::Float freq, const Ice::Current&)
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        if (freq < 0)
        {
            throw InvalidArgumentException{std::string{__FILE__} + ": " + BOOST_CURRENT_FUNCTION +
                                           ": illegal frequency:" + std::to_string(freq)};
        }
        checkFrequency = freq;
    }
 
    bool
    SelfCollisionChecker::isSelfCollisionCheckEnabled(const Ice::Current&) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        return checkFrequency != 0;
    }
 
    float
    SelfCollisionChecker::getSelfCollisionAvoidanceFrequency(const Ice::Current&) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        return checkFrequency;
    }
 
    float
    SelfCollisionChecker::getSelfCollisionAvoidanceDistance(const Ice::Current&) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        return minSelfDistance;
    }
 
    void
    SelfCollisionChecker::_componentPropertiesUpdated(const std::set<std::string>& changed)
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        if (changed.count("SelfCollisionCheck_Frequency"))
        {
            setSelfCollisionAvoidanceFrequency(
                getProperty<float>("SelfCollisionCheck_Frequency").getValue());
        }
        if (changed.count("SelfCollisionCheck_MinSelfDistance"))
        {
            setSelfCollisionAvoidanceDistance(
                getProperty<float>("SelfCollisionCheck_MinSelfDistance").getValue());
        }
    }
 
    void
    SelfCollisionChecker::_postFinishControlThreadInitialization()
    {
        selfCollisionAvoidanceThread = std::thread{[&] { selfCollisionAvoidanceTask(); }};
    }
 
    void
    SelfCollisionChecker::selfCollisionAvoidanceTask()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        ARMARX_INFO << "Self collision checker: entered selfCollisionAvoidanceTask";
        ARMARX_ON_SCOPE_EXIT
        {
            ARMARX_INFO << "Self collision checker: leaving selfCollisionAvoidanceTask";
        };
        while (true)
        {
            const auto freq = checkFrequency.load();
 
            core::time::Metronome metronome(Frequency::Hertz(freq));
 
            //done
            if (isShuttingDown())
            {
                return;
            }
            const bool inEmergencyStop =
                _module<ControlThread>().getEmergencyStopState() == eEmergencyStopActive;
            if (inEmergencyStop || freq == 0)
            {
                ARMARX_VERBOSE << deactivateSpam() << "Self collision checker: skipping check "
                               << VAROUT(freq) << " " << VAROUT(inEmergencyStop);
                //currently wait
                std::this_thread::sleep_for(std::chrono::microseconds{1'000});
                continue;
            }
            //update robot + check
            {
                std::lock_guard<std::mutex> guard{selfCollisionDataMutex};
                //update robot
                _module<ControlThreadDataBuffer>().updateVirtualRobot(
                    selfCollisionAvoidanceRobot, selfCollisionAvoidanceRobotNodes);
 
                //check for all nodes 0
                {
                    bool allJoints0 = true;
                    for (const auto& node : selfCollisionAvoidanceRobotNodes)
                    {
                        ARMARX_CHECK_NOT_NULL(node);
                        if (0 != node->getJointValue())
                        {
                            allJoints0 = false;
                            break;
                        }
                    }
                    if (allJoints0)
                    {
                        continue;
                    }
                }
 
                bool collision = false;
                for (std::size_t idx = 0; idx < nodePairsToCheck.size(); ++idx)
                {
                    const auto& pair = nodePairsToCheck.at(idx);
 
                    ARMARX_CHECK_NOT_NULL(pair.first);
                    ARMARX_CHECK_NOT_NULL(pair.second);
 
                    if (selfCollisionAvoidanceRobot->getCollisionChecker()->checkCollision(
                            pair.first, pair.second))
                    {
                        collision = true;
                        lastCollisionPairIndex = idx;
                        ARMARX_WARNING << "Self collision checker: COLLISION: '"
                                       << pair.first->getName() << "' and '"
                                       << pair.second->getName() << "'";
                        _module<Units>().getEmergencyStopMaster()->setEmergencyStopState(
                            EmergencyStopState::eEmergencyStopActive);
                        // since at least one of the NJoint-Controllers is going to cause a collision, we just kick them all.
                        _module<ControlThreadDataBuffer>().setActivateControllersRequest({});
                        break;
                    }
                }
                if (!collision)
                {
                    ARMARX_VERBOSE << deactivateSpam()
                                   << "Self collision checker: no collision found between the "
                                   << nodePairsToCheck.size() << " pairs";
                }
            }
 
            //sleep remaining
            const auto duration = metronome.waitForNextTick();
 
            if (not duration.isPositive())
            {
                ARMARX_WARNING << deactivateSpam(10)
                               << "Self collision checking took too long. "
                                  "Exceeding time budget by "
                               << duration.toMilliSecondsDouble() << "ms.";
            }
        }
    }
 
    void
    SelfCollisionChecker::_preFinishRunning()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        ARMARX_INFO << "Stopping Self Collision Avoidance.";
        if (selfCollisionAvoidanceThread.joinable())
        {
            selfCollisionAvoidanceThread.join();
        }
    }
} // namespace armarx::RobotUnitModule