Base.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    ...
 * @author     Jianfeng Gao ( jianfeng dot gao at kit dot edu )
 * @date       2025
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "Base.h"
 
#include <SimoxUtility/color/Color.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
// #include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
 
#include <armarx/control/common/utils.h>
#include <armarx/control/ethercat/RTUtility.h>
 
/// for GUI
#include <ArmarXCore/core/PackagePath.h> // for GUI
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h> // for GUI
 
#include <armarx/control/common/aron_conversions.h> // for GUI
 
/// HandUnit
//#include <RobotAPI/interface/units/HandUnitInterface.h>
 
namespace armarx::control::njoint_controller::task_space
{
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::limbInit(
        const std::string& nodeSetName,
        ArmPtr& arm,
        Config& cfg,
        VirtualRobot::RobotPtr& nonRtRobotPtr)
    {
        arm->kinematicChainName = nodeSetName;
        VirtualRobot::RobotNodeSetPtr rtRns =
            rtGetRobot()->getRobotNodeSet(arm->kinematicChainName);
        VirtualRobot::RobotNodeSetPtr nonRtRns =
            nonRtRobotPtr->getRobotNodeSet(arm->kinematicChainName);
        ARMARX_CHECK_EXPRESSION(rtRns) << arm->kinematicChainName;
        ARMARX_CHECK_EXPRESSION(nonRtRns) << arm->kinematicChainName;
        ARMARX_IMPORTANT << "--> Creating Taskspace controller with kinematic chain: "
                         << arm->kinematicChainName << " with num of joints: (RT robot) "
                         << rtRns->getSize() << ", and (non-RT robot) " << nonRtRns->getSize();
 
        std::vector<size_t> jointIDTorqueMode;
        std::vector<size_t> jointIDVelocityMode;
        arm->jointNames.clear();
        for (size_t i = 0; i < rtRns->getSize(); ++i)
        {
            std::string jointName = rtRns->getNode(i)->getName();
            arm->jointNames.push_back(jointName);
 
            bool foundControlDevice = false;
            auto it = std::find(
                cfg.jointNameListTorque.begin(), cfg.jointNameListTorque.end(), jointName);
            if (it != cfg.jointNameListTorque.end())
            {
                ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
                ARMARX_CHECK_EXPRESSION(ct);
                auto* casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>();
                ARMARX_CHECK_EXPRESSION(casted_ct);
                arm->targetsTorque.push_back(casted_ct);
                jointIDTorqueMode.push_back(i);
                foundControlDevice = true;
            }
 
            it = std::find(
                cfg.jointNameListVelocity.begin(), cfg.jointNameListVelocity.end(), jointName);
            if (it != cfg.jointNameListVelocity.end())
            {
                ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
                ARMARX_CHECK_EXPRESSION(ct);
                auto* casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
                ARMARX_CHECK_EXPRESSION(casted_ct);
                arm->targetsVel.push_back(casted_ct);
                jointIDVelocityMode.push_back(i);
                foundControlDevice = true;
            }
            if (not foundControlDevice)
            {
                auto namesTorque = armarx::control::common::sVecToString(cfg.jointNameListTorque);
                auto namesVelocity =
                    armarx::control::common::sVecToString(cfg.jointNameListVelocity);
                ARMARX_ERROR << "Does not found valid control device for joint name: " << jointName
                             << "Please check torque controlled joints: [" << namesTorque
                             << "] and velocity controlled joints: [" << namesVelocity << "].";
            }
 
            const SensorValueBase* sv = useSensorValue(jointName);
            ARMARX_CHECK_EXPRESSION(sv);
            arm->sensorDevices.append(sv, jointName);
        };
        ARMARX_DEBUG << "Number of torque controlled joints " << jointIDTorqueMode.size();
        ARMARX_DEBUG << "Number of velocity controlled joints " << jointIDVelocityMode.size();
        ARMARX_CHECK_EQUAL(rtRns->getNodeNames().size(),
                           jointIDTorqueMode.size() + jointIDVelocityMode.size());
 
        arm->controller.initialize(nonRtRns, rtRns);
        arm->controller.ftsensor.initialize(rtRns, robotUnit, cfg.ftConfig);
 
        validateConfigData(cfg, arm);
        arm->rtConfig = cfg;
        arm->nonRtConfig = cfg;
        arm->rtStatus.reset(rtRns->getSize(), jointIDTorqueMode, jointIDVelocityMode);
        ARMARX_INFO << "<-- Limb " << arm->kinematicChainName << " created";
    }
 
