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/Robot.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>
 
#include <simox/control/environment/CollisionRobot.h>
#include <simox/control/geodesics/metric/inertia.h>
#include <simox/control/impl/simox/robot/Robot.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 SimoxRobot = ::simox::control::simox::robot::Robot;
        using SimoxRobotPtr = std::shared_ptr<SimoxRobot>;
        using DistanceResults = std::vector<::simox::control::environment::DistanceResult>;
        using DistanceResultsPtr = std::shared_ptr<DistanceResults>;
        using CollisionRobot = ::simox::control::environment::CollisionRobot<hpp::fcl::OBBRSS>;
        using CollisionRobotPtr = std::unique_ptr<CollisionRobot>;
        using InertiaPtr = std::shared_ptr<simox::control::geodesics::metric::Inertia>;
        using CtrlConfig = armarx::control::common::control_law::arondto::CollisionAvoidanceConfig;
        using Config =
            ::armarx::control::common::control_law::arondto::CollisionAvoidanceConfigDict;
        using ConfigPtr = std::shared_ptr<Config>;
        using RtStatus = law::CollisionAvoidanceController::RtStatusForSafetyStrategy;
 
        //        using ConfigDict = std::map<std::string, ConfigPtr>;
 
        struct CollisionAvoidanceArmData
        {
            std::string nodeSetName;
 
            /// !!!!!!! Note that, all the data below should only be accessed in rt thread !!!!!!!
            VirtualRobot::RobotNodeSetPtr rtRNS;
            std::string tcpName;
            std::vector<std::string> jointNames;
 
            /// controller (maths)
            law::CollisionAvoidanceController controller;
            RtStatus rts; // should only be used in rt thread
            armarx::TripleBuffer<RtStatus> bufferRTtoPublish;
 
            /// self-collision avoidance
            InertiaPtr inertiaPtr;
            DistanceResultsPtr collisionPairsPtr;
            std::shared_ptr<std::vector<int>> pointsOnArm;
            int pointsOnArmIndex;
 
            // reduced robot required to get node where contact point is located to compute Jacobian
            SimoxRobotPtr simoxReducedRobotPtr;
 
            CollisionAvoidanceArmData(VirtualRobot::RobotPtr rtRobot,
                                      const std::string& robotNodeSetName,
                                      SimoxRobotPtr simoxReducedRobot,
                                      SimoxRobotPtr simoxControlRobot,
                                      DistanceResultsPtr collisionPairs,
                                      Config& c);
            void reset(Config& c);
        };
 
        using CollArmPtr = std::shared_ptr<CollisionAvoidanceArmData>;
 
        CollisionAvoidanceBase(const VirtualRobot::RobotPtr& rtRobot, const Config& cfg);
        ~CollisionAvoidanceBase();
 
        void rtCollisionChecking();
        void rtLimbControllerRun(const std::string& robotNodeSetName,
                                 Eigen::VectorXf& qpos,
                                 Eigen::VectorXf& qvelFiltered,
                                 float torqueLimit,
                                 Eigen::VectorXf& impedanceJointTorque);
        void rtPreActivate();
        void rtPostDeactivate();
        void updateRtConfigFromUser();
 
        std::map<std::string, CollArmPtr> collLimb;
        Config userCfg; // used in nonRT
        armarx::TripleBuffer<Config> bufferCfgUserToRT;
 
        void validateConfigData(Config& cfg);
        void setUserCfg(const Config& cfg);
 
    protected:
        CollisionRobotPtr collisionRobotPtr;
        SimoxRobotPtr simoxControlRobotPtr;
        SimoxRobotPtr simoxReducedRobotPtr;
        DistanceResultsPtr collisionPairs;
 
        std::vector<std::string> actuatedJointNames; /// TODO, rename to actuatedJointNames
        Eigen::VectorXd qposActuatedJoints;
        std::vector<int> translationJointIndex;
        //int qposActuatedJointsIndex;
 
    private:
        float preFilterDistance = 0.6; // in meter
        //        int maxCollisionPairs = 100;
 
        Config rtCfg; // used in RT
        VirtualRobot::RobotPtr rtRobot;
    };
 
    using CollisionAvoidanceBasePtr = std::shared_ptr<CollisionAvoidanceBase>;
} // namespace armarx::control::njoint_controller::core