RobotIKGuiPlugin.cpp

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package    ArmarX::
* @author     Philipp Schmidt
* @date       2015
* @license    http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
 
*/
 
#include "RobotIKGuiPlugin.h"
#include "RobotViewer.h"
 
/* ArmarX includes */
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/application/Application.h>
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
 
/* Virtual Robot includes */
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
 
/* Coin includes */
#include <Inventor/actions/SoGetMatrixAction.h>
#include <Inventor/actions/SoSearchAction.h>
#include <Inventor/SbViewportRegion.h>
#include <ArmarXCore/util/json/JSONObject.h>
#include <QClipboard>
 
// Gui Includes
#include "SetDesiredPoseDialog.h"
 
#define ROBOT_UPDATE_TIMER_MS 33
#define TEXTFIELD_UPDATE_TIMER_MS 200
#define AUTO_FOLLOW_UPDATE 333
#define CARTESIAN_CONTROLLER_LOOP_AMOUNT 100
 
armarx::RobotIKGuiPlugin::RobotIKGuiPlugin()
{
    addWidget<RobotIKWidgetController>();
}
 
armarx::RobotIKWidgetController::RobotIKWidgetController() : manipulatorMoved(false), startUpCameraPositioningFlag(true)
{
 
 
    //Component names
    robotStateComponentName = "";
    robotIKComponentName = "";
    kinematicUnitComponentName = "";
 
    visualization = NULL;
    robotUpdateSensor = NULL;
    textFieldUpdateSensor = NULL;
}
 
void armarx::RobotIKWidgetController::onInitComponent()
{
    ARMARX_INFO << "RobotIKWidgetController on init";
    QMetaObject::invokeMethod(this, "initWidget");
    //Prepare proxies
    usingProxy(robotStateComponentName);
    usingProxy(robotIKComponentName);
    usingProxy(kinematicUnitComponentName);
}
 
void armarx::RobotIKWidgetController::onConnectComponent()
{
    //Get all proxies
    robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName);
    kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitComponentName);
    robotIKPrx = getProxy<RobotIKInterfacePrx>(robotIKComponentName);
 
    //Load robot
    robot = loadRobot(getIncludePaths());
    ARMARX_INFO << VAROUT(robot->getName());
 
    if (!robot)
    {
        getObjectScheduler()->terminate();
 
        if (getWidget()->parentWidget())
        {
            getWidget()->parentWidget()->close();
        }
 
        ARMARX_ERROR << "RobotIKPlugin: Unable to load robot file! Terminating." << std::endl;
        return;
    }
 
    //Add in our visualization for manipulators
    visualization = new ManipulatorVisualization;
    this->ui.robotViewer->getRobotViewer()->getRootNode()->addChild((SoNode*)visualization);
 
    //Get visualization for our robot and add it to scene graph
    currentRobotVisualization = robot->getVisualization<VirtualRobot::CoinVisualization>();
    this->ui.robotViewer->getRobotViewer()->getRootNode()->addChild(currentRobotVisualization->getCoinVisualization());
 
    // Get visualization for preview robot
    previewRobot = loadRobot(getIncludePaths());
    previewRobotVisualization = previewRobot->getVisualization<VirtualRobot::CoinVisualization>();
 
    // Setup cartesianControlRobot
    cartesianControlRobot = previewRobot->clone();
 
    //Make a timer update robo pose every time tick
    SoSensorManager* sensor_mgr = SoDB::getSensorManager();
    robotUpdateSensor = new SoTimerSensor(robotUpdateTimerCB, this);
    robotUpdateSensor->setInterval(SbTime(ROBOT_UPDATE_TIMER_MS / 1000.0f));
    sensor_mgr->insertTimerSensor(robotUpdateSensor);
 
    //And a timer to update the labels
    textFieldUpdateSensor = new SoTimerSensor(textFieldUpdateTimerCB, this);
    textFieldUpdateSensor->setInterval(SbTime(TEXTFIELD_UPDATE_TIMER_MS / 1000.0f));
    sensor_mgr->insertTimerSensor(textFieldUpdateSensor);
 
    //Get all kinematic chain descriptions and add them to the combo box
    auto robotNodeSets = robot->getRobotNodeSets();
 
    for (VirtualRobot::RobotNodeSetPtr s : robotNodeSets)
    {
        this->ui.comboBox->addItem(QString::fromStdString(s->getName()));
    }
 
    //Make sure comboBox is enabled so you can select a kinematic chain
    this->ui.comboBox->setEnabled(true);
    this->ui.cartesianselection->clear();
    //Get all caertesian selections and add them
    this->ui.cartesianselection->addItem(QString::fromStdString("Orientation and Position"));
    this->ui.cartesianselection->addItem(QString::fromStdString("Position"));
    this->ui.cartesianselection->addItem(QString::fromStdString("Orientation"));
    this->ui.cartesianselection->addItem(QString::fromStdString("X position"));
    this->ui.cartesianselection->addItem(QString::fromStdString("Y position"));
    this->ui.cartesianselection->addItem(QString::fromStdString("Z position"));
 
    //Make sure it is enabled
    this->ui.cartesianselection->setEnabled(true);
    this->ui.cartesianselection->setCurrentIndex(0);
 
    connectSlots();
    enableMainWidgetAsync(true);
}
 
