SimulatorControlGuiPlugin.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    MemoryX::SimulatorControlGui
* @author     Nikolaus Vahrenkamp (vahrenkamp at kit dot edu)
* @date       2012
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "SimulatorControlGuiPlugin.h"
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <RobotAPI/libraries/core/Pose.h>
 
 
// Qt headers
#include <QBrush>
#include <QCheckBox>
#include <QMessageBox>
#include <QPushButton>
#include <QSlider>
#include <QSpinBox>
#include <QStringList>
#include <QTableView>
#include <Qt>
#include <QtGlobal>
 
// Coin3D headers
#include <Inventor/Qt/SoQt.h>
#include <Inventor/SoDB.h>
#include <Inventor/nodes/SoUnits.h>
 
// System
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
 
#include <iostream>
#include <string>
 
// Simox-VirtualRobot
#include <SimoxUtility/math/convert/pos_rpy_to_mat4f.h>
#include <VirtualRobot/Grasping/Grasp.h>
#include <VirtualRobot/Grasping/GraspSet.h>
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/XML/ObjectIO.h>
#include <VirtualRobot/XML/RobotIO.h>
 
using namespace armarx;
using namespace VirtualRobot;
 
#define DEFAULT_SETTINGS_SIMULATOR_PROXY_NAME "Simulator"
#define DEFAULT_SETTINGS_SIMULATORVIEWER_PROXY_NAME "SimulatorViewer_SimulationWindow"
#define DEFAULT_SETTINGS_TIMER_MS_UPDATE_SIM_INFO 500.0f
 
static const int POLL_DELAY_MS = 1000;
 
SimulatorControlGuiPlugin::SimulatorControlGuiPlugin()
{
    addWidget<SimulatorControlController>();
}
 
SimulatorControlController::SimulatorControlController()
{
    // init gui
    ui.setupUi(getWidget());
 
    ui.layerTable->setColumnWidth(0, 200);
    ui.layerTable->setColumnWidth(1, 40);
    ui.layerTable->setColumnWidth(2, 40);
    ui.layerTable->setColumnWidth(3, 80);
    ui.layerTable->setColumnWidth(4, 80);
    ui.layerTable->horizontalHeader()->setResizeMode(0, QHeaderView::Stretch);
    ui.layerTable->horizontalHeader()->setResizeMode(1, QHeaderView::Fixed);
    ui.layerTable->horizontalHeader()->setResizeMode(2, QHeaderView::Fixed);
    ui.layerTable->horizontalHeader()->setResizeMode(3, QHeaderView::Fixed);
    ui.layerTable->horizontalHeader()->setResizeMode(4, QHeaderView::Fixed);
    for (int i = ui.layerTable->rowCount(); i; --i)
    {
        ui.layerTable->removeRow(0);
    }
 
    ARMARX_INFO << "Finished setup of SimulatorControlGuiPlugin" << flush;
}
 
void
SimulatorControlController::onInitComponent()
{
    verbose = true;
 
    ARMARX_INFO << "Init SimulatorControlController " << flush;
 
    settings_simulatorPrxName = DEFAULT_SETTINGS_SIMULATOR_PROXY_NAME;
    settings_simulatorViewerPrxName = DEFAULT_SETTINGS_SIMULATORVIEWER_PROXY_NAME;
 
    usingProxy(settings_simulatorPrxName);
    connectSlots();
}
 
void
SimulatorControlController::onConnectComponent()
{
    // subscribe topic in order to receive the simulator reports
    ARMARX_INFO << "Using simulator proxy: " << settings_simulatorPrxName << flush;
    simulatorPrx = getProxy<SimulatorInterfacePrx>(settings_simulatorPrxName);
 
    timerId = startTimer(DEFAULT_SETTINGS_TIMER_MS_UPDATE_SIM_INFO);
    //start timer
    layerPollTimer.start(POLL_DELAY_MS);
    enableMainWidgetAsync(true);
}
 
void
SimulatorControlController::onDisconnectComponent()
{
    killTimer(timerId);
    layerPollTimer.stop();
    enableMainWidgetAsync(false);
}
 
void
SimulatorControlController::onExitComponent()
{
}
 
void
SimulatorControlController::loadSettings(QSettings* settings)
{
    /*robotFile = settings->value("RobotFile", QString::fromStdString(robotFile_default)).toString().toStdString();
    objectFile = settings->value("ObjectFile", QString::fromStdString(objectFile_default)).toString().toStdString();
    show3DViewer = settings->value("ViewerEnabled", "false") == "true";*/
}
 
