KinematicUnitGuiPlugin.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "KinematicUnitGuiPlugin.h"
 
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/json.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/services/tasks/RunningTask.h"
#include "ArmarXCore/core/time/Metronome.h"
#include "ArmarXCore/core/time/forward_declarations.h"
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/util/algorithm.h>
#include <ArmarXCore/util/json/JSONObject.h>
 
#include <RobotAPI/gui-plugins/KinematicUnitPlugin/ui_KinematicUnitConfigDialog.h>
#include <RobotAPI/interface/core/NameValueMap.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
 
#include "KinematicUnitConfigDialog.h"
 
// Qt headers
#include <QCheckBox>
#include <QClipboard>
#include <QInputDialog>
#include <QPushButton>
#include <QSlider>
#include <QSpinBox>
#include <QStringList>
#include <QTableView>
#include <QTableWidget>
#include <Qt>
#include <QtGlobal>
#include <qtimer.h>
 
#include <ArmarXCore/observers/filters/MedianFilter.h>
 
#include <Inventor/Qt/SoQt.h>
#include <Inventor/SoDB.h>
 
// System
#include <cmath>
#include <cstddef>
#include <cstdio>
#include <cstdlib>
#include <filesystem>
#include <iostream>
#include <memory>
#include <optional>
#include <stdexcept>
#include <string>
 
 
//#define KINEMATIC_UNIT_FILE_DEFAULT std::string("RobotAPI/robots/Armar3/ArmarIII.xml")
//#define KINEMATIC_UNIT_FILE_DEFAULT_PACKAGE std::string("RobotAPI")
#define KINEMATIC_UNIT_NAME_DEFAULT "Robot"
//#define TOPIC_NAME_DEFAULT "RobotState"
 
constexpr float SLIDER_POS_DEG_MULTIPLIER = 5;
constexpr float SLIDER_POS_RAD_MULTIPLIER = 100;
constexpr float SLIDER_POS_HEMI_MULTIPLIER = 100;
 
namespace armarx
{
 
    KinematicUnitGuiPlugin::KinematicUnitGuiPlugin()
    {
 
        qRegisterMetaType<DebugInfo>("DebugInfo");
 
        addWidget<KinematicUnitWidgetController>();
    }
 
    KinematicUnitWidgetController::KinematicUnitWidgetController() :
        kinematicUnitNode(nullptr),
        enableValueValidator(true),
        historyTime(100000), // 1/10 s
        currentValueMax(5.0f)
    {
        rootVisu = NULL;
        debugLayerVisu = NULL;
 
        // init gui
        ui.setupUi(getWidget());
        getWidget()->setEnabled(false);
 
        ui.tableJointList->setItemDelegateForColumn(eTabelColumnAngleProgressbar, &delegate);
 
        ui.radioButtonUnknown->setHidden(true);
    }
 
    void
    KinematicUnitWidgetController::onInitComponent()
    {
        ARMARX_INFO << flush;
        verbose = true;
 
 
        rootVisu = new SoSeparator;
        rootVisu->ref();
        robotVisu = new SoSeparator;
        robotVisu->ref();
        rootVisu->addChild(robotVisu);
 
        // create the debugdrawer component
        std::string debugDrawerComponentName = "KinemticUnitGUIDebugDrawer_" + getName();
        ARMARX_INFO << "Creating component " << debugDrawerComponentName;
        debugDrawer =
            Component::create<DebugDrawerComponent>(getIceProperties(), debugDrawerComponentName);
        showVisuLayers(false);
 
        if (mutex3D)
        {
            //ARMARX_IMPORTANT << "mutex3d:" << mutex3D.get();
            debugDrawer->setMutex(mutex3D);
        }
        else
        {
            ARMARX_ERROR << " No 3d mutex available...";
        }
 
        ArmarXManagerPtr m = getArmarXManager();
        m->addObject(debugDrawer, false);
 
 
        {
            std::unique_lock lock(*mutex3D);
            debugLayerVisu = new SoSeparator();
            debugLayerVisu->ref();
            debugLayerVisu->addChild(debugDrawer->getVisualization());
            rootVisu->addChild(debugLayerVisu);
        }
 
        connectSlots();
 
        usingProxy(kinematicUnitName);
    }
 
