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>
#include <ArmarXCore/util/json/JSONObject.h>
#include <QClipboard>

#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() : manipulatorMoved(false), startUpCameraPositioningFlag(true)
{


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

    visualization = NULL;
    robotUpdateSensor = NULL;
    textFieldUpdateSensor = NULL;
}

void armarx::RobotIKWidgetController::onInitComponent()
{
    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());

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

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::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()
{
    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 callbacks
    visualization->addManipFinishCallback(manipFinishCallback, this);
    visualization->addManipMovedCallback(manipMovedCallback, 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);
    connect(ui.btnCopyCurrentPoseToClipboard, SIGNAL(clicked()), this, SLOT(on_btnCopyCurrentPoseToClipboard_clicked()), 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);

    if (controller)
    {
        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::manipMovedCallback(void* data, SoDragger* dragger)
{
    RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);

    if (controller)
    {
        controller->manipulatorMoved = true;
    }
}

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

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

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