RobotUnitModuleSelfCollisionChecker.h

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
 */
 
#pragma once
 
#include <thread>
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
 
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
 
#include "RobotUnitModuleBase.h"
 
namespace armarx::RobotUnitModule
{
    class SelfCollisionCheckerPropertyDefinitions : public ModuleBasePropertyDefinitions
    {
    public:
        SelfCollisionCheckerPropertyDefinitions(std::string prefix) :
            ModuleBasePropertyDefinitions(prefix)
        {
            defineOptionalProperty<float>("SelfCollisionCheck_Frequency",
                                          10,
                                          "Frequency [Hz] of self collision checking (default = "
                                          "10). If set to 0, no cllisions will be checked.",
                                          PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<float>("SelfCollisionCheck_MinSelfDistance",
                                          20,
                                          "Minimum allowed distance (mm) between each collision "
                                          "model before entering emergency stop (default = 20).",
                                          PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<std::string>(
                "SelfCollisionCheck_ModelGroupsToCheck",
                "",
                "Comma seperated list of groups (two or more) of collision models (RobotNodeSets "
                "or RobotNodes) that "
                "should be checked against each other by collision avoidance \n"
                "(e.g. {rnsColModel1,rnsColModel2};{rnsColModel3,rnsColModel4}; .... "
                "says that rnsColModel1 will be only checked against rnsColModel2 and rnsColModel3 "
                "will only be checked against rnsColModel4). \n"
                "Following predefined descriptors can be used: 'FLOOR' which represents a virtual "
                "collision model of the floor.");
        }
    };
 
    /**
     * @ingroup Library-RobotUnit-Modules
     * @brief This \ref ModuleBase "Module" manages self collision avoidance.
     * If the distance between a pair of bodies falls below a threshold, the the emergency stop is activated.
     *
     * @see ModuleBase
     */
    class SelfCollisionChecker :
        virtual public ModuleBase,
        virtual public RobotUnitSelfCollisionCheckerInterface
    {
        friend class ModuleBase;
 
    public:
        /**
         * @brief Returns the singleton instance of this class
         * @return The singleton instance of this class
         */
        static SelfCollisionChecker&
        Instance()
        {
            return ModuleBase::Instance<SelfCollisionChecker>();
        }
 
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        /// @see ModuleBase::_preOnInitRobotUnit
        void _preOnInitRobotUnit();
        /// @see ModuleBase::_postFinishControlThreadInitialization
        void _postFinishControlThreadInitialization();
        /// @see ModuleBase::_preFinishRunning
        void _preFinishRunning();
        /// @see ModuleBase::_componentPropertiesUpdated
        void _componentPropertiesUpdated(const std::set<std::string>& changed);
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////////// ice interface //////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    public:
        /**
         * @brief Sets the minimal distance (mm) between a configured pair of bodies to 'distance'.
         * @param distance The minimal distance (mm) between a pair of bodies.
         */
        void setSelfCollisionAvoidanceDistance(Ice::Float distance,
                                               const Ice::Current& = Ice::emptyCurrent) override;
        /**
         * @brief Sets the frequency of self collision checks.
         * @param freq The frequency of self collision checks.
         */
        void setSelfCollisionAvoidanceFrequency(Ice::Float freq,
                                                const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief Returns whether the frequency of self collision checks is above 0.
         * @return Whether the frequency of self collision checks is above 0.
         */
        bool isSelfCollisionCheckEnabled(const Ice::Current& = Ice::emptyCurrent) const override;
        /**
         * @brief Returns the frequency of self collision checks.
         * @return The frequency of self collision checks.
         */
        float
        getSelfCollisionAvoidanceFrequency(const Ice::Current& = Ice::emptyCurrent) const override;
        /**
         * @brief Returns the minimal distance (mm) between a pair of bodies.
         * @return The minimal distance (mm) between a pair of bodies.
         */
        float
        getSelfCollisionAvoidanceDistance(const Ice::Current& = Ice::emptyCurrent) const override;
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // //////////////////////////////////// implementation //////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        /// @brief The Loop executing the self collision check
        void selfCollisionAvoidanceTask();
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////////////// Data ///////////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        /// @brief The thread executing \ref selfCollisionAvoidanceTask
        std::thread selfCollisionAvoidanceThread;
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // //////////////////////////////////////// config //////////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        /// @brief The list of pairs of collision models that should be checked agaisnt each other (names only)
        std::set<std::pair<std::string, std::string>> namePairsToCheck;
        /// @brief The frequency of self collision checks.
        std::atomic<float> checkFrequency{0};
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // /////////////////////////////////////// col data /////////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        ///@ brief The mutex guarding data used for self collision checks.
        std::mutex selfCollisionDataMutex;
        /// @brief Min allowed distance (mm) between each pair of collision models
        std::atomic<float> minSelfDistance{0};
        /// @brief The list of pairs of collision models that should be checked agaisnt each other (model nodes only)
        std::vector<std::pair<VirtualRobot::SceneObjectPtr, VirtualRobot::SceneObjectPtr>>
            nodePairsToCheck;
        /// @brief The Robot used for self collison checks
        VirtualRobot::RobotPtr selfCollisionAvoidanceRobot;
        /// @brief The Robot Nodes of the Robot used for collision checks
        std::vector<VirtualRobot::RobotNodePtr> selfCollisionAvoidanceRobotNodes;
        /// @brief A Collision object for the floor
        VirtualRobot::SceneObjectSetPtr floor;
        /// @brief The pair of nodes causing the last detected collision (as index in \ref nodePairsToCheck or -1)
        std::atomic_size_t lastCollisionPairIndex{std::numeric_limits<std::size_t>::max()};
    };
} // namespace armarx::RobotUnitModule