    void
    KinematicUnitWidgetController::onConnectComponent()
    {
        // ARMARX_INFO << "Kinematic Unit Gui :: onConnectComponent()";
        jointCurrentHistory.clear();
        jointCurrentHistory.set_capacity(5);
 
        // jointAnglesUpdateFrequency = new filters::MedianFilter(100);
        kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
 
        lastJointAngleUpdateTimestamp = Clock::Now();
        robotVisu->removeAllChildren();
 
        robot.reset();
 
        std::string rfile;
        Ice::StringSeq includePaths;
 
        // Get robot filename
        try
        {
            Ice::StringSeq packages = kinematicUnitInterfacePrx->getArmarXPackages();
            packages.push_back(Application::GetProjectName());
            ARMARX_VERBOSE << "ArmarX packages " << packages;
 
            for (const std::string& projectName : packages)
            {
                if (projectName.empty())
                {
                    continue;
                }
 
                CMakePackageFinder project(projectName);
                auto pathsString = project.getDataDir();
                ARMARX_VERBOSE << "Data paths of ArmarX package " << projectName << ": "
                               << pathsString;
                Ice::StringSeq projectIncludePaths = Split(pathsString, ";,", true, true);
                ARMARX_VERBOSE << "Result: Data paths of ArmarX package " << projectName << ": "
                               << projectIncludePaths;
                includePaths.insert(
                    includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
            }
 
            rfile = kinematicUnitInterfacePrx->getRobotFilename();
            ARMARX_VERBOSE << "Relative robot file " << rfile;
            ArmarXDataPath::getAbsolutePath(rfile, rfile, includePaths);
            ARMARX_VERBOSE << "Absolute robot file " << rfile;
 
            robotNodeSetName = kinematicUnitInterfacePrx->getRobotNodeSetName();
        }
        catch (...)
        {
            ARMARX_ERROR << "Unable to retrieve robot filename.";
        }
 
        try
        {
            ARMARX_INFO << "Loading robot from file " << rfile;
            robot = loadRobotFile(rfile);
        }
        catch (const std::exception& e)
        {
            ARMARX_ERROR << "Failed to init robot: " << e.what();
        }
        catch (...)
        {
            ARMARX_ERROR << "Failed to init robot";
        }
 
        if (!robot || !robot->hasRobotNodeSet(robotNodeSetName))
        {
            getObjectScheduler()->terminate();
            if (getWidget()->parentWidget())
            {
                getWidget()->parentWidget()->close();
            }
            return;
        }
 
        // Check robot name and disable setZero Button if necessary
        if (not simox::alg::starts_with(robot->getName(), "Armar3"))
        {
            ARMARX_VERBOSE << "Disable the SetZero button because the robot name is '"
                           << robot->getName() << "'.";
            ui.pushButtonKinematicUnitPos1->setDisabled(true);
        }
 
        kinematicUnitFile = rfile;
        robotNodeSet = robot->getRobotNodeSet(robotNodeSetName);
 
        kinematicUnitVisualization = getCoinVisualization(robot);
        kinematicUnitNode = kinematicUnitVisualization->getCoinVisualization();
        robotVisu->addChild(kinematicUnitNode);
 
        // Fetch the current joint angles.
        synchronizeRobotJointAngles();
 
        initGUIComboBox(robotNodeSet); // init the pull down menu (QT: ComboBox)
        initGUIJointListTable(robotNodeSet);
 
        const auto initialDebugInfo = kinematicUnitInterfacePrx->getDebugInfo();
 
        initializeUi(initialDebugInfo);
 
        QMetaObject::invokeMethod(this, "resetSlider");
        enableMainWidgetAsync(true);
 
        updateTask = new RunningTask<KinematicUnitWidgetController>(
            this, &KinematicUnitWidgetController::runUpdate);
        updateTask->start();
    }
 
    void
    KinematicUnitWidgetController::runUpdate()
    {
        Metronome metronome(Frequency::Hertz(10));
 
        while (kinematicUnitInterfacePrx)
        {
            fetchData();
            metronome.waitForNextTick();
        }
 
        ARMARX_INFO << "Connection to kinemetic unit lost. Update task terminates.";
    }
 
    void
    KinematicUnitWidgetController::onDisconnectComponent()
    {
        kinematicUnitInterfacePrx = nullptr;
 
        if (updateTask)
        {
            updateTask->stop();
            updateTask->join();
            updateTask = nullptr;
        }
 
        // killTimer(updateTimerId);
        enableMainWidgetAsync(false);
 
        {
            std::unique_lock lock(mutexNodeSet);
            robot.reset();
            robotNodeSet.reset();
            currentNode.reset();
        }
 
        {
            std::unique_lock lock(*mutex3D);
            robotVisu->removeAllChildren();
            debugLayerVisu->removeAllChildren();
        }
    }
 
    void
    KinematicUnitWidgetController::onExitComponent()
    {
        kinematicUnitInterfacePrx = nullptr;
 
        if (updateTask)
        {
            updateTask->stop();
            updateTask->join();
            updateTask = nullptr;
        }
 
        enableMainWidgetAsync(false);
 
        {
            std::unique_lock lock(*mutex3D);
 
            if (robotVisu)
            {
                robotVisu->removeAllChildren();
                robotVisu->unref();
                robotVisu = NULL;
            }
 
            if (debugLayerVisu)
            {
                debugLayerVisu->removeAllChildren();
                debugLayerVisu->unref();
                debugLayerVisu = NULL;
            }
 
            if (rootVisu)
            {
                rootVisu->removeAllChildren();
                rootVisu->unref();
                rootVisu = NULL;
            }
        }
 
        /*
        if (debugDrawer && debugDrawer->getObjectScheduler())
        {
            ARMARX_INFO << "Removing DebugDrawer component...";
            debugDrawer->getObjectScheduler()->terminate();
            ARMARX_INFO << "Removing DebugDrawer component...done";
        }
    */
    }
 
    QPointer<QDialog>
    KinematicUnitWidgetController::getConfigDialog(QWidget* parent)
    {
        if (!dialog)
        {
            dialog = new KinematicUnitConfigDialog(parent);
            dialog->setName(dialog->getDefaultName());
        }
 
        return qobject_cast<KinematicUnitConfigDialog*>(dialog);
    }
 
    void
    KinematicUnitWidgetController::configured()
    {
        ARMARX_VERBOSE << "KinematicUnitWidget::configured()";
        kinematicUnitName = dialog->proxyFinder->getSelectedProxyName().toStdString();
        enableValueValidator = dialog->ui->checkBox->isChecked();
        viewerEnabled = dialog->ui->checkBox3DViewerEnabled->isChecked();
        historyTime = dialog->ui->spinBoxHistory->value() * 1000;
        currentValueMax = dialog->ui->doubleSpinBoxMaxMinCurrent->value();
    }
 
    void
    KinematicUnitWidgetController::loadSettings(QSettings* settings)
    {
        kinematicUnitName = settings->value("kinematicUnitName", KINEMATIC_UNIT_NAME_DEFAULT)
                                .toString()
                                .toStdString();
        enableValueValidator = settings->value("enableValueValidator", true).toBool();
        viewerEnabled = settings->value("viewerEnabled", true).toBool();
        historyTime = settings->value("historyTime", 100).toInt() * 1000;
        currentValueMax = settings->value("currentValueMax", 5.0).toFloat();
    }
 
    void
    KinematicUnitWidgetController::saveSettings(QSettings* settings)
    {
        settings->setValue("kinematicUnitName", QString::fromStdString(kinematicUnitName));
        settings->setValue("enableValueValidator", enableValueValidator);
        settings->setValue("viewerEnabled", viewerEnabled);
        assert(historyTime % 1000 == 0);
        settings->setValue("historyTime", static_cast<int>(historyTime / 1000));
        settings->setValue("currentValueMax", currentValueMax);
    }
 
    void
    KinematicUnitWidgetController::showVisuLayers(bool show)
    {
        if (debugDrawer)
        {
            if (show)
            {
                debugDrawer->enableAllLayers();
            }
            else
            {
                debugDrawer->disableAllLayers();
            }
        }
    }
 
    void
    KinematicUnitWidgetController::copyToClipboard()
    {
        NameValueMap values;
        {
            std::unique_lock lock(mutexNodeSet);
 
            ARMARX_CHECK_NOT_NULL(kinematicUnitInterfacePrx);
            const auto debugInfo = kinematicUnitInterfacePrx->getDebugInfo();
 
            const auto selectedControlMode = getSelectedControlMode();
 
            if (selectedControlMode == ePositionControl)
            {
                values = debugInfo.jointAngles;
            }
            else if (selectedControlMode == eVelocityControl)
            {
                values = debugInfo.jointVelocities;
            }
        }
 
        JSONObjectPtr serializer = new JSONObject();
        for (auto& kv : values)
        {
            serializer->setFloat(kv.first, kv.second);
        }
        const QString json = QString::fromStdString(serializer->asString(true));
        QClipboard* clipboard = QApplication::clipboard();
        clipboard->setText(json);
        QApplication::processEvents();
    }
 
    void
    KinematicUnitWidgetController::updateGuiElements()
    {
        // modelUpdateCB();
    }
 
    void
    KinematicUnitWidgetController::updateKinematicUnitListInDialog()
    {
    }
 
    void
    KinematicUnitWidgetController::modelUpdateCB()
    {
    }
 
    SoNode*
    KinematicUnitWidgetController::getScene()
    {
        if (viewerEnabled)
        {
            ARMARX_INFO << "Returning scene ";
            return rootVisu;
        }
        else
        {
            ARMARX_INFO << "viewer disabled - returning null scene";
            return NULL;
        }
    }
 
    void
    KinematicUnitWidgetController::connectSlots()
    {
        connect(ui.pushButtonKinematicUnitPos1,
                SIGNAL(clicked()),
                this,
                SLOT(kinematicUnitZeroPosition()));
 
        connect(
            ui.nodeListComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectJoint(int)));
        connect(ui.horizontalSliderKinematicUnitPos,
                SIGNAL(valueChanged(int)),
                this,
                SLOT(sliderValueChanged(int)));
 
        connect(ui.horizontalSliderKinematicUnitPos,
                SIGNAL(sliderReleased()),
                this,
                SLOT(resetSliderToZeroPosition()));
 
        connect(ui.radioButtonPositionControl,
                SIGNAL(clicked(bool)),
                this,
                SLOT(setControlModePosition()));
        connect(ui.radioButtonVelocityControl,
                SIGNAL(clicked(bool)),
                this,
                SLOT(setControlModeVelocity()));
        connect(
            ui.radioButtonTorqueControl, SIGNAL(clicked(bool)), this, SLOT(setControlModeTorque()));
        connect(
            ui.pushButtonFromJson, SIGNAL(clicked()), this, SLOT(on_pushButtonFromJson_clicked()));
 
        connect(ui.copyToClipboard, SIGNAL(clicked()), this, SLOT(copyToClipboard()));
        connect(ui.showDebugLayer,
                SIGNAL(toggled(bool)),
                this,
                SLOT(showVisuLayers(bool)),
                Qt::QueuedConnection);
 
        connect(this,
                SIGNAL(jointAnglesReported()),
                this,
                SLOT(updateJointAnglesTable()),
                Qt::QueuedConnection);
        connect(this,
                SIGNAL(jointVelocitiesReported()),
                this,
                SLOT(updateJointVelocitiesTable()),
                Qt::QueuedConnection);
        connect(this,
                SIGNAL(jointTorquesReported()),
                this,
                SLOT(updateJointTorquesTable()),
                Qt::QueuedConnection);
        connect(this,
                SIGNAL(jointCurrentsReported()),
                this,
                SLOT(updateJointCurrentsTable()),
                Qt::QueuedConnection);
        connect(this,
                SIGNAL(jointMotorTemperaturesReported()),
                this,
                SLOT(updateMotorTemperaturesTable()),
                Qt::QueuedConnection);
        connect(this,
                SIGNAL(jointControlModesReported()),
                this,
                SLOT(updateControlModesTable()),
                Qt::QueuedConnection);
        connect(this,
                SIGNAL(jointStatusesReported()),
                this,
                SLOT(updateJointStatusesTable()),
                Qt::QueuedConnection);
 
        connect(ui.tableJointList,
                SIGNAL(cellDoubleClicked(int, int)),
                this,
                SLOT(selectJointFromTableWidget(int, int)),
                Qt::QueuedConnection);
 
        connect(ui.checkBoxUseDegree,
                SIGNAL(clicked()),
                this,
                SLOT(resetSlider()),
                Qt::QueuedConnection);
        connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModePosition()));
        connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModeVelocity()));
        connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModeTorque()));
 
        connect(this,
                SIGNAL(onDebugInfoReceived(const DebugInfo&)),
                this,
                SLOT(debugInfoReceived(const DebugInfo&)));
    }
 
    void
    KinematicUnitWidgetController::initializeUi(const DebugInfo& debugInfo)
    {
        //signal clicked is not emitted if you call setDown(), setChecked() or toggle().
 
        // there is no default control mode
        setControlModeRadioButtonGroup(ControlMode::eUnknown);
 
        ui.widgetSliderFactor->setVisible(false);
 
        fetchData();
    }
 
    void
    KinematicUnitWidgetController::kinematicUnitZeroVelocity()
    {
        if (!robotNodeSet)
        {
            return;
        }
 
        std::unique_lock lock(mutexNodeSet);
        std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
        NameValueMap vels;
        NameControlModeMap jointModes;
 
        for (unsigned int i = 0; i < rn.size(); i++)
        {
            jointModes[rn[i]->getName()] = eVelocityControl;
            vels[rn[i]->getName()] = 0.0f;
        }
 
        try
        {
            kinematicUnitInterfacePrx->switchControlMode(jointModes);
            kinematicUnitInterfacePrx->setJointVelocities(vels);
        }
        catch (...)
        {
        }
 
        const auto selectedControlMode = getSelectedControlMode();
        if (selectedControlMode == eVelocityControl)
        {
            ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
        }
    }
 
    void
    KinematicUnitWidgetController::resetSlider()
    {
        const auto selectedControlMode = getSelectedControlMode();
 
        if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
        {
            resetSliderToZeroPosition();
        }
        else if (selectedControlMode == ePositionControl)
        {
            if (currentNode)
            {
                if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or
                    currentNode->isFourBarJoint())
                {
                    const bool isDeg = ui.checkBoxUseDegree->isChecked();
                    const auto factor =
                        isDeg ? SLIDER_POS_DEG_MULTIPLIER : SLIDER_POS_RAD_MULTIPLIER;
                    const float conversionFactor = isDeg ? 180.0 / M_PI : 1.0f;
                    const float pos = currentNode->getJointValue() * conversionFactor;
 
                    ui.lcdNumberKinematicUnitJointValue->display((int)pos);
                    ui.horizontalSliderKinematicUnitPos->setSliderPosition((int)(pos * factor));
                }
 
                if (currentNode->isTranslationalJoint())
                {
                    const auto factor = SLIDER_POS_DEG_MULTIPLIER;
                    const float pos = currentNode->getJointValue();
 
                    ui.lcdNumberKinematicUnitJointValue->display((int)pos);
                    ui.horizontalSliderKinematicUnitPos->setSliderPosition((int)(pos * factor));
                }
            }
        }
    }
 
    void
    KinematicUnitWidgetController::resetSliderToZeroPosition()
    {
        const auto selectedControlMode = getSelectedControlMode();
 
        if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
        {
            ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
            ui.lcdNumberKinematicUnitJointValue->display(SLIDER_ZERO_POSITION);
        }
    }
 
    void
    KinematicUnitWidgetController::setControlModeRadioButtonGroup(const ControlMode& controlMode)
    {
        ARMARX_VERBOSE << "Setting control mode of radio button group to " << controlMode;
 
        switch (controlMode)
        {
            case eDisabled:
            case eUnknown:
            case ePositionVelocityControl:
                ui.radioButtonUnknown->setChecked(true);
                break;
            case ePositionControl:
                ui.radioButtonPositionControl->setChecked(true);
                break;
            case eVelocityControl:
                ui.radioButtonVelocityControl->setChecked(true);
                break;
            case eTorqueControl:
                ui.radioButtonTorqueControl->setChecked(true);
                break;
        }
    }
 
    void
    KinematicUnitWidgetController::setControlModePosition()
    {
        if (!ui.radioButtonPositionControl->isChecked())
        {
            return;
        }
        NameControlModeMap jointModes;
        // selectedControlMode = ePositionControl;
        ui.widgetSliderFactor->setVisible(false);
 
        // FIXME currentNode should be passed to this function!
 
        if (currentNode)
        {
            const QString unit = [&]() -> QString
            {
                if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or
                    currentNode->isFourBarJoint())
                {
                    if (ui.checkBoxUseDegree->isChecked())
                    {
                        return "deg";
                    }
 
                    return "rad";
                }
 
                if (currentNode->isTranslationalJoint())
                {
                    return "mm";
                }
 
                throw std::invalid_argument("unknown/unsupported joint type");
            }();
 
            ui.labelUnit->setText(unit);
 
 
            const auto [factor, conversionFactor] = [&]() -> std::pair<float, float>
            {
                if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or
                    currentNode->isFourBarJoint())
                {
                    const bool isDeg = ui.checkBoxUseDegree->isChecked();
                    if (isDeg)
                    {
                        return {SLIDER_POS_DEG_MULTIPLIER, 180.0 / M_PI};
                    }
                    return {SLIDER_POS_RAD_MULTIPLIER, 1};
                }
 
                if (currentNode->isTranslationalJoint())
                {
                    return {SLIDER_POS_DEG_MULTIPLIER, 1};
                }
 
                throw std::invalid_argument("unknown/unsupported joint type");
            }();
 
            jointModes[currentNode->getName()] = ePositionControl;
 
            if (kinematicUnitInterfacePrx)
            {
                kinematicUnitInterfacePrx->switchControlMode(jointModes);
            }
 
            const float lo = currentNode->getJointLimitLo() * conversionFactor;
            const float hi = currentNode->getJointLimitHi() * conversionFactor;
 
            if (hi - lo <= 0.0f)
            {
                return;
            }
 
            {
                // currentNode->getJointValue() can we wrong after we re-connected to the robot unit.
                // E.g., it can be 0 although the torso joint was at -365 before the unit disconnected.
                // Therefore, we first have to fetch the actual joint values and use that one.
                // However, this should actually not be necessary, as the robot model should be updated
                // via the topics.
                synchronizeRobotJointAngles();
            }
 
            const float pos = currentNode->getJointValue() * conversionFactor;
            ARMARX_INFO << "Setting position control for current node "
                        << "(name '" << currentNode->getName() << "' with current value " << pos
                        << ")";
 
            // Setting the slider position to pos will set the position to the slider tick closest to pos
            // This will initially send a position target with a small delta to the joint.
            ui.horizontalSliderKinematicUnitPos->blockSignals(true);
 
            const float sliderMax = hi * factor;
            const float sliderMin = lo * factor;
 
            ui.horizontalSliderKinematicUnitPos->setMaximum(sliderMax);
            ui.horizontalSliderKinematicUnitPos->setMinimum(sliderMin);
 
            const std::size_t desiredNumberOfTicks = 1'000;
 
            const float tickInterval = (sliderMax - sliderMin) / desiredNumberOfTicks;
            ARMARX_INFO << VAROUT(tickInterval);
 
            ui.horizontalSliderKinematicUnitPos->setTickInterval(tickInterval);
            ui.lcdNumberKinematicUnitJointValue->display(pos);
 
            ui.horizontalSliderKinematicUnitPos->blockSignals(false);
            resetSlider();
        }
    }
 
    void
    KinematicUnitWidgetController::setControlModeVelocity()
    {
        if (!ui.radioButtonVelocityControl->isChecked())
        {
            return;
        }
        NameControlModeMap jointModes;
        NameValueMap jointVelocities;
 
        if (currentNode)
        {
            jointModes[currentNode->getName()] = eVelocityControl;
 
            // set the velocity to zero to stop any previous controller (e.g. torque controller)
            jointVelocities[currentNode->getName()] = 0;
 
 
            const QString unit = [&]() -> QString
            {
                if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or
                    currentNode->isFourBarJoint())
                {
                    if (ui.checkBoxUseDegree->isChecked())
                    {
                        return "deg/s";
                    }
 
                    return "rad/(100*s)";
                }
 
                if (currentNode->isTranslationalJoint())
                {
                    return "mm/s";
                }
 
                throw std::invalid_argument("unknown/unsupported joint type");
            }();
 
 
            ui.labelUnit->setText(unit);
            ARMARX_INFO << "setting velocity control for current Node Name: "
                        << currentNode->getName() << flush;
 
            const bool isDeg = ui.checkBoxUseDegree->isChecked();
            const bool isRot = currentNode->isRotationalJoint() or
                               currentNode->isHemisphereJoint() or currentNode->isFourBarJoint();
 
            const float lo = isRot ? (isDeg ? -90 : -M_PI * 100) : -1000;
            const float hi = isRot ? (isDeg ? +90 : +M_PI * 100) : 1000;
 
            try
            {
                if (kinematicUnitInterfacePrx)
                {
                    kinematicUnitInterfacePrx->switchControlMode(jointModes);
                    kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
                }
            }
            catch (...)
            {
            }
 
            ui.widgetSliderFactor->setVisible(true);
 
            ui.horizontalSliderKinematicUnitPos->blockSignals(true);
            ui.horizontalSliderKinematicUnitPos->setMaximum(hi);
            ui.horizontalSliderKinematicUnitPos->setMinimum(lo);
            ui.horizontalSliderKinematicUnitPos->blockSignals(false);
            resetSlider();
        }
    }
 
    ControlMode
    KinematicUnitWidgetController::getSelectedControlMode() const
    {
        if (ui.radioButtonPositionControl->isChecked())
        {
            return ControlMode::ePositionControl;
        }
 
        if (ui.radioButtonVelocityControl->isChecked())
        {
            return ControlMode::eVelocityControl;
        }
 
        if (ui.radioButtonTorqueControl->isChecked())
        {
            return ControlMode::eTorqueControl;
        }
 
        // if no button is checked, then the joint is likely initialized but no controller has been loaded yet
        // (well, the no movement controller should be active)
        return ControlMode::eUnknown;
    }
 
    void
    KinematicUnitWidgetController::setControlModeTorque()
    {
        if (!ui.radioButtonTorqueControl->isChecked())
        {
            return;
        }
        NameControlModeMap jointModes;
 
        if (currentNode)
        {
            jointModes[currentNode->getName()] = eTorqueControl;
            ui.labelUnit->setText("Ncm");
            ARMARX_INFO << "setting torque control for current Node Name: "
                        << currentNode->getName() << flush;
 
            if (kinematicUnitInterfacePrx)
            {
                try
                {
                    kinematicUnitInterfacePrx->switchControlMode(jointModes);
                }
                catch (...)
                {
                }
            }
 
            ui.horizontalSliderKinematicUnitPos->blockSignals(true);
            ui.horizontalSliderKinematicUnitPos->setMaximum(20000.0);
            ui.horizontalSliderKinematicUnitPos->setMinimum(-20000.0);
 
            ui.widgetSliderFactor->setVisible(true);
 
            ui.horizontalSliderKinematicUnitPos->blockSignals(false);
            resetSlider();
        }
    }
 
    VirtualRobot::RobotPtr
    KinematicUnitWidgetController::loadRobotFile(std::string fileName)
    {
        VirtualRobot::RobotPtr robot;
 
        if (verbose)
        {
            ARMARX_INFO << "Loading KinematicUnit " << kinematicUnitName << " from "
                        << kinematicUnitFile << " ..." << flush;
        }
 
        if (!ArmarXDataPath::getAbsolutePath(fileName, fileName))
        {
            ARMARX_INFO << "Could not find Robot XML file with name " << fileName << flush;
        }
 
        robot = VirtualRobot::RobotIO::loadRobot(fileName);
 
        if (!robot)
        {
            ARMARX_INFO << "Could not find Robot XML file with name " << fileName << "("
                        << kinematicUnitName << ")" << flush;
        }
 
        return robot;
    }
 
    VirtualRobot::CoinVisualizationPtr
    KinematicUnitWidgetController::getCoinVisualization(VirtualRobot::RobotPtr robot)
    {
        VirtualRobot::CoinVisualizationPtr coinVisualization;
 
        if (robot != NULL)
        {
            ARMARX_VERBOSE << "getting coin visualization" << flush;
            coinVisualization = robot->getVisualization<VirtualRobot::CoinVisualization>();
 
            if (!coinVisualization || !coinVisualization->getCoinVisualization())
            {
                ARMARX_INFO << "could not get coin visualization" << flush;
            }
        }
 
        return coinVisualization;
    }
 
    VirtualRobot::RobotNodeSetPtr
    KinematicUnitWidgetController::getRobotNodeSet(VirtualRobot::RobotPtr robot,
                                                   std::string nodeSetName)
    {
        VirtualRobot::RobotNodeSetPtr nodeSetPtr;
 
        if (robot)
        {
            nodeSetPtr = robot->getRobotNodeSet(nodeSetName);
 
            if (!nodeSetPtr)
            {
                ARMARX_INFO << "RobotNodeSet with name " << nodeSetName << " is not defined"
                            << flush;
            }
        }
 
        return nodeSetPtr;
    }
 
    bool
    KinematicUnitWidgetController::initGUIComboBox(VirtualRobot::RobotNodeSetPtr robotNodeSet)
    {
        ui.nodeListComboBox->clear();
 
        if (robotNodeSet)
        {
            std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
 
            for (unsigned int i = 0; i < rn.size(); i++)
            {
                //            ARMARX_INFO << "adding item to joint combo box" << rn[i]->getName() << flush;
                QString name(rn[i]->getName().c_str());
                ui.nodeListComboBox->addItem(name);
            }
            ui.nodeListComboBox->setCurrentIndex(-1);
            return true;
        }
        return false;
    }
 
    bool
    KinematicUnitWidgetController::initGUIJointListTable(VirtualRobot::RobotNodeSetPtr robotNodeSet)
    {
        uint numberOfColumns = 10;
 
        //dont use clear! It is not required here and somehow causes the tabel to have
        //numberOfColumns additional empty columns and rn.size() additional empty rows.
        //Somehow columncount (rowcount) stay at numberOfColumns (rn.size())
        //ui.tableJointList->clear();
 
        if (robotNodeSet)
        {
            std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
 
            //set dimension of table
            //ui.tableJointList->setColumnWidth(0,110);
 
            //ui.tableJointList->horizontalHeader()->setResizeMode(QHeaderView::ResizeToContents);
            ui.tableJointList->setRowCount(rn.size());
            ui.tableJointList->setColumnCount(eTabelColumnCount);
 
 
            //ui.tableJointList->setSizePolicy(QSizePolicy::Expanding,QSizePolicy::Expanding);
 
            // set table header
            // if the order is changed dont forget to update the order in the enum JointTabelColumnIndex
            // in theheader file
            QStringList s;
            s << "Joint Name"
              << "Control Mode"
              << "Angle [deg]/Position [mm]"
              << "Velocity [deg/s]/[mm/s]"
              << "Torque [Nm] / PWM"
              << "Current [A]"
              << "Temperature [C]"
              << "Operation"
              << "Error"
              << "Enabled"
              << "Emergency Stop";
            ui.tableJointList->setHorizontalHeaderLabels(s);
            ARMARX_CHECK_EXPRESSION(ui.tableJointList->columnCount() == eTabelColumnCount)
                << "Current table size: " << ui.tableJointList->columnCount();
 
 
            // fill in joint names
            for (unsigned int i = 0; i < rn.size(); i++)
            {
                //         ARMARX_INFO << "adding item to joint table" << rn[i]->getName() << flush;
                QString name(rn[i]->getName().c_str());
 
                QTableWidgetItem* newItem = new QTableWidgetItem(name);
                ui.tableJointList->setItem(i, eTabelColumnName, newItem);
            }
 
            // init missing table fields with default values
            for (unsigned int i = 0; i < rn.size(); i++)
            {
                for (unsigned int j = 1; j < numberOfColumns; j++)
                {
                    QString state = "--";
                    QTableWidgetItem* newItem = new QTableWidgetItem(state);
                    ui.tableJointList->setItem(i, j, newItem);
                }
            }
 
            //hide columns Operation, Error, Enabled and Emergency Stop
            //they will be shown when changes occur
            ui.tableJointList->setColumnHidden(eTabelColumnTemperature, true);
            ui.tableJointList->setColumnHidden(eTabelColumnOperation, true);
            ui.tableJointList->setColumnHidden(eTabelColumnError, true);
            ui.tableJointList->setColumnHidden(eTabelColumnEnabled, true);
            ui.tableJointList->setColumnHidden(eTabelColumnEmergencyStop, true);
 
            return true;
        }
 
        return false;
    }
 
    void
    KinematicUnitWidgetController::selectJoint(int i)
    {
        std::unique_lock lock(mutexNodeSet);
 
        ARMARX_INFO << "Selected index: " << ui.nodeListComboBox->currentIndex();
 
        if (!robotNodeSet || i < 0 || i >= static_cast<int>(robotNodeSet->getSize()))
        {
            return;
        }
 
        currentNode = robotNodeSet->getAllRobotNodes()[i];
        ARMARX_IMPORTANT << "Selected joint is `" << currentNode->getName() << "`.";
 
        const auto controlModes = kinematicUnitInterfacePrx->getControlModes();
        if (controlModes.count(currentNode->getName()) == 0)
        {
            ARMARX_ERROR << "Could not retrieve control mode for joint `" << currentNode->getName()
                         << "` from kinematic unit!";
            return;
        }
 
        const auto controlMode = controlModes.at(currentNode->getName());
        setControlModeRadioButtonGroup(controlMode);
 
        if (controlMode == ePositionControl)
        {
            setControlModePosition();
        }
        else if (controlMode == eVelocityControl)
        {
            setControlModeVelocity();
            ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
        }
        else if (controlMode == eTorqueControl)
        {
            setControlModeTorque();
            ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
        }
    }
 
    void
    KinematicUnitWidgetController::selectJointFromTableWidget(int row, int column)
    {
        if (column == eTabelColumnName)
        {
            ui.nodeListComboBox->setCurrentIndex(row);
            //        selectJoint(row);
        }
    }
 
    void
    KinematicUnitWidgetController::sliderValueChanged(int pos)
    {
        std::unique_lock lock(mutexNodeSet);
 
        if (!currentNode)
        {
            return;
        }
 
        const float value = static_cast<float>(ui.horizontalSliderKinematicUnitPos->value());
 
        const ControlMode currentControlMode = getSelectedControlMode();
 
        const bool isDeg = ui.checkBoxUseDegree->isChecked();
        const bool isRot = currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or
                           currentNode->isFourBarJoint();
 
        if (currentControlMode == ePositionControl)
        {
            const auto factor =
                isRot && !isDeg ? SLIDER_POS_RAD_MULTIPLIER : SLIDER_POS_DEG_MULTIPLIER;
            float conversionFactor = isRot && isDeg ? 180.0 / M_PI : 1.0f;
 
            NameValueMap jointAngles;
 
            jointAngles[currentNode->getName()] = value / conversionFactor / factor;
            ui.lcdNumberKinematicUnitJointValue->display(value / factor);
            if (kinematicUnitInterfacePrx)
            {
                try
                {
                    kinematicUnitInterfacePrx->setJointAngles(jointAngles);
                }
                catch (...)
                {
                }
            }
        }
        else if (currentControlMode == eVelocityControl)
        {
            float conversionFactor = isRot ? (isDeg ? 180.0 / M_PI : 100.f) : 1.0f;
            NameValueMap jointVelocities;
            jointVelocities[currentNode->getName()] =
                value / conversionFactor *
                static_cast<float>(ui.doubleSpinBoxKinematicUnitPosFactor->value());
            ui.lcdNumberKinematicUnitJointValue->display(value);
 
            if (kinematicUnitInterfacePrx)
            {
                try
                {
                    kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
                }
                catch (...)
                {
                }
            }
        }
        else if (currentControlMode == eTorqueControl)
        {
            NameValueMap jointTorques;
            float torqueTargetValue =
                value / 100.0f *
                static_cast<float>(ui.doubleSpinBoxKinematicUnitPosFactor->value());
            jointTorques[currentNode->getName()] = torqueTargetValue;
            ui.lcdNumberKinematicUnitJointValue->display(torqueTargetValue);
 
            if (kinematicUnitInterfacePrx)
            {
                try
                {
                    kinematicUnitInterfacePrx->setJointTorques(jointTorques);
                }
                catch (...)
                {
                }
            }
        }
        else
        {
            ARMARX_INFO << "current ControlModes unknown" << flush;
        }
    }
 
    void
    KinematicUnitWidgetController::updateControlModesTable(
        const NameControlModeMap& reportedJointControlModes)
    {
        if (!getWidget() || !robotNodeSet)
        {
            return;
        }
 
        std::unique_lock lock(mutexNodeSet);
        std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
 
        for (unsigned int i = 0; i < rn.size(); i++)
        {
            NameControlModeMap::const_iterator it;
            it = reportedJointControlModes.find(rn[i]->getName());
            QString state;
 
            if (it == reportedJointControlModes.end())
            {
                state = "unknown";
            }
            else
            {
                ControlMode currentMode = it->second;
 
 
                switch (currentMode)
                {
                    /*case eNoMode:
                    state = "None";
                    break;
 
                case eUnknownMode:
                    state = "Unknown";
                    break;
                */
                    case eDisabled:
                        state = "Disabled";
                        break;
 
                    case eUnknown:
                        state = "Unknown";
                        break;
 
                    case ePositionControl:
                        state = "Position";
                        break;
 
                    case eVelocityControl:
                        state = "Velocity";
                        break;
 
                    case eTorqueControl:
                        state = "Torque";
                        break;
 
 
                    case ePositionVelocityControl:
                        state = "Position + Velocity";
                        break;
 
                    default:
                        //show the value of the mode so it can be implemented
                        state = QString("<nyi Mode: %1>").arg(static_cast<int>(currentMode));
                        break;
                }
            }
 
            QTableWidgetItem* newItem = new QTableWidgetItem(state);
            ui.tableJointList->setItem(i, eTabelColumnControlMode, newItem);
        }
    }
 
    void
    KinematicUnitWidgetController::updateJointStatusesTable(
        const NameStatusMap& reportedJointStatuses)
    {
        if (!getWidget() || !robotNodeSet || reportedJointStatuses.empty())
        {
            return;
        }
 
        std::unique_lock lock(mutexNodeSet);
        std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
 
        for (unsigned int i = 0; i < rn.size(); i++)
        {
 
            auto it = reportedJointStatuses.find(rn[i]->getName());
            if (it == reportedJointStatuses.end())
            {
                ARMARX_VERBOSE << deactivateSpam(5, rn[i]->getName()) << "Joint Status for "
                               << rn[i]->getName() << " was not reported!";
                continue;
            }
            JointStatus currentStatus = it->second;
 
            QString state = translateStatus(currentStatus.operation);
            QTableWidgetItem* newItem = new QTableWidgetItem(state);
            ui.tableJointList->setItem(i, eTabelColumnOperation, newItem);
 
            state = translateStatus(currentStatus.error);
            newItem = new QTableWidgetItem(state);
            ui.tableJointList->setItem(i, eTabelColumnError, newItem);
 
            state = currentStatus.enabled ? "yes" : "no";
            newItem = new QTableWidgetItem(state);
            ui.tableJointList->setItem(i, eTabelColumnEnabled, newItem);
 
            state = currentStatus.emergencyStop ? "yes" : "no";
            newItem = new QTableWidgetItem(state);
            ui.tableJointList->setItem(i, eTabelColumnEmergencyStop, newItem);
        }
 
        //show columns
        ui.tableJointList->setColumnHidden(eTabelColumnOperation, false);
        ui.tableJointList->setColumnHidden(eTabelColumnError, false);
        ui.tableJointList->setColumnHidden(eTabelColumnEnabled, false);
        ui.tableJointList->setColumnHidden(eTabelColumnEmergencyStop, false);
    }
 
    QString
    KinematicUnitWidgetController::translateStatus(OperationStatus status)
    {
        switch (status)
        {
            case eOffline:
                return "Offline";
 
            case eOnline:
                return "Online";
 
            case eInitialized:
                return "Initialized";
 
            default:
                return "?";
        }
    }
 
    QString
    KinematicUnitWidgetController::translateStatus(ErrorStatus status)
    {
        switch (status)
        {
            case eOk:
                return "Ok";
 
            case eWarning:
                return "Wr";
 
            case eError:
                return "Er";
 
            default:
                return "?";
        }
    }
 
    void
    KinematicUnitWidgetController::updateJointAnglesTable(const NameValueMap& reportedJointAngles)
    {
        std::unique_lock lock(mutexNodeSet);
 
        if (!robotNodeSet)
        {
            return;
        }
        std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
 
 
        for (unsigned int i = 0; i < rn.size(); i++)
        {
            NameValueMap::const_iterator it;
            VirtualRobot::RobotNodePtr node = rn[i];
            it = reportedJointAngles.find(node->getName());
 
            if (it == reportedJointAngles.end())
            {
                continue;
            }
 
            const float currentValue = it->second;
 
            QModelIndex index = ui.tableJointList->model()->index(i, eTabelColumnAngleProgressbar);
            float conversionFactor = ui.checkBoxUseDegree->isChecked() &&
                                             (node->isRotationalJoint() or
                                              node->isHemisphereJoint() or node->isFourBarJoint())
                                         ? 180.0 / M_PI
                                         : 1;
            ui.tableJointList->model()->setData(
                index,
                (int)(cutJitter(currentValue * conversionFactor) * 100) / 100.0f,
                eJointAngleRole);
            ui.tableJointList->model()->setData(
                index, node->getJointLimitHigh() * conversionFactor, eJointHiRole);
            ui.tableJointList->model()->setData(
                index, node->getJointLimitLow() * conversionFactor, eJointLoRole);
        }
    }
 
    void
    KinematicUnitWidgetController::updateJointVelocitiesTable(
        const NameValueMap& reportedJointVelocities)
    {
        if (!getWidget())
        {
            return;
        }
 
        std::unique_lock lock(mutexNodeSet);
        if (!robotNodeSet)
        {
            return;
        }
        std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
        QTableWidgetItem* newItem;
 
        for (unsigned int i = 0; i < rn.size(); i++)
        {
            NameValueMap::const_iterator it;
            it = reportedJointVelocities.find(rn[i]->getName());
 
            if (it == reportedJointVelocities.end())
            {
                continue;
            }
 
            float currentValue = it->second;
            if (ui.checkBoxUseDegree->isChecked() &&
                (rn[i]->isRotationalJoint() or rn[i]->isHemisphereJoint() or
                 rn[i]->isFourBarJoint()))
            {
                currentValue *= 180.0 / M_PI;
            }
            const QString Text = QString::number(cutJitter(currentValue), 'g', 2);
            newItem = new QTableWidgetItem(Text);
            ui.tableJointList->setItem(i, eTabelColumnVelocity, newItem);
        }
    }
 
    void
    KinematicUnitWidgetController::updateJointTorquesTable(const NameValueMap& reportedJointTorques)
    {
 
 
        std::unique_lock lock(mutexNodeSet);
        if (!getWidget() || !robotNodeSet)
        {
            return;
        }
        std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
        QTableWidgetItem* newItem;
        NameValueMap::const_iterator it;
 
        for (unsigned int i = 0; i < rn.size(); i++)
        {
            it = reportedJointTorques.find(rn[i]->getName());
 
            if (it == reportedJointTorques.end())
            {
                continue;
            }
 
            const float currentValue = it->second;
            newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
            ui.tableJointList->setItem(i, eTabelColumnTorque, newItem);
        }
    }
 
    void
    KinematicUnitWidgetController::updateJointCurrentsTable(
        const NameValueMap& reportedJointCurrents,
        const NameStatusMap& reportedJointStatuses)
    {
 
 
        std::unique_lock lock(mutexNodeSet);
        if (!getWidget() || !robotNodeSet || jointCurrentHistory.size() == 0)
        {
            return;
        }
        std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
        QTableWidgetItem* newItem;
 
        // FIXME history!
        // NameValueMap reportedJointCurrents = jointCurrentHistory.back().second;
        NameValueMap::const_iterator it;
 
        for (unsigned int i = 0; i < rn.size(); i++)
        {
            it = reportedJointCurrents.find(rn[i]->getName());
 
            if (it == reportedJointCurrents.end())
            {
                continue;
            }
 
            const float currentValue = it->second;
            newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
            ui.tableJointList->setItem(i, eTabelColumnCurrent, newItem);
        }
 
        highlightCriticalValues(reportedJointStatuses);
    }
 
    void
    KinematicUnitWidgetController::updateMotorTemperaturesTable(
        const NameValueMap& reportedJointTemperatures)
    {
 
 
        std::unique_lock lock(mutexNodeSet);
        if (!getWidget() || !robotNodeSet || reportedJointTemperatures.empty())
        {
            return;
        }
        std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
        QTableWidgetItem* newItem;
        NameValueMap::const_iterator it;
 
        for (unsigned int i = 0; i < rn.size(); i++)
        {
            it = reportedJointTemperatures.find(rn[i]->getName());
 
            if (it == reportedJointTemperatures.end())
            {
                continue;
            }
 
            const float currentValue = it->second;
            newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
            ui.tableJointList->setItem(i, eTabelColumnTemperature, newItem);
        }
        ui.tableJointList->setColumnHidden(eTabelColumnTemperature, false);
    }
 
    void
    KinematicUnitWidgetController::updateModel(const NameValueMap& reportedJointAngles)
    {
        //    ARMARX_INFO << "updateModel()" << flush;
        std::unique_lock lock(mutexNodeSet);
        if (!robotNodeSet)
        {
            return;
        }
        robot->setJointValues(reportedJointAngles);
    }
 
    std::optional<float>
    mean(const boost::circular_buffer<NameValueMap>& buffer, const std::string& key)
    {
        float sum = 0;
        std::size_t count = 0;
 
        for (const auto& element : buffer)
        {
            if (element.count(key) > 0)
            {
                sum += element.at(key);
            }
        }
 
        if (count == 0)
        {
            return std::nullopt;
        }
 
        return sum / static_cast<float>(count);
    }
 
    void
    KinematicUnitWidgetController::highlightCriticalValues(
        const NameStatusMap& reportedJointStatuses)
    {
        if (!enableValueValidator)
        {
            return;
        }
 
        std::unique_lock lock(mutexNodeSet);
 
        std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
 
        // get standard line colors
        static std::vector<QBrush> standardColors;
        if (standardColors.size() == 0)
        {
            for (unsigned int i = 0; i < rn.size(); i++)
            {
                // all cells of a row have the same color
                standardColors.push_back(
                    ui.tableJointList->item(i, eTabelColumnCurrent)->background());
            }
        }
 
        // check robot current value of nodes
        for (unsigned int i = 0; i < rn.size(); i++)
        {
            const auto& jointName = rn[i]->getName();
 
            const auto currentSmoothValOpt = mean(jointCurrentHistory, jointName);
            if (not currentSmoothValOpt.has_value())
            {
                continue;
            }
 
            const float smoothValue = std::fabs(currentSmoothValOpt.value());
 
            if (jointCurrentHistory.front().count(jointName) == 0)
            {
                continue;
            }
 
            const float startValue = jointCurrentHistory.front().at(jointName);
            const bool isStatic = (smoothValue == startValue);
 
            NameStatusMap::const_iterator it;
            it = reportedJointStatuses.find(rn[i]->getName());
            JointStatus currentStatus = it->second;
 
            if (isStatic)
            {
                if (currentStatus.operation != eOffline)
                {
                    // current value is zero, but joint is not offline
                    ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::yellow);
                }
            }
            else if (std::abs(smoothValue) > currentValueMax)
            {
                // current value is too high
                ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::red);
            }
            else
            {
                // everything seems to work as expected
                ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(standardColors[i]);
            }
        }
    }
 
    void
    KinematicUnitWidgetController::setMutex3D(RecursiveMutexPtr const& mutex3D)
    {
        this->mutex3D = mutex3D;
 
        if (debugDrawer)
        {
            debugDrawer->setMutex(mutex3D);
        }
    }
 
    QPointer<QWidget>
    KinematicUnitWidgetController::getCustomTitlebarWidget(QWidget* parent)
    {
        if (customToolbar)
        {
            customToolbar->setParent(parent);
        }
        else
        {
            customToolbar = new QToolBar(parent);
            customToolbar->addAction("ZeroVelocity", this, SLOT(kinematicUnitZeroVelocity()));
        }
        return customToolbar.data();
    }
 
    float
    KinematicUnitWidgetController::cutJitter(float value)
    {
        return (abs(value) < static_cast<float>(ui.jitterThresholdSpinBox->value())) ? 0 : value;
    }
 
    void
    KinematicUnitWidgetController::fetchData()
    {
        ARMARX_DEBUG << "updateGui";
 
        if (not kinematicUnitInterfacePrx)
        {
            ARMARX_WARNING << "KinematicUnit is not available!";
            return;
        }
 
        const auto debugInfo = kinematicUnitInterfacePrx->getDebugInfo();
 
        emit onDebugInfoReceived(debugInfo);
    }
 
    void
    KinematicUnitWidgetController::debugInfoReceived(const DebugInfo& debugInfo)
    {
        ARMARX_DEBUG << "debug info received";
 
        updateModel(debugInfo.jointAngles);
 
        updateJointAnglesTable(debugInfo.jointAngles);
        updateJointVelocitiesTable(debugInfo.jointVelocities);
        updateJointTorquesTable(debugInfo.jointTorques);
        updateJointCurrentsTable(debugInfo.jointCurrents, debugInfo.jointStatus);
        updateControlModesTable(debugInfo.jointModes);
        updateJointStatusesTable(debugInfo.jointStatus);
        updateMotorTemperaturesTable(debugInfo.jointMotorTemperatures);
    }
 
    void
    RangeValueDelegate::paint(QPainter* painter,
                              const QStyleOptionViewItem& option,
                              const QModelIndex& index) const
    {
        if (index.column() == KinematicUnitWidgetController::eTabelColumnAngleProgressbar)
        {
            float jointValue = index.data(KinematicUnitWidgetController::eJointAngleRole).toFloat();
            float loDeg = index.data(KinematicUnitWidgetController::eJointLoRole).toFloat();
            float hiDeg = index.data(KinematicUnitWidgetController::eJointHiRole).toFloat();
 
            if (hiDeg - loDeg <= 0)
            {
                QStyledItemDelegate::paint(painter, option, index);
                return;
            }
 
            QStyleOptionProgressBar progressBarOption;
            progressBarOption.rect = option.rect;
            progressBarOption.minimum = loDeg;
            progressBarOption.maximum = hiDeg;
            progressBarOption.progress = jointValue;
            progressBarOption.text = QString::number(jointValue);
            progressBarOption.textVisible = true;
            QPalette pal;
            pal.setColor(QPalette::Background, Qt::red);
            progressBarOption.palette = pal;
            QApplication::style()->drawControl(QStyle::CE_ProgressBar, &progressBarOption, painter);
        }
        else
        {
            QStyledItemDelegate::paint(painter, option, index);
        }
    }
 
    KinematicUnitWidgetController::~KinematicUnitWidgetController()
    {
        kinematicUnitInterfacePrx = nullptr;
 
        if (updateTask)
        {
            updateTask->stop();
            updateTask->join();
            updateTask = nullptr;
        }
    }
 
    void
    KinematicUnitWidgetController::on_pushButtonFromJson_clicked()
    {
        bool ok;
        const auto text = QInputDialog::getMultiLineText(
                              __widget, tr("JSON Joint values"), tr("Json:"), "{\n}", &ok)
                              .toStdString();
 
        if (!ok || text.empty())
        {
            return;
        }
 
        NameValueMap jointAngles;
        try
        {
            jointAngles = simox::json::json2NameValueMap(text);
        }
        catch (...)
        {
            ARMARX_ERROR << "invalid json";
        }
 
        NameControlModeMap jointModes;
        for (const auto& [key, _] : jointAngles)
        {
            jointModes[key] = ePositionControl;
        }
 
        try
        {
            kinematicUnitInterfacePrx->switchControlMode(jointModes);
            kinematicUnitInterfacePrx->setJointAngles(jointAngles);
        }
        catch (...)
        {
            ARMARX_ERROR << "failed to switch mode or set angles";
        }
    }
 
    void
    KinematicUnitWidgetController::synchronizeRobotJointAngles()
    {
        const NameValueMap currentJointAngles = kinematicUnitInterfacePrx->getJointAngles();
        robot->setJointValues(currentJointAngles);
    }
 
} // namespace armarx