RobotUnitModuleControlThread.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::ArmarXObjects::RobotUnit
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "RobotUnitModuleControlThread.h"
 
#include <SimoxUtility/math/convert/pos_rpy_to_mat4f.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/util/OnScopeExit.h>
#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
#include <ArmarXCore/observers/filters/rtfilters/ButterworthFilter.h>
 
#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
#include <RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h>
 
#include "../Devices/RTThreadTimingsSensorDevice.h"
 
namespace armarx::RobotUnitModule
{
    /**
     * \brief This class allows minimal access to private members of \ref NJointControllerBase in a sane fashion for \ref ControlThread.
     * \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
     */
    class NJointControllerAttorneyForControlThread
    {
        friend class ControlThread;
 
        static void
        RtDeactivateController(const NJointControllerBasePtr& nJointCtrl)
        {
            nJointCtrl->rtDeactivateController();
        }
 
        static void
        RtActivateController(const NJointControllerBasePtr& nJointCtrl)
        {
            nJointCtrl->rtActivateController();
        }
 
        static void
        RtDeactivateControllerBecauseOfError(const NJointControllerBasePtr& nJointCtrl)
        {
            nJointCtrl->rtDeactivateControllerBecauseOfError();
        }
    };
 
    /**
     * \brief This class allows minimal access to private members of \ref ControlThreadDataBuffer in a sane fashion for \ref ControlThread.
     * \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
     */
    class ControlThreadDataBufferAttorneyForControlThread
    {
        friend class ControlThread;
 
        static std::vector<JointController*>&
        GetActivatedJointControllers(ControlThread* p)
        {
            return p->_module<ControlThreadDataBuffer>()
                .controllersActivated.getWriteBuffer()
                .jointControllers;
        }
 
        static std::vector<NJointControllerBase*>&
        GetActivatedNJointControllers(ControlThread* p)
        {
            return p->_module<ControlThreadDataBuffer>()
                .controllersActivated.getWriteBuffer()
                .nJointControllers;
        }
 
        static std::vector<std::size_t>&
        GetActivatedJointToNJointControllerAssignement(ControlThread* p)
        {
            return p->_module<ControlThreadDataBuffer>()
                .controllersActivated.getWriteBuffer()
                .jointToNJointControllerAssignement;
        }
 
        static const std::vector<JointController*>&
        GetActivatedJointControllers(const ControlThread* p)
        {
            return p->_module<ControlThreadDataBuffer>()
                .controllersActivated.getWriteBuffer()
                .jointControllers;
        }
 
        static const std::vector<NJointControllerBase*>&
        GetActivatedNJointControllers(const ControlThread* p)
        {
            return p->_module<ControlThreadDataBuffer>()
                .controllersActivated.getWriteBuffer()
                .nJointControllers;
        }
 
        static const std::vector<std::size_t>&
        GetActivatedJointToNJointControllerAssignement(const ControlThread* p)
        {
            return p->_module<ControlThreadDataBuffer>()
                .controllersActivated.getWriteBuffer()
                .jointToNJointControllerAssignement;
        }
 
        static void
        AcceptRequestedJointToNJointControllerAssignement(ControlThread* p)
        {
            p->_module<ControlThreadDataBuffer>()
                .controllersActivated.getWriteBuffer()
                .jointToNJointControllerAssignement = p->_module<ControlThreadDataBuffer>()
                                                          .controllersRequested.getReadBuffer()
                                                          .jointToNJointControllerAssignement;
        }
 
        static void
        CommitActivatedControllers(ControlThread* p)
        {
            return p->_module<ControlThreadDataBuffer>().controllersActivated.commitWrite();
        }
 
        static void
        ResetActivatedControllerAssignement(ControlThread* p)
        {
            return p->_module<ControlThreadDataBuffer>()
                .controllersActivated.getWriteBuffer()
                .resetAssignement();
        }
 
        static const std::vector<JointController*>&
        GetRequestedJointControllers(const ControlThread* p)
        {
            //do NOT update here!
            return p->_module<ControlThreadDataBuffer>()
                .controllersRequested.getReadBuffer()
                .jointControllers;
        }
 
        static const std::vector<NJointControllerBase*>&
        GetRequestedNJointControllers(const ControlThread* p)
        {
            //do NOT update here!
            return p->_module<ControlThreadDataBuffer>()
                .controllersRequested.getReadBuffer()
                .nJointControllers;
        }
 
        static const std::vector<std::size_t>&
        GetRequestedJointToNJointControllerAssignement(const ControlThread* p)
        {
            //do NOT update here!
            return p->_module<ControlThreadDataBuffer>()
                .controllersRequested.getReadBuffer()
                .jointToNJointControllerAssignement;
        }
 
        static bool
        RequestedControllersChanged(const ControlThread* p)
        {
            //only place allowed to update this buffer!
            return p->_module<ControlThreadDataBuffer>().controllersRequested.updateReadBuffer();
        }
 
        /// Activate a joint controller from the rt loop (only works in switch mode RTThread)
        static void
        RTSetJointController(ControlThread* p, JointController* c, std::size_t index)
        {
            ARMARX_CHECK_NOT_NULL(c);
            //do NOT update here!
            auto& readbuf =
                p->_module<ControlThreadDataBuffer>().controllersRequested._getNonConstReadBuffer();
            auto& j = readbuf.jointControllers;
            auto& assig = readbuf.jointToNJointControllerAssignement;
            auto& nj = readbuf.nJointControllers;
            ARMARX_CHECK_LESS(index, j.size());
            const auto assigNJ = assig.at(index);
            if (assigNJ != JointAndNJointControllers::Sentinel())
            {
                //an NJointController is activated! Deactivate it and reset all joit devs
                nj.at(assigNJ) = nullptr;
                for (std::size_t i = 0; i < assig.size(); ++i)
                {
                    if (assig.at(i) == assigNJ)
                    {
                        j.at(i) = j.at(i)->rtGetParent().rtGetJointStopMovementController();
                    }
                }
            }
            j.at(index) = c;
        }
    };
 
