RobotStateComponent.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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    RobotStateComponent::
 * @author     ( at kit dot edu)
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "RobotStateComponent.h"
 
#include <Ice/ObjectAdapter.h>
 
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/math/Helpers.h>
 
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/components/units/RobotUnit/util/NonRtTiming.h>
#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
 
 
using namespace Eigen;
using namespace Ice;
using namespace VirtualRobot;
 
namespace armarx
{
    RobotStateComponent::~RobotStateComponent()
    {
        try
        {
            if (_synchronizedPrx)
            {
                _synchronizedPrx->unref();
            }
        }
        catch (...)
        {
        }
    }
 
    RobotStatePropertyDefinitions::RobotStatePropertyDefinitions(std::string prefix) :
        ComponentPropertyDefinitions(prefix)
    {
        defineRequiredProperty<std::string>("RobotNodeSetName",
                                            "Set of nodes that is controlled by the KinematicUnit");
        defineRequiredProperty<std::string>(
            "RobotFileName", "Filename of VirtualRobot robot model (e.g. robot_model.xml)");
        defineRequiredProperty<std::string>(
            "AgentName", "Name of the agent for which the sensor values are provided");
        defineOptionalProperty<std::string>(
            "RobotStateReportingTopic",
            "RobotStateUpdates",
            "Name of the topic on which updates of the robot state are reported.");
        defineOptionalProperty<int>(
            "HistoryLength", 10000, "Number of entries in the robot state history")
            .setMin(0);
        defineOptionalProperty<float>("RobotModelScaling", 1.0f, "Scaling of the robot model");
        defineOptionalProperty<std::string>(
            "TopicPrefix", "", "Prefix for the sensor value topic name.");
        defineOptionalProperty<std::string>("GlobalRobotPoseLocalizationTopicName",
                                            "GlobalRobotPoseLocalization",
                                            "Topic where the global robot pose can be reported.");
    }
 
    std::string
    RobotStateComponent::getDefaultName() const
    {
        return "RobotStateComponent";
    }
 
    void
    RobotStateComponent::onInitComponent()
    {
        robotStateTopicName = getProperty<std::string>("RobotStateReportingTopic").getValue();
        offeringTopic(getProperty<std::string>("RobotStateReportingTopic"));
        const std::size_t historyLength =
            static_cast<std::size_t>(getProperty<int>("HistoryLength").getValue());
 
        jointHistory.clear();
        jointHistoryLength = historyLength;
 
        poseHistory.clear();
        poseHistoryLength = historyLength;
 
        relativeRobotFile = getProperty<std::string>("RobotFileName").getValue();
 
        if (!ArmarXDataPath::getAbsolutePath(relativeRobotFile, robotFile))
        {
            throw UserException("Could not find robot file " + robotFile);
        }
 
        this->_synchronized =
            VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
        _synchronized->setName(getProperty<std::string>("AgentName").getValue());
 
        robotModelScaling = getProperty<float>("RobotModelScaling").getValue();
        ARMARX_INFO << "scale factor: " << robotModelScaling;
        if (robotModelScaling != 1.0f)
        {
            ARMARX_INFO << "Scaling robot model with scale factor " << robotModelScaling;
            _synchronized = _synchronized->clone(
                _synchronized->getName(), _synchronized->getCollisionChecker(), robotModelScaling);
        }
 
        if (this->_synchronized)
        {
            ARMARX_VERBOSE << "Loaded robot from file " << robotFile
                           << ". Robot name: " << this->_synchronized->getName();
            this->_synchronized->setPropagatingJointValuesEnabled(false);
        }
        else
        {
            ARMARX_VERBOSE << "Failed loading robot from file " << robotFile;
        }
 
        robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
 
 
        if (robotNodeSetName.empty())
        {
            throw UserException("RobotNodeSet not defined");
        }
 
        VirtualRobot::RobotNodeSetPtr rns = this->_synchronized->getRobotNodeSet(robotNodeSetName);
 
        if (!rns)
        {
            throw UserException("RobotNodeSet not defined");
        }
 
        std::vector<RobotNodePtr> nodes = rns->getAllRobotNodes();
 
        ARMARX_VERBOSE << "Using RobotNodeSet " << robotNodeSetName << " with " << nodes.size()
                       << " robot nodes.";
        for (const RobotNodePtr& node : nodes)
        {
            ARMARX_VERBOSE << "Node: " << node->getName();
        }
        const auto topicPrefix = getProperty<std::string>("TopicPrefix").getValue();
        usingTopic(topicPrefix + robotNodeSetName + "State");
 
        usingTopic(topicPrefix +
                   getProperty<std::string>("GlobalRobotPoseLocalizationTopicName").getValue());
 
        try
        {
            readRobotInfo(robotFile);
        }
        catch (const std::exception& e)
        {
            ARMARX_WARNING << "Failed to read robot info from file: " << robotFile
                           << ".\nReason: " << e.what();
        }
        catch (...)
        {
            ARMARX_WARNING << "Failed to read robot info from file: " << robotFile
                           << " for unknown reason.";
        }
        usingTopic("SimulatorResetEvent");
    }
 
