RobotIKGuiPlugin.cpp

/*
* 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>

/* 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>

#define ROBOT_UPDATE_TIMER_MS 33
#define TEXTFIELD_UPDATE_TIMER_MS 200

armarx::RobotIKGuiPlugin::RobotIKGuiPlugin()
{
    addWidget<RobotIKWidgetController>();
}

armarx::RobotIKWidgetController::RobotIKWidgetController() : startUpCameraPositioningFlag(true)
{
    //Initialize Gui
    ui.setupUi(getWidget());
    getWidget()->setEnabled(false);

    //Alignment for ui
    this->ui.verticalLayout->setAlignment(Qt::AlignTop);

    //Label color can not be set in designer, so we do it here
    this->ui.reachableLabel->setStyleSheet("QLabel { color : red; }");

    //Component names
    robotStateComponentName = "";
    robotIKComponentName = "";
    kinematicUnitComponentName = "";

    manipSeparator = NULL;
    currentSeparator = NULL;
    tcpManip = NULL;
    tcpManipTransform = NULL;
    tcpManipColor = NULL;
    tcpManipSphere = NULL;
    tcpCurrentTransform = NULL;
    tcpCurrentColor = NULL;
    tcpCurrentSphere = NULL;
    robotUpdateSensor = NULL;
    textFieldUpdateSensor = NULL;
}

void armarx::RobotIKWidgetController::onInitComponent()
{
    //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());
    if(!robot)
    {
        getObjectScheduler()->terminate();
        if(getWidget()->parentWidget())
        {
            getWidget()->parentWidget()->close();
        }
        ARMARX_ERROR << "RobotIKPlugin: Unable to load robot file! Terminating." << std::endl;
        return;
    }

    //Get visualization for our robot and add it to scene graph
    CoinVisualizationPtr robotViewerVisualization = robot->getVisualization<VirtualRobot::CoinVisualization>();
    this->ui.robotViewer->getRobotViewer()->getRootNode()->addChild(robotViewerVisualization->getCoinVisualization());

    //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);

    //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::onDisconnectComponent()
{
    //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.currentPose->setEnabled(false);
    this->ui.currentPoseMatrix->setEnabled(false);
    this->ui.desiredPose->setEnabled(false);
    this->ui.desiredPoseMatrix->setEnabled(false);
    this->ui.resetManip->setEnabled(false);

    //Remove all visualization
    this->ui.robotViewer->getRobotViewer()->getRootNode()->removeAllChildren();

    //Reset visualization pointers
    manipSeparator = NULL;
    currentSeparator = NULL;
    tcpManip = NULL;
    tcpManipTransform = NULL;
    tcpManipColor = NULL;
    tcpManipSphere = NULL;
    tcpCurrentTransform = NULL;
    tcpCurrentColor = NULL;
    tcpCurrentSphere = NULL;
    robotUpdateSensor = NULL;
    textFieldUpdateSensor = NULL;

    //Delete proxies
    robotStateComponentPrx = NULL;
    kinematicUnitInterfacePrx = NULL;
    robotIKPrx = NULL;
}

void armarx::RobotIKWidgetController::onExitComponent()
{
    enableMainWidgetAsync(false);
}

QPointer<QDialog> armarx::RobotIKWidgetController::getConfigDialog(QWidget *parent)
{
    if(!dialog)
    {
        dialog = new RobotIKConfigDialog(parent);
    }
    return qobject_cast<RobotIKConfigDialog*>(dialog);
}

void armarx::RobotIKWidgetController::loadSettings(QSettings *settings)
{

}

void armarx::RobotIKWidgetController::saveSettings(QSettings *settings)
{

}

void armarx::RobotIKWidgetController::configured()
{
    robotStateComponentName = dialog->robotStateComponentProxyFinder->getSelectedProxyName().trimmed().toStdString();
    robotIKComponentName = dialog->robotIKComponentProxyFinder->getSelectedProxyName().trimmed().toStdString();
    kinematicUnitComponentName = dialog->kinematicUnitComponentProxyFinder->getSelectedProxyName().trimmed().toStdString();
}

void armarx::RobotIKWidgetController::solveIK()
{
    auto targetJointAngles = getIKSolution();

    if(targetJointAngles.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(targetJointAngles.jointAngles);
    }
}

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);

    //Remove all current tcp and desired tcp pose visualization if present
    if(currentSeparator) {
        this->ui.robotViewer->getRobotViewer()->getRootNode()->removeChild(currentSeparator);
    }
    if(manipSeparator) {
        this->ui.robotViewer->getRobotViewer()->getRootNode()->removeChild(manipSeparator);
    }

    //Create new Visualization
    manipSeparator = new SoSeparator;
    currentSeparator = new SoSeparator;

    tcpCurrentColor = new SoMaterial;
    tcpCurrentColor->transparency = 0.6f;
    tcpCurrentSphere = new SoSphere;
    tcpCurrentSphere->radius = 0.06f;
    tcpManipColor = new SoMaterial;
    tcpManipColor->ambientColor.setValue(0, 1, 0);
    tcpManipColor->transparency = 0.3f;
    tcpManipSphere = new SoSphere;
    tcpManipSphere->radius = 0.06f;
    tcpManip = new SoTransformerManip;

    //We need this null separator to remove the scale knobs
    //This won't lead to memory leak, because this guy
    //will be deleted when the separator itself gets removed
    //from the scene graph
    SoSeparator* nullSep = new SoSeparator;

    //Make all scale knobs disappear
    tcpManip->getDragger()->setPart("scale1", nullSep);
    tcpManip->getDragger()->setPart("scale2", nullSep);
    tcpManip->getDragger()->setPart("scale3", nullSep);
    tcpManip->getDragger()->setPart("scale4", nullSep);
    tcpManip->getDragger()->setPart("scale5", nullSep);
    tcpManip->getDragger()->setPart("scale6", nullSep);
    tcpManip->getDragger()->setPart("scale7", nullSep);
    tcpManip->getDragger()->setPart("scale8", nullSep);

    tcpManip->getDragger()->addFinishCallback(manipFinishCallback, this);
    tcpCurrentTransform = new SoTransform;
    tcpManipTransform = new SoTransform;

    manipSeparator->addChild(tcpManipTransform);
    manipSeparator->addChild(tcpManip);
    manipSeparator->addChild(tcpManipColor);
    manipSeparator->addChild(tcpManipSphere);

    currentSeparator->addChild(tcpCurrentTransform);
    currentSeparator->addChild(tcpCurrentColor);
    currentSeparator->addChild(tcpCurrentSphere);

    this->ui.robotViewer->getRobotViewer()->getRootNode()->insertChild(currentSeparator, 0);
    this->ui.robotViewer->getRobotViewer()->getRootNode()->insertChild(manipSeparator, 0);

    //Get TCP pose
    Eigen::Matrix4f mat = robot->getRobotNodeSet(arg1.toStdString())->getTCP()->getGlobalPose();
    mat(0, 3) /= 1000;
    mat(1, 3) /= 1000;
    mat(2, 3) /= 1000;

    //Apply to current and desired visualizer
    tcpCurrentTransform->setMatrix(VirtualRobot::CoinVisualizationFactory::getSbMatrix(mat));
    tcpManipTransform->setMatrix(VirtualRobot::CoinVisualizationFactory::getSbMatrix(mat));

    /*tcpManip->translation.setValue(mat(0, 3) / 1000, mat(1, 3) / 1000, mat(2, 3) / 1000);
    tcpManip->rotation.setValue(SbRotation(VirtualRobot::CoinVisualizationFactory::getSbMatrix(mat)));*/
}

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(tcpManip) {
        manipFinishCallback(this, NULL);
    }
}

