RobotStateComponentPlugin.cpp

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
 */
 
#include "RobotStateComponentPlugin.h"
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/Component.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
namespace armarx::plugins
{
    RobotNameHelperPtr
    RobotStateComponentPlugin::getRobotNameHelper() const
    {
        ARMARX_CHECK_NOT_NULL(_nameHelper);
        return _nameHelper;
    }
 
    void
    RobotStateComponentPlugin::setRobotStateComponent(const RobotStateComponentInterfacePrx& rsc)
    {
        ARMARX_CHECK_NOT_NULL(rsc);
        ARMARX_CHECK_NULL(_robotStateComponent);
        _robotStateComponent = rsc;
    }
 
    bool
    RobotStateComponentPlugin::hasRobot(const std::string& id) const
    {
        std::lock_guard g{_robotsMutex};
        return _robots.count(id);
    }
 
    VirtualRobot::RobotPtr
    RobotStateComponentPlugin::addRobot(const std::string& id,
                                        const VirtualRobot::RobotPtr& robot,
                                        const VirtualRobot::RobotNodeSetPtr& rns,
                                        const VirtualRobot::RobotNodePtr& node)
    {
        ARMARX_CHECK_NOT_NULL(robot);
        if (rns)
        {
            ARMARX_CHECK_EXPRESSION(robot == rns->getRobot());
        }
        if (node)
        {
            ARMARX_CHECK_EXPRESSION(robot == node->getRobot());
        }
        std::lock_guard g {_robotsMutex};
        ARMARX_CHECK_EXPRESSION(!hasRobot(id)) << "Robot with id '" << id << "' was already added";
        if (rns && !node)
        {
            _robots[id] = {robot, rns, rns->getTCP()};
        }
        else
        {
            _robots[id] = {robot, rns, node};
        }
        return robot;
    }
 
    VirtualRobot::RobotPtr
    RobotStateComponentPlugin::addRobot(const std::string& id,
                                        const VirtualRobot::RobotPtr& robot,
                                        const std::string& rnsName,
                                        const std::string& nodeName)
    {
        ARMARX_CHECK_NOT_NULL(robot) << "robot added with id '" << id << "' is null";
        VirtualRobot::RobotNodeSetPtr rns;
        VirtualRobot::RobotNodePtr node;
        if (!rnsName.empty())
        {
            rns = robot->getRobotNodeSet(rnsName);
            ARMARX_CHECK_NOT_NULL(rns) << "The robot has no node set with name '" << rnsName
                                       << "'. Available RNS: " << robot->getRobotNodeSetNames();
        }
        if (!nodeName.empty())
        {
            auto node = robot->getRobotNode(nodeName);
            ARMARX_CHECK_NOT_NULL(node) << "The robot has no node with name '" << nodeName
                                        << "'. Available nodes: " << robot->getRobotNodeNames();
        }
        else if (rns)
        {
            node = rns->getTCP();
        }
        return addRobot(id, robot, rns, node);
    }
 
    VirtualRobot::RobotPtr
    RobotStateComponentPlugin::addRobot(const std::string& id,
                                        VirtualRobot::RobotIO::RobotDescription loadMode,
                                        const std::string& rnsName,
                                        const std::string& nodeName)
    {
        return addRobot(id,
                        RemoteRobot::createLocalCloneFromFile(_robotStateComponent, loadMode),
                        rnsName,
                        nodeName);
    }
 
