CollisionAvoidanceCore.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    ...
 * @author     Jianfeng Gao ( jianfeng dot gao at kit dot edu )
 * @date       2024
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <simox/control/dynamics/RBDLModel.h>
#include <simox/control/environment/CollisionRobotInterface.h>
#include <simox/control/environment/fwd.h>
#include <simox/control/impl/simox/robot/Robot.h>
#include <simox/control/robot/RobotInterface.h>
 
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
 
#include <armarx/control/common/control_law/CollisionAvoidance.h>
#include <armarx/control/common/control_law/aron/CollisionAvoidanceConfig.aron.generated.h>
 
namespace armarx::control::njoint_controller::core
{
 
    namespace law = armarx::control::common::control_law;
 
    /**
    * @defgroup Library-CollisionAvoidanceBase CollisionAvoidanceBase
    * @ingroup Library-RobotUnit-NJointControllers
    * A description of the library CollisionAvoidanceBase.
    *
    * @class CollisionAvoidanceBase
    * @ingroup Library-CollisionAvoidanceBase
    * @brief Brief description of class CollisionAvoidanceBase.
    *
    * Detailed description of class CollisionAvoidanceBase.
    */
    class CollisionAvoidanceBase
    {
 
    public:
        using SCRobot = law::CollisionAvoidanceController::SCRobot;
        using DistanceResults = law::CollisionAvoidanceController::DistanceResults;
        using CollisionRobotIndices = law::CollisionAvoidanceController::CollisionRobotIndices;
        using CtrlConfig = law::arondto::CollisionAvoidanceConfig;
        using Config = law::arondto::CollisionAvoidanceConfigDict;
        using RtStatus = law::CollisionAvoidanceController::RtStatusForSafetyStrategy;
        using RecoveryState = law::CollisionAvoidanceController::RecoveryState;
 
        using CollisionRobot = simox::control::environment::CollisionRobotInterface;
        using DynamicsModel = simox::control::dynamics::RBDLModel;
 
        struct NodeSetData
        {
            std::string nodeSetName;
 
            /// !!!!!!! Note that, all the data below should only be accessed in rt thread !!!!!!!
            VirtualRobot::RobotNodeSetPtr rtRNS;
 
            /// controller (maths)
            law::CollisionAvoidanceController controller;
            RtStatus rts; // should only be used in rt thread
            RecoveryState rStateSelfColl;
            armarx::TripleBuffer<RtStatus> bufferRTtoPublish;
 
            /// self-collision avoidance
            std::unique_ptr<DynamicsModel> dynamicsModel;
            const DistanceResults& collisionPairs;
            CollisionRobotIndices collisionRobotIndices;
 
            NodeSetData(const VirtualRobot::RobotPtr& rtRobot,
                        const std::string& robotNodeSetName,
                        const SCRobot& scRobot,
                        const DistanceResults& collisionPairs,
                        const Config& c,
                        const std::vector<size_t>& velCtrlIndex);
 
            void reset(const Config& c, const std::vector<size_t>& velCtrlIndex);
        };
 
        CollisionAvoidanceBase(const VirtualRobot::RobotPtr& rtRobot,
                               const Config& cfg,
                               const std::map<std::string, std::vector<size_t>>& velCtrlIndices);
        virtual ~CollisionAvoidanceBase();
 
        virtual void rtCollisionChecking();
        void rtLimbControllerRun(const std::string& robotNodeSetName,
                                 const Eigen::VectorXf& qpos,
                                 const Eigen::VectorXf& qvelFiltered,
                                 float torqueLimit,
                                 const Eigen::VectorXf& impedanceJointTorque,
                                 double deltaT);
        void rtPreActivate();
        void rtPostDeactivate();
        void updateRtConfigFromUser();
 
        std::map<std::string, NodeSetData> collLimb;
        Config userCfg; // used in nonRT
        armarx::TripleBuffer<Config> bufferCfgUserToRT;
 
        void validateConfigData(const Config& cfg);
        void setUserCfg(const Config& cfg);
 
    protected:
        std::unique_ptr<CollisionRobot> collisionRobot;
        std::unique_ptr<SCRobot> scRobot;
        VirtualRobot::RobotNodeSetPtr scRobotNodeSet;
        DistanceResults collisionPairs;
 
        std::vector<std::string>
            actuatedJointNames; /// TODO, rename to actuatedJointNames, Not needed anymore?
        Eigen::VectorXd qposActuatedJoints;
        std::vector<unsigned int> translationJointIndex;
        //int qposActuatedJointsIndex;
 
        /// !!! individualColObjects has to be set to true !!!
        /// if not set to true, the body and the arms become attractive, be aware
        const simox::control::environment::CompDistArgs compDistArgs =
            simox::control::environment::CompDistArgs{.nearestPoints = true,
                                                      .individualColObjects = true};
 
        Config rtCfg; // used in RT
 
    private:
        float preFilterDistance = 0.6; // in meter
        std::map<std::string, std::vector<size_t>> velCtrlIdx;
        //        int maxCollisionPairs = 100;
 
        const bool reduceModel = false; // not needed anymore
 
        simox::control::environment::CompDistArgs selfColAvoidanceArgs;
 
        VirtualRobot::RobotPtr rtRobot; // TODO not needed?
        VirtualRobot::RobotNodeSetPtr rtRobotNodeSet;
    };
 
    using CollisionAvoidanceBasePtr = std::shared_ptr<CollisionAvoidanceBase>;
} // namespace armarx::control::njoint_controller::core