void armarx::RobotIKWidgetController::onDisconnect()
{
    //Stop timers
    SoSensorManager* sensor_mgr = SoDB::getSensorManager();
    sensor_mgr->removeTimerSensor(textFieldUpdateSensor);
    sensor_mgr->removeTimerSensor(robotUpdateSensor);
 
    //Remove all options in comboBox
    this->ui.comboBox->clear();
 
    //Disable ui controls
    this->ui.comboBox->setEnabled(false);
    this->ui.moveTCP->setEnabled(false);
    this->ui.reachableLabel->setEnabled(false);
    this->ui.reachableLabel->setText(QString::fromStdString("No kinematic chain selected"));
    this->ui.reachableLabel->setStyleSheet("QLabel { color : red; }");
    this->ui.errorValue->setEnabled(false);
    this->ui.errorValue->setText(QString::fromStdString("Calculated error: 0"));
    this->ui.errorValue->setEnabled(false);
    this->ui.pushButton->setEnabled(false);
    this->ui.checkBox->setChecked(false);
    this->ui.checkBox->setEnabled(false);
    this->ui.currentPose->setEnabled(false);
    this->ui.currentPoseMatrix->setEnabled(false);
    this->ui.desiredPose->setEnabled(false);
    this->ui.desiredPoseMatrix->setEnabled(false);
    this->ui.resetManip->setEnabled(false);
    this->ui.btnCopyCurrentPoseToClipboard->setEnabled(false);
    this->ui.pushButton_setDesiredPose->setEnabled(false);
 
    //Remove all visualization
    this->ui.robotViewer->getRobotViewer()->getRootNode()->removeAllChildren();
 
    visualization = NULL;
    robotUpdateSensor = NULL;
    textFieldUpdateSensor = NULL;
}
 
void armarx::RobotIKWidgetController::onDisconnectComponent()
{
    QMetaObject::invokeMethod(this, "onDisconnect");
}
 
void armarx::RobotIKWidgetController::onExitComponent()
{
    //    enableMainWidgetAsync(false);
}
 
QPointer<QDialog> armarx::RobotIKWidgetController::getConfigDialog(QWidget* parent)
{
    if (!dialog)
    {
        dialog = new SimpleConfigDialog(parent);
        dialog->addProxyFinder<RobotStateComponentInterfacePrx>({"RobotStateComponent", "", "RobotState*"});
        dialog->addProxyFinder<KinematicUnitInterfacePrx>({"KinematicUnit", "", "*KinematicUnit|KinematicUnit*"});
        dialog->addProxyFinder<RobotIKInterfacePrx>({"RobotIK", "", "RobotIK*"});
    }
    return qobject_cast<SimpleConfigDialog*>(dialog);
}
 
void armarx::RobotIKWidgetController::loadSettings(QSettings* settings)
{
    robotStateComponentName = settings->value("robotStateComponentName").toString().toStdString();
    robotIKComponentName = settings->value("robotIKComponentName").toString().toStdString();
    kinematicUnitComponentName = settings->value("kinematicUnitComponentName").toString().toStdString();
}
 
