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 <ArmarXCore/core/Component.h>
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include "RobotStateComponentPlugin.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 {_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 {_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 {_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 {_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
{
    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);
    }
 
}