TCPControlUnit.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::
* @author     Mirko Waechter ( mirko.waechter at kit dot edu)
* @date       2013
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "TCPControlUnit.h"
 
#include <cfloat>
#include <memory>
 
#include <Eigen/Core>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/libraries/core/LinkedPose.h>
 
using namespace VirtualRobot;
using namespace Eigen;
 
namespace armarx
{
    TCPControlUnit::TCPControlUnit() :
        maxJointVelocity(30.f / 180 * 3.141),
        cycleTime(30),
        requested(false),
        calculationRunning(false)
    {
    }
 
    void
    TCPControlUnit::onInitComponent()
    {
        topicName = getName() + "State";
        usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
        usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
        offeringTopic("DebugObserver");
        offeringTopic(topicName);
        usingTopic(getProperty<std::string>("RobotStateTopicName").getValue());
 
        cycleTime = getProperty<int>("CycleTime").getValue();
        maxJointVelocity = getProperty<float>("MaxJointVelocity").getValue();
    }
 
    void
    TCPControlUnit::onConnectComponent()
    {
        std::unique_lock lock(dataMutex);
 
        debugObs = getTopic<DebugObserverInterfacePrx>("DebugObserver");
        kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(
            getProperty<std::string>("KinematicUnitName").getValue());
        robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(
            getProperty<std::string>("RobotStateComponentName").getValue());
 
 
        //remoteRobotPrx = robotStateComponentPrx->getSynchronizedRobot();
        robotName = robotStateComponentPrx->getRobotName();
        localRobot = RemoteRobot::createLocalClone(robotStateComponentPrx,
                                                   robotStateComponentPrx->getRobotFilename(),
                                                   robotStateComponentPrx->getArmarXPackages());
        jointExistenceCheckRobot =
            RemoteRobot::createLocalClone(robotStateComponentPrx,
                                          robotStateComponentPrx->getRobotFilename(),
                                          robotStateComponentPrx->getArmarXPackages());
 
        localReportRobot = localRobot->clone(localRobot->getName());
 
        std::string nodesetsString = getProperty<std::string>("TCPsToReport").getValue();
        nodesToReport.clear();
        if (!nodesetsString.empty())
        {
            if (nodesetsString == "*")
            {
                auto nodesets = localReportRobot->getRobotNodeSets();
 
                for (RobotNodeSetPtr& set : nodesets)
                {
                    if (set->getTCP())
                    {
                        nodesToReport.push_back(set->getTCP());
                    }
                }
            }
            else
            {
                std::vector<std::string> nodesetNames = simox::alg::split(nodesetsString, ",");
 
                for (auto& name : nodesetNames)
                {
                    auto node = localReportRobot->getRobotNode(name);
 
                    if (node)
                    {
                        nodesToReport.push_back(node);
                    }
                    else
                    {
                        ARMARX_ERROR << "Could not find node with name: " << name;
                    }
                }
            }
        }
 
        std::vector<RobotNodePtr> nodes = localRobot->getRobotNodes();
 
        for (unsigned int i = 0; i < nodes.size(); i++)
        {
            ARMARX_VERBOSE << nodes.at(i)->getName();
        }
 
 
        listener = getTopic<TCPControlUnitListenerPrx>(topicName);
 
        requested = false;
 
        if (execTask)
        {
            execTask->stop();
        }
 
        execTask = new PeriodicTask<TCPControlUnit>(
            this, &TCPControlUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
        execTask->start();
        execTask->setDelayWarningTolerance(100);
    }
 
    void
    TCPControlUnit::onDisconnectComponent()
    {
 
        try
        {
            release();
        }
        catch (std::exception& e)
        {
            ARMARX_WARNING << "Releasing TCP Unit failed";
        }
 
        if (execTask)
        {
            execTask->stop();
        }
    }
 
    void
    TCPControlUnit::onExitComponent()
    {
    }
 
    void
    TCPControlUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current& c)
    {
        std::unique_lock lock(taskMutex);
        cycleTime = milliseconds;
 
        if (execTask)
        {
            execTask->changeInterval(cycleTime);
        }
    }
 
    void
    TCPControlUnit::setTCPVelocity(const std::string& nodeSetName,
                                   const std::string& tcpName,
                                   const FramedDirectionBasePtr& translationVelocity,
                                   const FramedDirectionBasePtr& orientationVelocityRPY,
                                   const Ice::Current& c)
    {
        if (!isRequested())
        {
            ARMARX_WARNING << "Implicitly requesting TCPControlUnit! Please call request before "
                              "setting TCPVelocities!";
            request();
        }
 
        std::unique_lock lock(dataMutex);
        ARMARX_CHECK_EXPRESSION(jointExistenceCheckRobot->hasRobotNodeSet(nodeSetName))
            << "The robot does not have the node set: " + nodeSetName;
 
 
        if (translationVelocity)
        {
            ARMARX_DEBUG << "Setting new Velocity for " << nodeSetName << " in frame "
                         << translationVelocity->frame << ":\n"
                         << FramedDirectionPtr::dynamicCast(translationVelocity)->toEigen();
        }
 
        if (orientationVelocityRPY)
        {
            ARMARX_DEBUG << "Orientation Velo in frame " << orientationVelocityRPY->frame << ": \n"
                         << FramedDirectionPtr::dynamicCast(orientationVelocityRPY)->toEigen();
        }
 
        TCPVelocityData data;
        data.nodeSetName = nodeSetName;
        data.translationVelocity = FramedDirectionPtr::dynamicCast(translationVelocity);
        data.orientationVelocity = FramedDirectionPtr::dynamicCast(orientationVelocityRPY);
 
        if (tcpName.empty())
        {
            data.tcpName =
                jointExistenceCheckRobot->getRobotNodeSet(nodeSetName)->getTCP()->getName();
        }
        else
        {
            ARMARX_CHECK_EXPRESSION(jointExistenceCheckRobot->hasRobotNode(tcpName))
                << "The robot does not have the node: " + tcpName;
 
            data.tcpName = tcpName;
        }
 
        tcpVelocitiesMap[data.tcpName] = data;
    }
 
    void
    TCPControlUnit::init(const Ice::Current& c)
    {
    }
 
    void
    TCPControlUnit::start(const Ice::Current& c)
    {
    }
 
    void
    TCPControlUnit::stop(const Ice::Current& c)
    {
    }
 