void
SimulatorControlController::saveSettings(QSettings* settings)
{
    /*settings->setValue("RobotFile", QString::fromStdString(robotFile));
    settings->setValue("ObjectFile", QString::fromStdString(objectFile));
    settings->setValue("ViewerEnabled", QString(show3DViewer?"true":"false"));*/
}
 
QPointer<QWidget>
SimulatorControlController::getCustomTitlebarWidget(QWidget* parent)
{
    if (customToolbar)
    {
        customToolbar->setParent(parent);
    }
    else
    {
        customToolbar = new QToolBar(parent);
        customToolbar->addAction("Play / Stop", this, SLOT(playStopSim()));
        customToolbar->addSeparator();
        customToolbar->addAction("Reinit", this, SLOT(reInit()));
    }
    return customToolbar.data();
}
 
void
SimulatorControlController::connectSlots()
{
    connect(
        ui.cbWindow, SIGNAL(toggled(bool)), this, SLOT(showSimWindow(bool)), Qt::QueuedConnection);
    connect(ui.cbShowCoordSystem,
            SIGNAL(toggled(bool)),
            this,
            SLOT(showCoordSystem(bool)),
            Qt::QueuedConnection);
    connect(
        ui.cbContacts, SIGNAL(toggled(bool)), this, SLOT(showContacts(bool)), Qt::QueuedConnection);
    connect(ui.rbFull, SIGNAL(toggled(bool)), this, SLOT(selectVisuType()), Qt::QueuedConnection);
    connect(ui.rbCol, SIGNAL(toggled(bool)), this, SLOT(selectVisuType()), Qt::QueuedConnection);
    connect(
        ui.pushButtonPlayStop, SIGNAL(clicked()), this, SLOT(playStopSim()), Qt::QueuedConnection);
    connect(ui.pushButtonStep, SIGNAL(clicked()), this, SLOT(stepSim()), Qt::QueuedConnection);
    connect(ui.pushButtonReInit, SIGNAL(clicked()), this, SLOT(reInit()), Qt::QueuedConnection);
 
    connect(ui.spinBoxAntiAliasing,
            SIGNAL(valueChanged(int)),
            this,
            SLOT(antiAliasing(int)),
            Qt::QueuedConnection);
 
    //connect signal mapper
    QObject::connect(&layerSignalMapperVisible,
                     SIGNAL(mapped(QString)),
                     this,
                     SLOT(layerToggleVisibility(QString)));
    QObject::connect(
        &layerSignalMapperClear, SIGNAL(mapped(QString)), this, SLOT(layerClear(QString)));
    QObject::connect(
        &layerSignalMapperRemove, SIGNAL(mapped(QString)), this, SLOT(layerRemove(QString)));
    //connect poll timer
    QObject::connect(&layerPollTimer, SIGNAL(timeout()), this, SLOT(layerPoll()));
 
    connect(ui.pushButtonAddRobot, SIGNAL(clicked()), this, SLOT(on_pushButtonAddRobot_clicked()));
    connect(ui.pushButtonManipRobotUpdate,
            SIGNAL(clicked()),
            this,
            SLOT(on_pushButtonManipRobotUpdate_clicked()));
    connect(ui.pushButtonManipRobSetPose,
            SIGNAL(clicked()),
            this,
            SLOT(on_pushButtonManipRobSetPose_clicked()));
 
    ui.tableObjects->horizontalHeader()->setResizeMode(QHeaderView::ResizeToContents);
    ui.layerTable->horizontalHeader()->setResizeMode(QHeaderView::ResizeToContents);
}
 