    void
    RobotStateComponent::readRobotInfo(const std::string& robotFile)
    {
        RapidXmlReaderPtr reader = RapidXmlReader::FromFile(robotFile);
        RapidXmlReaderNode robotNode = reader->getRoot("Robot");
        robotInfo = readRobotInfo(robotNode.first_node("RobotInfo"));
    }
 
    RobotInfoNodePtr
    RobotStateComponent::readRobotInfo(const RapidXmlReaderNode& infoNode)
    {
        std::string name = infoNode.name();
        std::string profile = infoNode.attribute_value_or_default("profile", "");
        std::string value = infoNode.attribute_value_or_default("value", "");
        //ARMARX_IMPORTANT << "name: " << name << "; profile: " << profile << "; value: " << value;
        std::vector<RobotInfoNodePtr> children;
        for (const RapidXmlReaderNode& childNode : infoNode.nodes())
        {
            children.push_back(readRobotInfo(childNode));
        }
        return new RobotInfoNode(name, profile, value, children);
    }
 
    void
    RobotStateComponent::onConnectComponent()
    {
        robotStateListenerPrx = getTopic<RobotStateListenerInterfacePrx>(
            getProperty<std::string>("RobotStateReportingTopic"));
        _sharedRobotServant =
            new SharedRobotServant(this->_synchronized,
                                   RobotStateComponentInterfacePrx::uncheckedCast(getProxy()),
                                   robotStateListenerPrx);
        _sharedRobotServant->ref();
 
        _sharedRobotServant->setRobotStateComponent(
            RobotStateComponentInterfacePrx::uncheckedCast(getProxy()));
        this->_synchronizedPrx = SharedRobotInterfacePrx::uncheckedCast(
            getObjectAdapter()->addWithUUID(_sharedRobotServant));
        ARMARX_INFO << "Started RobotStateComponent" << flush;
        if (robotStateObs)
        {
            robotStateObs->setRobot(_synchronized);
        }
    }
 
    void
    RobotStateComponent::onDisconnectComponent()
    {
    }
 
    SharedRobotInterfacePrx
    RobotStateComponent::getSynchronizedRobot(const Current&) const
    {
        if (!this->_synchronizedPrx)
        {
            throw NotInitializedException("Shared Robot is NULL", "getSynchronizedRobot");
        }
        return this->_synchronizedPrx;
    }
 
