RobotStateObserver.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    ArmarX::Core
* @author     Stefan Ulbrich <stefan dot ulbrich at kit dot edu>, Kai Welke (welke _at_ kit _dot_ edu)
* @date       2011 Kai Welke
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "RobotStateObserver.h"
#include <RobotAPI/interface/core/RobotState.h>
#include <ArmarXCore/core/util/algorithm.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
#include <VirtualRobot/IK/DifferentialIK.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#define TCP_POSE_CHANNEL "TCPPose"
#define TCP_TRANS_VELOCITIES_CHANNEL "TCPVelocities"
 
 
using namespace armarx;
using namespace VirtualRobot;
 
// ********************************************************************
// observer framework hooks
// ********************************************************************
RobotStateObserver::RobotStateObserver()
{
 
}
 
void RobotStateObserver::onInitObserver()
{
 
    // register all checks
    offerConditionCheck("equals", new ConditionCheckEquals());
    offerConditionCheck("inrange", new ConditionCheckInRange());
    offerConditionCheck("larger", new ConditionCheckLarger());
    offerConditionCheck("smaller", new ConditionCheckSmaller());
}
 
void RobotStateObserver::onConnectObserver()
{
 
 
    offerChannel(TCP_POSE_CHANNEL, "TCP poses of the robot.");
    offerChannel(TCP_TRANS_VELOCITIES_CHANNEL, "TCP velocities of the robot.");
}
 
// ********************************************************************
// private methods
// ********************************************************************
 
 
 
 
void RobotStateObserver::updatePoses()
{
    if (getState() < eManagedIceObjectStarting)
    {
        return;
    }
 
    if (!robot)
    {
        return;
    }
 
    std::unique_lock lock(dataMutex);
    ReadLockPtr lock2 = robot->getReadLock();
    FramedPoseBaseMap tcpPoses;
    std::string rootFrame =  robot->getRootNode()->getName();
 
    //IceUtil::Time start = IceUtil::Time::now();
    for (unsigned int i = 0; i < nodesToReport.size(); i++)
    {
        VirtualRobot::RobotNodePtr& node = nodesToReport.at(i).first;
        const std::string& tcpName  = node->getName();
        const Eigen::Matrix4f& currentPose = node->getPoseInRootFrame();
        tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, robot->getName());
        FramedPosePtr::dynamicCast(tcpPoses[tcpName])->changeFrame(robot, nodesToReport.at(i).second);
 
    }
 
    udpatePoseDatafields(tcpPoses);
 
}
 