void
SimulatorControlController::timerEvent(QTimerEvent*)
{
    QString textSimTime("SimulationTime:");
    QString textComTime("CommunicationTime:");
    QString textRobots("Robots:");
    QString textJoints("Actuated Joints:");
    QString textObjects("Objects:");
    QString textContacts("Contacts:");
    QString textDraw("DrawTime:");
    QString textSync("SyncTime:");
    QString curTime;
    QString textCam("Camera ");
 
    try
    {
        if (simulatorPrx)
        {
            SimulatorInformation i = simulatorPrx->getSimulatorInformation();
 
            textSimTime += QString::number((double)i.simTimeStepMeasuredMS, 'f', 2);
            textSimTime += QString(" ms (x ");
            textSimTime += QString::number((double)i.simTimeFactor, 'f', 2);
            textSimTime += QString(")");
 
            textComTime += QString::number((double)i.comTimeMS, 'f', 2);
            textComTime += QString(" ms");
 
            textSync += QString::number((double)i.syncEngineTimeMS, 'f', 2);
            textSync += QString(" ms");
 
            textRobots += QString::number(i.nrRobots);
            textJoints += QString::number(i.nrActuatedJoints);
            textObjects += QString::number(i.nrObjects);
            textContacts += QString::number(i.nrContacts);
            curTime = QString::number(i.currentSimulatorTimeSec, 'f', 3);
 
            // update object info
 
            ui.tableObjects->setRowCount(i.objects.size());
 
            for (std::size_t ob = 0; ob < i.objects.size(); ++ob)
            {
                const auto& o = i.objects.at(ob);
                QString name = QString::fromStdString(o.name);
                //ARMARX_INFO << "objects:" <<  VAROUT(o.objectPoses);
                PoseBasePtr pose = o.objectPoses.begin()->second;
                Eigen::Matrix4f gp = PosePtr::dynamicCast(pose)->toEigen();
                float x = gp(0, 3);
                float y = gp(1, 3);
                float z = gp(2, 3);
                QString pos = QString::number(x, 'f', 2) + "," + QString::number(y, 'f', 2) + "," +
                              QString::number(z, 'f', 2);
                Eigen::Vector3f rpy;
                VirtualRobot::MathTools::eigen4f2rpy(gp, rpy);
                QString ori = QString::number(rpy(0), 'f', 2) + "," +
                              QString::number(rpy(1), 'f', 2) + "," +
                              QString::number(rpy(2), 'f', 2);
 
                QString status;
 
                if (o.staticObject)
                {
                    status = "static";
                }
                else
                {
                    status = "dynamic";
                }
 
                //add name and number of elements
                ui.tableObjects->setItem(ob, 0, new QTableWidgetItem{name});
                ui.tableObjects->setItem(ob, 1, new QTableWidgetItem{pos});
                ui.tableObjects->setItem(ob, 2, new QTableWidgetItem{ori});
                ui.tableObjects->setItem(ob, 3, new QTableWidgetItem{status});
            }
        }
 
        if (!simulatorViewerPrx)
        {
            simulatorViewerPrx =
                getProxy<SimulatorViewerControlInterfacePrx>(settings_simulatorViewerPrxName);
        }
 
        if (simulatorViewerPrx)
        {
            SimulatorViewerInformation i = simulatorViewerPrx->getSimulatorInformation();
            textDraw += QString::number((double)i.drawTimeMS + (double)i.comTimeMS, 'f', 2);
            textDraw += QString(" ms");
 
            //textDraw += " / ";
            //textDraw += QString::number((double)i.comTimeMS, 'f', 2);
 
            PoseBasePtr p = simulatorViewerPrx->getCameraPose();
            PosePtr p2 = PosePtr::dynamicCast(p);
            Eigen::Matrix4f p3 = p2->toEigen();
            Eigen::Vector3f rpy;
            MathTools::eigen4f2rpy(p3, rpy);
            textCam += "pos:";
            textCam += QString::number(p2->position->x * 1000.0f, 'f', 2);
            textCam += ",";
            textCam += QString::number(p2->position->y * 1000.0f, 'f', 2);
            textCam += ",";
            textCam += QString::number(p2->position->z * 1000.0f, 'f', 2);
            textCam += ", rpy:";
            textCam += QString::number(rpy(0), 'f', 2);
            textCam += ",";
            textCam += QString::number(rpy(1), 'f', 2);
            textCam += ",";
            textCam += QString::number(rpy(2), 'f', 2);
        }
    }
    catch (...)
    {
        // silently ignore lost connections
    }
 
    ui.lSimTime->setText(textSimTime);
    ui.lComTime->setText(textComTime);
    ui.lRobots->setText(textRobots);
    ui.lJoints->setText(textJoints);
    ui.lObjects->setText(textObjects);
    ui.lContacts->setText(textContacts);
    ui.lDrawTime->setText(textDraw);
    ui.lSyncTime->setText(textSync);
    ui.lcdNumberSimTime->setText(curTime);
 
    ui.labelCamPose->setText(textCam);
}
 