void armarx::RobotIKWidgetController::saveSettings(QSettings* settings)
{
    settings->setValue("robotStateComponentName", robotStateComponentName.c_str());
    settings->setValue("robotIKComponentName", robotIKComponentName.c_str());
    settings->setValue("kinematicUnitComponentName", kinematicUnitComponentName.c_str());
}
 
void armarx::RobotIKWidgetController::configured()
{
    robotStateComponentName = dialog->getProxyName("RobotStateComponent");
    robotIKComponentName = dialog->getProxyName("RobotIK");
    kinematicUnitComponentName = dialog->getProxyName("KinematicUnit");
}
 
void armarx::RobotIKWidgetController::initWidget()
{
    ARMARX_INFO << "RobotIKWidgetController initWidget";
    //Initialize Gui
    ui.setupUi(getWidget());
    getWidget()->setEnabled(false);
    //Alignment for ui
    this->ui.gridLayout->setAlignment(Qt::AlignTop);
 
    //Label color can not be set in designer, so we do it here
    this->ui.reachableLabel->setStyleSheet("QLabel { color : red; }");
}
 
void armarx::RobotIKWidgetController::solveIK()
{
    robotIKPrx->begin_computeExtendedIKGlobalPose(this->ui.comboBox->currentText().toStdString(), new armarx::Pose(visualization->getUserDesiredPose()),
            convertOption(this->ui.cartesianselection->currentText().toStdString()),
            Ice::newCallback(this, &RobotIKWidgetController::ikCallbackExecuteMotion));
}
 
void armarx::RobotIKWidgetController::ikCallbackExecuteMotion(const Ice::AsyncResultPtr& r)
{
    RobotIKInterfacePrx prx = RobotIKInterfacePrx::uncheckedCast(r->getProxy());
    ExtendedIKResult solution = prx->end_computeExtendedIKGlobalPose(r);
    {
        ScopedLock lock(solutionMutex);
        currentSolution = improveCurrentSolution(solution);
    }
    QMetaObject::invokeMethod(this, "updateSolutionDisplay");
    QMetaObject::invokeMethod(this, "executeMotion");
}
void armarx::RobotIKWidgetController::ikCallbackDisplaySolution(const Ice::AsyncResultPtr& r)
 
{
    RobotIKInterfacePrx prx = RobotIKInterfacePrx::uncheckedCast(r->getProxy());
    ExtendedIKResult solution = prx->end_computeExtendedIKGlobalPose(r);
    {
        ScopedLock lock(solutionMutex);
        currentSolution = improveCurrentSolution(solution);
        if (currentSolution.isReachable)
        {
            updatePreviewRobot();
        }
    }
    QMetaObject::invokeMethod(this, "updateSolutionDisplay");
}
 
 
 
void armarx::RobotIKWidgetController::kinematicChainChanged(const QString& arg1)
{
    //An item has been selected, so we can allow the user to use the ui now
    //The manipulator will be set to the position of the current tcp, so this pose
    //has to be reachable!
    this->ui.moveTCP->setEnabled(true);
    this->ui.reachableLabel->setEnabled(true);
    this->ui.reachableLabel->setText(QString::fromStdString("Pose reachable!"));
    this->ui.reachableLabel->setStyleSheet("QLabel { color : green; }");
    this->ui.errorValue->setText(QString::fromStdString("Calculated error: 0"));
    this->ui.errorValue->setEnabled(true);
    this->ui.pushButton->setEnabled(true);
    this->ui.currentPose->setEnabled(true);
    this->ui.currentPoseMatrix->setEnabled(true);
    this->ui.desiredPose->setEnabled(true);
    this->ui.desiredPoseMatrix->setEnabled(true);
    this->ui.resetManip->setEnabled(true);
    this->ui.checkBox->setEnabled(true);
    this->ui.btnCopyCurrentPoseToClipboard->setEnabled(true);
    this->ui.pushButton_setDesiredPose->setEnabled(true);
 
    //Add visualization
    if (visualization && robot->hasRobotNodeSet(arg1.toStdString()))
    {
        visualization->setVisualization(robot, robot->getRobotNodeSet(arg1.toStdString()));
        //Add callbacks
        visualization->addManipFinishCallback(manipFinishCallback, this);
        visualization->addManipMovedCallback(manipMovedCallback, this);
    }
 
    armarx::RemoteRobot::synchronizeLocalClone(previewRobot, robotStateComponentPrx);
}
 
