ObjectCollisionAvoidanceVelCore.h

Go to the documentation of this file.

// namespace armarx::control::njoint_controller::core
/*
 * 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 <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/ObjectCollisionAvoidanceVel.h>
#include <armarx/control/common/control_law/aron/ObjectCollisionAvoidanceVelConfig.aron.generated.h>
 
namespace armarx::control::njoint_controller::core
{
 
    namespace law = armarx::control::common::control_law;
 
    /**
    * @defgroup Library-ObjectCollisionAvoidanceVelBase ObjectCollisionAvoidanceVelBase
    * @ingroup Library-RobotUnit-NJointControllers
    * A description of the library ObjectCollisionAvoidanceVelBase.
    *
    * @class ObjectCollisionAvoidanceVelBase
    * @ingroup Library-CollisionAvoidanceBase
    * @brief Brief description of class ObjectCollisionAvoidanceVelBase.
    *
    * Detailed description of class ObjectCollisionAvoidanceVelBase.
    */
    class ObjectCollisionAvoidanceVelBase
    {
 
    public:
        using SCRobot = law::CollisionAvoidanceVelController::SCRobot;
        using DistanceResults = law::CollisionAvoidanceVelController::DistanceResults;
        using CollisionRobotIndices = law::CollisionAvoidanceVelController::CollisionRobotIndices;
        using CtrlConfig = law::arondto::ObjectCollisionAvoidanceVelConfig;
        using Config = law::arondto::ObjectCollisionAvoidanceVelConfigDict;
        using RtStatus = law::CollisionAvoidanceVelController::RtStatusForSafetyStrategy;
        using RecoveryState = law::CollisionAvoidanceVelController::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::ObjectCollisionAvoidanceVelController controller;
            RtStatus rts; // should only be used in rt thread
 
            /// recovery strategy
            RecoveryState recoveryStateSelfColl;
            RecoveryState recoveryStateObjColl;
 
            armarx::TripleBuffer<RtStatus> bufferRTtoPublish;
            armarx::TripleBuffer<RecoveryState> bufferRecoveryStateSelfColl;
 
            /// 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);
 
            void reset(const Config& c);
        };
 
        ObjectCollisionAvoidanceVelBase(const VirtualRobot::RobotPtr& rtRobot, const Config& cfg);
        virtual ~ObjectCollisionAvoidanceVelBase();
 
        virtual void rtCollisionChecking();
        void rtLimbControllerRun(const std::string& robotNodeSetName,
                                 const Eigen::VectorXf& qpos,
                                 const Eigen::VectorXf& qvelFiltered,
                                 float velocityLimit,
                                 const Eigen::VectorXf& trajFollowJointVel,
                                 double deltaT);
        void rtPreActivate();
        void rtPostDeactivate();
        void updateRtConfigFromUser();
 
        std::vector<hpp::fcl::CollisionObject> userCollisionObjects;
        DistanceResults userExternalCollisionPairs;
        void updateUserCollisionObjects(const std::vector<hpp::fcl::CollisionObject>&);
        void updateRtCollisionObjects();
        void deleteUserCollisionObjects();
 
        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);
 
        // TODO make protected again, publish collision geometry
        std::unique_ptr<CollisionRobot> collisionRobot;
        Eigen::Matrix<double, 4, 4> globalPose;
 
 
    protected:
        DistanceResults collisionPairs;
        std::unique_ptr<SCRobot> scRobot;
        VirtualRobot::RobotNodeSetPtr scRobotNodeSet;
 
        armarx::TripleBuffer<std::tuple<DistanceResults, std::vector<hpp::fcl::CollisionObject>>>
            bufferCollisionObjectsUserToRt;
        DistanceResults externalCollisionPairs;
 
        std::vector<std::string> actuatedJointNames;
        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
        //        int maxCollisionPairs = 100;
 
        const bool reduceModel = false; // TODO check here! not needed anymore for a7, but for a6
 
        simox::control::environment::CompDistArgs selfColAvoidanceArgs;
 
        VirtualRobot::RobotPtr rtRobot;
        std::vector<hpp::fcl::CollisionObject> rtCollisionObjects;
        VirtualRobot::RobotNodeSetPtr rtRobotNodeSet;
    };
 
    using ObjectCollisionAvoidanceVelBasePtr = std::shared_ptr<ObjectCollisionAvoidanceVelBase>;
} // namespace armarx::control::njoint_controller::core