void
SimulatorControlController::showSimWindow(bool enable)
{
    try
    {
        if (!simulatorViewerPrx)
        {
            simulatorViewerPrx =
                getProxy<SimulatorViewerControlInterfacePrx>(settings_simulatorViewerPrxName);
        }
 
        if (simulatorViewerPrx)
        {
            simulatorViewerPrx->enableSimulatorWindow(enable);
        }
    }
    catch (...)
    {
        handleExceptions();
    }
}
 
void
SimulatorControlController::showCoordSystem(bool enable)
{
    try
    {
        if (!simulatorViewerPrx)
        {
            simulatorViewerPrx =
                getProxy<SimulatorViewerControlInterfacePrx>(settings_simulatorViewerPrxName);
        }
 
        if (simulatorViewerPrx)
        {
            simulatorViewerPrx->showBaseCoordSystem(enable, 5.0f);
        }
    }
    catch (...)
    {
        handleExceptions();
    }
}
 
void
SimulatorControlController::showContacts(bool enable)
{
    try
    {
        simulatorPrx->showContacts(enable, "contacts");
        /*if (!simulatorViewerPrx)
            simulatorViewerPrx = getProxy<SimulatorViewerControlInterfacePrx>(settings_simulatorViewerPrxName);
        if (simulatorViewerPrx)
            simulatorViewerPrx->showContacts(enable);*/
    }
    catch (...)
    {
        handleExceptions();
    }
}
 
void
SimulatorControlController::selectVisuType()
{
    try
    {
        bool fullModel = ui.rbFull->isChecked();
 
        if (!simulatorViewerPrx)
        {
            simulatorViewerPrx =
                getProxy<SimulatorViewerControlInterfacePrx>(settings_simulatorViewerPrxName);
        }
 
        if (simulatorViewerPrx)
        {
            simulatorViewerPrx->selectVisuType(fullModel);
        }
    }
    catch (...)
    {
        handleExceptions();
    }
}
 
void
SimulatorControlController::playStopSim()
{
    try
    {
        bool running = simulatorPrx->isRunning();
 
        if (running)
        {
            simulatorPrx->pause();
            ui.pushButtonStep->setEnabled(true);
        }
        else
        {
            simulatorPrx->start();
            ui.pushButtonStep->setEnabled(false);
        }
    }
    catch (...)
    {
        handleExceptions();
    }
}
 
void
SimulatorControlController::stepSim()
{
    try
    {
        bool running = simulatorPrx->isRunning();
 
        if (!running)
        {
            simulatorPrx->step();
        }
    }
    catch (...)
    {
        handleExceptions();
    }
}
 
void
SimulatorControlController::reInit()
{
    if (!simulatorPrx)
    {
        return;
    }
 
    try
    {
        simulatorPrx->reInitialize();
    }
    catch (...)
    {
        handleExceptions();
    }
}
 
void
SimulatorControlController::antiAliasing(int steps)
{
    try
    {
        if (!simulatorViewerPrx)
        {
            simulatorViewerPrx =
                getProxy<SimulatorViewerControlInterfacePrx>(settings_simulatorViewerPrxName);
        }
 
        if (simulatorViewerPrx)
        {
            simulatorViewerPrx->setAntiAliasing(steps);
        }
    }
    catch (...)
    {
        handleExceptions();
    }
}
 
void
SimulatorControlController::layerRemove(QString layerName)
{
    try
    {
        if (!simulatorViewerPrx)
        {
            simulatorViewerPrx =
                getProxy<SimulatorViewerControlInterfacePrx>(settings_simulatorViewerPrxName);
        }
 
        if (simulatorViewerPrx)
        {
            std::string name = layerName.toStdString();
            ARMARX_VERBOSE << "Removing layer " << name << "...";
            simulatorViewerPrx->removeLayer(name);
            ARMARX_VERBOSE << "done!";
        }
    }
    catch (...)
    {
        handleExceptions();
    }
}
 
void
SimulatorControlController::layerClear(QString layerName)
{
    try
    {
        if (!simulatorViewerPrx)
        {
            simulatorViewerPrx =
                getProxy<SimulatorViewerControlInterfacePrx>(settings_simulatorViewerPrxName);
        }
 
        if (simulatorViewerPrx)
        {
            std::string name = layerName.toStdString();
            ARMARX_VERBOSE << "Clearing layer " << name << "...";
            simulatorViewerPrx->clearLayer(name);
            ARMARX_VERBOSE << "done!";
        }
    }
    catch (...)
    {
        handleExceptions();
    }
}
 