    UnitExecutionState
    TCPControlUnit::getExecutionState(const Ice::Current& c)
    {
        switch (getState())
        {
            case eManagedIceObjectStarted:
                return eUnitStarted;
 
            case eManagedIceObjectInitialized:
            case eManagedIceObjectStarting:
                return eUnitInitialized;
 
            case eManagedIceObjectExiting:
            case eManagedIceObjectExited:
                return eUnitStopped;
 
            default:
                return eUnitConstructed;
        }
    }
 
    void
    TCPControlUnit::request(const Ice::Current& c)
    {
        if (execTask)
        {
            //            while (calculationRunning)
            //            {
            //                IceUtil::ThreadControl::yield();
            //            }
 
            execTask->stop();
        }
 
        ARMARX_IMPORTANT << "Requesting TCPControlUnit";
        std::unique_lock lock(dataMutex);
        requested = true;
 
        execTask = new PeriodicTask<TCPControlUnit>(
            this, &TCPControlUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
        execTask->start();
        execTask->setDelayWarningTolerance(100);
        ARMARX_IMPORTANT << "Requested TCPControlUnit";
    }
 
    void
    TCPControlUnit::release(const Ice::Current& c)
    {
        ARMARX_IMPORTANT << "Releasing TCPControlUnit";
        std::unique_lock lock(dataMutex);
 
        //        while (calculationRunning)
        //        {
        //            IceUtil::ThreadControl::yield();
        //        }
 
        tcpVelocitiesMap.clear();
        localTcpVelocitiesMap.clear();
        requested = false;
        //kinematicUnitPrx->stop();
        ARMARX_IMPORTANT << "Released TCPControlUnit";
    }
 
    bool
    TCPControlUnit::isRequested(const Ice::Current& c)
    {
        // no lock needed to read a single bool value.
        return requested;
    }
 
    void
    TCPControlUnit::periodicExec()
    {
        std::unique_lock lock(dataMutex, std::defer_lock);
 
        if (lock.try_lock())
        {
 
            {
                localTcpVelocitiesMap.clear();
                TCPVelocityDataMap::iterator it = tcpVelocitiesMap.begin();
 
                for (; it != tcpVelocitiesMap.end(); it++)
                {
                    localTcpVelocitiesMap[it->first] = it->second;
                }
 
                localDIKMap.clear();
                DIKMap::iterator itDIK = dIKMap.begin();
 
                for (; itDIK != dIKMap.end(); itDIK++)
                {
                    localDIKMap[itDIK->first] = itDIK->second;
                }
 
                //ARMARX_DEBUG << "RN TCP R pose1:" << localRobot->getRobotNode("TCP R")->getGlobalPose();
 
                RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponentPrx);
                //ARMARX_DEBUG << "RN TCP R pose2:" << localRobot->getRobotNode("TCP R")->getGlobalPose();
            }
 
            if (requested)
            {
                calcAndSetVelocities();
            }
        }
    }
 
