RobotStateComponentPlugin.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    RobotAPI::ArmarXObjects::RobotAPIComponentPlugins
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <mutex>
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/ComponentPlugin.h>
 
#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
 
 
namespace armarx::plugins
{
    class RobotStateComponentPlugin : public ComponentPlugin
    {
    public:
        struct RobotData;
        using ComponentPlugin::ComponentPlugin;
 
        void setRobotStateComponentName(const std::string& name);
        void setRobotStateComponent(const RobotStateComponentInterfacePrx& rsc);
        const std::string& getRobotStateComponentName() const;
        void deactivate();
        //get / add
    public:
        bool hasRobot(const std::string& id) const;
 
        VirtualRobot::RobotPtr addRobot(
            const std::string& id,
            const VirtualRobot::RobotPtr& robot,
            const VirtualRobot::RobotNodeSetPtr& rns = {},
            const VirtualRobot::RobotNodePtr& node = {});
        VirtualRobot::RobotPtr addRobot(
            const std::string& id,
            const VirtualRobot::RobotPtr& robot,
            const std::string& rnsName,
            const std::string& nodeName = "");
        VirtualRobot::RobotPtr addRobot(
            const std::string& id,
            VirtualRobot::RobotIO::RobotDescription loadMode,
            const std::string& rnsName = "",
            const std::string& nodeName = "");
        VirtualRobot::RobotPtr addRobot(
            const std::string& id,
            const std::string& filename,
            const Ice::StringSeq packages,
            VirtualRobot::RobotIO::RobotDescription loadMode,
            const std::string& rnsName = "",
            const std::string& nodeName = "");
 
        VirtualRobot::RobotPtr getRobot(const std::string& id) const;
        RobotData getRobotData(const std::string& id) const;
        void setRobotRNSAndNode(const std::string& id, const std::string& rnsName, const std::string& nodeName);
 
        //querry
    public:
        const RobotStateComponentInterfacePrx& getRobotStateComponent() const;
        RobotNameHelperPtr getRobotNameHelper() const;
        Eigen::Matrix4f transformFromTo(const std::string& from,
                                        const std::string& to,
                                        const VirtualRobot::RobotPtr& rob);
 
        //sync
    public:
        bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const;
        bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot, Ice::Long timestamp) const;
        bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot, const RobotStateConfig& state) const;
 
        bool synchronizeLocalClone(const RobotData& rdata) const;
        bool synchronizeLocalClone(const RobotData& rdata, Ice::Long timestamp) const;
        bool synchronizeLocalClone(const RobotData& rdata, const RobotStateConfig& state) const;
 
        bool synchronizeLocalClone(const std::string& id) const;
        bool synchronizeLocalClone(const std::string& id, Ice::Long timestamp) const;
        bool synchronizeLocalClone(const std::string& id, const RobotStateConfig& state) const;
 
        //diffik
    public:
        SimpleDiffIK::Result       calculateRobotDiffIK(
            const std::string& id,
            const Eigen::Matrix4f& targetPose,
            const SimpleDiffIK::Parameters& params = {}) const;
        SimpleDiffIK::Reachability calculateRobotReachability(
            const std::string& id,
            const std::vector<Eigen::Matrix4f>& targets,
            const Eigen::VectorXf& initialJV,
            const SimpleDiffIK::Parameters& params = {}) const;
 
        //hooks
    protected:
        void preOnInitComponent() override;
        void preOnConnectComponent() override;
        void postOnDisconnectComponent() override;
        void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
 
        //structs
    public:
        struct RobotData
        {
            VirtualRobot::RobotPtr        robot;
            VirtualRobot::RobotNodeSetPtr rns;
            VirtualRobot::RobotNodePtr    node;
 
            SimpleDiffIK::Result calculateRobotDiffIK(
                const Eigen::Matrix4f& targetPose,
                const SimpleDiffIK::Parameters& params = {}) const;
            SimpleDiffIK::Reachability calculateRobotReachability(
                const std::vector<Eigen::Matrix4f>& targets,
                const Eigen::VectorXf& initialJV,
                const SimpleDiffIK::Parameters& params = {}) const;
        };
        //data
    private:
        static constexpr auto            _propertyName = "RemoteStateComponentName";
        std::string                      _robotStateComponentName;
        RobotStateComponentInterfacePrx  _robotStateComponent;
        mutable std::recursive_mutex     _robotsMutex;
        std::map<std::string, RobotData> _robots;
        RobotNameHelperPtr               _nameHelper;
        bool                             _deactivated = false;
    };
}
 
 
#include <ArmarXCore/core/ManagedIceObject.h>
 