    /**
     * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThread.
     * \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
     */
    class DevicesAttorneyForControlThread
    {
        friend class ControlThread;
 
        static const std::vector<ControlDevicePtr>&
        GetControlDevices(const ControlThread* p)
        {
            return p->_module<Devices>().controlDevices.values();
        }
 
        static const std::vector<SensorDevicePtr>&
        GetSensorDevices(const ControlThread* p)
        {
            return p->_module<Devices>().sensorDevices.values();
        }
 
        static const std::vector<const SensorValueBase*>&
        GetSensorValues(const ControlThread* p)
        {
            return p->_module<Devices>().sensorValues;
        }
 
        static const std::vector<std::vector<PropagateConst<ControlTargetBase*>>>&
        GetControlTargets(const ControlThread* p)
        {
            return p->_module<Devices>().controlTargets;
        }
 
        static RTThreadTimingsSensorDevice&
        GetThreadTimingsSensorDevice(const ControlThread* p)
        {
            return *(p->_module<Devices>().rtThreadTimingsSensorDevice);
        }
 
        static void
        UpdateRobotWithSensorValues(const ControlThread* p,
                                    const VirtualRobot::RobotPtr& robot,
                                    const std::vector<VirtualRobot::RobotNodePtr>& robotNodes)
        {
            p->_module<Devices>().updateVirtualRobotFromSensorValues(
                robot, robotNodes, p->_module<Devices>().sensorValues);
        }
    };
 
    /**
     * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThread.
     * \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
     */
    class ManagementAttorneyForControlThread
    {
        friend class ControlThread;
 
        static bool
        HeartbeatMissing(const ControlThread* p)
        {
            const Management& m = p->_module<Management>();
            long now = TimeUtil::GetTime(true).toMilliSeconds();
            if (!m.heartbeatRequired || now < m.controlLoopStartTime + m.heartbeatStartupMarginMS)
            {
                return false;
            }
            return (now - m.lastHeartbeat) > m.heartbeatMaxCycleMS;
        }
    };
} // namespace armarx::RobotUnitModule
 
