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
#define AUTO_FOLLOW_UPDATE 333

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 = "";

    visualization = 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;
    }

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

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

    visualization = 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);
    this->ui.checkBox->setEnabled(true);


    //Add visualization
    visualization->setVisualization(robot, robot->getRobotNodeSet(arg1.toStdString()));
    //Add callback
    visualization->addManipFinishCallback(manipFinishCallback, this);
}

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::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);
    connect(ui.checkBox, SIGNAL(toggled(bool)), this, SLOT(autoFollowChanged(bool)), 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->ui.reachableLabel->setText(QString::fromStdString("Pose reachable!"));
        controller->ui.reachableLabel->setStyleSheet("QLabel { color : green; }");
        controller->visualization->setColor(0, 1, 0);
    }
    else {
        //Red
        controller->ui.reachableLabel->setText(QString::fromStdString("Pose unreachable!"));
        controller->ui.reachableLabel->setStyleSheet("QLabel { color : red; }");
        controller->visualization->setColor(1, 0, 0);
    }

    //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->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()->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
        controller->ui.desiredPoseMatrix->setText(QString::fromStdString(controller->visualization->getUserDesiredPoseString()));
    }
}

void armarx::RobotIKWidgetController::autoFollowSensorTimerCB(void *data, SoSensor *sensor)
{
    RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
    if (!controller)
        return;

    controller->solveIK();
}

armarx::ExtendedIKResult armarx::RobotIKWidgetController::getIKSolution()
{
//    mat = robot->getRootNode()->toLocalCoordinateSystem(mat);
    return(robotIKPrx->computeExtendedIKGlobalPose(this->ui.comboBox->currentText().toStdString(),new armarx::Pose(visualization->getUserDesiredPose()),
                                                   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)

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