void armarx::RobotIKWidgetController::caertesianSelectionChanged(const QString& arg1)
{
    //If there is a manip in the scene we pretend it just moved to update color etc.
    if (visualization->getIsVisualizing())
    {
        manipFinishCallback(this, NULL);
    }
}
 
void armarx::RobotIKWidgetController::resetManip()
{
    //Triggers reset of manipulator in kinematicChainChanged
    kinematicChainChanged(this->ui.comboBox->currentText());
}
 
void armarx::RobotIKWidgetController::updateSolutionDisplay()
{
    if (getState() != eManagedIceObjectStarted)
    {
        return;
    }
 
 
    ScopedLock lock(solutionMutex);
 
    if (currentSolution.isReachable)
    {
        //Green
        ui.reachableLabel->setText(QString::fromStdString("Pose reachable!"));
        ui.reachableLabel->setStyleSheet("QLabel { color : green; }");
        visualization->setColor(0, 1, 0);
    }
    else
    {
        //Red
        ui.reachableLabel->setText(QString::fromStdString("Pose unreachable!"));
        ui.reachableLabel->setStyleSheet("QLabel { color : red; }");
        visualization->setColor(1, 0, 0);
    }
 
    //Display calculated error
    ui.errorValue->setText("Calculated error: " + QString::number(currentSolution.errorPosition, 'f', 1)
                           + " mm " + QString::number(currentSolution.errorOrientation * 180 / M_PI, 'f', 2) + " deg");
 
}
 
void armarx::RobotIKWidgetController::showCurrentRobotChanged(bool checked)
{
    auto visualizationRoot = this->ui.robotViewer->getRobotViewer()->getRootNode();
    if (checked)
    {
        if (visualizationRoot->findChild(currentRobotVisualization->getCoinVisualization()) == -1)
        {
            visualizationRoot->addChild(currentRobotVisualization->getCoinVisualization());
        }
    }
    else
    {
        if (visualizationRoot->findChild(currentRobotVisualization->getCoinVisualization()) != -1)
        {
            visualizationRoot->removeChild(currentRobotVisualization->getCoinVisualization());
        }
    }
}
 
void armarx::RobotIKWidgetController::showPreviewRobotChanged(bool checked)
{
    auto visualizationRoot = this->ui.robotViewer->getRobotViewer()->getRootNode();
    if (checked)
    {
        if (visualizationRoot->findChild(previewRobotVisualization->getCoinVisualization()) == -1)
        {
            visualizationRoot->addChild(previewRobotVisualization->getCoinVisualization());
        }
    }
    else
    {
        if (visualizationRoot->findChild(previewRobotVisualization->getCoinVisualization()) != -1)
        {
            visualizationRoot->removeChild(previewRobotVisualization->getCoinVisualization());
        }
    }
}
 
void armarx::RobotIKWidgetController::connectSlots()
{
    connect(ui.resetManip, SIGNAL(clicked()), this, SLOT(resetManip()), Qt::UniqueConnection);
    connect(ui.pushButton, SIGNAL(clicked()), this, SLOT(executeMotion()), Qt::UniqueConnection);
    connect(ui.comboBox, SIGNAL(currentIndexChanged(QString)), this, SLOT(kinematicChainChanged(QString)), Qt::UniqueConnection);
    connect(ui.cartesianselection, SIGNAL(currentIndexChanged(QString)), this, SLOT(caertesianSelectionChanged(QString)), Qt::UniqueConnection);
    connect(ui.checkBox, SIGNAL(toggled(bool)), this, SLOT(autoFollowChanged(bool)), Qt::UniqueConnection);
    connect(ui.btnCopyCurrentPoseToClipboard, SIGNAL(clicked()), this, SLOT(on_btnCopyCurrentPoseToClipboard_clicked()), Qt::UniqueConnection);
    connect(ui.checkBox_showCurrentRobot, SIGNAL(toggled(bool)), this, SLOT(showCurrentRobotChanged(bool)), Qt::UniqueConnection);
    connect(ui.checkBox_showPreviewRobot, SIGNAL(toggled(bool)), this, SLOT(showPreviewRobotChanged(bool)), Qt::UniqueConnection);
    connect(ui.pushButton_setDesiredPose, SIGNAL(clicked()), this, SLOT(on_btnSetDesiredPose_clicked()), Qt::UniqueConnection);
}
 
