HandControlBase.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 <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
 
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
 
#include <armarx/control/common/control_law/aron/HandStatus.aron.generated.h>
#include <armarx/control/interface/HandShapeControllerInterface.h>
 
namespace armarx::control::njoint_controller::core
{
    namespace law = armarx::control::common::control_law;
 
    /**
    * @defgroup Library-HandControlBase HandControlBase
    * @ingroup Library-RobotUnit-NJointControllers
    * A description of the library HandControlBase.
    *
    * @class HandControlBase
    * @ingroup Library-HandControlBase
    * @brief Brief description of class HandControlBase.
    *
    * Detailed description of class HandControlBase.
    */
    class HandControlBase
    {
 
    public:
        using HandConfig = law::arondto::HandStatus;
        using HandConfigDict = std::map<std::string, HandConfig>;
 
        struct HandData
        {
            /// names
            std::string kinematicChainName;
            std::vector<std::string> jointNames;
            size_t nJoints;
            std::map<std::string, float> desiredJointPositionMap;
 
            hand::HandShapeControllerInterfacePrx ctrl;
 
            class RTStatus
            {
            public:
                double deltaT = 0.0;
                double accumulateTime = 0.0;
            };
 
            Eigen::VectorXf jointLimMin;
            Eigen::VectorXf jointLimMax;
 
            HandConfig targetNonRT;
            HandConfig prevTargetNonRT;
            float deltaJointValueThreshold = 0.001f;
 
            RTStatus rts;
            RTStatus rtsInNonRT;
 
            double nonRTAccumulateTime = 0.0;
            double nonRTDeltaT = 0.0;
            TripleBuffer<RTStatus> bufferRTToNonRT;
            TripleBuffer<HandConfig> bufferUserToNonRT;
            TripleBuffer<HandConfig> bufferConfigNonRTToOnPublish;
            TripleBuffer<HandConfig> bufferConfigNonRTToUser;
 
            void validate();
            void setTarget();
            bool isTargetChanged();
 
            HandData(const RobotUnitPtr& robotUnit,
                     const VirtualRobot::RobotNodeSetPtr& robotNodeSet,
                     const HandConfig& cfg);
            void rtPreActivate();
        };
 
        using HandPtr = std::unique_ptr<HandData>;
 
        HandControlBase(const RobotUnitPtr& robotUnit,
                        const VirtualRobot::RobotPtr& rtRobot,
                        const HandConfigDict& cfg);
        ~HandControlBase(){};
 
        void nonRTSetTarget();
        void updateRTStatus(double deltaT);
        void nonRTUpdateStatus();
        void updateConfig(const HandConfigDict& cfg);
        void getConfig(HandConfigDict& cfgToUpdate);
        void reinitBuffer(HandConfigDict& cfgToUpdate);
        void nonRtActivateController();
        void rtPreActivate();
        void rtPostDeactivate(HandConfigDict& cfgToUpdate);
        void limbPublish(HandPtr& hand, const DebugObserverInterfacePrx& debugObs);
        void onPublish(const DebugObserverInterfacePrx& debugObs);
 
        std::map<std::string, HandPtr> hands;
    };
 
    using HandControlPtr = std::shared_ptr<HandControlBase>;
} // namespace armarx::control::njoint_controller::core