    VirtualRobot::RobotPtr
    RobotStateComponentPlugin::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)
    {
        return addRobot(
            id,
            RemoteRobot::createLocalClone(_robotStateComponent, filename, packages, loadMode),
            rnsName,
            nodeName);
    }
 
    VirtualRobot::RobotPtr
    RobotStateComponentPlugin::getRobot(const std::string& id) const
    {
        return getRobotData(id).robot;
    }
 
    RobotStateComponentPlugin::RobotData
    RobotStateComponentPlugin::getRobotData(const std::string& id) const
    {
        std::lock_guard g{_robotsMutex};
        ARMARX_CHECK_EXPRESSION(hasRobot(id)) << "No robot with id '" << id << "' loaded";
        return _robots.at(id);
    }
 
    void
    RobotStateComponentPlugin::setRobotRNSAndNode(const std::string& id,
                                                  const std::string& rnsName,
                                                  const std::string& nodeName)
    {
        std::lock_guard g{_robotsMutex};
        ARMARX_CHECK_EXPRESSION(hasRobot(id)) << "No robot with id '" << id << "' loaded";
        auto& r = _robots.at(id);
        ARMARX_CHECK_EXPRESSION(!rnsName.empty())
            << "There was no robot node set specified! "
            << "Available RNS: " << r.robot->getRobotNodeSetNames();
        auto rns = r.robot->getRobotNodeSet(rnsName);
        ARMARX_CHECK_NOT_NULL(rns) << "The robot has no node set with name '" << rnsName
                                   << "'. Available RNS: " << r.robot->getRobotNodeSetNames();
        if (nodeName.empty())
        {
            r.rns = rns;
            r.node = rns->getTCP();
        }
        else
        {
            auto node = r.robot->getRobotNode(nodeName);
            ARMARX_CHECK_NOT_NULL(node) << "The robot has no node with name '" << nodeName
                                        << "'. Available nodes: " << r.robot->getRobotNodeNames();
            r.rns = rns;
            r.node = node;
        }
    }
 
    const RobotStateComponentInterfacePrx&
    RobotStateComponentPlugin::getRobotStateComponent() const
    {
        return _robotStateComponent;
    }
 
    bool
    RobotStateComponentPlugin::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const
    {
        return RemoteRobot::synchronizeLocalClone(robot, _robotStateComponent);
    }
 
    bool
    RobotStateComponentPlugin::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot,
                                                     Ice::Long timestamp) const
    {
        return RemoteRobot::synchronizeLocalCloneToTimestamp(
            robot, _robotStateComponent, timestamp);
    }
 
    bool
    RobotStateComponentPlugin::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot,
                                                     const RobotStateConfig& state) const
    {
        return RemoteRobot::synchronizeLocalCloneToState(robot, state);
    }
 
    bool
    RobotStateComponentPlugin::synchronizeLocalClone(const RobotData& rdata) const
    {
        return synchronizeLocalClone(rdata.robot);
    }
 
    bool
    RobotStateComponentPlugin::synchronizeLocalClone(const RobotData& rdata,
                                                     Ice::Long timestamp) const
    {
        return synchronizeLocalClone(rdata.robot, timestamp);
    }
 
    bool
    RobotStateComponentPlugin::synchronizeLocalClone(const RobotData& rdata,
                                                     const RobotStateConfig& state) const
    {
        return synchronizeLocalClone(rdata.robot, state);
    }
 
    bool
    RobotStateComponentPlugin::synchronizeLocalClone(const std::string& id) const
    {
        return synchronizeLocalClone(getRobot(id));
    }
 
    bool
    RobotStateComponentPlugin::synchronizeLocalClone(const std::string& id,
                                                     Ice::Long timestamp) const
    {
        return synchronizeLocalClone(getRobot(id), timestamp);
    }
 
    bool
    RobotStateComponentPlugin::synchronizeLocalClone(const std::string& id,
                                                     const RobotStateConfig& state) const
    {
        return synchronizeLocalClone(getRobot(id), state);
    }
 
    SimpleDiffIK::Result
    RobotStateComponentPlugin::calculateRobotDiffIK(const std::string& id,
                                                    const Eigen::Matrix4f& targetPose,
                                                    const SimpleDiffIK::Parameters& params) const
    {
        return getRobotData(id).calculateRobotDiffIK(targetPose, params);
    }
 
    SimpleDiffIK::Reachability
    RobotStateComponentPlugin::calculateRobotReachability(
        const std::string& id,
        const std::vector<Eigen::Matrix4f>& targets,
        const Eigen::VectorXf& initialJV,
        const SimpleDiffIK::Parameters& params) const
    {
        return getRobotData(id).calculateRobotReachability(targets, initialJV, params);
    }
 
    void
    RobotStateComponentPlugin::preOnInitComponent()
    {
        if (!_deactivated && !_robotStateComponent && _robotStateComponentName.empty())
        {
            parent<Component>().getProperty(_robotStateComponentName,
                                            makePropertyName(_propertyName));
        }
 
        if (!_deactivated && !_robotStateComponent)
        {
            parent<Component>().usingProxy(_robotStateComponentName);
        }
    }
 
    void
    RobotStateComponentPlugin::preOnConnectComponent()
    {
        if (!_deactivated && !_robotStateComponent)
        {
            parent<Component>().getProxy(_robotStateComponent, _robotStateComponentName);
        }
        if (!_deactivated)
        {
            _nameHelper = std::make_shared<RobotNameHelper>(_robotStateComponent->getRobotInfo());
        }
    }
 
    void
    RobotStateComponentPlugin::postOnDisconnectComponent()
    {
        _robotStateComponent = nullptr;
    }
 
    void
    RobotStateComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
    {
        if (!properties->hasDefinition(makePropertyName(_propertyName)))
        {
            properties->defineOptionalProperty<std::string>(makePropertyName(_propertyName),
                                                            "RobotStateComponent",
                                                            "Name of the robot state component");
        }
    }
 
    SimpleDiffIK::Result
    RobotStateComponentPlugin::RobotData::calculateRobotDiffIK(
        const Eigen::Matrix4f& targetPose,
        const SimpleDiffIK::Parameters& params) const
    {
        ARMARX_CHECK_NOT_NULL(rns) << "No robot node set configured for the robot";
        ARMARX_CHECK_NOT_NULL(node) << "No tcp configured for the robot";
        return SimpleDiffIK::CalculateDiffIK(targetPose, rns, node, params);
    }
 
    SimpleDiffIK::Reachability
    RobotStateComponentPlugin::RobotData::calculateRobotReachability(
        const std::vector<Eigen::Matrix4f>& targets,
        const Eigen::VectorXf& initialJV,
        const SimpleDiffIK::Parameters& params) const
    {
        ARMARX_CHECK_NOT_NULL(rns) << "No robot node set configured for the robot";
        ARMARX_CHECK_NOT_NULL(node) << "No tcp configured for the robot ";
        return SimpleDiffIK::CalculateReachability(targets, initialJV, rns, node, params);
    }
 
    void
    RobotStateComponentPlugin::setRobotStateComponentName(const std::string& name)
    {
        ARMARX_CHECK_NOT_EMPTY(name);
        ARMARX_CHECK_EMPTY(_robotStateComponentName);
        _robotStateComponentName = name;
    }
 
    const std::string&
    RobotStateComponentPlugin::getRobotStateComponentName() const
    {
        return _robotStateComponentName;
    }
 
    void
    RobotStateComponentPlugin::deactivate()
    {
        _deactivated = true;
    }
 
    Eigen::Matrix4f
    RobotStateComponentPlugin::transformFromTo(const std::string& from,
                                               const std::string& to,
                                               const VirtualRobot::RobotPtr& rob)
    {
        ARMARX_CHECK_NOT_NULL(rob);
        armarx::FramedPose fp{Eigen::Matrix4f::Identity(), from, rob->getName()};
        fp.changeFrame(rob, to);
        return fp.toEigen();
    }
} // namespace armarx::plugins
 