void RobotStateObserver::udpatePoseDatafields(const FramedPoseBaseMap& poseMap)
{
    //        ARMARX_INFO << deactivateSpam() << "new tcp poses reported";
    FramedPoseBaseMap::const_iterator it = poseMap.begin();
 
    for (; it != poseMap.end(); it++)
    {
 
        FramedPosePtr vec = FramedPosePtr::dynamicCast(it->second);
        const std::string& tcpName = it->first;
 
        if (!existsDataField(TCP_POSE_CHANNEL, tcpName))
        {
            offerDataFieldWithDefault(TCP_POSE_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
        }
        else
        {
            setDataField(TCP_POSE_CHANNEL, tcpName, Variant(it->second));
        }
 
        updateChannel(TCP_POSE_CHANNEL);
 
        if (!existsChannel(tcpName))
        {
            offerChannel(tcpName, "pose components of " + tcpName);
            offerDataFieldWithDefault(tcpName, "x", Variant(vec->position->x), "X axis");
            offerDataFieldWithDefault(tcpName, "y", Variant(vec->position->y), "Y axis");
            offerDataFieldWithDefault(tcpName, "z", Variant(vec->position->z), "Z axis");
            offerDataFieldWithDefault(tcpName, "qx", Variant(vec->orientation->qx), "Quaternion part x");
            offerDataFieldWithDefault(tcpName, "qy", Variant(vec->orientation->qy), "Quaternion part y");
            offerDataFieldWithDefault(tcpName, "qz", Variant(vec->orientation->qz), "Quaternion part z");
            offerDataFieldWithDefault(tcpName, "qw", Variant(vec->orientation->qw), "Quaternion part w");
            offerDataFieldWithDefault(tcpName, "frame", Variant(vec->frame), "Reference Frame");
        }
        else
        {
            StringVariantBaseMap newValues;
            newValues["x"] =  new Variant(vec->position->x);
            newValues["y"] =  new Variant(vec->position->y);
            newValues["z"] =  new Variant(vec->position->z);
            newValues["qx"] =  new Variant(vec->orientation->qx);
            newValues["qy"] =  new Variant(vec->orientation->qy);
            newValues["qz"] =  new Variant(vec->orientation->qz);
            newValues["qw"] =  new Variant(vec->orientation->qw);
            newValues["frame"] =  new Variant(vec->frame);
            setDataFieldsFlatCopy(tcpName, newValues);
        }
 
        updateChannel(tcpName);
 
    }
}
 
void RobotStateObserver::getPoseDatafield_async(
    const AMD_RobotStateObserverInterface_getPoseDatafieldPtr& amd,
    const std::string& nodeName,
    const Ice::Current&) const
{
    addWorkerJob("RobotStateObserver::getPoseDatafield", [this, amd, nodeName]
    {
        return getDatafieldRefByName(TCP_POSE_CHANNEL, nodeName);
    });
}
 
void RobotStateObserver::updateNodeVelocities(const NameValueMap& jointVel, long timestampMicroSeconds)
{
    if (jointVel.empty())
    {
        return;
    }
    if (getState() < eManagedIceObjectStarting)
    {
        return;
    }
 
    std::unique_lock lock(dataMutex);
 
    if (!robot)
    {
        return;
    }
 
    ReadLockPtr lock2 = robot->getReadLock();
 
    if (!velocityReportRobot)
    {
        velocityReportRobot = robot->clone(robot->getName());
    }
 
    //    IceUtil::Time start = IceUtil::Time::now();
    //    ARMARX_INFO << jointVel;    FramedPoseBaseMap tcpPoses;
    FramedDirectionMap tcpTranslationVelocities;
    FramedDirectionMap tcpOrientationVelocities;
    std::string rootFrame =  robot->getRootNode()->getName();
    NameValueMap tempJointAngles = robot->getConfig()->getRobotNodeJointValueMap();
    FramedPoseBaseMap tcpPoses;
 
    for (unsigned int i = 0; i < nodesToReport.size(); i++)
    {
        RobotNodePtr node = nodesToReport.at(i).first;
        const std::string& tcpName  = node->getName();
        const Eigen::Matrix4f& currentPose = velocityReportRobot->getRobotNode(tcpName)->getGlobalPose();
        tcpPoses[tcpName] = new FramedPose(currentPose, GlobalFrame, "");
 
    }
 
 
    velocityReportRobot->setJointValues(tempJointAngles);
    velocityReportRobot->setGlobalPose(robot->getGlobalPose());
 
    Eigen::Matrix4f mat;
    Eigen::Vector3f rpy;
 
    auto keys = armarx::getMapKeys(jointVel);
    Eigen::VectorXf jointVelocities(jointVel.size());
    auto rootFrameName = velocityReportRobot->getRootNode()->getName();
    RobotNodeSetPtr rns = RobotNodeSet::createRobotNodeSet(velocityReportRobot, "All_Nodes", keys, rootFrameName);
    for (size_t i = 0; i < rns->getSize(); ++i)
    {
        jointVelocities(i) = jointVel.at(rns->getNode(i)->getName());
    }
    DifferentialIKPtr j(new DifferentialIK(rns));
 
 
    auto robotName = velocityReportRobot->getName();
    for (unsigned int i = 0; i < nodesToReport.size(); i++)
    {
        RobotNodePtr node = velocityReportRobot->getRobotNode(nodesToReport.at(i).first->getName());
        Eigen::MatrixXf jac = j->getJacobianMatrix(node, IKSolver::All);
        Eigen::VectorXf nodeVel = jac * jointVelocities;
 
        const std::string tcpName  = node->getName();
        tcpTranslationVelocities[tcpName] = new FramedDirection(nodeVel.block<3, 1>(0, 0), rootFrameName, robotName);
        tcpOrientationVelocities[tcpName] = new FramedDirection(nodeVel.block<3, 1>(3, 0), rootFrameName, robotName);
 
 
    }
    updateVelocityDatafields(tcpTranslationVelocities, tcpOrientationVelocities);
}
 
void RobotStateObserver::updateVelocityDatafields(const FramedDirectionMap& tcpTranslationVelocities, const FramedDirectionMap& tcpOrientationVelocities)
{
    FramedDirectionMap::const_iterator it = tcpTranslationVelocities.begin();
 
    for (; it != tcpTranslationVelocities.end(); it++)
    {
 
        FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(it->second);
        FramedDirectionPtr vecOri;
        FramedDirectionMap::const_iterator itOri = tcpOrientationVelocities.find(it->first);
 
        if (itOri != tcpOrientationVelocities.end())
        {
            vecOri = FramedDirectionPtr::dynamicCast(itOri->second);
        }
 
        const std::string& tcpName = it->first;
 
        ARMARX_CHECK_EXPRESSION(vec->frame == vecOri->frame);
 
        if (!existsDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName))
        {
            offerDataFieldWithDefault(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
        }
        else
        {
            setDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second));
        }
 
        updateChannel(TCP_TRANS_VELOCITIES_CHANNEL);
        const std::string channelName = tcpName + "Velocities";
 
        if (!existsChannel(channelName))
        {
            offerChannel(channelName, "pose components of " + tcpName);
            offerDataFieldWithDefault(channelName, "x", Variant(vec->x), "X axis");
            offerDataFieldWithDefault(channelName, "y", Variant(vec->y), "Y axis");
            offerDataFieldWithDefault(channelName, "z", Variant(vec->z), "Z axis");
            offerDataFieldWithDefault(channelName, "roll", Variant(vecOri->x), "Roll");
            offerDataFieldWithDefault(channelName, "pitch", Variant(vecOri->y), "Pitch");
            offerDataFieldWithDefault(channelName, "yaw", Variant(vecOri->z), "Yaw");
            offerDataFieldWithDefault(channelName, "frame", Variant(vecOri->frame), "Reference Frame");
        }
        else
        {
            StringVariantBaseMap newValues;
            newValues["x"] =  new Variant(vec->x);
            newValues["y"] =  new Variant(vec->y);
            newValues["z"] =  new Variant(vec->z);
            newValues["roll"] =  new Variant(vecOri->x);
            newValues["pitch"] =  new Variant(vecOri->y);
            newValues["yaw"] =  new Variant(vecOri->z);
            newValues["frame"] =  new Variant(vec->frame);
            setDataFieldsFlatCopy(channelName, newValues);
        }
 
        updateChannel(channelName);
 
    }
}
 
 
PropertyDefinitionsPtr RobotStateObserver::createPropertyDefinitions()
{
    return PropertyDefinitionsPtr(new RobotStateObserverPropertyDefinitions(
                                      getConfigIdentifier()));
}
 