    template <typename NJointTaskspaceControllerType>
    NJointTaskspaceController<NJointTaskspaceControllerType>::NJointTaskspaceController(
        const RobotUnitPtr& robotUnit,
        const NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr&) :
        robotUnit(robotUnit)
    {
        ARMARX_INFO << "================ creating controller ============================";
        ARMARX_CHECK_EXPRESSION(robotUnit);
 
        //        // Type: ::armarx::HandUnitInterfacePrx
        //        //        auto handUnit = robotUnit->getUnitPrx<::armarx::HandUnitInterface>();
        //        auto handUnit = robotUnit->getUnit<::armarx::HandUnitInterface>();
        //        auto jointValues = handUnit->getCurrentJointValues();
        //        for (auto& pair : jointValues)
        //        {
        //            ARMARX_INFO << " --- joint: " << pair.first << ": " << pair.second;
        //        }
        //        //        handUnit->setJointAngles();
        //        // Hand_L_EEF_NJointKITHandV2ShapeController
        //        // Hand_R_EEF_NJointKITHandV2ShapeController
        //        // NJointKITHandV2ShapeController
 
 
        ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
        ARMARX_CHECK_EXPRESSION(cfg);
        userConfig.fromAron(cfg->config);
 
        useSynchronizedRtRobot();
        nonRtRobot = robotUnit->cloneRobot();
        robotUnit->updateVirtualRobot(nonRtRobot);
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::createLimbs()
    {
        for (auto& pair : userConfig.limbs)
        {
            limb.emplace(pair.first, std::make_unique<ArmData>());
            limbInit(pair.first, limb.at(pair.first), pair.second, nonRtRobot);
            controllableNodeSets.emplace(pair.first, nonRtRobot->getRobotNodeSet(pair.first));
        }
 
        if (not userConfig.hands.empty())
        {
            createHands();
        }
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::createHands()
    {
 
        hands = std::make_shared<core::HandControlBase>(robotUnit, nonRtRobot, userConfig.hands);
        for (auto& [name, hand_cfg] : userConfig.hands)
        {
            ARMARX_INFO << "-- Creating hand: " << hands->hands.at(name)->kinematicChainName;
            controllableNodeSets.emplace(name, nonRtRobot->getRobotNodeSet(name));
        }
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::onInitNJointController()
    {
        std::string taskName = getClassName() + "AdditionalTask";
        runTask(taskName,
                [&]
                {
                    armarx::control::ethercat::RTUtility::pinThreadToCPU(
                        4); // please don't set this to 0 as it is the rtRun/control thread
                    armarx::control::ethercat::RTUtility::elevateThreadPriority(
                        armarx::control::ethercat::RTUtility::RT_THREAD_PRIORITY);
 
                    CycleUtil c(1);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    ARMARX_IMPORTANT << "Create a new thread for " << getClassName();
                    while (getState() == eManagedIceObjectStarted)
                    {
                        if (isControllerActive())
                        {
                            additionalTask();
                        }
                        c.waitForCycleDuration();
                    }
                });
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::limbNonRT(ArmPtr& arm)
    {
        arm->bufferConfigNonRtToRt.getWriteBuffer() = arm->nonRtConfig;
        arm->bufferConfigNonRtToRt.commitWrite();
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::additionalTask()
    {
        //        bool rtSafe = additionalTaskUpdateStatus();
        auto [rtTargetSafe, ftSafe] = additionalTaskUpdateStatus();
        additionalTaskSetTarget();
        if (not rtTargetSafe)
        {
            handleRTNotSafeInNonRT();
        }
    }
 
    template <typename NJointTaskspaceControllerType>
    std::tuple<bool, bool>
    NJointTaskspaceController<NJointTaskspaceControllerType>::additionalTaskUpdateStatus()
    {
        robotUnit->updateVirtualRobot(nonRtRobot);
        bool rtSafe = true;
        bool rtTargetSafe = true;
        bool forceTorqueSafe = true;
        for (auto& pair : limb)
        {
            auto& arm = pair.second;
            arm->nonRtConfig = arm->bufferConfigUserToNonRt.getUpToDateReadBuffer();
            arm->rtStatusInNonRT = arm->bufferRtStatusToNonRt.getUpToDateReadBuffer();
            rtSafe = rtSafe and pair.second->rtStatusInNonRT.rtSafe;
            rtTargetSafe = rtTargetSafe and pair.second->rtStatusInNonRT.rtTargetSafe;
            forceTorqueSafe = forceTorqueSafe and pair.second->rtStatusInNonRT.forceTorqueSafe;
        }
        if (hands)
        {
            hands->nonRTUpdateStatus();
        }
        return std::make_tuple(rtTargetSafe, forceTorqueSafe);
    }
 
    template <typename NJointTaskspaceControllerType>
    bool
    NJointTaskspaceController<NJointTaskspaceControllerType>::rtGetDesiredPoseSafeStatus()
    {
        bool rtDesiredPoseSafe = true;
        for (auto& [_, arm] : limb)
        {
            rtDesiredPoseSafe = rtDesiredPoseSafe and arm->rtStatusInNonRT.rtTargetSafe;
        }
        return rtDesiredPoseSafe;
    }
 
    template <typename NJointTaskspaceControllerType>
    bool
    NJointTaskspaceController<
        NJointTaskspaceControllerType>::rtGetDesiredPoseSafeStatusOnActivation()
    {
        bool rtDesiredPoseSafe = true;
        for (auto& [_, arm] : limb)
        {
            rtDesiredPoseSafe = rtDesiredPoseSafe and arm->desiredPoseSafeOnActivation.load();
        }
        return rtDesiredPoseSafe;
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::additionalTaskSetTarget()
    {
        for (auto& [_, arm] : limb)
        {
            limbNonRT(arm);
        }
        if (hands)
        {
            hands->nonRTSetTarget();
        }
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::handleRTNotSafeInNonRT()
    {
        for (auto& pair : limb)
        {
            if (not pair.second->rtReady)
            {
                continue;
            }
 
            if (not pair.second->rtStatusInNonRT.rtTargetSafe)
            {
                common::reportPoseDeviationWarning(pair.first,
                                                   pair.second->rtStatusInNonRT.currentPose,
                                                   pair.second->rtStatusInNonRT.desiredPoseUnsafe,
                                                   "current pose",
                                                   "desired pose");
            }
        }
    }
 
    /// -------------------------------- Real time cotnrol -----------------------------------------
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::limbRTUpdateStatus(
        ArmPtr& arm,
        const double deltaT)
    {
        arm->rtConfig = arm->bufferConfigNonRtToRt.getUpToDateReadBuffer();
        arm->rtStatus.deltaT = deltaT;
        arm->rtStatus.accumulateTime += deltaT;
        arm->rtStatus.globalPose = rtGetRobot()->getGlobalPose();
 
        arm->controller.ftsensor.updateStatus(arm->rtConfig.ftConfig,
                                              arm->rtStatus.currentForceTorque,
                                              arm->rtStatus.deltaT,
                                              arm->rtFirstRun.load());
        arm->rtStatus.safeFTGuardOffset.template head<3>() =
            arm->controller.ftsensor.getSafeGuardForceOffset();
 
        arm->sensorDevices.updateJointValues(
            arm->rtStatus.jointPosition, arm->rtStatus.jointVelocity, arm->rtStatus.jointTorque);
        //        rtGetRobot()->getRobotNodeSet(arm->kinematicChainName)
 
 
        arm->bufferRtStatusToNonRt.getWriteBuffer() = arm->rtStatus;
        arm->bufferRtStatusToNonRt.commitWrite();
        arm->bufferRtStatusToUser.getWriteBuffer() = arm->rtStatus;
        arm->bufferRtStatusToUser.commitWrite();
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::limbRTSetTarget(
        ArmPtr& arm,
        const size_t nDoFTorque,
        const size_t nDoFVelocity,
        const Eigen::VectorXf& targetTorque,
        const Eigen::VectorXf& targetVelocity)
    {
        for (size_t i = 0; i < nDoFTorque; i++)
        {
 
            auto jointIdx = arm->rtStatus.jointIDTorqueMode[i];
            arm->targetsTorque.at(i)->torque = targetTorque(jointIdx);
            if (!arm->targetsTorque.at(i)->isValid())
            {
                arm->targetsTorque.at(i)->torque = 0;
            }
        }
        for (size_t i = 0; i < nDoFVelocity; i++)
        {
            auto jointIdx = arm->rtStatus.jointIDVelocityMode[i];
            arm->targetsVel.at(i)->velocity = targetVelocity(jointIdx);
            // if (!targets.at(i)->isValid() || fabs(targets.at(i)->velocity) > cfg->maxJointVelocity)
            if (!arm->targetsVel.at(i)->isValid())
            {
                arm->targetsVel.at(i)->velocity = 0;
            }
        }
        arm->bufferRtStatusToOnPublish.getWriteBuffer() = arm->rtStatus;
        arm->bufferRtStatusToOnPublish.commitWrite();
 
        arm->bufferConfigRtToOnPublish.getWriteBuffer() = arm->rtConfig;
        arm->bufferConfigRtToOnPublish.commitWrite();
 
        arm->bufferConfigRtToUser.getWriteBuffer() = arm->rtConfig;
        arm->bufferConfigRtToUser.commitWrite();
 
        if (arm->rtFirstRun.load())
        {
            arm->rtFirstRun.store(false);
            arm->rtReady.store(true);
        }
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::limbRT(ArmPtr& arm,
                                                                     const double /*deltaT*/)
    {
        double time_measure = IceUtil::Time::now().toMicroSecondsDouble();
        // limbRTUpdateStatus(arm, deltaT);
        // double time_update_status = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
 
        arm->controller.run(arm->rtConfig, arm->rtStatus);
        double time_run_rt = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
 
        limbRTSetTarget(arm,
                        arm->rtStatus.nDoFTorque,
                        arm->rtStatus.nDoFVelocity,
                        arm->rtStatus.desiredJointTorque,
                        arm->rtStatus.desiredJointVelocity);
 
        double time_set_target = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
        time_measure = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
 
        if (time_measure > 200)
        {
            ARMARX_RT_LOGF_WARN("---- rt too slow: "
                                "run_rt_limb: %.2f\n"
                                "set_target_limb: %.2f\n"
                                "time all: %.2f\n",
                                time_run_rt - time_measure, //
                                time_set_target - time_run_rt, //
                                time_measure)
                .deactivateSpam(1.0f); // 0-1 us
        }
    }
 
    /// some method for coordinator
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::updateInputData(
        const std::string& key,
        const Eigen::Matrix4f& targetPose,
        const PoseFrameMode& targetPoseMode,
        const Eigen::Matrix4f& pose,
        const Eigen::Vector6f& vel,
        const Eigen::Vector6f& ft,
        const Eigen::Vector6f& stiffness)
    {
        if (coordinatorInputData.find(key) != coordinatorInputData.end())
        {
            // Update existing InputData
            auto& data = coordinatorInputData.at(key);
            data.pose = pose;
            data.targetPose = targetPose;
            data.targetPoseMode = targetPoseMode;
            data.vel = vel;
            data.ft = ft;
            data.stiffness = stiffness;
        }
        else
        {
            coordinatorInputData.emplace(key,
                                         common::coordination::InputData(
                                             targetPose, targetPoseMode, pose, vel, ft, stiffness));
        }
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::rtRunCoordinator(double deltaT)
    {
        if (not coordinatorEnabled.load() or not coordinator)
        {
            return;
        }
 
        bool rtReady = false;
        for (const auto& nodeset : coordinator->getNodesetList())
        {
            auto& rts = limb.at(nodeset)->rtStatus;
            auto& rtc = limb.at(nodeset)->rtConfig;
            rtReady = limb.at(nodeset)->rtReady.load();
            /// TODO, fix this for vel controller, as it does not have kpImpedance
            if constexpr (NJointTaskspaceControllerType::IsCompliant)
            {
                updateInputData(nodeset,
                                rtc.desiredPose,
                                rtc.desiredPoseFrameMode,
                                rts.currentPose,
                                rts.currentTwist,
                                rts.currentForceTorque,
                                rtc.kpImpedance);
            }
            else
            {
                ARMARX_WARNING << "Not implemented yet for controller types without kpImpedance";
                return;
            }
        }
        if (not rtReady)
        {
            return;
        }
        coordinator->runRT(coordinatorInputData, deltaT);
        for (const auto& nodeset : coordinator->getNodesetList())
        {
            limb.at(nodeset)->rtConfig.desiredPose = coordinatorInputData.at(nodeset).targetPose;
            limb.at(nodeset)->rtConfig.desiredPoseFrameMode = PoseFrameMode::root;
        }
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::rtRun(
        const IceUtil::Time& /*sensorValuesTimestamp*/,
        const IceUtil::Time& timeSinceLastIteration)
    {
        double deltaT = timeSinceLastIteration.toSecondsDouble();
        for (auto& pair : limb)
        {
            limbRTUpdateStatus(pair.second, deltaT);
        }
 
        rtRunCoordinator(deltaT);
 
        for (auto& pair : limb)
        {
            limbRT(pair.second, deltaT);
        }
        if (hands)
        {
            hands->updateRTStatus(deltaT);
            //            hands->setTargets();
        }
    }
 
    /// ------------------------------ update/get config -------------------------------------------
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::updateConfig(
        const ::armarx::aron::data::dto::DictPtr& dto,
        const Ice::Current& /*iceCurrent*/)
    {
        auto prevCfg = userConfig;
        userConfig.fromAron(dto);
 
        for (auto& pair : userConfig.limbs)
        {
            auto& arm = limb.at(pair.first);
            validateConfigData(pair.second, arm);
            bool desiredPoseFrameModeSame = prevCfg.limbs.at(pair.first).desiredPoseFrameMode ==
                                            pair.second.desiredPoseFrameMode;
            if (desiredPoseFrameModeSame and common::detectAndReportPoseDeviationWarning(
                                                 pair.second.desiredPose,
                                                 prevCfg.limbs.at(pair.first).desiredPose,
                                                 "new desired pose",
                                                 "previous desired pose",
                                                 pair.second.safeDistanceMMToGoal,
                                                 pair.second.safeRotAngleDegreeToGoal,
                                                 "updateConfig_" + pair.first))
            {
                ARMARX_INFO << deactivateSpam(2) << "-- use the existing target pose.";
                pair.second.desiredPose = prevCfg.limbs.at(pair.first).desiredPose;
            }
            arm->bufferConfigUserToNonRt.getWriteBuffer() = pair.second;
            if (not isControllerActive())
            {
                arm->nonRtConfig = arm->bufferConfigUserToNonRt.getWriteBuffer();
            }
            arm->bufferConfigUserToNonRt.commitWrite();
 
            /// When user updates the controller config before the controller is activated, we need
            /// to copy the userconfig to rt data struct directly, as upon activation, the rtConfig
            /// will be validated and used to reinitialize all buffers.
            if (not isControllerActive())
            {
                arm->rtConfig = arm->nonRtConfig;
            }
        }
 
        if (hands)
        {
            hands->updateConfig(userConfig.hands);
        }
        else
        {
            if (not userConfig.hands.empty())
            {
                createHands();
            }
        }
    }
 
    template <typename NJointTaskspaceControllerType>
    ::armarx::aron::data::dto::DictPtr
    NJointTaskspaceController<NJointTaskspaceControllerType>::getConfig(
        const Ice::Current& /*iceCurrent*/)
    {
        for (auto& pair : limb)
        {
            userConfig.limbs.at(pair.first) =
                pair.second->bufferConfigRtToUser.getUpToDateReadBuffer();
        }
        if (hands)
        {
            hands->getConfig(userConfig.hands);
        }
        return userConfig.toAronDTO();
    }
 
    template <typename NJointTaskspaceControllerType>
    ::armarx::aron::data::dto::DictPtr
    NJointTaskspaceController<NJointTaskspaceControllerType>::getRTStatus(
        const Ice::Current& /*iceCurrent*/)
    {
        for (auto& pair : limb)
        {
            userConfig.limbs.at(pair.first) =
                pair.second->bufferConfigRtToUser.getUpToDateReadBuffer();
        }
        if (hands)
        {
            hands->getConfig(userConfig.hands);
        }
        return userConfig.toAronDTO();
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::enableSafeGuardForceTorque(
        const std::string& nodeSetName,
        const bool forceGuard,
        const bool torqueGuard,
        const Ice::Current& /*iceCurrent*/)
    {
        auto it = userConfig.limbs.find(nodeSetName);
        if (it != userConfig.limbs.end())
        {
            it->second.ftConfig.enableSafeGuardForce = forceGuard;
            it->second.ftConfig.enableSafeGuardTorque = torqueGuard;
            limb.at(nodeSetName)->controller.ftsensor.enableSafeGuard(forceGuard, torqueGuard);
            ARMARX_IMPORTANT
                << "reset safe guard with force torque sensors: safe? "
                << limb.at(nodeSetName)->controller.ftsensor.isSafe(it->second.ftConfig);
            limb.at(nodeSetName)->bufferConfigUserToNonRt.getWriteBuffer() = it->second;
            limb.at(nodeSetName)->bufferConfigUserToNonRt.commitWrite();
        }
        else
        {
            ARMARX_WARNING << "no robot node set with name " << nodeSetName
                           << " found in the controllers.";
        }
    }
 
    /// -------------------------------- Other interaces -------------------------------------------
    template <typename NJointTaskspaceControllerType>
    bool
    NJointTaskspaceController<NJointTaskspaceControllerType>::isSafeForceTorque(
        const std::string& nodeSetName,
        const Ice::Current& /*iceCurrent*/)
    {
        auto it = userConfig.limbs.find(nodeSetName);
        if (it != userConfig.limbs.end())
        {
            return limb.at(nodeSetName)->controller.ftsensor.ftSafe.load();
        }
 
        ARMARX_WARNING << "no robot node set with name " << nodeSetName
                       << " found in the controllers.";
        return false;
    }
 
    template <typename NJointTaskspaceControllerType>
    bool
    NJointTaskspaceController<NJointTaskspaceControllerType>::wasNotSafeForceTorque(
        const std::string& nodeSetName,
        const Ice::Current& /*iceCurrent*/)
    {
        auto it = userConfig.limbs.find(nodeSetName);
        if (it != userConfig.limbs.end())
        {
            return limb.at(nodeSetName)->controller.ftsensor.ftWasNotSafe.load();
        }
 
        ARMARX_WARNING << "no robot node set with name " << nodeSetName
                       << " found in the controllers.";
        return false;
    }
 
    template <typename NJointTaskspaceControllerType>
    Ice::FloatSeq
    NJointTaskspaceController<NJointTaskspaceControllerType>::getTCPVel(
        const std::string& rns,
        const Ice::Current& /*iceCurrent*/)
    {
        std::vector<float> tcpVel;
        auto& arm = limb.at(rns);
        auto s = arm->bufferRtStatusToUser.getUpToDateReadBuffer();
        tcpVel.reserve(s.currentTwist.size());
        for (int i = 0; i < s.currentTwist.size(); i++)
        {
            tcpVel.push_back(s.currentTwist[i]);
        }
        return tcpVel;
    }
 
    template <typename NJointTaskspaceControllerType>
    Ice::DoubleSeq
    NJointTaskspaceController<NJointTaskspaceControllerType>::getCurrentTCPPose(
        const std::string& nodeSetName,
        const Ice::Current& /*iceCurrent*/)
    {
        auto search = limb.find(nodeSetName);
        if (search == limb.end())
        {
            ARMARX_WARNING << "requested node set " << nodeSetName
                           << "does not exist in the controller";
            return std::vector<double>{};
        }
 
        auto& arm = limb.at(nodeSetName);
        auto currentPose = arm->bufferRtStatusToOnPublish.getUpToDateReadBuffer().currentPose;
        ARMARX_DEBUG << "returning current pose" << VAROUT(currentPose);
        return common::mat4ToDVec(currentPose);
    }
 
    template <typename NJointTaskspaceControllerType>
    bool
    NJointTaskspaceController<NJointTaskspaceControllerType>::updateTargetPose(
        const TargetPoseMap& targetPoseMap,
        const TargetNullspaceMap& targetNullspaceMap,
        const Ice::Current& /*iceCurrent*/)
    {
        for (auto& pair : userConfig.limbs)
        {
            for (size_t i = 0; i < 4; ++i)
            {
                for (int j = 0; j < 4; ++j)
                {
                    pair.second.desiredPose(i, j) = targetPoseMap.at(pair.first)[i][j];
                }
            }
            if (targetNullspaceMap.at(pair.first).size() > 0)
            {
                const size_t nDoF = pair.second.desiredNullspaceJointAngles.value().size();
                ARMARX_CHECK_EQUAL(targetNullspaceMap.at(pair.first).size(), nDoF)
                    << "the joint numbers does not match";
                for (size_t i = 0; i < nDoF; ++i)
                {
                    pair.second.desiredNullspaceJointAngles.value()(i) =
                        targetNullspaceMap.at(pair.first)[i];
                }
            }
            limb.at(pair.first)->bufferConfigUserToNonRt.getWriteBuffer() = pair.second;
            limb.at(pair.first)->bufferConfigUserToNonRt.commitWrite();
        }
        return true;
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::validateConfigData(Config& configData,
                                                                                 ArmPtr& arm)
    {
        const auto nDoF = arm->jointNames.size();
        if (!configData.desiredNullspaceJointAngles.has_value())
        {
            if (!isControllerActive())
            {
                ARMARX_INFO << "-- No user defined nullspace target, reset to zero with "
                            << VAROUT(nDoF);
                configData.desiredNullspaceJointAngles = Eigen::VectorXf::Zero(nDoF);
                arm->reInitPreActivate.store(true);
            }
            else
            {
                auto currentValue =
                    arm->bufferConfigRtToUser.getUpToDateReadBuffer().desiredNullspaceJointAngles;
                ARMARX_VERBOSE
                    << "Controller is active, but no user defined nullspace target. \n"
                       "You should first get up-to-date config of this controller and then "
                       "update "
                       "it:\n"
                       "    auto cfg = ctrl->getConfig(); \n"
                       "    cfg.desiredPose = xxx;\n"
                       "    ctrl->updateConfig(cfg);\n"
                       "Now, I decide by myself to use the existing values of nullspace "
                       "target\n"
                    << currentValue.value();
                configData.desiredNullspaceJointAngles = currentValue;
            }
        }
        ARMARX_CHECK_GREATER(configData.safeDistanceMMToGoal, 100.0f);
        validateConfigDataCheckSize(configData, arm);
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::limbPublish(
        ArmPtr& arm,
        const DebugObserverInterfacePrx& debugObs)
    {
        StringVariantBaseMap datafields;
        auto rtStatus = arm->bufferRtStatusToOnPublish.getUpToDateReadBuffer();
        auto cfgNonRt = arm->bufferConfigNonRtToPublish.getUpToDateReadBuffer();
 
        limbPublishConfig(datafields, arm);
 
        datafields["nDoFTorque"] = new Variant(rtStatus.nDoFTorque);
        datafields["nDoFVelocity"] = new Variant(rtStatus.nDoFVelocity);
 
        common::debugEigenVec(datafields, "jointPosition", rtStatus.jointPosition);
        common::debugEigenVec(datafields, "jointVelocity", rtStatus.jointVelocity);
        common::debugEigenVec(datafields, "qvelFiltered", rtStatus.qvelFiltered);
 
        common::debugEigenPose(datafields, "current_pose", rtStatus.currentPose);
        common::debugEigenPose(datafields, "desired_pose", rtStatus.desiredPose);
        common::debugEigenPose(datafields, "desired_pose_user", cfgNonRt.desiredPose);
 
        datafields["cfg.velocity_limit"] = new Variant(cfgNonRt.velocityLimit);
        datafields["cfg.torque_limit"] = new Variant(cfgNonRt.torqueLimit);
        datafields["ft.safeGuardForceThreshold"] =
            new Variant(cfgNonRt.ftConfig.safeGuardForceThreshold);
        datafields["ft.safeGuardTorqueThreshold"] =
            new Variant(cfgNonRt.ftConfig.safeGuardTorqueThreshold);
 
 
        float positionError =
            (common::getPos(rtStatus.currentPose) - common::getPos(rtStatus.desiredPose)).norm();
        float angleError =
            common::getDeltaAngleBetweenPose(rtStatus.currentPose, rtStatus.desiredPose);
        datafields["poseError_position"] = new Variant(positionError);
        datafields["poseError_angle"] = new Variant(angleError);
 
        common::debugEigenVec(datafields, "current_twist", rtStatus.currentTwist);
        common::debugEigenVec(datafields, "poseErrorImp", rtStatus.poseErrorImp);
 
        common::debugEigenVec(datafields, "currentForceTorque", rtStatus.currentForceTorque);
        common::debugEigenVec(datafields, "safeFTGuardOffset", rtStatus.safeFTGuardOffset);
 
        /// admittance interface
        common::debugEigenMat(datafields, "inertia", rtStatus.inertia);
        common::debugEigenMat(datafields, "inertiaVelCtrlJoints", rtStatus.inertiaVelCtrlJoints);
        common::debugEigenMat(
            datafields, "inertiaInvVelCtrlJoints", rtStatus.inertiaInvVelCtrlJoints);
        common::debugEigenVec(datafields, "qdVel", rtStatus.qdVel);
        common::debugEigenVec(datafields, "qdAcc", rtStatus.qdAcc);
        common::debugEigenVec(datafields, "qTemp", rtStatus.qTemp);
 
        float currentForceNorm = rtStatus.currentForceTorque.template head<3>().norm();
        float currentTorqueNorm = rtStatus.currentForceTorque.template tail<3>().norm();
        float safeForceGuardOffsetNorm = rtStatus.safeFTGuardOffset.template head<3>().norm();
        float safeTorqueGuardOffsetNorm = rtStatus.safeFTGuardOffset.template tail<3>().norm();
        datafields["currentForceNorm"] = new Variant(currentForceNorm);
        datafields["currentTorqueNorm"] = new Variant(currentTorqueNorm);
        datafields["safeForceGuardOffsetNorm"] = new Variant(safeForceGuardOffsetNorm);
        datafields["safeTorqueGuardOffsetNorm"] = new Variant(safeTorqueGuardOffsetNorm);
        datafields["safeForceGuardDifference"] =
            new Variant(currentForceNorm - safeForceGuardOffsetNorm);
        datafields["safeTorqueGuardDifference"] =
            new Variant(currentTorqueNorm - safeTorqueGuardOffsetNorm);
 
        datafields["rtSafe"] = new Variant(rtStatus.rtSafe * 1.0);
        datafields["rtTargetSafe"] = new Variant(rtStatus.rtTargetSafe * 1.0);
        bool forceTrigger = (not rtStatus.rtSafe) and rtStatus.rtTargetSafe;
        datafields["forceTrigger"] = new Variant(forceTrigger * 1.0);
        datafields["forceTorqueSafe"] = new Variant(rtStatus.forceTorqueSafe * 1.0);
 
        common::debugEigenVec(datafields, "forceImpedance", rtStatus.forceImpedance);
        common::debugEigenVec(datafields, "cartesianVelTarget", rtStatus.cartesianVelTarget);
 
        common::debugEigenVec(datafields, "desiredJointTorques", rtStatus.desiredJointTorque);
        common::debugEigenVec(datafields, "desiredJointVelocity", rtStatus.desiredJointVelocity);
        common::debugEigenVec(datafields, "nullspaceTorque", rtStatus.nullspaceTorque);
        common::debugEigenVec(datafields, "nullspaceVelocity", rtStatus.nullspaceVelocity);
        debugObs->setDebugChannel(getClassName() + "_" + arm->kinematicChainName, datafields);
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::onPublish(
        const SensorAndControl& /*unused*/,
        const DebugDrawerInterfacePrx& /*unused*/,
        const DebugObserverInterfacePrx& debugObs)
    {
        for (auto& pair : limb)
        {
            if (not pair.second->rtReady.load())
            {
                continue;
            }
            limbPublish(pair.second, debugObs);
        }
        if (hands)
        {
            hands->onPublish(debugObs);
        }
        onPublishCoordinator(debugObs);
 
        if (++skipStepCountArviz_ >= skipStepsArviz_)
        {
            skipStepCountArviz_ = 0;
            commitArviz();
        }
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::onPublishCoordinator(
        const DebugObserverInterfacePrx& debugObs)
    {
        if (!coordinator)
        {
            return;
        }
        StringVariantBaseMap datafields;
        auto& cfg = coordinator->bufferUserCfgToPublish.getUpToDateReadBuffer();
        auto data = coordinator->bufferDataRtToPublish.getUpToDateReadBuffer();
        common::debugEigenPose(datafields, "current_pose", data.currentPose);
        common::debugEigenPose(datafields, "virtualPose", data.virtualPose);
        common::debugEigenPose(datafields, "targetPose", data.targetPose);
        common::debugEigenVec(datafields, "currentFT", data.currentFT);
        common::debugEigenVec(datafields, "pose_error", data.poseError);
        common::debugEigenVec(datafields, "tempAcc", data.tempAcc);
        common::debugEigenVec(datafields, "virtualVel", data.virtualVel);
        common::debugEigenVec(datafields, "virtualAcc", data.virtualAcc);
        //        ARMARX_INFO << VAROUT(data.obj2TcpInMeterL) << VAROUT(data.obj2TcpInMeterL);
 
        using namespace armarx::control::common::coordination::arondto;
        auto followerIsolation = cfg.followerConnectionMode == ConnectionMode::follower_isolated;
        auto leftAsLeader = cfg.leadershipMode == LeadershipMode::left_as_leader;
        auto rightAsLeader = cfg.leadershipMode == LeadershipMode::right_as_leader;
        auto followerInLeaderLocalFrame =
            cfg.followerFrameMode == common::arondto::PoseFrameMode::leader;
        datafields["followerIsolation"] = new Variant(followerIsolation);
        datafields["leftAsLeader"] = new Variant(leftAsLeader);
        datafields["rightAsLeader"] = new Variant(rightAsLeader);
        datafields["followerInLeaderLocalFrame"] = new Variant(followerInLeaderLocalFrame);
 
        debugObs->setDebugChannel("SyncModeCoordinator", datafields);
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::commitArviz()
    {
        if (scopedArviz.has_value())
        {
            viz::StagedCommit stage = scopedArviz->stage();
            if (resetArviz)
            {
                resetArviz = false;
                scopedArviz->clear();
                storedArvizLayers.clear();
            }
            collectArviz(stage);
            scopedArviz->commit(stage);
        }
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::collectArviz(
        viz::StagedCommit& stage) const
    {
        if (not scopedArviz.has_value())
        {
            return;
        }
 
        const double arrowWidth = 2.5F;
        if (coordinator)
        {
            const auto& rtStatus = coordinator->bufferDataRtToPublish.getUpToDateReadBuffer();
            const Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity(); // FIXME
            {
                viz::Layer layer = scopedArviz->layer("Pose_coordinator");
                layer.add(
                    viz::Pose("targetPose").pose(globalPose * rtStatus.targetPose).scale(1.5f));
                layer.add(
                    viz::Pose("virtualPose").pose(globalPose * rtStatus.virtualPose).scale(1.25f));
                layer.add(
                    viz::Pose("currentPose").pose(globalPose * rtStatus.currentPose).scale(1.0f));
                stage.add(layer);
            }
 
            const auto objCenter = common::getPos(globalPose * rtStatus.currentPose);
            {
                viz::Layer layer = scopedArviz->layer("Status_coordinator");
                layer.add(
                    viz::Arrow("object_vel")
                        .fromTo(objCenter, objCenter + 1000.0f * rtStatus.virtualVel.head<3>())
                        .color(simox::Color::blue())
                        .width(arrowWidth));
                layer.add(viz::Arrow("object_pose_error")
                              .fromTo(objCenter, objCenter + 1000.0f * rtStatus.poseError.head<3>())
                              .color(simox::Color::black())
                              .width(arrowWidth));
                stage.add(layer);
            }
            {
                viz::Layer layer = scopedArviz->layer("FT_coordinator");
                layer.add(viz::Arrow("object_force")
                              .fromTo(objCenter, objCenter + 100.0f * rtStatus.currentFT.head<3>())
                              .color(simox::Color::yellow())
                              .width(arrowWidth));
                layer.add(viz::Arrow("object_torque")
                              .fromTo(objCenter, objCenter + 100.0f * rtStatus.currentFT.tail<3>())
                              .color(simox::Color::green())
                              .width(arrowWidth));
                stage.add(layer);
            }
        }
        else
        {
            for (const auto& [_, arm] : limb)
            {
                const auto& cfgNonRt = arm->bufferConfigNonRtToPublish.getUpToDateReadBuffer();
                const auto& rtStatus = arm->bufferRtStatusToOnPublish.getUpToDateReadBuffer();
 
                const Eigen::Matrix4f& globalPose = rtStatus.globalPose;
                auto addPose = [&](viz::Layer& layer, const std::string& suffix = "")
                {
                    layer.add(viz::Pose("targetPose" + suffix)
                                  .pose(globalPose * rtStatus.desiredPose)
                                  .scale(1.5f));
                    layer.add(viz::Pose("currentPose" + suffix)
                                  .pose(globalPose * rtStatus.currentPose)
                                  .scale(1.0f));
 
                    const float positionError = (common::getPos(rtStatus.currentPose) -
                                                 common::getPos(rtStatus.desiredPose))
                                                    .norm();
                    const float positionErrorNormalized =
                        std::fmin((positionError / cfgNonRt.safeDistanceMMToGoal), 1.);
                    layer.add(viz::Arrow("positionError" + suffix)
                                  .fromTo(common::getPos(globalPose * rtStatus.currentPose),
                                          common::getPos(globalPose * rtStatus.desiredPose))
                                  .color(simox::Color(positionErrorNormalized,
                                                      1.0f - positionErrorNormalized,
                                                      0.0f))
                                  .width(arrowWidth));
                };
 
                {
                    viz::Layer layer = scopedArviz->layer("Pose_" + arm->kinematicChainName);
                    addPose(layer);
                    stage.add(layer);
                }
                const bool enableTrajVisualization = false;
                if (enableTrajVisualization)
                {
                    const std::string name = "Traj_" + arm->kinematicChainName;
                    if (storedArvizLayers.count(name) == 0)
                    {
                        storedArvizLayers[name] = scopedArviz->layer(name);
                    }
                    viz::Layer& layer = storedArvizLayers.at(name);
                    int index = layer.size() / 3;
                    addPose(layer, std::to_string(index));
                    stage.add(layer);
                }
                {
                    viz::Layer layer = scopedArviz->layer("Status_" + arm->kinematicChainName);
                    layer.add(viz::Arrow("tsImpForce")
                                  .fromTo(common::getPos(globalPose * rtStatus.currentPose),
                                          common::getPos(globalPose * rtStatus.currentPose) +
                                              rtStatus.forceImpedance.template head<3>() * 5.0)
                                  .color(simox::Color::cyan())
                                  .width(arrowWidth));
                    stage.add(layer);
                }
                // {
                //     viz::Layer layer = scopedArviz->layer("FT_" + arm->kinematicChainName);
                //     const auto curentPos = common::getPos(globalPose * rtStatus.currentPose);
                //     layer.add(
                //         viz::Arrow("TCP force")
                //             .fromTo(curentPos,
                //                     curentPos +
                //                         100.0f * rtStatus.currentForceTorque.template head<3>())
                //             .color(simox::Color::yellow())
                //             .width(arrowWidth));
                //     layer.add(
                //         viz::Arrow("TCP torque")
                //             .fromTo(curentPos,
                //                     curentPos +
                //                         100.0f * rtStatus.currentForceTorque.template tail<3>())
                //             .color(simox::Color::green())
                //             .width(arrowWidth));
                //     stage.add(layer);
                // }
            }
        }
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::calibrateFTSensor(const Ice::Current&)
    {
        for (auto& pair : limb)
        {
            pair.second->rtReady.store(false);
        }
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::useCoordinator(
        const std::string& type,
        const ::armarx::aron::data::dto::DictPtr& dto,
        const Ice::Current& /*unused*/)
    {
        ARMARX_INFO << "Adding coordinator of type " << type;
        /// TODO think about other types of coordinators
        if (!coordinator)
        {
            /// TODO add check if the current controller has all robotNodeSets
            ///  that the coordinator requires. currently, use check below.
            coordinator = std::make_shared<common::coordination::SyncCoordination>(dto);
        }
        else
        {
            coordinator->updateConfig(dto);
        }
 
        for (const auto& nodeset : coordinator->getNodesetList())
        {
            if (limb.find(nodeset) == limb.end())
            {
                coordinatorEnabled.store(false);
                ARMARX_WARNING << "The requested nodeset by coordinator " << nodeset
                               << " does not exist in the current controller, disable coordinator";
                return;
            }
        }
 
        //        for (auto& pair : limb)
        //        {
        //          enableSafeGuardForceTorque(pair.first, false, false);
        //            //            pair.second->controller.setForceTorqueGuard(false, false);
        //            pair.second->controller.ftsensor.enableSafeGuard(false, false);
        //        }
        for (const auto& nodeset : coordinator->getNodesetList())
        {
            enableSafeGuardForceTorque(nodeset, false, false);
            //            pair.second->controller.setForceTorqueGuard(false, false);
            //            pair.second->controller.ftsensor.enableSafeGuard(false, false);
        }
        coordinatorEnabled.store(true);
        // std::map<std::string, Eigen::Matrix4f> initPose;
        // for (const auto& nodeset: coord->getNodesetList())
        // {
        //     auto buff = limb.at(nodeset)->bufferConfigRtToUser.getUpToDateReadBuffer();
        //     initPose[nodeset] = buff.desiredPose;
        //     ARMARX_INFO << "using node set " << nodeset << " with pose " << buff.desiredPose;
        // }
        // coord->reset(initPose);
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::disableCoordinator(
        const Ice::Current& /*unused*/)
    {
        ARMARX_INFO << "Disabling coordinator!";
        if (not coordinator)
        {
            ARMARX_INFO << "Coordinator is not created yet, use `useCoordinator` to create!!";
            return;
        }
        for (const auto& nodeset : coordinator->getNodesetList())
        {
            auto& arm = limb.at(nodeset);
            auto& cfgRT = arm->bufferConfigRtToUser.getUpToDateReadBuffer();
            arm->bufferConfigUserToNonRt.getWriteBuffer().desiredPose = cfgRT.desiredPose;
            arm->bufferConfigUserToNonRt.commitWrite();
            usleep(2000);
        }
        coordinatorEnabled.store(false);
        ARMARX_VERBOSE << "Coordinator is disabled!!";
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::limbReInit(ArmPtr& arm)
    {
        /// Note: this function will always be called once ctrl_proxy->activateController() is called.
        /// For situations where heavy objects are hold in hands in torque control mode, user has
        /// to check if this controller is already activated, and only call acitvateContgroller
        /// when it is not active.
        /// Or you can use the controllerBuilder wrapper, which handles this automatically
        ARMARX_RT_LOGF_INFO("--> rtPreActivateController for %s", arm->kinematicChainName.c_str());
        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(arm->kinematicChainName);
        Eigen::Matrix4f currentPose = rns->getTCP()->getPoseInRootFrame();
        auto quat = VirtualRobot::MathTools::eigen4f2quat(currentPose);
        ARMARX_RT_LOGF_INFO("-- rt preactivate controller with target pose\n\n "
                            "[ %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f ].",
                            currentPose(0, 3),
                            currentPose(1, 3),
                            currentPose(2, 3),
                            quat.w,
                            quat.x,
                            quat.y,
                            quat.z);
 
        arm->desiredPoseSafeOnActivation =
            not common::poseDeviationTooLarge(currentPose,
                                              arm->rtConfig.desiredPose,
                                              arm->rtConfig.safeDistanceMMToGoal,
                                              arm->rtConfig.safeRotAngleDegreeToGoal);
 
        /// Is there a case where using current pose to initialize the RT desired pose is not suitable?
        if (arm->reInitPreActivate.load())
        {
            //            arm->nonRtConfig.desiredNullspaceJointAngles = rns->getJointValuesEigen();
            //            arm->nonRtConfig.desiredPose = currentPose;
 
            //            arm->bufferConfigUserToNonRt.reinitAllBuffers(arm->nonRtConfig);
 
            arm->rtConfig.desiredNullspaceJointAngles = rns->getJointValuesEigen();
 
            arm->reInitPreActivate.store(false);
        }
        {
            //            arm->rtConfig = arm->nonRtConfig;
            arm->rtConfig.desiredPose = currentPose;
            arm->nonRtConfig = arm->rtConfig;
 
            arm->bufferConfigUserToNonRt.reinitAllBuffers(arm->rtConfig);
            arm->bufferConfigNonRtToRt.reinitAllBuffers(arm->rtConfig);
            arm->bufferConfigRtToOnPublish.reinitAllBuffers(arm->rtConfig);
            arm->bufferConfigNonRtToPublish.reinitAllBuffers(arm->rtConfig);
            arm->bufferConfigRtToUser.reinitAllBuffers(arm->rtConfig);
        }
        {
            arm->sensorDevices.updateJointValues(arm->rtStatus.jointPosition,
                                                 arm->rtStatus.jointVelocity,
                                                 arm->rtStatus.jointTorque);
            arm->rtStatus.rtPreActivate(currentPose);
            arm->rtStatus.globalPose = rtGetRobot()->getGlobalPose();
            arm->rtStatusInNonRT = arm->rtStatus;
            arm->nonRTDeltaT = 0.0;
            arm->nonRTAccumulateTime = 0.0;
            arm->bufferRtStatusToOnPublish.reinitAllBuffers(arm->rtStatus);
            arm->bufferRtStatusToNonRt.reinitAllBuffers(arm->rtStatus);
            arm->bufferRtStatusToUser.reinitAllBuffers(arm->rtStatus);
        }
 
        // arm->controller.preactivateInit(rns);
        ARMARX_RT_LOGF_INFO("<-- done.");
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::activateController(
        const Ice::Current& current)
    {
        ARMARX_INFO
            << "NJointTaskspaceController<NJointTaskspaceControllerType>::activateController";
 
        if (not scopedArviz.has_value())
        {
            scopedArviz.emplace(createArVizClient());
            scopedArviz->updateComponentName("NJointTaskspaceController");
        }
        resetArviz = true;
 
        if (hands)
        {
            ARMARX_VERBOSE << "Calling hands->nonRtActivateController()";
            hands->nonRtActivateController();
        }
        NJointControllerBase::activateController(current);
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::deactivateController(
        const Ice::Current& current)
    {
        ARMARX_INFO
            << "NJointTaskspaceController<NJointTaskspaceControllerType>::deactivateController";
 
        NJointControllerBase::deactivateController(current);
 
        resetArviz = true;
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::rtPreActivateController()
    {
        ARMARX_RT_LOGF_IMPORTANT("--> rtPreActivateController");
        for (auto& pair : limb)
        {
 
            limbReInit(pair.second);
            userConfig.limbs.at(pair.first) = pair.second->rtConfig;
        }
        if (hands)
        {
            hands->rtPreActivate();
        }
        ARMARX_RT_LOGF_IMPORTANT("<-- Done");
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::rtPostDeactivateController()
    {
        for (auto& pair : userConfig.limbs)
        {
            limb.at(pair.first)->rtPostDeactivate();
        }
        if (hands)
        {
            hands->rtPostDeactivate(userConfig.hands);
        }
 
        /// @Andre Meixner: introduced the following line, which contradicts the use case where you
        /// disabled the controller during a coordination mode and reactivtion will not automatically
        /// activate the coordination mode again.
        coordinatorEnabled.store(false);
    }
 
    template <typename NJointTaskspaceControllerType>
    void
    NJointTaskspaceController<NJointTaskspaceControllerType>::ArmData::rtPostDeactivate()
    {
        nonRtConfig = rtConfig;
        bufferConfigUserToNonRt.reinitAllBuffers(rtConfig);
        bufferConfigNonRtToRt.reinitAllBuffers(rtConfig);
        bufferConfigRtToOnPublish.reinitAllBuffers(rtConfig);
        bufferConfigNonRtToPublish.reinitAllBuffers(rtConfig);
        bufferConfigRtToUser.reinitAllBuffers(rtConfig);
    }
 
    /// ----------------------------------- GUI Widget ---------------------------------------------
    template <typename NJointTaskspaceControllerType>
    WidgetDescription::WidgetPtr
    NJointTaskspaceController<NJointTaskspaceControllerType>::GenerateConfigDescription(
        const VirtualRobot::RobotPtr& /*robot*/,
        const std::map<std::string, ConstControlDevicePtr>& /*controlDevices*/,
        const std::map<std::string, ConstSensorDevicePtr>&)
    {
        using namespace armarx::WidgetDescription;
        HBoxLayoutPtr layout = new HBoxLayout;
 
 
        ::armarx::WidgetDescription::WidgetSeq widgets;
 
        /// select default config
        LabelPtr label = new Label;
        label->text = "select a controller config";
 
        StringComboBoxPtr cfgBox = new StringComboBox;
        cfgBox->name = "config_box";
        cfgBox->defaultIndex = 0;
        cfgBox->multiSelect = false;
 
        cfgBox->options = std::vector<std::string>{
            "default_a7_with_hands",
            "default_a7_left",
            "default_a7_right",
            "default_a7_zero_torque",
            "default_a6_left",
            "default_a6_with_hands",
            "default_a6_right",
            "default_a6_zero_torque",
        };
        ARMARX_TRACE;
 
        layout->children.emplace_back(label);
        layout->children.emplace_back(cfgBox);
        ARMARX_TRACE;
 
        layout->children.insert(layout->children.end(), widgets.begin(), widgets.end());
        ARMARX_VERBOSE_S << "Layout done";
        return layout;
    }
 
    template <typename NJointTaskspaceControllerType>
    typename NJointTaskspaceController<NJointTaskspaceControllerType>::ConfigPtrT
    NJointTaskspaceController<NJointTaskspaceControllerType>::GenerateConfigFromVariants(
        const StringVariantBaseMap& values)
    {
        auto cfgName = values.at("config_box")->getString();
        const armarx::PackagePath configPath(
            "armarx_control",
            "controller_config/" + std::string(NJointTaskspaceControllerType::TypeName) + "/" +
                cfgName + ".json");
        ARMARX_INFO_S << "Loading config from " << configPath.toSystemPath();
        ARMARX_CHECK(std::filesystem::exists(configPath.toSystemPath()));
 
        auto cfgDTO = armarx::readFromJson<ConfigDict>(configPath.toSystemPath());
 
        ARMARX_TRACE;
        return new ConfigurableNJointControllerConfig{cfgDTO.toAronDTO()};
    }
 
    /////////
    template class NJointTaskspaceController<law::TSMixImpVelController>;
 
    NJointControllerRegistration<NJointTSMixImpVelController>
        registrationControllerNJointTSMixImpVelController("TSMixImpVel");
 
    NJointTSMixImpVelController::NJointTSMixImpVelController(
        const RobotUnitPtr& robotUnit,
        const NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr& robot) :
        NJointTaskspaceController<law::TSMixImpVelController>(robotUnit, config, robot)
    {
        createLimbs();
    }
 
    std::string
    NJointTSMixImpVelController::getClassName(const Ice::Current&) const
    {
        return "TSMixImpVel";
    }
 
    void
    NJointTSMixImpVelController::validateConfigDataCheckSize(Config& configData, ArmPtr& arm)
    {
        const auto nDoF = static_cast<Eigen::Index>(arm->jointNames.size());
 
        const auto checkSize = [nDoF](const auto& v) { ARMARX_CHECK_EQUAL(v.rows(), nDoF); };
        const auto checkNonNegative = [](const auto& v) { ARMARX_CHECK((v.array() >= 0).all()); };
        checkSize(configData.desiredNullspaceJointAngles.value());
        checkSize(configData.kdNullspaceTorque);
        checkSize(configData.kpNullspaceTorque);
        checkSize(configData.kdNullspaceVel);
        checkSize(configData.kpNullspaceVel);
 
        checkNonNegative(configData.kdNullspaceTorque);
        checkNonNegative(configData.kpNullspaceTorque);
        checkNonNegative(configData.kpNullspaceVel);
        checkNonNegative(configData.kdNullspaceVel);
        checkNonNegative(configData.kdImpedance);
        checkNonNegative(configData.kpImpedance);
    }
 
    void
    NJointTSMixImpVelController::limbPublishConfig(StringVariantBaseMap& datafields, ArmPtr& arm)
    {
        auto cfg = arm->bufferConfigRtToOnPublish.getUpToDateReadBuffer();
        common::debugEigenVec(datafields, "kpImpedance", cfg.kpImpedance);
        common::debugEigenVec(datafields, "kdImpedance", cfg.kdImpedance);
        common::debugEigenVec(datafields, "kpCartesianVel", cfg.kpCartesianVel);
        common::debugEigenVec(datafields, "kdCartesianVel", cfg.kdCartesianVel);
        if (cfg.desiredNullspaceJointAngles.has_value())
        {
            common::debugEigenVec(
                datafields, "desiredNullspaceJointAngles", cfg.desiredNullspaceJointAngles.value());
        }
 
        datafields["poseError_threshold_position"] = new Variant(cfg.safeDistanceMMToGoal);
        datafields["poseError_threshold_angle"] = new Variant(cfg.safeRotAngleDegreeToGoal);
        datafields["safeForceGuardThreshold"] = new Variant(cfg.ftConfig.safeGuardForceThreshold);
        datafields["safeTorqueGuardThreshold"] = new Variant(cfg.ftConfig.safeGuardTorqueThreshold);
    }
 
    /// ================================== TSImp ==================================
    template class NJointTaskspaceController<law::TSImpController>;
 
    NJointControllerRegistration<NJointTSImpController>
        registrationControllerNJointTSImpController("TSImp");
 
    NJointTSImpController::NJointTSImpController(const RobotUnitPtr& robotUnit,
                                                 const NJointControllerConfigPtr& config,
                                                 const VirtualRobot::RobotPtr& robot) :
        NJointTaskspaceController<law::TSImpController>(robotUnit, config, robot)
    {
        createLimbs();
    }
 
    std::string
    NJointTSImpController::getClassName(const Ice::Current&) const
    {
        return "TSImp";
    }
 
    void
    NJointTSImpController::validateConfigDataCheckSize(Config& configData, ArmPtr& arm)
    {
        ARMARX_TRACE;
        const auto nDoF = static_cast<Eigen::Index>(arm->jointNames.size());
 
        const auto checkSize = [nDoF](const auto& v) { ARMARX_CHECK_EQUAL(v.rows(), nDoF); };
        const auto checkNonNegative = [](const auto& v) { ARMARX_CHECK((v.array() >= 0).all()); };
        checkSize(configData.desiredNullspaceJointAngles.value());
        checkSize(configData.kdNullspaceTorque);
        checkSize(configData.kpNullspaceTorque);
 
        checkNonNegative(configData.kdNullspaceTorque);
        checkNonNegative(configData.kpNullspaceTorque);
        checkNonNegative(configData.kdImpedance);
        checkNonNegative(configData.kpImpedance);
    }
 
    void
    NJointTSImpController::limbPublishConfig(StringVariantBaseMap& datafields, ArmPtr& arm)
    {
        auto cfg = arm->bufferConfigRtToOnPublish.getUpToDateReadBuffer();
        common::debugEigenVec(datafields, "kpImpedance", cfg.kpImpedance);
        common::debugEigenVec(datafields, "kdImpedance", cfg.kdImpedance);
        if (cfg.desiredNullspaceJointAngles.has_value())
        {
            common::debugEigenVec(
                datafields, "desiredNullspaceJointAngles", cfg.desiredNullspaceJointAngles.value());
        }
 
        datafields["poseError_threshold_position"] = new Variant(cfg.safeDistanceMMToGoal);
        datafields["poseError_threshold_angle"] = new Variant(cfg.safeRotAngleDegreeToGoal);
        datafields["safeForceGuardThreshold"] = new Variant(cfg.ftConfig.safeGuardForceThreshold);
        datafields["safeTorqueGuardThreshold"] = new Variant(cfg.ftConfig.safeGuardTorqueThreshold);
    }
 
    /// ================================== TSAdm ==================================
    template class NJointTaskspaceController<law::TSAdmController>;
 
    NJointControllerRegistration<NJointTSAdmController>
        registrationControllerNJointTSAdmController("TSAdm");
 
    NJointTSAdmController::NJointTSAdmController(const RobotUnitPtr& robotUnit,
                                                 const NJointControllerConfigPtr& config,
                                                 const VirtualRobot::RobotPtr& robot) :
        NJointTaskspaceController<law::TSAdmController>(robotUnit, config, robot)
    {
        createLimbs();
    }
 
    std::string
    NJointTSAdmController::getClassName(const Ice::Current&) const
    {
        return "TSAdm";
    }
 
    void
    NJointTSAdmController::validateConfigDataCheckSize(Config& configData, ArmPtr& arm)
    {
        ARMARX_TRACE;
        const auto nDoF = static_cast<Eigen::Index>(arm->jointNames.size());
 
        const auto checkSize = [nDoF](const auto& v) { ARMARX_CHECK_EQUAL(v.rows(), nDoF); };
        const auto checkNonNegative = [](const auto& v) { ARMARX_CHECK((v.array() >= 0).all()); };
        checkSize(configData.desiredNullspaceJointAngles.value());
        checkSize(configData.kdNullspaceTorque);
        checkSize(configData.kpNullspaceTorque);
 
        checkNonNegative(configData.kdNullspaceTorque);
        checkNonNegative(configData.kpNullspaceTorque);
        checkNonNegative(configData.kdImpedance);
        checkNonNegative(configData.kpImpedance);
    }
 
    void
    NJointTSAdmController::limbPublishConfig(StringVariantBaseMap& datafields, ArmPtr& arm)
    {
        auto cfg = arm->bufferConfigRtToOnPublish.getUpToDateReadBuffer();
        common::debugEigenVec(datafields, "kpImpedance", cfg.kpImpedance);
        common::debugEigenVec(datafields, "kdImpedance", cfg.kdImpedance);
        if (cfg.desiredNullspaceJointAngles.has_value())
        {
            common::debugEigenVec(
                datafields, "desiredNullspaceJointAngles", cfg.desiredNullspaceJointAngles.value());
        }
 
        datafields["poseError_threshold_position"] = new Variant(cfg.safeDistanceMMToGoal);
        datafields["poseError_threshold_angle"] = new Variant(cfg.safeRotAngleDegreeToGoal);
        datafields["safeForceGuardThreshold"] = new Variant(cfg.ftConfig.safeGuardForceThreshold);
        datafields["safeTorqueGuardThreshold"] = new Variant(cfg.ftConfig.safeGuardTorqueThreshold);
    }
 
    /// ================================== TSVel ==================================
    template class NJointTaskspaceController<law::TSVelController>;
 
    NJointControllerRegistration<NJointTSVelController>
        registrationControllerNJointTSVelController("TSVel");
 
    NJointTSVelController::NJointTSVelController(const RobotUnitPtr& robotUnit,
                                                 const NJointControllerConfigPtr& config,
                                                 const VirtualRobot::RobotPtr& robot) :
        NJointTaskspaceController<law::TSVelController>(robotUnit, config, robot)
    {
        createLimbs();
    }
 
    std::string
    NJointTSVelController::getClassName(const Ice::Current&) const
    {
        return "TSVel";
    }
 
    void
    NJointTSVelController::validateConfigDataCheckSize(Config& configData, ArmPtr& arm)
    {
        const auto nDoF = static_cast<Eigen::Index>(arm->jointNames.size());
 
        const auto checkSize = [nDoF](const auto& v) { ARMARX_CHECK_EQUAL(v.rows(), nDoF); };
        const auto checkNonNegative = [](const auto& v) { ARMARX_CHECK((v.array() >= 0).all()); };
        checkSize(configData.desiredNullspaceJointAngles.value());
        checkSize(configData.kdNullspaceVel);
        checkSize(configData.kpNullspaceVel);
 
        checkNonNegative(configData.kpNullspaceVel);
        checkNonNegative(configData.kdNullspaceVel);
    }
 
    void
    NJointTSVelController::limbPublishConfig(StringVariantBaseMap& datafields, ArmPtr& arm)
    {
        auto cfg = arm->bufferConfigRtToOnPublish.getUpToDateReadBuffer();
        common::debugEigenVec(datafields, "kpCartesianVel", cfg.kpCartesianVel);
        common::debugEigenVec(datafields, "kdCartesianVel", cfg.kdCartesianVel);
        if (cfg.desiredNullspaceJointAngles.has_value())
        {
            common::debugEigenVec(
                datafields, "desiredNullspaceJointAngles", cfg.desiredNullspaceJointAngles.value());
        }
 
        datafields["poseError_threshold_position"] = new Variant(cfg.safeDistanceMMToGoal);
        datafields["poseError_threshold_angle"] = new Variant(cfg.safeRotAngleDegreeToGoal);
        datafields["safeForceGuardThreshold"] = new Variant(cfg.ftConfig.safeGuardForceThreshold);
        datafields["safeTorqueGuardThreshold"] = new Variant(cfg.ftConfig.safeGuardTorqueThreshold);
    }
} // namespace armarx::control::njoint_controller::task_space