namespace armarx
{
    RobotStateComponentPluginUser::RobotStateComponentPluginUser()
    {
        addPlugin(_robotStateComponentPlugin);
    }
 
    const RobotStateComponentPluginUser::RobotStateComponentPlugin&
    RobotStateComponentPluginUser::getRobotStateComponentPlugin() const
    {
        return *_robotStateComponentPlugin;
    }
 
    RobotStateComponentPluginUser::RobotStateComponentPlugin&
    RobotStateComponentPluginUser::getRobotStateComponentPlugin()
    {
        return *_robotStateComponentPlugin;
    }
 
    const RobotStateComponentInterfacePrx&
    RobotStateComponentPluginUser::getRobotStateComponent() const
    {
        return getRobotStateComponentPlugin().getRobotStateComponent();
    }
 
    RobotNameHelperPtr
    RobotStateComponentPluginUser::getRobotNameHelper() const
    {
        return getRobotStateComponentPlugin().getRobotNameHelper();
    }
 
    bool
    RobotStateComponentPluginUser::hasRobot(const std::string& id) const
    {
        return getRobotStateComponentPlugin().hasRobot(id);
    }
 
    VirtualRobot::RobotPtr
    RobotStateComponentPluginUser::addRobot(const std::string& id,
                                            const VirtualRobot::RobotPtr& robot,
                                            const VirtualRobot::RobotNodeSetPtr& rns,
                                            const VirtualRobot::RobotNodePtr& node)
    {
        return getRobotStateComponentPlugin().addRobot(id, robot, rns, node);
    }
 
    VirtualRobot::RobotPtr
    RobotStateComponentPluginUser::addRobot(const std::string& id,
                                            const VirtualRobot::RobotPtr& robot,
                                            const std::string& rnsName,
                                            const std::string& nodeName)
    {
        return getRobotStateComponentPlugin().addRobot(id, robot, rnsName, nodeName);
    }
 