void
SimulatorControlController::layerToggleVisibility(QString layerName)
{
    try
    {
        if (!simulatorViewerPrx)
        {
            simulatorViewerPrx =
                getProxy<SimulatorViewerControlInterfacePrx>(settings_simulatorViewerPrxName);
        }
 
        if (simulatorViewerPrx)
        {
            auto name = layerName.toStdString();
 
            if (layerVisibilityCheckBoxes.count(name))
            {
                const bool vis = layerVisibilityCheckBoxes.at(name).second->isChecked();
                ARMARX_VERBOSE << "Toggling visibility from " << !vis << " to " << vis
                               << " for layer " << name << "...";
                simulatorViewerPrx->enableLayerVisu(name, vis);
                ARMARX_VERBOSE << "done!";
            }
        }
    }
    catch (...)
    {
        handleExceptions();
    }
}
 
void
SimulatorControlController::layerPoll()
{
    try
    {
        if (!simulatorViewerPrx)
        {
            simulatorViewerPrx =
                getProxy<SimulatorViewerControlInterfacePrx>(settings_simulatorViewerPrxName);
        }
 
        if (simulatorViewerPrx)
        {
            auto layerInfo = simulatorViewerPrx->layerInformation();
            ui.layerTable->setRowCount(layerInfo.size());
            LayerVisibilityCheckBoxesType newLayerVisibilityCheckBoxes;
 
            const bool hideEmptyLayers = ui.checkBoxHideEmptyLayers->isChecked();
            //fill & map signals
            for (const auto& layer : layerInfo)
            {
                auto& layerEntry = newLayerVisibilityCheckBoxes[layer.layerName];
                int tableIdx = 0;
                //create row if missing
                {
                    if (layerVisibilityCheckBoxes.count(layer.layerName))
                    {
                        //already there
                        layerEntry = layerVisibilityCheckBoxes.at(layer.layerName);
                        layerVisibilityCheckBoxes.erase(layer.layerName);
                        //search for idx
                        bool found = false;
                        for (; tableIdx < ui.layerTable->rowCount(); ++tableIdx)
                        {
                            const QTableWidgetItem* item = ui.layerTable->item(tableIdx, 0);
                            if (item)
                            {
                                found = (item->text() == QString::fromStdString(layer.layerName));
                            }
                            else
                            {
                                ARMARX_WARNING << "layerPoll(search old): item " << tableIdx
                                               << " is null";
                            }
                            if (found)
                            {
                                break;
                            }
                        }
                    }
                    else
                    {
                        ui.layerTable->insertRow(0);
                        //missing! -> create it
                        QString name = QString::fromStdString(layer.layerName);
                        //store visibility
                        //set text
                        ui.layerTable->setItem(0, 0, new QTableWidgetItem{name});
 
                        //add&connect checkbox
                        QCheckBox* box{new QCheckBox};
                        ui.layerTable->setCellWidget(0, 2, box);
                        layerEntry.second = box;
                        //                        layerEntry.first = ui.layerTable->item(0,0);
                        layerSignalMapperVisible.setMapping(box, name);
                        QObject::connect(
                            box, SIGNAL(stateChanged(int)), &layerSignalMapperVisible, SLOT(map()));
 
                        //add&connect button clear
                        QPushButton* clear{new QPushButton{"Clear"}};
                        ui.layerTable->setCellWidget(0, 3, clear);
                        layerSignalMapperClear.setMapping(clear, name);
                        QObject::connect(
                            clear, SIGNAL(clicked()), &layerSignalMapperClear, SLOT(map()));
 
                        //add&connect button remove
                        QPushButton* remove{new QPushButton{"Remove"}};
                        ui.layerTable->setCellWidget(0, 4, remove);
                        layerSignalMapperRemove.setMapping(remove, name);
                        QObject::connect(
                            remove, SIGNAL(clicked()), &layerSignalMapperRemove, SLOT(map()));
                    }
                }
                //                ARMARX_CHECK_NOT_NULL(layerEntry.first);
                ARMARX_CHECK_NOT_NULL(layerEntry.second);
                //                const auto tableIdx = ui.layerTable->row(layerEntry.first);
                QCheckBox* box = layerEntry.second;
                //update layer
                ui.layerTable->setItem(
                    tableIdx, 1, new QTableWidgetItem{QString::number(layer.elementCount)});
                box->blockSignals(true);
                box->setChecked(layer.visible);
                box->blockSignals(false);
 
                if (!layer.elementCount && hideEmptyLayers)
                {
                    //hide it
                    ui.layerTable->hideRow(tableIdx);
                    continue;
                }
                ui.layerTable->showRow(tableIdx);
            }
            //remove old entries
            for (const auto& pair : layerVisibilityCheckBoxes)
            {
                ui.layerTable->removeRow(ui.layerTable->row(pair.second.first));
            }
            for (int i = ui.layerTable->rowCount() - 1; i >= 0; --i)
            {
                const QTableWidgetItem* item = ui.layerTable->item(i, 0);
                if (item)
                {
                    if (layerVisibilityCheckBoxes.count(item->text().toStdString()))
                    {
                        ui.layerTable->removeRow(i);
                    }
                }
                else
                {
                    ARMARX_WARNING << "layerPoll(delete old): item " << i << " is null";
                }
            }
            layerVisibilityCheckBoxes = std::move(newLayerVisibilityCheckBoxes);
            ui.layerTable->setRowCount(layerVisibilityCheckBoxes.size());
        }
    }
    catch (...)
    {
        //handleExceptions();
    }
 
    /*if (!simulatorPrx)
    {
        return;
    }
 
    auto layerInfo = simulatorPrx->layerInformation();
    ui.layerTable->setRowCount(layerInfo.size());
    layerVisibility.clear();
    //fill&map signals
    for(std::size_t i=0;i<layerInfo.size();++i)
    {
        const auto& layer=layerInfo.at(i);
        QString name=QString::fromStdString(layer.layerName);
        //store visibility
        layerVisibility[layer.layerName]=layer.visible;
        //set text
        ui.layerTable->setItem(i,0,new QTableWidgetItem{name});
        ui.layerTable->setItem(i,1,new QTableWidgetItem{QString::number(layer.elementCount)});
        //add&connect checkbox
        std::unique_ptr<QCheckBox> box{new QCheckBox};
        box->setChecked(layer.visible);
        layerSignalMapperVisible.setMapping(box.get(),name);
        QObject::connect(box.get(), SIGNAL(stateChanged(int)), &layerSignalMapperVisible, SLOT(map()));
        ui.layerTable->setCellWidget(i,2,box.release());
        //add&connect button clear
        std::unique_ptr<QPushButton> clear{new QPushButton{"Clear"}};
        layerSignalMapperClear.setMapping(clear.get(),name);
        QObject::connect(clear.get(), SIGNAL(clicked()), &layerSignalMapperClear, SLOT(map()));
        ui.layerTable->setCellWidget(i,3,clear.release());
        //add&connect button remove
        std::unique_ptr<QPushButton> remove{new QPushButton{"Remove"}};
        layerSignalMapperRemove.setMapping(remove.get(),name);
        QObject::connect(remove.get(), SIGNAL(clicked()), &layerSignalMapperRemove, SLOT(map()));
        ui.layerTable->setCellWidget(i,4,remove.release());
    }*/
}
 