    void
    TCPControlUnit::calcAndSetVelocities()
    {
        TCPVelocityDataMap::iterator it = localTcpVelocitiesMap.begin();
 
        for (; it != localTcpVelocitiesMap.end(); it++)
        {
            const TCPVelocityData& data = it->second;
            RobotNodeSetPtr nodeSet = localRobot->getRobotNodeSet(data.nodeSetName);
            //            std::string refFrame = nodeSet->getTCP()->getName();
            DIKMap::iterator itDIK = localDIKMap.find(data.nodeSetName);
 
            if (itDIK == localDIKMap.end())
            {
                VirtualRobot::DifferentialIKPtr dIk(new EDifferentialIK(
                    nodeSet, localRobot->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
                float lambda = getProperty<float>("LambdaDampedSVD").getValue();
                dIk->setDampedSvdLambda(lambda);
                localDIKMap[data.nodeSetName] = dIk;
            }
 
            auto dIk = std::dynamic_pointer_cast<EDifferentialIK>(localDIKMap[data.nodeSetName]);
            dIk->clearGoals();
        }
 
        using namespace Eigen;
 
        it = localTcpVelocitiesMap.begin();
 
        for (; it != localTcpVelocitiesMap.end(); it++)
        {
 
            TCPVelocityData& data = it->second;
            //            RobotNodeSetPtr nodeSet = localRobot->getRobotNodeSet(data.nodeSetName);
            std::string refFrame = localRobot->getRootNode()->getName();
            IKSolver::CartesianSelection mode;
 
            if (data.translationVelocity && data.orientationVelocity)
            {
                mode = IKSolver::All;
                //                ARMARX_DEBUG << deactivateSpam(4) << "FullMode";
            }
            else if (data.translationVelocity && !data.orientationVelocity)
            {
                mode = IKSolver::Position;
            }
            else if (!data.translationVelocity && data.orientationVelocity)
            {
                mode = IKSolver::Orientation;
            }
            else
            {
                //                ARMARX_VERBOSE << deactivateSpam(2) << "No mode feasible for " << data.nodeSetName << " - skipping";
                continue;
            }
 
            RobotNodePtr tcpNode = localRobot->getRobotNode(data.tcpName);
            Eigen::Matrix4f m;
            m.setIdentity();
 
            if (data.orientationVelocity)
            {
                Eigen::Matrix3f rotInOriginalFrame, rotInRefFrame;
                rotInOriginalFrame =
                    Eigen::AngleAxisf(data.orientationVelocity->z * cycleTime * 0.001,
                                      Eigen::Vector3f::UnitZ()) *
                    Eigen::AngleAxisf(data.orientationVelocity->y * cycleTime * 0.001,
                                      Eigen::Vector3f::UnitY()) *
                    Eigen::AngleAxisf(data.orientationVelocity->x * cycleTime * 0.001,
                                      Eigen::Vector3f::UnitX());
 
                if (data.orientationVelocity->frame != refFrame)
                {
                    Eigen::Matrix4f trafoOriginalFrameToGlobal = Eigen::Matrix4f::Identity();
 
                    if (data.orientationVelocity->frame != GlobalFrame)
                    {
                        trafoOriginalFrameToGlobal =
                            localRobot->getRobotNode(data.orientationVelocity->frame)
                                ->getGlobalPose();
                    }
 
                    Eigen::Matrix4f trafoRefFrameToGlobal = Eigen::Matrix4f::Identity();
 
                    if (refFrame != GlobalFrame)
                    {
                        trafoRefFrameToGlobal = localRobot->getRobotNode(refFrame)->getGlobalPose();
                    }
 
                    Eigen::Matrix4f trafoOriginalToRef =
                        trafoRefFrameToGlobal.inverse() * trafoOriginalFrameToGlobal;
                    rotInRefFrame = trafoOriginalToRef.block<3, 3>(0, 0) * rotInOriginalFrame *
                                    trafoOriginalToRef.block<3, 3>(0, 0).inverse();
                }
                else
                {
                    rotInRefFrame = rotInOriginalFrame;
                }
 
                m.block(0, 0, 3, 3) = rotInRefFrame * (tcpNode->getGlobalPose().block(0, 0, 3, 3));
            }
 
            //            ARMARX_VERBOSE << deactivateSpam(1) << "Delta Mat: \n" << m;
 
 
            //            m =  m * tcpNode->getGlobalPose();
 
            if (data.translationVelocity)
            {
                data.translationVelocity =
                    FramedDirection::ChangeFrame(localRobot, *data.translationVelocity, refFrame);
                ARMARX_INFO << deactivateSpam(1) << "Translation in " << refFrame << ": "
                            << data.translationVelocity->toEigen();
 
                m.block<3, 1>(0, 3) = tcpNode->getGlobalPose().block<3, 1>(0, 3) +
                                      data.translationVelocity->toEigen() * cycleTime * 0.001;
            }
 
 
            DifferentialIKPtr dIK = localDIKMap[data.nodeSetName];
 
            if (!dIK)
            {
                ARMARX_WARNING << deactivateSpam(1)
                               << "DiffIK is NULL for robot node set: " << data.nodeSetName;
                continue;
            }
 
            //            ARMARX_VERBOSE << deactivateSpam(1) << "Old Pos: \n" << tcpNode->getGlobalPose();
            //            ARMARX_VERBOSE << deactivateSpam(1) << "New Goal: \n" << m;
            //            if(tcpNode != nodeSet->getTCP())
            //            {
            //                mode = IKSolver::Z;
            //                Eigen::VectorXf weight(1);
            //                weight << 0.2;
            //                boost::shared_dynamic_cast<EDifferentialIK>(dIK)->setGoal(m, tcpNode,mode, 0.001, 0.001/180.0f*3.14159, weight);
            //            }
            //            else
            dIK->setGoal(m, tcpNode, mode, 0.001, 0.001 / 180.0f * 3.14159);
 
            //            ARMARX_VERBOSE << deactivateSpam(1) << "Delta to Goal: " << dIK->getDeltaToGoal(tcpNode);
        }
 
 
        NameValueMap targetVelocities;
        NameControlModeMap controlModes;
        DIKMap::iterator itDIK = localDIKMap.begin();
 
        for (; itDIK != localDIKMap.end(); itDIK++)
        {
            RobotNodeSetPtr robotNodeSet = localRobot->getRobotNodeSet(itDIK->first);
 
            auto dIK = std::dynamic_pointer_cast<EDifferentialIK>(itDIK->second);
            //            ARMARX_VERBOSE << deactivateSpam(1) << "Old Pos: \n" << robotNodeSet->getTCP()->getGlobalPose();
            //            dIK->setVerbose(true);
            Eigen::VectorXf jointDelta;
            dIK->computeSteps(
                jointDelta, 0.8f, 0.001, 1, &EDifferentialIK::computeStep); // 1.0, 0.00001, 50
            //            ARMARX_VERBOSE << deactivateSpam(1) << "New Goal: \n" << robotNodeSet->getTCP()->getGlobalPose();
 
            jointDelta /= (cycleTime * 0.001);
 
            jointDelta = applyMaxJointVelocity(jointDelta, maxJointVelocity);
 
            lastTCPPose = robotNodeSet->getTCP()->getGlobalPose();
 
            // build name value map
 
            const std::vector<VirtualRobot::RobotNodePtr> nodes = robotNodeSet->getAllRobotNodes();
            std::vector<VirtualRobot::RobotNodePtr>::const_iterator iter = nodes.begin();
            int i = 0;
 
            while (iter != nodes.end() && i < jointDelta.rows())
            {
                if (targetVelocities.find((*iter)->getName()) != targetVelocities.end())
                {
                    ARMARX_WARNING
                        << deactivateSpam(2) << (*iter)->getName()
                        << " is set from two joint delta calculations - overwriting first value";
                }
 
                targetVelocities.insert(std::make_pair((*iter)->getName(), jointDelta(i)));
 
                controlModes.insert(std::make_pair((*iter)->getName(), eVelocityControl));
                i++;
                iter++;
            };
        }
 
 
        kinematicUnitPrx->switchControlMode(controlModes);
        kinematicUnitPrx->setJointVelocities(targetVelocities);
    }
 
    void
    TCPControlUnit::ContinuousAngles(const Eigen::AngleAxisf& oldAngle,
                                     Eigen::AngleAxisf& newAngle,
                                     double& offset)
    {
        //    if(oldAngle.axis().isApprox(newAngle.axis()*-1))
        const Eigen::Vector3f& v1 = oldAngle.axis();
        const Eigen::Vector3f& v2 = newAngle.axis();
        const Eigen::Vector3f& v2i = newAngle.axis() * -1;
        double angle = acos(v1.dot(v2) / (v1.norm() * v2.norm()));
        double angleInv = acos(v1.dot(v2i) / (v1.norm() * v2i.norm()));
        //        std::cout << "angle1: " << angle << std::endl;
        //        std::cout << "angleInv: " << angleInv << std::endl;
 
        //        Eigen::AngleAxisd result;
        if (angle > angleInv)
        {
            //            ARMARX_IMPORTANT_S << "inversion needed" << std::endl;
            newAngle = Eigen::AngleAxisf(2.0 * M_PI - newAngle.angle(), newAngle.axis() * -1);
        }
 
        //        else newAngle = newAngle;
 
        if (fabs(newAngle.angle() + offset - oldAngle.angle()) >
            fabs(newAngle.angle() + offset - (oldAngle.angle() + M_PI * 2)))
        {
            offset -= M_PI * 2;
        }
        else if (fabs(newAngle.angle() + offset - oldAngle.angle()) >
                 fabs((newAngle.angle() + M_PI * 2 + offset) - oldAngle.angle()))
        {
            offset += M_PI * 2;
        }
 
        newAngle.angle() += offset;
    }
 
    Eigen::VectorXf
    TCPControlUnit::CalcJointLimitAvoidanceDeltas(VirtualRobot::RobotNodeSetPtr robotNodeSet,
                                                  const Eigen::MatrixXf& jacobian,
                                                  const Eigen::MatrixXf& jacobianInverse,
                                                  Eigen::VectorXf desiredJointValues)
    {
        std::vector<VirtualRobot::RobotNodePtr> nodes = robotNodeSet->getAllRobotNodes();
        Eigen::VectorXf actualJointValues(nodes.size());
 
        if (desiredJointValues.rows() == 0)
        {
 
            desiredJointValues.resize(nodes.size());
 
            for (unsigned int i = 0; i < nodes.size(); i++)
            {
                VirtualRobot::RobotNodePtr node = nodes.at(i);
                desiredJointValues(i) =
                    (node->getJointLimitHigh() - node->getJointLimitLow()) * 0.5f +
                    node->getJointLimitLow();
            }
        }
 
        //        ARMARX_IMPORTANT << deactivateSpam(true, 0.5) << "desiredJointValues: "  << desiredJointValues;
        Eigen::VectorXf jointCompensationDeltas(desiredJointValues.rows());
 
        for (unsigned int i = 0; i < desiredJointValues.rows(); i++)
        {
            VirtualRobot::RobotNodePtr node = nodes.at(i);
            actualJointValues(i) = node->getJointValue();
            jointCompensationDeltas(i) = (node->getJointValue() - desiredJointValues(i)) /
                                         (node->getJointLimitHigh() - node->getJointLimitLow());
            jointCompensationDeltas(i) =
                -pow(jointCompensationDeltas(i), 3) * pow(nodes.size() - i, 2);
        }
 
        //        ARMARX_IMPORTANT << deactivateSpam(true, 0.5) << "actualJointValues: "  << actualJointValues;
 
        return CalcNullspaceJointDeltas(jointCompensationDeltas, jacobian, jacobianInverse);
    }
 
    Eigen::VectorXf
    TCPControlUnit::CalcNullspaceJointDeltas(const Eigen::VectorXf& desiredJointDeltas,
                                             const Eigen::MatrixXf& jacobian,
                                             const Eigen::MatrixXf& jacobianInverse)
    {
        Eigen::MatrixXf I(jacobianInverse.rows(), jacobian.cols());
        I.setIdentity();
 
        Eigen::MatrixXf nullspaceProjection = I - jacobianInverse * jacobian;
 
        Eigen::VectorXf delta = nullspaceProjection * desiredJointDeltas;
        return delta;
    }
 
    Eigen::VectorXf
    TCPControlUnit::applyMaxJointVelocity(const Eigen::VectorXf& jointVelocity,
                                          float maxJointVelocity)
    {
        double currentMaxJointVel = std::numeric_limits<double>::min();
 
        for (unsigned int i = 0; i < jointVelocity.rows(); i++)
        {
            currentMaxJointVel =
                std::max(static_cast<double>(std::abs(jointVelocity(i))), currentMaxJointVel);
        }
 
        if (currentMaxJointVel > maxJointVelocity)
        {
            ARMARX_IMPORTANT << deactivateSpam(1)
                             << "max joint velocity too high: " << currentMaxJointVel
                             << " allowed: " << maxJointVelocity;
            return jointVelocity * (maxJointVelocity / currentMaxJointVel);
        }
        else
        {
            return jointVelocity;
        }
    }
 
    PropertyDefinitionsPtr
    TCPControlUnit::createPropertyDefinitions()
    {
 
        return PropertyDefinitionsPtr(new TCPControlUnitPropertyDefinitions(getConfigIdentifier()));
    }
 
    EDifferentialIK::EDifferentialIK(RobotNodeSetPtr rns,
                                     RobotNodePtr coordSystem,
                                     JacobiProvider::InverseJacobiMethod invJacMethod) :
        DifferentialIK(rns, coordSystem, invJacMethod)
    {
    }
 
    Eigen::MatrixXf
    EDifferentialIK::calcFullJacobian()
    {
        if (nRows == 0)
        {
            this->setNRows();
        }
 
        size_t nDoF = nodes.size();
 
        using namespace Eigen;
        MatrixXf Jacobian(nRows, nDoF);
 
        size_t index = 0;
 
        for (size_t i = 0; i < tcp_set.size(); i++)
        {
            SceneObjectPtr tcp = tcp_set[i];
 
            if (this->targets.find(tcp) != this->targets.end())
            {
                IKSolver::CartesianSelection mode = this->modes[tcp];
                MatrixXf partJacobian = this->getJacobianMatrix(tcp, mode);
                Jacobian.block(index, 0, partJacobian.rows(), nDoF) = partJacobian;
            }
            else
            {
                ARMARX_ERROR << "Internal error?!" << std::endl; // Error
            }
        }
 
        return Jacobian;
    }
 
    void
    EDifferentialIK::clearGoals()
    {
        targets.clear();
        modes.clear();
        tolerancePosition.clear();
        toleranceRotation.clear();
        parents.clear();
        tcpWeights.clear();
    }
 
    void
    EDifferentialIK::setRefFrame(RobotNodePtr coordSystem)
    {
        this->coordSystem = coordSystem;
    }
 
    void
    EDifferentialIK::setGoal(const Matrix4f& goal,
                             RobotNodePtr tcp,
                             IKSolver::CartesianSelection mode,
                             float tolerancePosition,
                             float toleranceRotation,
                             VectorXf tcpWeight)
    {
        if (mode <= IKSolver::Z)
        {
            ARMARX_CHECK_EXPRESSION(tcpWeight.rows() == 1)
                << "The tcpWeight vector must be of size 1";
        }
        else if (mode <= IKSolver::Orientation)
        {
            ARMARX_CHECK_EXPRESSION(tcpWeight.rows() == 3)
                << "The tcpWeight vector must be of size 3";
        }
        else if (mode == IKSolver::All)
        {
            ARMARX_CHECK_EXPRESSION(tcpWeight.rows() == 6)
                << "The tcpWeight vector must be of size 6";
        }
 
        tcpWeights[tcp] = tcpWeight;
        DifferentialIK::setGoal(goal, tcp, mode, tolerancePosition, toleranceRotation);
    }
 
    void
    EDifferentialIK::setDOFWeights(Eigen::VectorXf dofWeights)
    {
        this->dofWeights = dofWeights;
    }
 
    bool
    EDifferentialIK::computeSteps(float stepSize, float mininumChange, int maxNStep)
    {
        VectorXf jointDelta;
        return computeSteps(
            jointDelta, stepSize, mininumChange, maxNStep, &DifferentialIK::computeStep);
    }
 
    //    void EDifferentialIK::setTCPWeights(Eigen::VectorXf tcpWeights)
    //    {
    //        this->tcpWeights = tcpWeights;
    //    }
 
    bool
    EDifferentialIK::computeSteps(VectorXf& resultJointDelta,
                                  float stepSize,
                                  float mininumChange,
                                  int maxNStep,
                                  ComputeFunction computeFunction)
    {
        VR_ASSERT(rns);
        VR_ASSERT(nodes.size() == rns->getSize());
        //std::vector<RobotNodePtr> rn = this->nodes;
        RobotPtr robot = rns->getRobot();
        VR_ASSERT(robot);
        std::vector<float> jv(nodes.size(), 0.0f);
        std::vector<float> jvBest = rns->getJointValues();
        int step = 0;
        checkTolerances();
 
        std::vector<std::pair<float, VectorXf>> jointDeltaIterations;
        float lastDist = FLT_MAX;
        VectorXf oldJointValues;
        rns->getJointValues(oldJointValues);
        VectorXf tempDOFWeights = dofWeights;
        //        VectorXf dThetaSum(nodes.size());
        resultJointDelta.resize(nodes.size());
        resultJointDelta.setZero();
 
        do
        {
            VectorXf dTheta = (this->*computeFunction)(stepSize);
 
            if (adjustDOFWeightsToJointLimits(dTheta))
            {
                dTheta = computeStep(stepSize);
            }
 
 
            for (unsigned int i = 0; i < nodes.size(); i++)
            {
                ARMARX_DEBUG << VAROUT(nodes[i]->getJointValue()) << VAROUT(dTheta[i]);
                jv[i] = (nodes[i]->getJointValue() + dTheta[i]);
 
                if (std::isnan(jv[i]) || std::isinf(jv[i]))
                {
                    ARMARX_WARNING_S << "Aborting, invalid joint value (nan)" << std::endl;
                    dofWeights = tempDOFWeights;
                    return false;
                }
 
                //nodes[i]->setJointValue(nodes[i]->getJointValue() + dTheta[i]);
            }
 
            robot->setJointValues(rns, jv);
 
            VectorXf newJointValues;
            rns->getJointValues(newJointValues);
            resultJointDelta = newJointValues - oldJointValues;
 
 
            //            ARMARX_DEBUG << "joint angle deltas:\n" << dThetaSum;
 
            // check tolerances
            if (checkTolerances())
            {
                if (verbose)
                {
                    ARMARX_INFO << deactivateSpam(1) << "Tolerances ok, loop:" << step << std::endl;
                }
 
                break;
            }
 
            float d = dTheta.norm();
            float posDist = getMeanErrorPosition();
            float oriErr = getErrorRotation(rns->getTCP());
 
            if (dTheta.norm() < mininumChange)
            {
                //                if (verbose)
                ARMARX_INFO << deactivateSpam(1)
                            << "Could not improve result any more (dTheta.norm()=" << d
                            << "), loop:" << step << " Resulting error: pos " << posDist
                            << " orientation: " << oriErr << std::endl;
                break;
            }
 
            if (checkImprovement && posDist > lastDist)
            {
                //                if (verbose)
                ARMARX_INFO << deactivateSpam(1)
                            << "Could not improve result any more (current position error="
                            << posDist << ", last loop's error:" << lastDist << "), loop:" << step
                            << std::endl;
                robot->setJointValues(rns, jvBest);
                break;
            }
 
            jvBest = jv;
            lastDist = posDist;
            step++;
 
            jointDeltaIterations.push_back(std::make_pair(getWeightedError(), resultJointDelta));
        } while (step < maxNStep);
 
 
        float bestJVError = std::numeric_limits<float>::max();
 
        for (unsigned int i = 0; i < jointDeltaIterations.size(); i++)
        {
            if (jointDeltaIterations.at(i).first < bestJVError)
            {
                bestJVError = jointDeltaIterations.at(i).first;
                resultJointDelta = jointDeltaIterations.at(i).second;
            }
        }
 
        robot->setJointValues(rns, oldJointValues + resultJointDelta);
 
        //        ARMARX_DEBUG << "best try: " <<  bestIndex << " with error: " << bestJVError;
        //        rns->setJointValues(oldJointValues);
        //        Matrix4f oldPose = rns->getTCP()->getGlobalPose();
        //        rns->setJointValues(oldJointValues+dThetaSum);
        //        Matrix4f newPose = rns->getTCP()->getGlobalPose();
        //        ARMARX_IMPORTANT << "tcp delta:\n" << newPose.block(0,3,3,1) - oldPose.block(0,3,3,1);
        dofWeights = tempDOFWeights;
 
        if (step >= maxNStep && verbose)
        {
            ARMARX_INFO << deactivateSpam(1) << "IK failed, loop:" << step << std::endl;
            ARMARX_INFO << deactivateSpam(1) << "pos error:" << getMeanErrorPosition() << std::endl;
            ARMARX_INFO << deactivateSpam(1) << "rot error:" << getErrorRotation(rns->getTCP())
                        << std::endl;
            return false;
        }
 
        return true;
    }
 
    VectorXf
    EDifferentialIK::computeStep(float stepSize)
    {
 
        if (nRows == 0)
        {
            this->setNRows();
        }
 
        size_t nDoF = nodes.size();
 
        MatrixXf Jacobian(nRows, nDoF);
        VectorXf error(nRows);
        size_t index = 0;
 
        for (size_t i = 0; i < tcp_set.size(); i++)
        {
            SceneObjectPtr tcp = tcp_set[i];
 
            if (this->targets.find(tcp) != this->targets.end())
            {
                Eigen::VectorXf delta = getDeltaToGoal(tcp);
                //ARMARX_DEBUG << VAROUT(delta);
                IKSolver::CartesianSelection mode = this->modes[tcp];
                MatrixXf partJacobian = this->getJacobianMatrix(tcp, mode);
                //ARMARX_DEBUG << VAROUT(partJacobian);
 
                Jacobian.block(index, 0, partJacobian.rows(), nDoF) = partJacobian;
                Vector3f position = delta.head(3);
                position *= stepSize;
 
                if (mode & IKSolver::X)
                {
                    error(index) = position(0);
                    index++;
                }
 
                if (mode & IKSolver::Y)
                {
                    error(index) = position(1);
                    index++;
                }
 
                if (mode & IKSolver::Z)
                {
                    error(index) = position(2);
                    index++;
                }
 
                if (mode & IKSolver::Orientation)
                {
                    error.segment(index, 3) = delta.tail(3) * stepSize;
                    index += 3;
                }
            }
            else
            {
                ARMARX_ERROR << "Internal error?!" << std::endl; // Error
            }
        }
 
        //        applyDOFWeightsToJacobian(Jacobian);
        ARMARX_DEBUG << VAROUT(Jacobian);
        MatrixXf pseudo = computePseudoInverseJacobianMatrix(Jacobian);
        ARMARX_DEBUG << VAROUT(pseudo);
        ARMARX_DEBUG << VAROUT(error);
        return pseudo * error;
    }
 
    VectorXf
    EDifferentialIK::computeStepIndependently(float stepSize)
    {
        if (nRows == 0)
        {
            this->setNRows();
        }
 
        size_t nDoF = nodes.size();
 
        std::map<float, MatrixXf> partJacobians;
 
        VectorXf thetaSum(nDoF);
        thetaSum.setZero();
        size_t index = 0;
 
        for (size_t i = 0; i < tcp_set.size(); i++)
        {
            SceneObjectPtr tcp = tcp_set[i];
 
            if (this->targets.find(tcp) != this->targets.end())
            {
                Eigen::VectorXf delta = getDeltaToGoal(tcp);
                IKSolver::CartesianSelection mode = this->modes[tcp];
                MatrixXf partJacobian = this->getJacobianMatrix(tcp, mode);
 
                // apply tcp weights
                //                applyTCPWeights(tcp, partJacobian);
                int tcpDOF = 0;
 
                if (mode <= IKSolver::Z)
                {
                    tcpDOF = 1;
                }
                else if (mode <= IKSolver::Orientation)
                {
                    tcpDOF = 3;
                }
                else if (mode == IKSolver::All)
                {
                    tcpDOF = 6;
                }
 
                index = 0;
                VectorXf partError(tcpDOF);
                Vector3f position = delta.head(3);
                //                ARMARX_DEBUG << tcp->getName() << "'s error: " << position << " weighted error: " << position*stepSize;
                position *= stepSize;
 
                if (mode & IKSolver::X)
                {
                    partError(index) = position(0);
                    index++;
                }
 
                if (mode & IKSolver::Y)
                {
                    partError(index) = position(1);
                    index++;
                }
 
                if (mode & IKSolver::Z)
                {
                    partError(index) = position(2);
                    index++;
                }
 
                if (mode & IKSolver::Orientation)
                {
                    partError.segment(index, 3) = delta.tail(3) * stepSize;
                    index += 3;
                }
 
 
                ARMARX_DEBUG << deactivateSpam(0.05) << "step size adjusted error to goal:\n"
                             << partError;
 
                applyDOFWeightsToJacobian(partJacobian);
                //                ARMARX_DEBUG <<  "Jac:\n" << partJacobian;
 
 
                MatrixXf pseudo = computePseudoInverseJacobianMatrix(partJacobian);
 
                Eigen::VectorXf tcpWeightVec;
                std::map<VirtualRobot::SceneObjectPtr, Eigen::VectorXf>::iterator it =
                    tcpWeights.find(tcp);
 
                if (it != tcpWeights.end())
                {
                    tcpWeightVec = it->second;
                }
                else
                {
                    IKSolver::CartesianSelection mode = modes[tcp_set.at(i)];
                    int size = 1;
 
                    if (mode <= IKSolver::Z)
                    {
                        size = 1;
                    }
                    else if (mode <= IKSolver::Orientation)
                    {
                        size = 3;
                    }
                    else if (mode == IKSolver::All)
                    {
                        size = 6;
                    }
 
                    Eigen::VectorXf tmptcpWeightVec(size);
                    tmptcpWeightVec.setOnes();
                    tcpWeightVec = tmptcpWeightVec;
                }
 
 
                if (pseudo.cols() == tcpWeightVec.rows())
                {
 
                    Eigen::MatrixXf weightMat(tcpWeightVec.rows(), tcpWeightVec.rows());
                    weightMat.setIdentity();
                    weightMat.diagonal() = tcpWeightVec;
                    //            ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "tcpWeightVec:\n" << tcpWeightVec;
                    //            ARMARX_DEBUG << /*deactivateSpam(1) << */"InvJac Before:\n" << invJacobian;
                    pseudo = pseudo * weightMat;
                    //                    ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "InvJac after:\n" << pseudo;
                }
                else
                {
                    ARMARX_WARNING << deactivateSpam(3)
                                   << "Wrong dimension of tcp weights: " << tcpWeightVec.rows()
                                   << ", but should be: " << pseudo.cols();
                }
 
 
                thetaSum += pseudo * partError;
            }
            else
            {
                ARMARX_ERROR << "Internal error?!" << std::endl; // Error
            }
        }
 
        return thetaSum;
    }
 
    bool
    EDifferentialIK::solveIK(float stepSize,
                             float minChange,
                             int maxSteps,
                             bool useAlternativeOnFail)
    {
        Eigen::VectorXf jointValuesBefore;
        rns->getJointValues(jointValuesBefore);
        Eigen::VectorXf resultJointDelta;
        Eigen::VectorXf jointDeltaAlternative;
        bool result = computeSteps(resultJointDelta, stepSize, minChange, maxSteps);
        float error = getWeightedError();
 
        if (useAlternativeOnFail && error > 5)
        {
            result = computeSteps(jointDeltaAlternative,
                                  stepSize,
                                  minChange,
                                  maxSteps,
                                  &EDifferentialIK::computeStepIndependently);
        }
 
        if (getWeightedError() < error)
        {
            resultJointDelta = jointDeltaAlternative;
        }
 
        return result;
    }
 
    void
    EDifferentialIK::applyDOFWeightsToJacobian(MatrixXf& Jacobian)
    {
        if (dofWeights.rows() == Jacobian.cols())
        {
            Eigen::MatrixXf weightMat(dofWeights.rows(), dofWeights.rows());
            weightMat.setIdentity();
            weightMat.diagonal() = dofWeights;
            //            ARMARX_DEBUG << deactivateSpam(1) << "Jac before:\n" << Jacobian;
            //            ARMARX_DEBUG << deactivateSpam(1) << "Weights:\n" << weightMat;
            Jacobian = Jacobian * weightMat;
            //            ARMARX_DEBUG << deactivateSpam(1) << "Jac after:\n" << Jacobian;
        }
    }
 
    void
    EDifferentialIK::applyTCPWeights(RobotNodePtr tcp, MatrixXf& partJacobian)
    {
 
        std::map<VirtualRobot::SceneObjectPtr, Eigen::VectorXf>::iterator it = tcpWeights.find(tcp);
 
        if (it != tcpWeights.end())
        {
            Eigen::VectorXf& tcpWeightVec = it->second;
 
            if (partJacobian.rows() == tcpWeightVec.rows())
            {
 
                Eigen::MatrixXf weightMat(tcpWeightVec.rows(), tcpWeightVec.rows());
                weightMat.setIdentity();
                weightMat.diagonal() = tcpWeightVec;
                ARMARX_DEBUG << deactivateSpam(1) << "Jac Before: " << partJacobian;
                partJacobian = weightMat * partJacobian;
                ARMARX_DEBUG << deactivateSpam(1) << "Jac after: " << partJacobian;
            }
            else
            {
                ARMARX_WARNING << deactivateSpam(3)
                               << "Wrong dimension of tcp weights: " << tcpWeightVec.rows()
                               << ", but should be: " << partJacobian.rows();
            }
        }
    }
 
    void
    EDifferentialIK::applyTCPWeights(MatrixXf& invJacobian)
    {
        if (tcpWeightVec.rows() == 0)
            for (size_t i = 0; i < tcp_set.size(); i++)
            {
                std::map<VirtualRobot::SceneObjectPtr, Eigen::VectorXf>::iterator it =
                    tcpWeights.find(tcp_set.at(i));
 
                if (it != tcpWeights.end())
                {
                    Eigen::VectorXf& tmptcpWeightVec = it->second;
                    Eigen::VectorXf oldVec = tcpWeightVec;
                    tcpWeightVec.resize(tcpWeightVec.rows() + tmptcpWeightVec.rows());
 
                    if (oldVec.rows() > 0)
                    {
                        tcpWeightVec.head(oldVec.rows()) = oldVec;
                    }
 
                    tcpWeightVec.tail(tmptcpWeightVec.rows()) = tmptcpWeightVec;
                }
                else
                {
                    IKSolver::CartesianSelection mode = modes[tcp_set.at(i)];
                    int size = 1;
 
                    if (mode <= IKSolver::Z)
                    {
                        size = 1;
                    }
                    else if (mode <= IKSolver::Orientation)
                    {
                        size = 3;
                    }
                    else if (mode == IKSolver::All)
                    {
                        size = 6;
                    }
 
                    Eigen::VectorXf oldVec = tcpWeightVec;
                    tcpWeightVec.resize(tcpWeightVec.rows() + size);
 
                    if (oldVec.rows() > 0)
                    {
                        tcpWeightVec.head(oldVec.rows()) = oldVec;
                    }
 
                    Eigen::VectorXf tmptcpWeightVec(size);
                    tmptcpWeightVec.setOnes();
                    tcpWeightVec.tail(size) = tmptcpWeightVec;
                }
            }
 
        if (invJacobian.cols() == tcpWeightVec.rows())
        {
 
            Eigen::MatrixXf weightMat(tcpWeightVec.rows(), tcpWeightVec.rows());
            weightMat.setIdentity();
            weightMat.diagonal() = tcpWeightVec;
            //            ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "tcpWeightVec:\n" << tcpWeightVec;
            //            ARMARX_DEBUG << /*deactivateSpam(1) << */"InvJac Before:\n" << invJacobian;
            invJacobian = invJacobian * weightMat;
            ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "InvJac after:\n" << invJacobian;
        }
        else
        {
            ARMARX_WARNING << deactivateSpam(3)
                           << "Wrong dimension of tcp weights: " << tcpWeightVec.rows()
                           << ", but should be: " << invJacobian.cols();
        }
    }
 
    float
    EDifferentialIK::getWeightedError()
    {
        float result = 0.0f;
        float positionOrientationRatio = 3.f / 180.f * M_PI;
 
        for (size_t i = 0; i < tcp_set.size(); i++)
        {
            SceneObjectPtr tcp = tcp_set[i];
            result +=
                getWeightedErrorPosition(tcp) + getErrorRotation(tcp) * positionOrientationRatio;
        }
 
        return result;
    }
 
    float
    EDifferentialIK::getWeightedErrorPosition(SceneObjectPtr tcp)
    {
        if (modes[tcp] == IKSolver::Orientation)
        {
            return 0.0f; // ignoring position
        }
 
        if (!tcp)
        {
            tcp = getDefaultTCP();
        }
 
        Vector3f position = targets[tcp].block(0, 3, 3, 1) - tcp->getGlobalPose().block(0, 3, 3, 1);
        //cout << "Error Position <" << tcp->getName() << ">: " << position.norm() << std::endl;
        //cout << boost::format("Error Position < %1% >: %2%") % tcp->getName() % position.norm()  << std::endl;
        float result = 0.0f;
        Eigen::VectorXf tcpWeight(3);
 
        if (tcpWeights.find(tcp) != tcpWeights.end())
        {
            tcpWeight = tcpWeights.find(tcp)->second;
        }
        else
        {
            tcpWeight.setOnes();
        }
 
        int weightIndex = 0;
 
        if (modes[tcp] & IKSolver::X)
        {
            //            ARMARX_DEBUG << "error x: " << position(0)*tcpWeight(weightIndex);
            result += position(0) * position(0) * tcpWeight(weightIndex++);
        }
 
        if (modes[tcp] & IKSolver::Y)
        {
            //            ARMARX_DEBUG << "error y: " << position(1)*tcpWeight(weightIndex);
            result += position(1) * position(1) * tcpWeight(weightIndex++);
        }
 
        if (modes[tcp] & IKSolver::Z)
        {
            //            ARMARX_DEBUG << "error z: " << position(2)*tcpWeight(weightIndex);
            result += position(2) * position(2) * tcpWeight(weightIndex++);
        }
 
        return sqrtf(result);
    }
 
    bool
    EDifferentialIK::adjustDOFWeightsToJointLimits(const VectorXf& plannedJointDeltas)
    {
        if (dofWeights.rows() != plannedJointDeltas.rows())
        {
            dofWeights.resize(plannedJointDeltas.rows());
            dofWeights.setOnes();
        }
 
        bool result = false;
 
        for (unsigned int i = 0; i < nodes.size(); i++)
        {
            float angle = nodes[i]->getJointValue() + plannedJointDeltas[i] * 0.1;
 
            if (angle > nodes[i]->getJointLimitHi() || angle < nodes[i]->getJointLimitLo())
            {
                ARMARX_VERBOSE << deactivateSpam(3) << nodes[i]->getName()
                               << " joint deactivated because of joint limit";
                dofWeights(i) = 0;
                result = true;
            }
        }
 
        return result;
    }
 
 
} // namespace armarx
 
void
armarx::TCPControlUnit::reportControlModeChanged(const NameControlModeMap&,
                                                 Ice::Long timestamp,
                                                 bool,
                                                 const Ice::Current&)
{
}
 
void
armarx::TCPControlUnit::reportJointAngles(const NameValueMap& jointAngles,
                                          Ice::Long timestamp,
                                          bool,
                                          const Ice::Current&)
{
    std::unique_lock lock(reportMutex);
    FramedPoseBaseMap tcpPoses;
    std::string rootFrame = localReportRobot->getRootNode()->getName();
    auto it = jointAngles.find("timestamp");
    if (it == jointAngles.end())
    {
        localReportRobot->setJointValues(jointAngles);
    }
    else
    {
        NameValueMap tempMap = jointAngles;
        tempMap.erase("timestamp");
        localReportRobot->setJointValues(tempMap);
    }
    for (unsigned int i = 0; i < nodesToReport.size(); i++)
    {
        RobotNodePtr& node = nodesToReport.at(i);
        const std::string& tcpName = node->getName();
        const Eigen::Matrix4f& currentPose = node->getPoseInRootFrame();
        tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, robotName);
    }
 
    listener->reportTCPPose(tcpPoses);
}
 
void
armarx::TCPControlUnit::reportJointVelocities(const NameValueMap& jointVel,
                                              Ice::Long timestamp,
                                              bool,
                                              const Ice::Current&)
{
    if ((TimeUtil::GetTime() - lastTopicReportTime).toMilliSeconds() < cycleTime)
    {
        return;
    }
 
    lastTopicReportTime = TimeUtil::GetTime();
    std::unique_lock lock(reportMutex);
 
    if (!localVelReportRobot)
    {
        localVelReportRobot = localReportRobot->clone(robotName);
    }
 
    //    ARMARX_DEBUG << jointVel;    FramedPoseBaseMap tcpPoses;
    FramedDirectionMap tcpTranslationVelocities;
    FramedDirectionMap tcpOrientationVelocities;
    std::string rootFrame = localReportRobot->getRootNode()->getName();
    NameValueMap tempJointAngles = localReportRobot->getConfig()->getRobotNodeJointValueMap();
    FramedPoseBaseMap tcpPoses;
 
    for (unsigned int i = 0; i < nodesToReport.size(); i++)
    {
        RobotNodePtr node = nodesToReport.at(i);
        const std::string& tcpName = node->getName();
        const Eigen::Matrix4f& currentPose = node->getPoseInRootFrame();
        tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, robotName);
    }
 