namespace armarx
{
    class RobotStateComponentPluginUser : virtual public ManagedIceObject
    {
    public:
        using RobotStateComponentPlugin = armarx::plugins::RobotStateComponentPlugin;
    private:
        RobotStateComponentPlugin* _robotStateComponentPlugin{nullptr};
    public:
        RobotStateComponentPluginUser();
 
        const RobotStateComponentPlugin& getRobotStateComponentPlugin() const;
        RobotStateComponentPlugin& getRobotStateComponentPlugin();
 
        const RobotStateComponentInterfacePrx& getRobotStateComponent() const;
 
        RobotNameHelperPtr getRobotNameHelper() const;
        //get / add
    public:
        bool hasRobot(const std::string& id) const;
 
        VirtualRobot::RobotPtr addRobot(
            const std::string& id,
            const VirtualRobot::RobotPtr& robot,
            const VirtualRobot::RobotNodeSetPtr& rns = {},
            const VirtualRobot::RobotNodePtr& node = {});
        VirtualRobot::RobotPtr addRobot(
            const std::string& id,
            const VirtualRobot::RobotPtr& robot,
            const std::string& rnsName,
            const std::string& nodeName = "");
        VirtualRobot::RobotPtr addRobot(
            const std::string& id,
            VirtualRobot::RobotIO::RobotDescription loadMode,
            const std::string& rnsName = "",
            const std::string& nodeName = "");
        VirtualRobot::RobotPtr addRobot(
            const std::string& id,
            const std::string& filename,
            const Ice::StringSeq packages,
            VirtualRobot::RobotIO::RobotDescription loadMode,
            const std::string& rnsName = "",
            const std::string& nodeName = "");
 
        template<class...Ts>
        VirtualRobot::RobotPtr addOrGetRobot(const std::string& id, Ts&& ...ts)
        {
            if (hasRobot(id))
            {
                return getRobot(id);
            }
            return addRobot(id, std::forward<Ts>(ts)...);
        }
 
        VirtualRobot::RobotPtr getRobot(const std::string& id) const;
        RobotStateComponentPlugin::RobotData getRobotData(const std::string& id) const;
        void setRobotRNSAndNode(const std::string& id, const std::string& rnsName, const std::string& nodeName);
 
        RobotStateComponentInterfacePrx getRobotStateComponent();
 
        //sync
    public:
        bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const;
        bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot, Ice::Long timestamp) const;
        bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot, const RobotStateConfig& state) const;
 
        bool synchronizeLocalClone(const RobotStateComponentPlugin::RobotData& rdata) const;
        bool synchronizeLocalClone(const RobotStateComponentPlugin::RobotData& rdata, Ice::Long timestamp) const;
        bool synchronizeLocalClone(const RobotStateComponentPlugin::RobotData& rdata, const RobotStateConfig& state) const;
 
        bool synchronizeLocalClone(const std::string& id) const;
        bool synchronizeLocalClone(const std::string& id, Ice::Long timestamp) const;
        bool synchronizeLocalClone(const std::string& id, const RobotStateConfig& state) const;
 
        //diffik
    public:
        SimpleDiffIK::Result calculateRobotDiffIK(
            const std::string& id,
            const Eigen::Matrix4f& targetPose,
            const SimpleDiffIK::Parameters& params = {});
        SimpleDiffIK::Reachability calculateRobotReachability(
            const std::string& id,
            const std::vector<Eigen::Matrix4f> targets,
            const Eigen::VectorXf& initialJV,
            const SimpleDiffIK::Parameters& params = {});
    };
}