    VirtualRobot::RobotPtr
    RobotStateComponentPluginUser::addRobot(const std::string& id,
                                            VirtualRobot::RobotIO::RobotDescription loadMode,
                                            const std::string& rnsName,
                                            const std::string& nodeName)
    {
        return getRobotStateComponentPlugin().addRobot(id, loadMode, rnsName, nodeName);
    }
 
    VirtualRobot::RobotPtr
    RobotStateComponentPluginUser::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)
    {
        return getRobotStateComponentPlugin().addRobot(
            id, filename, packages, loadMode, rnsName, nodeName);
    }
 
    VirtualRobot::RobotPtr
    RobotStateComponentPluginUser::getRobot(const std::string& id) const
    {
        return getRobotStateComponentPlugin().getRobot(id);
    }
 
    plugins::RobotStateComponentPlugin::RobotData
    RobotStateComponentPluginUser::getRobotData(const std::string& id) const
    {
        return getRobotStateComponentPlugin().getRobotData(id);
    }
 
    void
    RobotStateComponentPluginUser::setRobotRNSAndNode(const std::string& id,
                                                      const std::string& rnsName,
                                                      const std::string& nodeName)
    {
        getRobotStateComponentPlugin().setRobotRNSAndNode(id, rnsName, nodeName);
    }
 
    RobotStateComponentInterfacePrx
    RobotStateComponentPluginUser::getRobotStateComponent()
    {
        return getRobotStateComponentPlugin().getRobotStateComponent();
    }
 
    bool
    RobotStateComponentPluginUser::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const
    {
        return getRobotStateComponentPlugin().synchronizeLocalClone(robot);
    }
 
    bool
    RobotStateComponentPluginUser::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot,
                                                         Ice::Long timestamp) const
    {
        return getRobotStateComponentPlugin().synchronizeLocalClone(robot, timestamp);
    }
 
    bool
    RobotStateComponentPluginUser::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot,
                                                         const RobotStateConfig& state) const
    {
        return getRobotStateComponentPlugin().synchronizeLocalClone(robot, state);
    }
 
    bool
    RobotStateComponentPluginUser::synchronizeLocalClone(
        const plugins::RobotStateComponentPlugin::RobotData& rdata) const
    {
        return getRobotStateComponentPlugin().synchronizeLocalClone(rdata);
    }
 
    bool
    RobotStateComponentPluginUser::synchronizeLocalClone(
        const plugins::RobotStateComponentPlugin::RobotData& rdata,
        Ice::Long timestamp) const
    {
        return getRobotStateComponentPlugin().synchronizeLocalClone(rdata, timestamp);
    }
 
    bool
    RobotStateComponentPluginUser::synchronizeLocalClone(
        const plugins::RobotStateComponentPlugin::RobotData& rdata,
        const RobotStateConfig& state) const
    {
        return getRobotStateComponentPlugin().synchronizeLocalClone(rdata, state);
    }
 
    bool
    RobotStateComponentPluginUser::synchronizeLocalClone(const std::string& id) const
    {
        return getRobotStateComponentPlugin().synchronizeLocalClone(id);
    }
 
    bool
    RobotStateComponentPluginUser::synchronizeLocalClone(const std::string& id,
                                                         Ice::Long timestamp) const
    {
        return getRobotStateComponentPlugin().synchronizeLocalClone(id, timestamp);
    }
 
    bool
    RobotStateComponentPluginUser::synchronizeLocalClone(const std::string& id,
                                                         const RobotStateConfig& state) const
    {
        return getRobotStateComponentPlugin().synchronizeLocalClone(id, state);
    }
 
    SimpleDiffIK::Result
    RobotStateComponentPluginUser::calculateRobotDiffIK(const std::string& id,
                                                        const Eigen::Matrix4f& targetPose,
                                                        const SimpleDiffIK::Parameters& params)
    {
        return getRobotStateComponentPlugin().calculateRobotDiffIK(id, targetPose, params);
    }
 
    SimpleDiffIK::Reachability
    RobotStateComponentPluginUser::calculateRobotReachability(
        const std::string& id,
        const std::vector<Eigen::Matrix4f> targets,
        const Eigen::VectorXf& initialJV,
        const SimpleDiffIK::Parameters& params)
    {
        return getRobotStateComponentPlugin().calculateRobotReachability(
            id, targets, initialJV, params);
    }
 
} // namespace armarx