Ice::StringSeq armarx::RobotIKWidgetController::getIncludePaths() const
{
    Ice::StringSeq includePaths;
 
    try
    {
        Ice::StringSeq packages = robotStateComponentPrx->getArmarXPackages();
        packages.push_back(Application::GetProjectName());
 
        for (const std::string& projectName : packages)
        {
            if (projectName.empty())
            {
                continue;
            }
 
            CMakePackageFinder project(projectName);
            auto pathsString = project.getDataDir();
            Ice::StringSeq projectIncludePaths = armarx::Split(pathsString, ",;", true, true);
            includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
        }
    }
    catch (...)
    {
        ARMARX_ERROR << "Unable to retrieve robot filename." << std::endl;
    }
 
    return includePaths;
}
 
VirtualRobot::RobotPtr armarx::RobotIKWidgetController::loadRobot(Ice::StringSeq includePaths)
{
    try
    {
        std::string rfile = robotStateComponentPrx->getRobotFilename();
        ArmarXDataPath::getAbsolutePath(rfile, rfile, includePaths);
        return VirtualRobot::RobotIO::loadRobot(rfile);
    }
    catch (...)
    {
        ARMARX_ERROR << "Unable to load robot from file" << std::endl;
        return VirtualRobot::RobotPtr();
    }
}
 
void armarx::RobotIKWidgetController::manipFinishCallback(void* data, SoDragger* dragger)
{
    RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
    if (controller)
    {
        startIKSolving(controller);
    }
}
 
void armarx::RobotIKWidgetController::manipMovedCallback(void* data, SoDragger* dragger)
{
    RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
 
    if (controller)
    {
        controller->manipulatorMoved = true;
        QMetaObject::invokeMethod(controller, "showPreviewRobotChanged", Qt::QueuedConnection, Q_ARG(bool, false));
    }
}
 
void armarx::RobotIKWidgetController::robotUpdateTimerCB(void* data, SoSensor* sensor)
{
    RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
 
    if (!controller || !controller->robotStateComponentPrx || !controller->robot)
    {
        return;
    }
 
    try
    {
        armarx::RemoteRobot::synchronizeLocalClone(controller->robot, controller->robotStateComponentPrx);
 
        if (controller->startUpCameraPositioningFlag)
        {
            controller->ui.robotViewer->getRobotViewer()->cameraViewAll();
            controller->startUpCameraPositioningFlag = false;
        }
    }
    catch (...) {};
}
 
void armarx::RobotIKWidgetController::textFieldUpdateTimerCB(void* data, SoSensor* sensor)
{
    RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
 
    if (!controller)
    {
        return;
    }
 
    if (controller->visualization->getIsVisualizing())
    {
        Eigen::Matrix4f tcpMatrix = controller->robot->getRobotNodeSet(controller->ui.comboBox->currentText().toStdString())->getTCP()->getPoseInRootFrame();
        FramedPose actualPose(tcpMatrix,
                              controller->robot->getRootNode()->getName(),
                              controller->robot->getName());
        //Set text label to tcp matrix
        controller->ui.currentPoseMatrix->setText(QString::fromStdString(actualPose.output()));
 
        //Set text label for desired tcp pose
        FramedPose desired(controller->robot->getRootNode()->toLocalCoordinateSystem(controller->visualization->getUserDesiredPose()),
                           controller->robot->getRootNode()->getName(),
                           controller->robot->getName());
        controller->ui.desiredPoseMatrix->setText(QString::fromStdString(desired.output()));
    }
}
 
void armarx::RobotIKWidgetController::autoFollowSensorTimerCB(void* data, SoSensor* sensor)
{
    RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
 
    if (controller && controller->manipulatorMoved)
    {
        controller->solveIK();
        controller->manipulatorMoved = false;
    }
}
 