    SharedRobotInterfacePrx
    RobotStateComponent::getRobotSnapshot(const std::string& deprecated, const Current&)
    {
        (void)deprecated;
 
        if (!_synchronized)
        {
            throw NotInitializedException("Shared Robot is NULL", "getRobotSnapshot");
        }
 
        auto clone = this->_synchronized->clone(_synchronized->getName());
 
        SharedRobotServantPtr p = new SharedRobotServant(
            clone, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()), nullptr);
        p->setTimestamp(
            IceUtil::Time::microSecondsDouble(_sharedRobotServant->getTimestamp()->timestamp));
        auto result = getObjectAdapter()->addWithUUID(p);
        // virtal robot clone is buggy -> set global pose here
        p->setGlobalPose(new Pose(_synchronized->getGlobalPose()));
        return SharedRobotInterfacePrx::uncheckedCast(result);
    }
 
    SharedRobotInterfacePrx
    RobotStateComponent::getRobotSnapshotAtTimestamp(double _time, const Current&)
    {
        const IceUtil::Time time = IceUtil::Time::microSecondsDouble(_time);
 
        if (!_synchronized)
        {
            throw NotInitializedException("Shared Robot is NULL", "getRobotSnapshot");
        }
 
        VirtualRobot::RobotPtr clone = this->_synchronized->clone(_synchronized->getName());
        auto config = interpolate(time);
        if (!config)
        {
            ARMARX_WARNING << "Could not find or interpolate a robot state for time "
                           << time.toDateTime();
            return nullptr;
        }
 
        clone->setJointValues(config->jointMap);
        SharedRobotServantPtr p = new SharedRobotServant(
            clone, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()), nullptr);
        p->setTimestamp(time);
        auto result = getObjectAdapter()->addWithUUID(p);
        // Virtal robot clone is buggy -> set global pose here.
        p->setGlobalPose(config->globalPose);
 
        return SharedRobotInterfacePrx::uncheckedCast(result);
    }
 
    NameValueMap
    RobotStateComponent::getJointConfigAtTimestamp(double timestamp, const Current&) const
    {
        auto jointAngles = interpolateJoints(IceUtil::Time::microSecondsDouble(timestamp));
        return jointAngles ? jointAngles->value : NameValueMap();
    }
 
    RobotStateConfig
    RobotStateComponent::getRobotStateAtTimestamp(double timestamp, const Current&) const
    {
        auto config = interpolate(IceUtil::Time::microSecondsDouble(timestamp));
        return config ? *config : RobotStateConfig();
    }
 
    std::string
    RobotStateComponent::getRobotFilename(const Ice::Current&) const
    {
        return relativeRobotFile;
    }
 
    float
    RobotStateComponent::getScaling(const Ice::Current&) const
    {
        return robotModelScaling;
    }
 
    RobotInfoNodePtr
    RobotStateComponent::getRobotInfo(const Ice::Current&) const
    {
        return robotInfo;
    }
 
    void
    RobotStateComponent::reportJointAngles(const NameValueMap& jointAngles,
                                           Ice::Long timestamp,
                                           bool aValueChanged,
                                           const Current&)
    {
        if (timestamp <= 0)
        {
            timestamp = armarx::rtNow().toMicroSeconds();
        }
 
        IceUtil::Time time = mapRtTimestampToNonRtTimestamp(IceUtil::Time::microSeconds(timestamp));
 
        ARMARX_DEBUG << deactivateSpam(1) << "Got new jointangles: " << jointAngles
                     << " from timestamp " << time.toDateTime()
                     << " a value changed: " << aValueChanged;
 
        if (aValueChanged)
        {
            {
                WriteLockPtr lock = _synchronized->getWriteLock();
 
                _synchronized->setJointValues(jointAngles);
            }
 
            if (robotStateObs)
            {
                robotStateObs->updatePoses();
            }
        }
        if (_sharedRobotServant)
        {
            _sharedRobotServant->setTimestamp(time);
        }
 
        {
            std::unique_lock lock(jointHistoryMutex);
            jointHistory.emplace_hint(jointHistory.end(), time, jointAngles);
 
            if (jointHistory.size() > jointHistoryLength)
            {
                jointHistory.erase(jointHistory.begin());
            }
        }
 
        robotStateListenerPrx->reportJointValues(jointAngles, timestamp, aValueChanged);
    }
 
    void
    RobotStateComponent::reportGlobalRobotPose(const TransformStamped& globalRobotPose,
                                               const Ice::Current&)
    {
        if (_synchronized)
        {
            std::string localRobotName = _synchronized->getName();
            ARMARX_DEBUG << "Comparing " << localRobotName << " and "
                         << globalRobotPose.header.agent << ".";
            if (localRobotName == globalRobotPose.header.agent)
            {
                const IceUtil::Time time =
                    IceUtil::Time::microSeconds(globalRobotPose.header.timestampInMicroSeconds);
 
                insertPose(time, globalRobotPose.transform);
                _synchronized->setGlobalPose(globalRobotPose.transform);
 
                if (_sharedRobotServant)
                {
                    _sharedRobotServant->setGlobalPose(new Pose(globalRobotPose.transform));
                    _sharedRobotServant->setTimestamp(time);
                }
            }
        }
        else
        {
            throw NotInitializedException("Robot Ptr is NULL", "reportGlobalRobotPose");
        }
    }
 
    std::vector<std::string>
    RobotStateComponent::getArmarXPackages(const Current&) const
    {
        std::vector<std::string> result;
        auto packages = armarx::Application::GetProjectDependencies();
        packages.push_back(Application::GetProjectName());
 
        for (const std::string& projectName : packages)
        {
            if (projectName.empty())
            {
                continue;
            }
 
            result.push_back(projectName);
        }
 
        return result;
    }
 
    void
    RobotStateComponent::reportControlModeChanged(const NameControlModeMap& jointModes,
                                                  Ice::Long timestamp,
                                                  bool aValueChanged,
                                                  const Current&)
    {
        // Unused.
        (void)jointModes, (void)timestamp, (void)aValueChanged;
    }
 
    void
    RobotStateComponent::reportJointVelocities(const NameValueMap& jointVelocities,
                                               Ice::Long timestamp,
                                               bool aValueChanged,
                                               const Current&)
    {
        // Unused.
        (void)aValueChanged;
        if (robotStateObs)
        {
            robotStateObs->updateNodeVelocities(jointVelocities, timestamp);
        }
    }
 
    void
    RobotStateComponent::reportJointTorques(const NameValueMap& jointTorques,
                                            Ice::Long timestamp,
                                            bool aValueChanged,
                                            const Current&)
    {
        // Unused.
        (void)jointTorques, (void)timestamp, (void)aValueChanged;
    }
 
    void
    RobotStateComponent::reportJointAccelerations(const NameValueMap& jointAccelerations,
                                                  Ice::Long timestamp,
                                                  bool aValueChanged,
                                                  const Current&)
    {
        // Unused.
        (void)jointAccelerations, (void)timestamp, (void)aValueChanged;
    }
 
    void
    RobotStateComponent::reportJointCurrents(const NameValueMap& jointCurrents,
                                             Ice::Long timestamp,
                                             bool aValueChanged,
                                             const Current&)
    {
        // Unused.
        (void)jointCurrents, (void)timestamp, (void)aValueChanged;
    }
 
    void
    RobotStateComponent::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures,
                                                      Ice::Long timestamp,
                                                      bool aValueChanged,
                                                      const Current&)
    {
        // Unused.
        (void)jointMotorTemperatures, (void)timestamp, (void)aValueChanged;
    }
 
    void
    RobotStateComponent::reportJointStatuses(const NameStatusMap& jointStatuses,
                                             Ice::Long timestamp,
                                             bool aValueChanged,
                                             const Current&)
    {
        // Unused.
        (void)jointStatuses, (void)timestamp, (void)aValueChanged;
    }
 
    void
    RobotStateComponent::simulatorWasReset(const Current&)
    {
        {
            std::lock_guard lock(poseHistoryMutex);
            poseHistory.clear();
        }
        {
            std::lock_guard lock(jointHistoryMutex);
            jointHistory.clear();
        }
    }
 
    PropertyDefinitionsPtr
    RobotStateComponent::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(new RobotStatePropertyDefinitions(getConfigIdentifier()));
    }
 
    void
    RobotStateComponent::setRobotStateObserver(RobotStateObserverPtr observer)
    {
        robotStateObs = observer;
    }
 
    std::string
    RobotStateComponent::getRobotName(const Current&) const
    {
        if (_synchronized)
        {
            return _synchronized->getName(); // _synchronizedPrx->getName();
        }
        else
        {
            throw NotInitializedException("Robot Ptr is NULL", "getName");
        }
    }
 
    std::string
    RobotStateComponent::getRobotNodeSetName(const Current&) const
    {
        if (robotNodeSetName.empty())
        {
            throw NotInitializedException("RobotNodeSetName is empty", "getRobotNodeSetName");
        }
        return robotNodeSetName;
    }
 
    std::string
    RobotStateComponent::getRobotStateTopicName(const Current&) const
    {
        return robotStateTopicName;
    }
 
    void
    RobotStateComponent::insertPose(IceUtil::Time timestamp, const Matrix4f& globalPose)
    {
        IceUtil::Time duration;
        {
            IceUtil::Time start = IceUtil::Time::now();
            std::unique_lock lock(poseHistoryMutex);
            duration = IceUtil::Time::now() - start;
 
            poseHistory.emplace_hint(
                poseHistory.end(), timestamp, new FramedPose(globalPose, GlobalFrame, ""));
 
            if (poseHistory.size() > poseHistoryLength)
            {
                poseHistory.erase(poseHistory.begin());
            }
        }
 
        if (robotStateObs)
        {
            robotStateObs->updatePoses();
        }
    }
 
    std::optional<RobotStateConfig>
    RobotStateComponent::interpolate(IceUtil::Time time) const
    {
        auto joints = interpolateJoints(time);
        auto globalPose = interpolatePose(time);
 
        if (joints && globalPose)
        {
            RobotStateConfig config;
            // Use time stamp from interpolation.
            // config.timestamp = time.toMicroSeconds();
            config.timestamp = std::min(joints->timestamp, globalPose->timestamp).toMicroSeconds();
            config.jointMap = joints->value;
            config.globalPose = globalPose->value;
            return config;
        }
        else
        {
            return std::nullopt;
        }
    }
 
    auto
    RobotStateComponent::interpolateJoints(IceUtil::Time time) const
        -> std::optional<Timestamped<NameValueMap>>
    {
        std::shared_lock lock(jointHistoryMutex);
        if (jointHistory.empty())
        {
            ARMARX_WARNING << deactivateSpam(1)
                           << "Joint history of robot state component is empty!";
            return std::nullopt;
        }
 
        const IceUtil::Time newestTimeInHistory = jointHistory.rbegin()->first;
        if (time > newestTimeInHistory)
        {
            const IceUtil::Time minOffset = IceUtil::Time::milliSeconds(25);
            const IceUtil::Time maxOffset = IceUtil::Time::seconds(2);
            if (time > newestTimeInHistory + maxOffset)
            {
                ARMARX_WARNING << deactivateSpam(5)
                               << "Requested joint timestamp is substantially newer (>"
                               << maxOffset.toSecondsDouble()
                               << " sec) than newest available timestamp!"
                               << "\n- requested timestamp: \t" << time.toDateTime()
                               << "\n- newest timestamp:    \t" << newestTimeInHistory.toDateTime();
                return std::nullopt;
            }
            else if (time > newestTimeInHistory + minOffset)
            {
                ARMARX_INFO << deactivateSpam(10)
                            << "Requested joint timestamp is newer than newest available timestamp!"
                            << "\n- requested timestamp: \t" << time.toDateTime()
                            << "\n- newest timestamp:    \t" << newestTimeInHistory.toDateTime()
                            << "\n- difference:          \t"
                            << (time - newestTimeInHistory).toMicroSeconds() << " us";
            }
 
            return Timestamped<NameValueMap>{jointHistory.rbegin()->first,
                                             jointHistory.rbegin()->second};
        }
 
        // Get the oldest entry whose time >= time.
        auto nextIt = jointHistory.lower_bound(time);
        if (nextIt == jointHistory.end())
        {
            ARMARX_WARNING << deactivateSpam(1)
                           << "Could not find or interpolate a robot joint angles for time "
                           << time.toDateTime() << "\n- oldest available value: "
                           << jointHistory.begin()->first.toDateTime()
                           << "\n- newest available value: " << newestTimeInHistory.toDateTime();
            return std::nullopt;
        }
 
        if (nextIt == jointHistory.begin())
        {
            // Next was oldest entry.
            return Timestamped<NameValueMap>{nextIt->first, nextIt->second};
        }
 
        const NameValueMap& next = nextIt->second;
        auto prevIt = std::prev(nextIt);
 
 
        // Interpolate.
 
        IceUtil::Time prevTime = prevIt->first;
        IceUtil::Time nextTime = nextIt->first;
 
        float t = static_cast<float>((time - prevTime).toSecondsDouble() /
                                     (nextTime - prevTime).toSecondsDouble());
 
        NameValueMap jointAngles;
        auto prevJointIt = prevIt->second.begin();
        for (const auto& [name, nextAngle] : next)
        {
            float value = (1 - t) * prevJointIt->second + t * nextAngle;
            prevJointIt++;
 
            jointAngles.emplace(name, value);
        }
 
        return Timestamped<NameValueMap>{time, std::move(jointAngles)};
    }
 
    auto
    RobotStateComponent::interpolatePose(IceUtil::Time time) const
        -> std::optional<Timestamped<FramedPosePtr>>
    {
        std::shared_lock lock(poseHistoryMutex);
 
        if (poseHistory.empty())
        {
            ARMARX_INFO << deactivateSpam(10)
                        << "Pose history is empty. This can happen if there is no platform unit.";
 
            ReadLockPtr readLock = _synchronized->getReadLock();
 
            FramedPosePtr pose =
                new FramedPose(_synchronized->getGlobalPose(), armarx::GlobalFrame, "");
            return Timestamped<FramedPosePtr>{IceUtil::Time::seconds(0), pose};
        }
 
 
        const IceUtil::Time newestTimeInHistory = poseHistory.rbegin()->first;
        if (time > newestTimeInHistory)
        {
            IceUtil::Time maxOffset = IceUtil::Time::seconds(2);
            if (time <= newestTimeInHistory + maxOffset)
            {
                ARMARX_INFO << deactivateSpam(5)
                            << "Requested pose timestamp is newer than newest available timestamp!"
                            << "\n- requested timestamp: \t" << time.toDateTime()
                            << "\n- newest timestamp:    \t" << newestTimeInHistory.toDateTime()
                            << "\n- difference:          \t"
                            << (time - newestTimeInHistory).toMicroSeconds() << " us";
            }
            else
            {
                ARMARX_WARNING << deactivateSpam(1)
                               << "Requested pose timestamp is substantially newer (>"
                               << maxOffset.toSecondsDouble()
                               << " sec) than newest available timestamp!"
                               << "\n- requested timestamp: \t" << time.toDateTime()
                               << "\n- newest timestamp:    \t" << newestTimeInHistory.toDateTime();
                return std::nullopt;
            }
            return Timestamped<FramedPosePtr>{poseHistory.rbegin()->first,
                                              poseHistory.rbegin()->second};
        }
 
 
        auto nextIt = poseHistory.lower_bound(time);
        if (nextIt == poseHistory.end())
        {
            ARMARX_WARNING << deactivateSpam(1)
                           << "Could not find or interpolate platform pose for time "
                           << time.toDateTime() << "\n- oldest available value: "
                           << jointHistory.begin()->first.toDateTime()
                           << "\n- newest available value: " << newestTimeInHistory.toDateTime();
            return std::nullopt;
        }
 
 
        if (nextIt == poseHistory.begin())
        {
            // Next was oldest entry.
            return Timestamped<FramedPosePtr>{nextIt->first, nextIt->second};
        }
 
        auto prevIt = std::prev(nextIt);
        ARMARX_CHECK_EXPRESSION(prevIt->second);
 
        const FramedPosePtr& next = nextIt->second;
        const FramedPosePtr& prev = prevIt->second;
 
 
        // Interpolate.
 
        IceUtil::Time prevTime = prevIt->first;
        IceUtil::Time nextTime = nextIt->first;
 
        float t = static_cast<float>((time - prevTime).toSecondsDouble() /
                                     (nextTime - prevTime).toSecondsDouble());
 
        Eigen::Matrix4f globalPoseNext = next->toEigen();
        Eigen::Matrix4f globalPosePrev = prev->toEigen();
 
 
        Eigen::Matrix4f globalPose;
 
        math::Helpers::Position(globalPose) = (1 - t) * math::Helpers::Position(globalPosePrev) +
                                              t * math::Helpers::Position(globalPoseNext);
 
        Eigen::Quaternionf rotNext(math::Helpers::Orientation(globalPoseNext));
        Eigen::Quaternionf rotPrev(math::Helpers::Orientation(globalPosePrev));
        Eigen::Quaternionf rotNew = rotPrev.slerp(t, rotNext);
 
        math::Helpers::Orientation(globalPose) = rotNew.toRotationMatrix();
 
        return Timestamped<FramedPosePtr>{time,
                                          new FramedPose(globalPose, armarx::GlobalFrame, "")};
    }
 
} // namespace armarx