void armarx::RobotIKWidgetController::resetManip()
{
    //Triggers reset of manipulator in kinematicChainChanged
   kinematicChainChanged(this->ui.comboBox->currentText());
}

void armarx::RobotIKWidgetController::connectSlots()
{
    connect(ui.resetManip, SIGNAL(clicked()), this, SLOT(resetManip()), Qt::QueuedConnection);
    connect(ui.pushButton, SIGNAL(clicked()), this, SLOT(solveIK()), Qt::QueuedConnection);
    connect(ui.comboBox, SIGNAL(currentIndexChanged(QString)), this, SLOT(kinematicChainChanged(QString)), Qt::UniqueConnection);
    connect(ui.cartesianselection, SIGNAL(currentIndexChanged(QString)), this, SLOT(caertesianSelectionChanged(QString)), Qt::UniqueConnection);
}

armarx::StringList armarx::RobotIKWidgetController::getIncludePaths()
{
    StringList includePaths;

    try {
        StringList packages = robotStateComponentPrx->getArmarXPackages();
        packages.push_back(Application::GetProjectName());
        for(const std::string &projectName : packages)
        {
            if(projectName.empty())
                continue;
            CMakePackageFinder project(projectName);
            StringList projectIncludePaths;
            auto pathsString = project.getDataDir();
            boost::split(projectIncludePaths,
                         pathsString,
                         boost::is_any_of(";,"),
                         boost::token_compress_on);
            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(StringList 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);

    auto targetJointAngles = controller->getIKSolution();

    if(targetJointAngles.isReachable) {
        //Green
        controller->tcpManipColor->ambientColor.setValue(0, 1, 0);
        controller->ui.reachableLabel->setText(QString::fromStdString("Pose reachable!"));
        controller->ui.reachableLabel->setStyleSheet("QLabel { color : green; }");
    }
    else {
        //Red
        controller->tcpManipColor->ambientColor.setValue(1, 0, 0);
        controller->ui.reachableLabel->setText(QString::fromStdString("Pose unreachable!"));
        controller->ui.reachableLabel->setStyleSheet("QLabel { color : red; }");
    }

    //Display calculated error
    controller->ui.errorValue->setText(QString::fromStdString("Calculated error: " + boost::lexical_cast<std::string>(targetJointAngles.error)));
}

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->currentSeparator) {
            Eigen::Matrix4f tcpMatrix = controller->robot->getRobotNodeSet(controller->ui.comboBox->currentText().toStdString())->getTCP()->getGlobalPose();
            //Apply tcp position to indicator
            tcpMatrix(0, 3) /= 1000;
            tcpMatrix(1, 3) /= 1000;
            tcpMatrix(2, 3) /= 1000;
            controller->tcpCurrentTransform->setMatrix(VirtualRobot::CoinVisualizationFactory::getSbMatrix(tcpMatrix));
        }

        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->currentSeparator) {
        Eigen::Matrix4f tcpMatrix = controller->robot->getRobotNodeSet(controller->ui.comboBox->currentText().toStdString())->getTCP()->getGlobalPose();

        //Set text label to tcp matrix
        std::stringstream buffer;
        buffer << tcpMatrix;
        std::string matrixText = buffer.str();
        controller->ui.currentPoseMatrix->setText(QString::fromStdString(matrixText));

        //Set text label for desired tcp pose
        //Therefore calculate current manipulator pose
        SoGetMatrixAction* action = new SoGetMatrixAction(SbViewportRegion());
        SoSearchAction sa;
        sa.setNode(controller->tcpManipSphere);
        sa.setSearchingAll( TRUE );              // Search all nodes
        SoBaseKit::setSearchingChildren( TRUE ); // Even inside nodekits
        sa.apply(controller->ui.robotViewer->getRobotViewer()->getRootNode());

        action->apply(sa.getPath());

        SbMatrix matrix = action->getMatrix();

        Eigen::Matrix4f mat = Eigen::Matrix4f::Identity();
        mat (0,0) = matrix[0][0];
        mat (0,1) = matrix[1][0];
        mat (0,2) = matrix[2][0];
        mat (0,3) = matrix[3][0] * 1000;

        mat (1,0) = matrix[0][1];
        mat (1,1) = matrix[1][1];
        mat (1,2) = matrix[2][1];
        mat (1,3) = matrix[3][1] * 1000;

        mat (2,0) = matrix[0][2];
        mat (2,1) = matrix[1][2];
        mat (2,2) = matrix[2][2];
        mat (2,3) = matrix[3][2] * 1000;

        mat (3,0) = matrix[0][3];
        mat (3,1) = matrix[1][3];
        mat (3,2) = matrix[2][3];
        mat (3,3) = matrix[3][3];

        //Set text label to manip matrix
        std::stringstream buffer2;
        buffer2 << mat;
        std::string matrixText2 = buffer2.str();
        controller->ui.desiredPoseMatrix->setText(QString::fromStdString(matrixText2));

        delete(action);
    }
}