namespace armarx::RobotUnitModule
{
    bool
    ControlThread::rtSwitchControllerSetup(SwitchControllerMode mode)
    {
        rtGetThreadTimingsSensorDevice().rtMarkRtSwitchControllerSetupStart();
        ARMARX_ON_SCOPE_EXIT
        {
            rtGetThreadTimingsSensorDevice().rtMarkRtSwitchControllerSetupEnd();
        };
 
        //store controllers activated before switching controllers
        {
            preSwitchSetup_ActivatedJointControllers = rtGetActivatedJointControllers();
            preSwitchSetup_ActivatedJointToNJointControllerAssignement =
                rtGetActivatedJointToNJointControllerAssignement();
            preSwitchSetup_ActivatedNJointControllers = rtGetActivatedNJointControllers();
        }
 
        ARMARX_ON_SCOPE_EXIT
        {
            const auto& actJC = rtGetActivatedJointControllers();
            const auto& assig = rtGetActivatedJointToNJointControllerAssignement();
            const auto& actNJC = rtGetActivatedNJointControllers();
 
            //store controllers activated after switching controllers
            {
                postSwitchSetup_ActivatedJointControllers = actJC;
                postSwitchSetup_ActivatedJointToNJointControllerAssignement = assig;
                postSwitchSetup_ActivatedNJointControllers = actNJC;
            }
 
            std::size_t numSyncNj = 0;
            std::size_t numAsyncNj = 0;
 
            for (std::size_t i = 0; i < actJC.size(); ++i)
            {
                if (actNJC.at(i) == nullptr)
                {
                    continue;
                }
                if (dynamic_cast<SynchronousNJointController*>(actNJC.at(i)))
                {
                    _activatedSynchronousNJointControllersIdx.at(numSyncNj++) = i;
                }
                else if (dynamic_cast<AsynchronousNJointController*>(actNJC.at(i)))
                {
                    _activatedAsynchronousNJointControllersIdx.at(numAsyncNj++) = i;
                }
                else
                {
                    ARMARX_RT_LOGF_ERROR(
                        "NJoint controller that is neither SynchronousNJointController"
                        " nor AsynchronousNJointController: %s",
                        actNJC.at(i)->rtGetClassName().c_str());
                    // Throwing exceptions in a destructor causes std::abort to be called
                    //throw std::logic_error{};
                }
            }
            for (std::size_t i = numSyncNj;
                 i < _maxControllerCount &&
                 _activatedSynchronousNJointControllersIdx.at(i) != _maxControllerCount;
                 ++i)
            {
                _activatedSynchronousNJointControllersIdx.at(i) = _maxControllerCount;
            }
            for (std::size_t i = numAsyncNj;
                 i < _maxControllerCount &&
                 _activatedAsynchronousNJointControllersIdx.at(i) != _maxControllerCount;
                 ++i)
            {
                _activatedAsynchronousNJointControllersIdx.at(i) = _maxControllerCount;
            }
        };
 
 
        rtSwitchControllerSetupChangedControllers = false;
 
        // a missing heartbeat (if required by the config) is interpreted as emergencyStop == true
        if (!emergencyStop && ManagementAttorneyForControlThread::HeartbeatMissing(this))
        {
            rtSetEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
            ARMARX_RT_LOGF_ERROR("Emergency Stop was activated because heartbeat is missing!")
                .deactivateSpam(1);
        }
 
        const bool rtThreadOverridesControl = mode != SwitchControllerMode::IceRequests;
        // mode == RTThread                         -> control flow in hand of rt thread (ignoring estop)
        // !emergencyStop && !rtIsInEmergencyStop() -> normal control flow
        // !emergencyStop &&  rtIsInEmergencyStop() -> force switch to reactivate old ( + reset flag)
        //  emergencyStop && !rtIsInEmergencyStop() -> deactivate all + set flag
        //  emergencyStop &&  rtIsInEmergencyStop() -> nothing to do
        if (emergencyStop && !rtThreadOverridesControl)
        {
            if (rtIsInEmergencyStop())
            {
                return false;
            }
            rtIsInEmergencyStop_ = true;
            //deactivate all nJointCtrl
            for (auto& nJointCtrl : rtGetActivatedNJointControllers())
            {
                if (nJointCtrl)
                {
                    NJointControllerAttorneyForControlThread::RtDeactivateController(nJointCtrl);
                    nJointCtrl = nullptr;
                    rtSwitchControllerSetupChangedControllers = true;
                }
            }
            //set all JointCtrl to emergency stop (except stop movement)
            for (std::size_t i = 0; i < rtGetControlDevices().size(); ++i)
            {
                const ControlDevicePtr& controlDevice = rtGetControlDevices().at(i);
                const auto active = controlDevice->rtGetActiveJointController();
                const auto stopMov = controlDevice->rtGetJointStopMovementController();
                const auto emergency = controlDevice->rtGetJointEmergencyStopController();
                if (active != stopMov && active != emergency)
                {
                    controlDevice->rtSetActiveJointController(emergency);
                    rtGetActivatedJointControllers().at(i) = emergency;
                    rtSwitchControllerSetupChangedControllers = true;
                }
            }
            if (rtSwitchControllerSetupChangedControllers)
            {
                ControlThreadDataBufferAttorneyForControlThread::
                    ResetActivatedControllerAssignement(this);
                // the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
            }
            return rtSwitchControllerSetupChangedControllers;
        }
 
        if (!rtThreadOverridesControl &&
            !ControlThreadDataBufferAttorneyForControlThread::RequestedControllersChanged(this) &&
            !rtIsInEmergencyStop())
        {
            return false;
        }
        rtIsInEmergencyStop_ = false;
 
        //handle nJointCtrl
        {
            const auto& allReqNJ = rtGetRequestedNJointControllers();
            auto& allActdNJ = rtGetActivatedNJointControllers();
            //"merge"
            std::size_t n = rtGetControlDevices().size();
            std::size_t idxAct = 0;
            std::size_t idxReq = 0;
            for (std::size_t i = 0; i < 2 * n; ++i)
            {
                //skip nullptrs in act
                while (idxAct < n && !allActdNJ.at(idxAct))
                {
                    ++idxAct;
                }
                const NJointControllerBasePtr& req =
                    idxReq < n ? allReqNJ.at(idxReq) : nullptr; //may be null
                const NJointControllerBasePtr& act =
                    idxAct < n ? allActdNJ.at(idxAct) : nullptr; //may be null if it is the last
                const auto reqId = reinterpret_cast<std::uintptr_t>(req.get());
                const auto actId = reinterpret_cast<std::uintptr_t>(act.get());
 
                if (reqId > actId)
                {
                    //new ctrl
                    rtSyncNJointControllerRobot(req.get());
                    NJointControllerAttorneyForControlThread::RtActivateController(req);
                    ++idxReq;
                    rtSwitchControllerSetupChangedControllers = true;
                }
                else if (reqId < actId)
                {
                    NJointControllerAttorneyForControlThread::RtDeactivateController(act);
                    rtSwitchControllerSetupChangedControllers = true;
                    ++idxAct;
                }
                else //if(reqId == actId)
                {
                    //same ctrl or both null ctrl
                    ++idxReq;
                    ++idxAct;
                }
                if (idxAct >= n && !req)
                {
                    break;
                }
            }
            allActdNJ = allReqNJ;
        }
 
        //handle Joint Ctrl
        {
            const auto& allReqJ = rtGetRequestedJointControllers();
            auto& allActdJ = rtGetActivatedJointControllers();
            ARMARX_CHECK_EQUAL(allReqJ.size(), rtGetControlDevices().size());
            for (std::size_t i = 0; i < rtGetControlDevices().size(); ++i)
            {
                auto& controlDevice = rtGetControlDevices().at(i);
                const auto requestedJointCtrl = allReqJ.at(i);
                controlDevice->rtSetActiveJointController(requestedJointCtrl);
                allActdJ.at(i) = requestedJointCtrl;
            }
        }
        ControlThreadDataBufferAttorneyForControlThread::
            AcceptRequestedJointToNJointControllerAssignement(this);
        // the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
        return true;
    }
 
    void
    ControlThread::rtResetAllTargets()
    {
        rtGetThreadTimingsSensorDevice().rtMarkRtResetAllTargetsStart();
        for (const ControlDevicePtr& controlDev : rtGetControlDevices())
        {
            controlDev->rtGetActiveJointController()->rtResetTarget();
        }
        rtGetThreadTimingsSensorDevice().rtMarkRtResetAllTargetsEnd();
    }
 
    void
    ControlThread::rtHandleInvalidTargets()
    {
        rtGetThreadTimingsSensorDevice().rtMarkRtHandleInvalidTargetsStart();
        numberOfInvalidTargetsInThisIteration = 0;
        const auto& cdevs = rtGetControlDevices();
        for (std::size_t i = 0; i < cdevs.size(); ++i)
        {
            if (!rtGetActivatedJointControllers().at(i)->rtIsTargetValid())
            {
                ARMARX_RT_LOGF_ERROR("INVALID TARGET for JointController (idx = %lu) '%s'",
                                     i,
                                     cdevs.at(i)->rtGetDeviceName());
                ARMARX_ERROR << ">>>INVALID TARGET for JointController (idx = " << i << ") '"
                             << cdevs.at(i)->getDeviceName() << "'";
                rtDeactivateAssignedNJointControllerBecauseOfError(i);
                ++numberOfInvalidTargetsInThisIteration;
            }
        }
        // the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
        rtGetThreadTimingsSensorDevice().rtMarkRtHandleInvalidTargetsEnd();
    }
 