void armarx::RobotIKWidgetController::startIKSolving(RobotIKWidgetController* controller)
{
    if (controller)
    {
        controller->ui.reachableLabel->setText(QString::fromStdString("Calculating..."));
        controller->ui.reachableLabel->setStyleSheet("QLabel { color : black; }");
        controller->robotIKPrx->begin_computeExtendedIKGlobalPose(controller->ui.comboBox->currentText().toStdString(),
                new armarx::Pose(controller->visualization->getUserDesiredPose()),
                controller->convertOption(controller->ui.cartesianselection->currentText().toStdString()),
                Ice::newCallback(controller, &RobotIKWidgetController::ikCallbackDisplaySolution));
    }
}
 
armarx::ExtendedIKResult armarx::RobotIKWidgetController::improveCurrentSolution(armarx::ExtendedIKResult& solution)
{
    if (!ui.checkBox_cartesianControl->isChecked())
    {
        return solution;
    }
 
    if (!solution.isReachable)
    {
        return solution;
    }
 
    VirtualRobot::IKSolver::CartesianSelection vrmode;
    armarx::CartesianSelection mode = convertOption(ui.cartesianselection->currentText().toStdString());
    if (mode == CartesianSelection::eAll)
    {
        vrmode = VirtualRobot::IKSolver::CartesianSelection::All;
    }
    else if (mode == CartesianSelection::ePosition)
    {
        vrmode = VirtualRobot::IKSolver::CartesianSelection::Position;
    }
    else if (mode == CartesianSelection::eOrientation)
    {
        vrmode = VirtualRobot::IKSolver::CartesianSelection::Orientation;
    }
    else
    {
        return solution;
    }
 
    armarx::RemoteRobot::synchronizeLocalClone(cartesianControlRobot, robotStateComponentPrx);
    cartesianControlRobot->setJointValues(solution.jointAngles);
    VirtualRobot::RobotNodeSetPtr kc = cartesianControlRobot->getRobotNodeSet(ui.comboBox->currentText().toStdString());
    VirtualRobot::RobotNodePtr tcp = kc->getTCP();
    Eigen::Matrix4f globalTargetTCPPose = visualization->getUserDesiredPose();
    Eigen::Matrix4f localTarget = cartesianControlRobot->getRootNode()->toLocalCoordinateSystem(globalTargetTCPPose);
 
    CartesianPositionControllerPtr cartesianPositionController;
    cartesianPositionController.reset(new CartesianPositionController(tcp));
 
    CartesianVelocityControllerPtr cartesianVelocityController;
    cartesianVelocityController.reset(new CartesianVelocityController(kc, tcp, VirtualRobot::JacobiProvider::eSVD));
 
 
    float errorOriInitial = cartesianPositionController->getOrientationError(localTarget);
    float errorPosInitial = cartesianPositionController->getPositionError(localTarget);
 
    float stepLength = 0.05f;
    float eps = 0.001;
 
    for (int i = 0; i < CARTESIAN_CONTROLLER_LOOP_AMOUNT; i++)
    {
        Eigen::VectorXf tcpVelocities = cartesianPositionController->calculate(localTarget, vrmode);
        Eigen::VectorXf nullspaceVel = cartesianVelocityController->calculateJointLimitAvoidance(); // calculateNullspaceVelocity(tcpVelocities, 1.0f, vrmode);
        float nullspaceLen = nullspaceVel.norm();
        if (nullspaceLen > stepLength)
        {
            nullspaceVel = nullspaceVel / nullspaceLen * stepLength;
        }
        Eigen::VectorXf jointVelocities = cartesianVelocityController->calculate(tcpVelocities, nullspaceVel, vrmode);
 
        //jointVelocities = jointVelocities * stepLength;
        float len = jointVelocities.norm();
        if (len > stepLength)
        {
            jointVelocities = jointVelocities / len * stepLength;
        }
 
        for (size_t n = 0; n < kc->getSize(); n++)
        {
            kc->getNode(n)->setJointValue(kc->getNode(n)->getJointValue() + jointVelocities(n));
        }
        if (len < eps)
        {
            break;
        }
    }
 
    float errorOriAfter = cartesianPositionController->getOrientationError(localTarget);
    float errorPosAfter = cartesianPositionController->getPositionError(localTarget);
    if (errorOriAfter < errorOriInitial + 1.f / 180 * M_PI && errorPosAfter < errorPosInitial + 1.f)
    {
        solution.jointAngles = kc->getJointValueMap();
        solution.errorOrientation = errorOriAfter;
        solution.errorPosition = errorPosAfter;
    }
 
    return solution;
}
 