armarx::ExtendedIKResult armarx::RobotIKWidgetController::getIKSolution()
{
    SoGetMatrixAction* action = new SoGetMatrixAction(SbViewportRegion());
    SoSearchAction sa;
    sa.setNode(tcpManipSphere);
    sa.setSearchingAll( TRUE );              // Search all nodes
    SoBaseKit::setSearchingChildren( TRUE ); // Even inside nodekits
    sa.apply(this->ui.robotViewer->getRobotViewer()->getRootNode());

    action->apply(sa.getPath());

    SbMatrix matrix = action->getMatrix();

    Eigen::Matrix4f mat = Eigen::Matrix4f::Identity();
    mat (0,0) = matrix[0][0];
    mat (0,1) = matrix[1][0];
    mat (0,2) = matrix[2][0];
    mat (0,3) = matrix[3][0] * 1000;

    mat (1,0) = matrix[0][1];
    mat (1,1) = matrix[1][1];
    mat (1,2) = matrix[2][1];
    mat (1,3) = matrix[3][1] * 1000;

    mat (2,0) = matrix[0][2];
    mat (2,1) = matrix[1][2];
    mat (2,2) = matrix[2][2];
    mat (2,3) = matrix[3][2] * 1000;

    mat (3,0) = matrix[0][3];
    mat (3,1) = matrix[1][3];
    mat (3,2) = matrix[2][3];
    mat (3,3) = matrix[3][3];

//    mat = robot->getRootNode()->toLocalCoordinateSystem(mat);
    return(robotIKPrx->computeExtendedIKGlobalPose(this->ui.comboBox->currentText().toStdString(),new armarx::Pose(mat),
                                                   convertOption(this->ui.cartesianselection->currentText().toStdString())));
}

armarx::CartesianSelection armarx::RobotIKWidgetController::convertOption(std::string option)
{
    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;
}

Q_EXPORT_PLUGIN2(armarx_gui_RobotIKGuiPlugin, armarx::RobotIKGuiPlugin)