    void
    ControlThread::rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp,
                                            const IceUtil::Time& timeSinceLastIteration)
    {
        rtGetThreadTimingsSensorDevice().rtMarkRtReadSensorDeviceValuesStart();
        for (const SensorDevicePtr& device : rtGetSensorDevices())
        {
            device->rtReadSensorValues(sensorValuesTimestamp, timeSinceLastIteration);
        }
        DevicesAttorneyForControlThread::UpdateRobotWithSensorValues(this, rtRobot, rtRobotNodes);
        rtPostReadSensorDeviceValues(sensorValuesTimestamp, timeSinceLastIteration);
        rtGetThreadTimingsSensorDevice().rtMarkRtReadSensorDeviceValuesEnd();
    }
 
    void
    ControlThread::rtPostReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp,
                                                const IceUtil::Time& timeSinceLastIteration)
    {
        if (dynamicsHelpers)
        {
            //                auto start = IceUtil::Time::now();
            dynamicsHelpers->update(sensorValuesTimestamp, timeSinceLastIteration);
            //                auto end = IceUtil::Time::now();
            //                ARMARX_INFO << "Dynamics duration: " << (end-start).toMicroSeconds();
        }
    }
 
    void
    ControlThread::rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp,
                                         const IceUtil::Time& timeSinceLastIteration)
    {
        rtGetThreadTimingsSensorDevice().rtMarkRtRunJointControllersStart();
        for (const ControlDevicePtr& device : rtGetControlDevices())
        {
            device->rtRun(sensorValuesTimestamp, timeSinceLastIteration);
        }
        rtGetThreadTimingsSensorDevice().rtMarkRtRunJointControllersEnd();
    }
 
    void
    ControlThread::rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp,
                                          const IceUtil::Time& timeSinceLastIteration)
    {
        rtGetThreadTimingsSensorDevice().rtMarkRtRunNJointControllersStart();
        //            bool activeControllersChanged = false;
        auto activatedNjointCtrls = rtGetActivatedNJointControllers();
        //start async
        for (std::size_t nJointCtrlIndex : _activatedAsynchronousNJointControllersIdx)
        {
            if (nJointCtrlIndex == _maxControllerCount)
            {
                break;
            }
            auto nJointCtrl = static_cast<AsynchronousNJointController*>(
                activatedNjointCtrls.at(nJointCtrlIndex));
            if (!nJointCtrl)
            {
                continue;
            }
            try
            {
                if (nJointCtrl->rtGetErrorState())
                {
                    ARMARX_RT_LOGF_ERROR(
                        "NJointControllerBase '%s' requested deactivation while activating it",
                        nJointCtrl->rtGetInstanceName().c_str());
                    rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
                }
                auto start = TimeUtil::GetTime(true);
                rtSyncNJointControllerRobot(nJointCtrl);
                nJointCtrl->rtRunIterationBegin(sensorValuesTimestamp, timeSinceLastIteration);
                auto duration = TimeUtil::GetTime(true) - start;
                if (nJointCtrl->rtGetErrorState())
                {
                    ARMARX_RT_LOGF_ERROR(
                        "NJointControllerBase '%s' requested deactivation while running it",
                        nJointCtrl->rtGetInstanceName().c_str());
                    rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
                }
                if (static_cast<std::size_t>(duration.toMicroSeconds()) >
                    nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
                {
                    ARMARX_RT_LOGF_ERROR("The NJointControllerBase '%s' took %ld µs to run!",
                                         nJointCtrl->rtGetInstanceName().c_str(),
                                         duration.toMicroSeconds())
                        .deactivateSpam(5);
                }
                else if (static_cast<std::size_t>(duration.toMicroSeconds()) >
                         nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
                {
                    ARMARX_RT_LOGF_WARNING("The NJointControllerBase '%s' took %ld µs to run!",
                                           nJointCtrl->rtGetInstanceName().c_str(),
                                           duration.toMicroSeconds())
                        .deactivateSpam(5);
                }
            }
            catch (...)
            {
                ARMARX_ERROR << "NJoint Controller " << nJointCtrl->getInstanceName()
                             << " threw an exception and is now deactivated: "
                             << GetHandledExceptionString();
                rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
            }
        }
 
        //start sync
        for (std::size_t nJointCtrlIndex : _activatedSynchronousNJointControllersIdx)
        {
            if (nJointCtrlIndex == _maxControllerCount)
            {
                break;
            }
            auto nJointCtrl =
                static_cast<SynchronousNJointController*>(activatedNjointCtrls.at(nJointCtrlIndex));
            try
            {
                if (nJointCtrl)
                {
                    if (nJointCtrl->rtGetErrorState())
                    {
                        ARMARX_RT_LOGF_ERROR(
                            "NJointControllerBase '%s' requested deactivation while activating it",
                            nJointCtrl->rtGetInstanceName().c_str());
                        rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
                        //                            activeControllersChanged = true;
                    }
 
                    auto start = TimeUtil::GetTime(true);
                    rtSyncNJointControllerRobot(nJointCtrl);
                    nJointCtrl->rtSwapBufferAndRun(sensorValuesTimestamp, timeSinceLastIteration);
                    auto duration = TimeUtil::GetTime(true) - start;
                    if (nJointCtrl->rtGetErrorState())
                    {
                        ARMARX_RT_LOGF_ERROR(
                            "NJointControllerBase '%s' requested deactivation while running it",
                            nJointCtrl->rtGetInstanceName().c_str());
                        rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
                        //                            activeControllersChanged = true;
                    }
                    if (static_cast<std::size_t>(duration.toMicroSeconds()) >
                        nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
                    {
                        ARMARX_RT_LOGF_ERROR("The NJointControllerBase '%s' took %ld µs to run!",
                                             nJointCtrl->rtGetInstanceName().c_str(),
                                             duration.toMicroSeconds())
                            .deactivateSpam(5);
                    }
                    else if (static_cast<std::size_t>(duration.toMicroSeconds()) >
                             nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
                    {
                        ARMARX_RT_LOGF_WARNING("The NJointControllerBase '%s' took %ld µs to run!",
                                               nJointCtrl->rtGetInstanceName().c_str(),
                                               duration.toMicroSeconds())
                            .deactivateSpam(5);
                    }
                }
            }
            catch (...)
            {
                ARMARX_ERROR << "NJoint Controller " << nJointCtrl->getInstanceName()
                             << " threw an exception and is now deactivated: "
                             << GetHandledExceptionString();
                rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
                //                    activeControllersChanged = true;
            }
        }
        //stop async
        for (std::size_t nJointCtrlIndex : _activatedAsynchronousNJointControllersIdx)
        {
            if (nJointCtrlIndex == _maxControllerCount)
            {
                break;
            }
            auto nJointCtrl = static_cast<AsynchronousNJointController*>(
                activatedNjointCtrls.at(nJointCtrlIndex));
            if (!nJointCtrl)
            {
                continue;
            }
            try
            {
                if (nJointCtrl->rtGetErrorState())
                {
                    ARMARX_RT_LOGF_ERROR(
                        "NJointControllerBase '%s' requested deactivation while activating it",
                        nJointCtrl->rtGetInstanceName().c_str());
                    rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
                }
                auto start = TimeUtil::GetTime(true);
                rtSyncNJointControllerRobot(nJointCtrl);
                nJointCtrl->rtRunIterationEnd(sensorValuesTimestamp, timeSinceLastIteration);
                auto duration = TimeUtil::GetTime(true) - start;
                if (nJointCtrl->rtGetErrorState())
                {
                    ARMARX_RT_LOGF_ERROR(
                        "NJointControllerBase '%s' requested deactivation while running it",
                        nJointCtrl->rtGetInstanceName().c_str());
                    rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
                }
                if (static_cast<std::size_t>(duration.toMicroSeconds()) >
                    nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
                {
                    ARMARX_RT_LOGF_ERROR("The NJointControllerBase '%s' took %ld µs to run!",
                                         nJointCtrl->rtGetInstanceName().c_str(),
                                         duration.toMicroSeconds())
                        .deactivateSpam(5);
                }
                else if (static_cast<std::size_t>(duration.toMicroSeconds()) >
                         nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
                {
                    ARMARX_RT_LOGF_WARNING("The NJointControllerBase '%s' took %ld µs to run!",
                                           nJointCtrl->rtGetInstanceName().c_str(),
                                           duration.toMicroSeconds())
                        .deactivateSpam(5);
                }
            }
            catch (...)
            {
                ARMARX_ERROR << "NJoint Controller " << nJointCtrl->getInstanceName()
                             << " threw an exception and is now deactivated: "
                             << GetHandledExceptionString();
                rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
            }
        }
        // the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
        rtGetThreadTimingsSensorDevice().rtMarkRtRunNJointControllersEnd();
    }
 
    void
    ControlThread::rtDeactivateNJointControllerBecauseOfError(std::size_t nJointCtrlIndex)
    {
        const NJointControllerBasePtr& nJointCtrl =
            rtGetActivatedNJointControllers().at(nJointCtrlIndex);
        NJointControllerAttorneyForControlThread::RtDeactivateControllerBecauseOfError(nJointCtrl);
        for (auto ctrlDevIdx : nJointCtrl->rtGetControlDeviceUsedIndices())
        {
            const ControlDevicePtr& controlDevice = rtGetControlDevices().at(ctrlDevIdx);
            JointController* es = controlDevice->rtGetJointEmergencyStopController();
 
            ARMARX_CHECK_EQUAL(rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIdx),
                               nJointCtrlIndex)
                << VAROUT(ctrlDevIdx) << "\n"
                << VAROUT(controlDevice->getDeviceName()) << "\n"
                << dumpRtState();
 
            controlDevice->rtSetActiveJointController(es);
            rtGetActivatedJointControllers().at(ctrlDevIdx) = es;
            rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIdx) = IndexSentinel();
        }
        rtGetActivatedNJointControllers().at(nJointCtrlIndex) = nullptr;
        //check ControlDeviceHardwareControlModeGroups
        {
            for (auto ctrlDevIdx : nJointCtrl->rtGetControlDeviceUsedIndices())
            {
                const auto& ctrlModeGroups = _module<Devices>().getControlModeGroups();
                const auto groupIdx = ctrlModeGroups.groupIndices.at(ctrlDevIdx);
                if (groupIdx == IndexSentinel())
                {
                    continue;
                }
                ControlDevice* const dev = rtGetControlDevices().at(ctrlDevIdx).get();
                JointController* const jointCtrl = dev->rtGetActiveJointController();
                const auto hwModeHash = jointCtrl->rtGetHardwareControlModeHash();
                //this device is in a group!
                // -> check all other devices
                for (const auto otherIdx : ctrlModeGroups.deviceIndices.at(groupIdx))
                {
                    ControlDevice* const otherDev = rtGetControlDevices().at(otherIdx).get();
                    JointController* const otherJointCtrl = otherDev->rtGetActiveJointController();
                    const auto otherHwModeHash = otherJointCtrl->rtGetHardwareControlModeHash();
                    if (hwModeHash == otherHwModeHash)
                    {
                        //the assigend ctrl has the same hwMode -> don't do anything
                        continue;
                    }
                    const auto otherNJointCtrl1Idx =
                        rtGetActivatedJointToNJointControllerAssignement().at(otherIdx);
                    if (otherNJointCtrl1Idx == IndexSentinel())
                    {
                        //the hwmodes are different! (hence the other ctrl must be in stop movement
                        ARMARX_CHECK_EXPRESSION(otherJointCtrl ==
                                                otherDev->rtGetJointStopMovementController());
                        //we need to activate the es contoller
                        JointController* const es = otherDev->rtGetJointEmergencyStopController();
                        otherDev->rtSetActiveJointController(es);
                        ARMARX_CHECK_EXPRESSION(es->rtGetHardwareControlModeHash() == hwModeHash);
                        rtGetActivatedJointControllers().at(otherIdx) = es;
                        continue;
                    }
                    //the devs NJoint controller needs to be deactivated
                    rtDeactivateAssignedNJointControllerBecauseOfError(otherIdx);
                }
            }
        }
        rtDeactivatedNJointControllerBecauseOfError(nJointCtrl);
    }
 
    void
    ControlThread::rtDeactivateAssignedNJointControllerBecauseOfError(std::size_t ctrlDevIndex)
    {
        std::size_t nJointCtrlIndex =
            rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIndex);
        ARMARX_CHECK_LESS(nJointCtrlIndex, rtGetControlDevices().size())
            << "no NJoint controller controls this device (name = "
            << rtGetControlDevices().at(ctrlDevIndex)->getDeviceName() << ", ControlMode = "
            << rtGetActivatedJointControllers().at(ctrlDevIndex)->getControlMode() << ")!"
            << "\n"
            << "This means an invariant is violated! Dumping data for debugging:\n"
            << VAROUT(ctrlDevIndex) << "\n"
            << dumpRtState();
 
        rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
    }
 
    void
    ControlThread::rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp,
                                                  const IceUtil::Time& timeSinceLastIteration)
    {
        rtGetThreadTimingsSensorDevice().rtMarkRtUpdateSensorAndControlBufferStart();
        //commit all changes to activated controllers (joint, njoint, assignement)
        {
            ControlThreadDataBufferAttorneyForControlThread::CommitActivatedControllers(this);
        }
 
        SensorAndControl& sc = _module<ControlThreadDataBuffer>().rtGetSensorAndControlBuffer();
        sc.writeTimestamp = armarx::rtNow(); // this has to be in real time
        sc.sensorValuesTimestamp = sensorValuesTimestamp;
        sc.timeSinceLastIteration = timeSinceLastIteration;
        ARMARX_CHECK_EQUAL(rtGetSensorDevices().size(), sc.sensors.size());
        for (std::size_t sensIdx = 0; sensIdx < rtGetSensorDevices().size(); ++sensIdx)
        {
            rtGetSensorDevices().at(sensIdx)->getSensorValue()->_copyTo(sc.sensors.at(sensIdx));
        }
 
        ARMARX_CHECK_EQUAL(rtGetControlDevices().size(), sc.control.size());
        for (std::size_t ctrlIdx = 0; ctrlIdx < rtGetControlDevices().size(); ++ctrlIdx)
        {
            ControlDevice& controlDevice = *rtGetControlDevices().at(ctrlIdx);
            ARMARX_CHECK_EQUAL(controlDevice.rtGetJointControllers().size(),
                               sc.control.at(ctrlIdx).size());
            for (std::size_t targIdx = 0; targIdx < controlDevice.rtGetJointControllers().size();
                 ++targIdx)
            {
                JointController& jointCtrl = *controlDevice.rtGetJointControllers().at(targIdx);
                jointCtrl.getControlTarget()->_copyTo(sc.control.at(ctrlIdx).at(targIdx));
            }
        }
        _module<ControlThreadDataBuffer>().rtSensorAndControlBufferCommitWrite();
 
 
        rtGetThreadTimingsSensorDevice().rtMarkRtUpdateSensorAndControlBufferEnd();
    }
 
    const std::vector<ControlDevicePtr>&
    ControlThread::rtGetControlDevices() const
    {
        return DevicesAttorneyForControlThread::GetControlDevices(this);
    }
 
    const std::vector<SensorDevicePtr>&
    ControlThread::rtGetSensorDevices()
    {
        return DevicesAttorneyForControlThread::GetSensorDevices(this);
    }
 
    RTThreadTimingsSensorDevice&
    ControlThread::rtGetThreadTimingsSensorDevice()
    {
        return DevicesAttorneyForControlThread::GetThreadTimingsSensorDevice(this);
    }
 
    void
    ControlThread::rtSetEmergencyStopState(EmergencyStopState state)
    {
        if (state == EmergencyStopState::eEmergencyStopActive)
        {
            emergencyStopStateRequest = EmergencyStopStateRequest::RequestActive;
        }
        else
        {
            emergencyStopStateRequest = EmergencyStopStateRequest::RequestInactive;
        }
    }
 
    void
    ControlThread::_preFinishControlThreadInitialization()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        controlThreadId = std::this_thread::get_id();
 
        _maxControllerCount = rtGetActivatedJointControllers().size();
 
        ARMARX_CHECK_EQUAL(_maxControllerCount,
                           rtGetActivatedJointToNJointControllerAssignement().size());
        ARMARX_CHECK_EQUAL(_maxControllerCount, rtGetActivatedNJointControllers().size());
        //resize buffers used for error oputput
        preSwitchSetup_ActivatedJointControllers.resize(_maxControllerCount);
        postSwitchSetup_ActivatedJointControllers.resize(_maxControllerCount);
 
        preSwitchSetup_ActivatedJointToNJointControllerAssignement.resize(_maxControllerCount);
        postSwitchSetup_ActivatedJointToNJointControllerAssignement.resize(_maxControllerCount);
 
        preSwitchSetup_ActivatedNJointControllers.resize(_maxControllerCount);
        postSwitchSetup_ActivatedNJointControllers.resize(_maxControllerCount);
 
        _activatedSynchronousNJointControllersIdx.resize(_maxControllerCount, _maxControllerCount);
        _activatedAsynchronousNJointControllersIdx.resize(_maxControllerCount, _maxControllerCount);
 
        // setup inverse dynamics
        if (getProperty<bool>("EnableInverseDynamics").getValue())
        {
            RobotUnit* robotUnit = dynamic_cast<RobotUnit*>(this);
            ARMARX_CHECK_EXPRESSION(robotUnit);
            std::shared_ptr<DynamicsHelper> dynamicsHelpers(new DynamicsHelper(robotUnit));
            auto bodySetName = getProperty<std::string>("InverseDynamicsRobotBodySet").getValue();
            auto rtRobotBodySet = rtGetRobot()->getRobotNodeSet(bodySetName);
            ARMARX_CHECK_EXPRESSION(rtRobotBodySet)
                << "could not find robot node set with name: " << bodySetName
                << " - Check property InverseDynamicsRobotBodySet";
            //                rtfilters::RTFilterBasePtr accFilter(new rtfilters::ButterworthFilter(100, 1000, Lowpass, 1));
            //                rtfilters::RTFilterBasePtr velFilter(new rtfilters::ButterworthFilter(100, 1000, Lowpass, 1));
            rtfilters::RTFilterBasePtr accFilter(new rtfilters::AverageFilter(30));
            rtfilters::RTFilterBasePtr velFilter(new rtfilters::AverageFilter(30));
 
            auto setList =
                armarx::Split(getProperty<std::string>("InverseDynamicsRobotJointSets").getValue(),
                              ",",
                              true,
                              true);
            for (auto& set : setList)
            {
                auto rns = rtGetRobot()->getRobotNodeSet(set);
                ARMARX_CHECK_EXPRESSION(rns) << "could not find robot node set with name: " << set
                                             << " - Check property InverseDynamicsRobotJointSets";
                dynamicsHelpers->addRobotPart(rns, rtRobotBodySet, velFilter, accFilter);
                ARMARX_VERBOSE << "Added nodeset " << set << " for inverse dynamics";
            }
 
            this->dynamicsHelpers = dynamicsHelpers;
        }
    }
 
    void
    ControlThread::_preOnInitRobotUnit()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        try
        {
            rtRobot = VirtualRobot::RobotIO::loadRobot(_module<RobotData>().getRobotFileName(),
                                                       VirtualRobot::RobotIO::eStructure);
            rtRobot->setUpdateCollisionModel(false);
            rtRobot->setUpdateVisualization(false);
            rtRobot->setThreadsafe(false);
            rtRobotNodes = rtRobot->getRobotNodes();
        }
        catch (VirtualRobot::VirtualRobotException& e)
        {
            throw UserException(e.what());
        }
        usPerDevUntilWarn = getProperty<std::size_t>(
                                "NjointController_AllowedExecutionTimePerControlDeviceUntilWarning")
                                .getValue();
        usPerDevUntilError = getProperty<std::size_t>(
                                 "NjointController_AllowedExecutionTimePerControlDeviceUntilError")
                                 .getValue();
    }
 
    void
    ControlThread::setEmergencyStopState(EmergencyStopState state, const Ice::Current&)
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        _module<Units>().getEmergencyStopMaster()->setEmergencyStopState(state);
    }
 
    EmergencyStopState
    ControlThread::getEmergencyStopState(const Ice::Current&) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        return emergencyStop ? EmergencyStopState::eEmergencyStopActive
                             : EmergencyStopState::eEmergencyStopInactive;
    }
 
    EmergencyStopState
    ControlThread::getRtEmergencyStopState(const Ice::Current&) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        return rtIsInEmergencyStop() ? EmergencyStopState::eEmergencyStopActive
                                     : EmergencyStopState::eEmergencyStopInactive;
    }
 
    void
    ControlThread::setEmergencyStopStateNoReportToTopic(EmergencyStopState state)
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        emergencyStop = (state == EmergencyStopState::eEmergencyStopActive);
    }
 
    void
    ControlThread::processEmergencyStopRequest()
    {
        const auto state =
            emergencyStopStateRequest.exchange(EmergencyStopStateRequest::RequestNone);
        switch (state)
        {
            case EmergencyStopStateRequest::RequestActive:
                setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
                break;
            case EmergencyStopStateRequest::RequestInactive:
                setEmergencyStopState(EmergencyStopState::eEmergencyStopInactive);
                break;
            case EmergencyStopStateRequest::RequestNone:
                break;
            default:
                ARMARX_CHECK_EXPRESSION(!static_cast<int>(state))
                    << "Unkown value for EmergencyStopStateRequest";
        }
    }
 
    const std::vector<JointController*>&
    ControlThread::rtGetActivatedJointControllers() const
    {
        return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointControllers(this);
    }
 
    const std::vector<NJointControllerBase*>&
    ControlThread::rtGetActivatedNJointControllers() const
    {
        return ControlThreadDataBufferAttorneyForControlThread::GetActivatedNJointControllers(this);
    }
 
    const std::vector<std::size_t>&
    ControlThread::rtGetActivatedJointToNJointControllerAssignement() const
    {
        return ControlThreadDataBufferAttorneyForControlThread::
            GetActivatedJointToNJointControllerAssignement(this);
    }
 
    std::vector<JointController*>&
    ControlThread::rtGetActivatedJointControllers()
    {
        return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointControllers(this);
    }
 
    std::vector<NJointControllerBase*>&
    ControlThread::rtGetActivatedNJointControllers()
    {
        return ControlThreadDataBufferAttorneyForControlThread::GetActivatedNJointControllers(this);
    }
 
    std::vector<std::size_t>&
    ControlThread::rtGetActivatedJointToNJointControllerAssignement()
    {
        return ControlThreadDataBufferAttorneyForControlThread::
            GetActivatedJointToNJointControllerAssignement(this);
    }
 
    const std::vector<JointController*>&
    ControlThread::rtGetRequestedJointControllers() const
    {
        return ControlThreadDataBufferAttorneyForControlThread::GetRequestedJointControllers(this);
    }
 
    const std::vector<NJointControllerBase*>&
    ControlThread::rtGetRequestedNJointControllers() const
    {
        return ControlThreadDataBufferAttorneyForControlThread::GetRequestedNJointControllers(this);
    }
 
    const std::vector<std::size_t>&
    ControlThread::rtGetRequestedJointToNJointControllerAssignement() const
    {
        return ControlThreadDataBufferAttorneyForControlThread::
            GetRequestedJointToNJointControllerAssignement(this);
    }
 
    void
    ControlThread::rtSyncNJointControllerRobot(NJointControllerBase* njctrl)
    {
        if (njctrl->rtGetRobot())
        {
            // update joints / nodes
            auto& from = rtRobotNodes;
            auto& to = njctrl->rtGetRobotNodes();
            for (std::size_t i = 0; i < from.size(); ++i)
            {
                to.at(i)->copyPoseFrom(from.at(i));
            }
 
            // update global root pose
            njctrl->rtGetRobot()->setGlobalPose(rtRobot->getGlobalPose(), false);
        }
    }
 
    void
    ControlThread::dumpRtControllerSetup(
        std::ostream& out,
        const std::string& indent,
        const std::vector<JointController*>& activeJCtrls,
        const std::vector<std::size_t>& assignement,
        const std::vector<NJointControllerBase*>& activeNJCtrls) const
    {
        out << indent << "JointControllers:\n";
        {
            const auto& cdevs = rtGetControlDevices();
            for (std::size_t i = 0; i < cdevs.size(); ++i)
            {
                const JointController* jctrl = activeJCtrls.at(i);
                out << indent << "\t(" << i << ")\t" << cdevs.at(i)->getDeviceName() << ":\n"
                    << indent << "\t\t Controller: " << jctrl->getControlMode() << " (" << jctrl
                    << ")\n"
                    << indent << "\t\t Assigned NJoint: " << assignement.at(i) << "\n";
            }
        }
        out << indent << "NJointControllers:\n";
        {
            for (std::size_t i = 0; i < activeNJCtrls.size(); ++i)
            {
                const auto* njctrl = activeNJCtrls.at(i);
                out << indent << "\t(" << i << ")\t";
                if (njctrl)
                {
                    out << njctrl->rtGetInstanceName() << " (" << njctrl << "):"
                        << "\t Class: " << njctrl->rtGetClassName() << "\n";
                }
                else
                {
                    out << " (" << njctrl << ")\n";
                }
            }
        }
    }
 
    std::string
    ControlThread::dumpRtState() const
    {
        std::stringstream str;
        str << "state requested\n";
        dumpRtControllerSetup(str,
                              "\t",
                              rtGetRequestedJointControllers(),
                              rtGetRequestedJointToNJointControllerAssignement(),
                              rtGetRequestedNJointControllers());
 
        str << "state before rtSwitchControllerSetup() was called\n";
        dumpRtControllerSetup(str,
                              "\t",
                              preSwitchSetup_ActivatedJointControllers,
                              preSwitchSetup_ActivatedJointToNJointControllerAssignement,
                              preSwitchSetup_ActivatedNJointControllers);
 
        str << "state after rtSwitchControllerSetup() was called\n";
        dumpRtControllerSetup(str,
                              "\t",
                              postSwitchSetup_ActivatedJointControllers,
                              postSwitchSetup_ActivatedJointToNJointControllerAssignement,
                              postSwitchSetup_ActivatedNJointControllers);
 
        str << "state now\n";
        dumpRtControllerSetup(str,
                              "\t",
                              rtGetActivatedJointControllers(),
                              rtGetActivatedJointToNJointControllerAssignement(),
                              rtGetActivatedNJointControllers());
 
        str << VAROUT(rtSwitchControllerSetupChangedControllers) << "\n";
        str << VAROUT(numberOfInvalidTargetsInThisIteration) << "\n";
        return str.str();
    }
 
    void
    ControlThread::rtUpdateRobotGlobalPose()
    {
        const ConstSensorDevicePtr globalPoseSensorDevice =
            _module<Devices>().getSensorDevice(GlobalRobotPoseSensorDevice::DeviceName());
        if (not globalPoseSensorDevice)
        {
            return;
        }
 
        const auto* const sensorValue =
            globalPoseSensorDevice->getSensorValue()->asA<SensorValueGlobalRobotPose>();
        if (sensorValue == nullptr)
        {
            return;
        }
 
        const auto global_T_robot = sensorValue->global_T_root;
        rtSetRobotGlobalPose(global_T_robot, false);
    }
 
    void
    ControlThread::rtSetRobotGlobalPose(const Eigen::Matrix4f& gp, bool updateRobot)
    {
        rtRobot->setGlobalPose(gp, updateRobot);
    }
 
    void
    ControlThread::rtSetJointController(JointController* c, std::size_t index)
    {
        ControlThreadDataBufferAttorneyForControlThread::RTSetJointController(this, c, index);
    }
} // namespace armarx::RobotUnitModule