Component.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    control::ArmarXObjects::controller_creator
 * @author     Jianfeng Gao ( jianfeng dot gao at kit dot edu )
 * @date       2023
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#pragma once
 
 
// #include <mutex>
// #include <Ice/Optional.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/Component.h>
// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
 
// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
 
// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
 
#include <experimental/memory>
 
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
#include <RobotAPI/libraries/armem/client/plugins.h>
#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
 
#include <armarx/control/client/ComponentPlugin.h>
#include <armarx/control/client/ControllerBuilder.h>
#include <armarx/control/common/ft/utils/ForceTorqueUtility.h>
#include <armarx/control/common/type.h>
#include <armarx/control/components/controller_creator/ComponentInterface.h>
#include <armarx/control/njoint_controller/controller_descriptions.h>
#include <armarx/control/njoint_mp_controller/controller_descriptions.h>
 
namespace armarx::control::components::controller_creator
{
 
    class Component :
        virtual public armarx::Component,
        virtual public ComponentInterface,
        virtual public armarx::RobotStateComponentPluginUser,
        virtual public control::client::ComponentPluginUser,
        virtual public armem::ListeningClientPluginUser
    // , virtual public armarx::DebugObserverComponentPluginUser
    // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
    // , virtual public armarx::ArVizComponentPluginUser
 
    {
    public:
        Component();
 
        ~Component() override;
 
        /// @see armarx::ManagedIceObject::getDefaultName()
        std::string getDefaultName() const override;
 
        /// Get the component's default name.
        static std::string GetDefaultName();
 
 
    protected:
        /// @see PropertyUser::createPropertyDefinitions()
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        /// @see armarx::ManagedIceObject::onInitComponent()
        void onInitComponent() override;
 
        /// @see armarx::ManagedIceObject::onConnectComponent()
        void onConnectComponent() override;
 
        /// @see armarx::ManagedIceObject::onDisconnectComponent()
        void onDisconnectComponent() override;
 
        /// @see armarx::ManagedIceObject::onExitComponent()
        void onExitComponent() override;
 
 
        /* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
        /// This function should be called once in onConnect() or when you
        /// need to re-create the Remote GUI tab.
        void createRemoteGuiTab();
 
        /// After calling `RemoteGui_startRunningTask`, this function is
        /// called periodically in a separate thread. If you update variables,
        /// make sure to synchronize access to them.
        void RemoteGui_update() override;
        */
 
    public:
        std::string createController(const std::string& namePrefix,
                                     const std::string& controllerType,
                                     const aron::data::dto::DictPtr& configDict = nullptr,
                                     const std::string& configFilename = "",
                                     bool activate = false,
                                     bool allowReuse = true,
                                     bool cfgFromMemory = false,
                                     const Ice::Current& = Ice::emptyCurrent) override;
 
        Ice::FloatSeq getPoseInRootFrame(const std::string& nodeName,
                                         const Ice::Current& = Ice::emptyCurrent) override;
        Ice::FloatSeq getPoseInGlobalFrame(const std::string& nodeName,
                                           const Ice::Current& = Ice::emptyCurrent) override;
 
        std::string kinestheticTeaching(const std::string& kinematicChainName,
                                        const std::string& side,
                                        const std::string& fileName,
                                        const std::string& nameSuffix,
                                        const Ice::Current& = Ice::emptyCurrent) override;
 
        void enableVelocityMode(const Ice::Current& = Ice::emptyCurrent) override;
        void toggleZeroTorque(const std::string& leftNodesetName, const std::string& rightNodesetName, const Ice::Current& = Ice::emptyCurrent) override;
        void createZeroTorqueController(const std::string& kinematicChainName);
        template <control::common::ControllerType T>
        std::string
        createComplianceController(const std::string& namePrefix,
                                   const ::armarx::aron::data::dto::DictPtr& configDict = nullptr,
                                   const std::string& configFilename = "",
                                   bool flagActivate = true,
                                   bool flagAllowReuse = true,
                                   bool flagFromMemory = false);
 
    private:
        // Private methods go here.
 
        // Forward declare `Properties` if you used it before its defined.
        // struct Properties;
 
        /* (Requires the armarx::ArVizComponentPluginUser.)
        /// Draw some boxes in ArViz.
        void drawBoxes(const Properties& p, viz::Client& arviz);
        */
 
        // std::string createTSBiImpedanceController(
        //         const std::string &controllerNamePrefix,
        //         const std::string &configFilename,
        //         bool activate,
        //         bool allowReuse);
 
    private:
        static const std::string defaultName;
 
        // Private member variables go here.
        RobotUnitInterfacePrx m_robot_unit = nullptr;
        VirtualRobot::RobotPtr m_robot = nullptr;
        // armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* m_virtualRobotReaderPlugin = nullptr;
 
        /// observer
        /*ForceTorqueUnitObserverInterfacePrx m_force_torque_observer;
        armarx::control::common::ft::ForceTorqueUtilityPtr m_ft_l = nullptr;
        armarx::control::common::ft::ForceTorqueUtilityPtr m_ft_r = nullptr;*/
 
        /// robot node sets
        VirtualRobot::RobotNodeSetPtr m_rns_l;
        VirtualRobot::RobotNodeSetPtr m_rns_r;
 
        std::map<std::string, VirtualRobot::RobotNodeSetPtr> m_robotNodeSets;
        std::map<std::string, unsigned int> m_controllerJointNumbers;
 
        // std::map<std::string, TargetPoseMap> m_prevTargetPose;
        // std::map<std::string, TargetNullspaceMap > m_prevNullspaceJointTarget;
        std::map<std::string, std::map<std::string, Eigen::Matrix4f>> m_prevTargetPose;
        std::map<std::string, std::map<std::string, Eigen::VectorXf>> m_prevNullspaceJointTarget;
 
        /// Properties shown in the Scenario GUI.
        struct Properties
        {
            std::string robotName = "Armar6";
            // std::string robotUnitName = "Armar6Unit";
            //std::string ftObserverName = "Armar6ForceTorqueObserver";
            std::string ftSensorNameLeft = "FT L_ArmL_FT";
            std::string ftSensorNameRight = "FT R_ArmR_FT";
 
            std::string defaultRobotNodeSetLeft = "LeftArm";
            std::string defaultRobotNodeSetRight = "RightArm";
        };
 
        Properties properties;
        /* Use a mutex if you access variables from different threads
         * (e.g. ice functions and RemoteGui_update()).
        std::mutex propertiesMutex;
        */
 
 
        /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
        /// Tab shown in the Remote GUI.
        struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
        {
            armarx::RemoteGui::Client::LineEdit boxLayerName;
            armarx::RemoteGui::Client::IntSpinBox numBoxes;
 
            armarx::RemoteGui::Client::Button drawBoxes;
        };
        RemoteGuiTab tab;
        */
        template <typename T>
        using obs_ptr = std::experimental::observer_ptr<T>;
 
        template <typename T>
        using rw_plugin = armarx::armem::client::plugins::ReaderWriterPlugin<T>;
 
        obs_ptr<rw_plugin<armarx::armem::robot_state::VirtualRobotReader>> virtualRobotReader_;
 
        //armarx::kinesthetic_learning::kinesthetic_teaching::trigger::FTWristSensorTrigger m_trigger;
    };
 
} // namespace armarx::control::components::controller_creator