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 <VirtualRobot/Robot.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 <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.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 <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
 
#include <armarx/control/client/ComponentPlugin.h>
#include <armarx/control/client/ControllerBuilder.h>
#include <armarx/control/common/type.h>
#include <armarx/control/njoint_controller/controller_descriptions.h>
#include <armarx/control/njoint_mp_controller/controller_descriptions.h>
#include <armarx/control/components/controller_creator/ComponentInterface.h>
#include <armarx/control/common/ft/utils/ForceTorqueUtility.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& controllerNamePrefix, const std::string &robotNodeSet, const std::string& controllerType, const std::string& configFilename, const Ice::Current& = Ice::emptyCurrent) override;
        Ice::FloatSeq getPoseInRootFrame(const std::string& nodeName, const Ice::Current& = Ice::emptyCurrent) override;
        Ice::FloatSeq getPrevTargetPose(const std::string& controllerName, const Ice::Current& = Ice::emptyCurrent) override;
        Ice::FloatSeq getPrevNullspaceTargetAngle(const std::string& controllerName, const Ice::Current& = Ice::emptyCurrent) override;
        std::string kinestheticTeaching(const std::string& kinematicChainName, const std::string& side, const std::string &fileName, const Ice::Current& = Ice::emptyCurrent) override;
        void enableVelocityMode(const Ice::Current& = Ice::emptyCurrent) override;
 
        bool updateTargetPose(const std::string &controllerName, const std::string& controllerType, const Ice::FloatSeq& targetPoseVector, const Ice::Current & = Ice::emptyCurrent) override;
        bool updateTargetNullspaceJointAngles(const std::string &controllerName, const std::string &controllerType, const Ice::FloatSeq& targetNullspaceJointAngle, const Ice::Current & = Ice::emptyCurrent) override;
        bool updateTarget(const std::string &controllerName, const std::string &controllerType, const Ice::FloatSeq& targetPoseVector, const Ice::FloatSeq& targetNullspaceJointAngle, const Ice::Current & = Ice::emptyCurrent) override;
 
    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 createImpedanceController(const std::string &controllerNamePrefix, const std::string &robotNodeSet, const std::string &configFilename);
        std::string createImpedanceMPController(const std::string &controllerNamePrefix, const std::string &robotNodeSet, const std::string &configFilename);
        std::string createAdmittanceController(const std::string &controllerNamePrefix, const std::string &robotNodeSet, const std::string &configFilename);
 
 
    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, std::optional<control::client::ControllerWrapper<control::common::ControllerType::TSImpedance>>> m_ctrlTSImpedance;
        std::map<std::string, std::optional<control::client::ControllerWrapper<control::common::ControllerType::TSAdmittance>>> m_ctrlTSAdmittance;
        std::map<std::string, std::optional<control::client::ControllerWrapper<control::common::ControllerType::TSImpedanceMP>>> m_ctrlTSImpedanceMP;
        std::map<std::string, VirtualRobot::RobotNodeSetPtr> m_robotNodeSets;
        std::map<std::string, unsigned int> m_controllerJointNumbers;
 
        std::map<std::string, Eigen::Matrix4f> m_prevTargetPose;
        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;
        */
 
    };
 
}  /// namespace armarx::control::components::controller_creator