armarx::ExtendedIKResult armarx::RobotIKWidgetController::getIKSolution() const
{
    return ExtendedIKResult();
}
 
armarx::CartesianSelection armarx::RobotIKWidgetController::convertOption(std::string option) const
{
    if (option == "Orientation and Position")
    {
        return eAll;
    }
    else if (option == "Position")
    {
        return ePosition;
    }
    else if (option == "Orientation")
    {
        return eOrientation;
    }
    else if (option == "X position")
    {
        return eX;
    }
    else if (option == "Y position")
    {
        return eY;
    }
    else if (option == "Z position")
    {
        return eZ;
    }
 
    return eAll;
}
 
void armarx::RobotIKWidgetController::updatePreviewRobot()
{
    armarx::RemoteRobot::synchronizeLocalClone(previewRobot, robotStateComponentPrx);
    previewRobot->setJointValues(currentSolution.jointAngles);
    showPreviewRobotChanged(ui.checkBox_showPreviewRobot->isChecked());
}
 
void armarx::RobotIKWidgetController::on_btnSetDesiredPose_clicked()
{
    SetDesiredPoseDialog d;
 
    if (d.exec() == QDialog::Accepted)
    {
        FramedPosePtr newDesiredPose = d.getDesiredPose();
        if (newDesiredPose->frame.empty())
        {
            ARMARX_WARNING << "Frame of a FramedPose cannot be empty!";
        }
        else if (newDesiredPose->agent.empty())
        {
            ARMARX_WARNING << "Agent of a FramedPose cannot be empty!";
 
        }
        else
        {
            try
            {
                visualization->setUserDesiredPose(newDesiredPose->toGlobalEigen(robot));
                startIKSolving(this);
            }
            catch (...)
            {
                handleExceptions();
            }
        }
    }
}
 
void armarx::RobotIKWidgetController::executeMotion()
{
    if (currentSolution.isReachable)
    {
        //Switch all control modes to ePositionControl
        std::vector<VirtualRobot::RobotNodePtr> rn = robot->getRobotNodeSet(this->ui.comboBox->currentText().toStdString())->getAllRobotNodes();
        NameControlModeMap jointModes;
        NameValueMap jointAngles;
 
        for (unsigned int i = 0; i < rn.size(); i++)
        {
            jointModes[rn[i]->getName()] = ePositionControl;
            jointAngles[rn[i]->getName()] = 0.0f;
        }
 
        kinematicUnitInterfacePrx->switchControlMode(jointModes);
        kinematicUnitInterfacePrx->setJointAngles(currentSolution.jointAngles);
    }
}
 
void armarx::RobotIKWidgetController::autoFollowChanged(bool checked)
{
    if (checked)
    {
        //Make a timer update robo pose every time tick
        SoSensorManager* sensor_mgr = SoDB::getSensorManager();
        autoFollowSensor = new SoTimerSensor(autoFollowSensorTimerCB, this);
        autoFollowSensor->setInterval(SbTime(AUTO_FOLLOW_UPDATE / 1000.0f));
        sensor_mgr->insertTimerSensor(autoFollowSensor);
    }
    else
    {
        SoSensorManager* sensor_mgr = SoDB::getSensorManager();
        sensor_mgr->removeTimerSensor(autoFollowSensor);
    }
}
 
void armarx::RobotIKWidgetController::on_btnCopyCurrentPoseToClipboard_clicked()
{
 
    if (visualization->getIsVisualizing())
    {
 
        Eigen::Matrix4f tcpMatrix = robot->getRobotNodeSet(ui.comboBox->currentText().toStdString())->getTCP()->getPoseInRootFrame();
        FramedPosePtr pose = new FramedPose(tcpMatrix, robot->getRootNode()->getName(), robot->getName());
        JSONObjectPtr obj = new JSONObject();
        obj->serializeIceObject(pose);
 
 
        QClipboard* clipboard = QApplication::clipboard();
        clipboard->setText(QString::fromStdString(obj->asString(true)));
    }
}