void RobotStateObserver::setRobot(RobotPtr robot)
{
    std::unique_lock lock(dataMutex);
    this->robot = robot;
 
    std::vector< VirtualRobot::RobotNodeSetPtr > robotNodes;
    robotNodes = robot->getRobotNodeSets();
 
    std::string nodesetsString = getProperty<std::string>("TCPsToReport").getValue();
    nodesToReport.clear();
    //    nodesToReport.push_back(std::make_pair(robot->getRootNode(), GlobalFrame));
    if (!nodesetsString.empty())
    {
        if (nodesetsString == "*")
        {
            auto nodesets = robot->getRobotNodeSets();
 
            for (RobotNodeSetPtr& set : nodesets)
            {
                if (set->getTCP())
                {
                    nodesToReport.push_back(std::make_pair(set->getTCP(), robot->getRootNode()->getName()));
                }
            }
        }
        else
        {
            std::vector<std::string> nodesetNames = simox::alg::split(nodesetsString, ",");
 
            for (auto name : nodesetNames)
            {
                simox::alg::trim(name);
                auto node = robot->getRobotNode(name);
 
                if (node)
                {
                    nodesToReport.push_back(std::make_pair(node, robot->getRootNode()->getName()));
                }
                else
                {
                    ARMARX_ERROR << "Could not find node with name: " << name;
                }
            }
        }
    }
}