    double tDelta = 0.001;
 
    for (NameValueMap::iterator it = tempJointAngles.begin(); it != tempJointAngles.end(); it++)
    {
        NameValueMap::const_iterator itSrc = jointVel.find(it->first);
        if (itSrc != jointVel.end())
        {
            if (itSrc->first == "timestamp")
            {
                continue;
            }
            it->second += itSrc->second * tDelta;
        }
    }
 
    localVelReportRobot->setJointValues(tempJointAngles);
 
    for (unsigned int i = 0; i < nodesToReport.size(); i++)
    {
        RobotNodePtr node = localVelReportRobot->getRobotNode(nodesToReport.at(i)->getName());
        const std::string& tcpName = node->getName();
        const Eigen::Matrix4f& currentPose = node->getPoseInRootFrame();
 
 
        FramedPosePtr lastPose = FramedPosePtr::dynamicCast(tcpPoses[tcpName]);
 
        tcpTranslationVelocities[tcpName] = new FramedDirection(
            (currentPose.block(0, 3, 3, 1) - lastPose->toEigen().block(0, 3, 3, 1)) / tDelta,
            rootFrame,
            localReportRobot->getName());
 
        const Eigen::Matrix4f mat = currentPose * lastPose->toEigen().inverse();
        //        const Eigen::Matrix3f rot = currentPose.block(0,0,3,3) * lastPose->toEigen().block(0,0,3,3).inverse();
        Eigen::Vector3f rpy;
        VirtualRobot::MathTools::eigen4f2rpy(mat, rpy);
        //        Eigen::AngleAxisf orient(rot.block<3,3>(0,0));
 
        tcpOrientationVelocities[tcpName] = new FramedDirection(rpy / tDelta, rootFrame, robotName);
    }
 
    listener->reportTCPVelocities(tcpTranslationVelocities, tcpOrientationVelocities);
}
 
void
armarx::TCPControlUnit::reportJointTorques(const NameValueMap&,
                                           Ice::Long timestamp,
                                           bool,
                                           const Ice::Current&)
{
}
 
void
armarx::TCPControlUnit::reportJointAccelerations(const armarx::NameValueMap& jointAccelerations,
                                                 Ice::Long timestamp,
                                                 bool aValueChanged,
                                                 const Ice::Current& c)
{
}
 
void
armarx::TCPControlUnit::reportJointCurrents(const NameValueMap&,
                                            Ice::Long timestamp,
                                            bool,
                                            const Ice::Current&)
{
}
 
void
armarx::TCPControlUnit::reportJointMotorTemperatures(const NameValueMap&,
                                                     Ice::Long timestamp,
                                                     bool,
                                                     const Ice::Current&)
{
}
 
void
armarx::TCPControlUnit::reportJointStatuses(const NameStatusMap&,
                                            Ice::Long timestamp,
                                            bool,
                                            const Ice::Current&)
{
}