void
armarx::SimulatorControlController::on_pushButtonAddRobot_clicked()
{
    if (!simulatorPrx)
    {
        return;
    }
    const auto result = simulatorPrx->addScaledRobot(ui.lineEditAddRobotXML->text().toStdString(),
                                                     ui.doubleSpinBoxAddRobotScale->value());
    ui.labelAddRobotResult->setText(QString::fromStdString(result));
}
 
void
armarx::SimulatorControlController::on_pushButtonManipRobotUpdate_clicked()
{
    if (!simulatorPrx)
    {
        return;
    }
    ui.comboBoxManipRobotName->clear();
    for (const auto& n : simulatorPrx->getRobotNames())
    {
        ui.comboBoxManipRobotName->addItem(QString::fromStdString(n));
    }
}
 
void
armarx::SimulatorControlController::on_pushButtonManipRobSetPose_clicked()
{
    if (!simulatorPrx)
    {
        return;
    }
    const Eigen::Matrix4f p = simox::math::pos_rpy_to_mat4f(ui.doubleSpinBoxManipRobTX->value(),
                                                            ui.doubleSpinBoxManipRobTY->value(),
                                                            ui.doubleSpinBoxManipRobTZ->value(),
 
                                                            ui.doubleSpinBoxManipRobRX->value(),
                                                            ui.doubleSpinBoxManipRobRY->value(),
                                                            ui.doubleSpinBoxManipRobRZ->value());
    simulatorPrx->setRobotPose(ui.comboBoxManipRobotName->currentText().toStdString(), new Pose{p});
}