HandUnitGuiPlugin.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "HandUnitGuiPlugin.h"
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
 
#include <RobotAPI/gui-plugins/HandUnitPlugin/ui_HandUnitConfigDialog.h>
 
#include "HandUnitConfigDialog.h"
 
// Qt headers
#include <cmath>
 
#include <QLabel>
#include <QLineEdit>
#include <QMessageBox>
#include <QPushButton>
#include <QString>
#include <QTimer>
#include <Qt>
#include <QtGlobal>
#include <QtWidgets/QSlider>
#include <QtWidgets/QTableWidgetItem>
 
namespace armarx
{
    HandUnitGuiPlugin::HandUnitGuiPlugin()
    {
        addWidget<HandUnitWidget>();
    }
 
    HandUnitWidget::HandUnitWidget() :
        leftHandName("NOT SET YET"),
        rightHandName("NOT SET YET"),
        leftHandUnitProxyName(""),
        rightHandUnitProxyName(""),
        setLeftHandJointAnglesFlag(false),
        setRightHandJointAnglesFlag(false)
    {
        // init gui
        ARMARX_INFO << "Setup UI";
        ui.setupUi(getWidget());
 
        ARMARX_INFO << "Done: Setup UI";
 
 
        setLeftHandJointAngleUpdateTask =
            new PeriodicTask<HandUnitWidget>(this, &HandUnitWidget::setLeftHandJointAngles, 50);
        setRightHandJointAngleUpdateTask =
            new PeriodicTask<HandUnitWidget>(this, &HandUnitWidget::setRightHandJointAngles, 50);
        updateInfoTimer = new QTimer(this);
    }
 
    void
    HandUnitWidget::onInitComponent()
    {
        usingProxy(leftHandUnitProxyName);
        usingProxy(rightHandUnitProxyName);
        //usingTopic(handName + "State");
        //ARMARX_WARNING << "Listening on Topic: " << handName + "State";
    }
 
    void
    HandUnitWidget::onConnectComponent()
    {
        updateInfoTimer->start(50);
 
 
        setLeftHandJointAngleUpdateTask->start();
 
        leftHandUnitProxy = getProxy<HandUnitInterfacePrx>(leftHandUnitProxyName);
        leftHandName = leftHandUnitProxy->getHandName();
 
        if (leftHandName == "Hand L" || leftHandName == "TCP L")
        {
            leftHandConversionFactor *= M_PI / 2;
        }
        else if (leftHandName != "Hand_L_EEF")
        {
            ARMARX_WARNING << "Left hand with name \"" << leftHandName << "\" is not supported.";
        }
 
        SingleTypeVariantListPtr leftHandPreshapeStrings =
            SingleTypeVariantListPtr::dynamicCast(leftHandUnitProxy->getShapeNames());
        QStringList leftHandList;
        int leftHandPreshapeCount = leftHandPreshapeStrings->getSize();
 
        for (int i = 0; i < leftHandPreshapeCount; ++i)
        {
            std::string shape = ((leftHandPreshapeStrings->getVariant(i))->get<std::string>());
            //            ARMARX_INFO << VAROUT(shape);
            leftHandList << QString::fromStdString(shape);
        }
 
        ui.comboLeftHandPreshapes->clear();
        ui.comboLeftHandPreshapes->addItems(leftHandList);
 
 
        setRightHandJointAngleUpdateTask->start();
 
        rightHandUnitProxy = getProxy<HandUnitInterfacePrx>(rightHandUnitProxyName);
        rightHandName = rightHandUnitProxy->getHandName();
 
        if (rightHandName == "Hand R" || rightHandName == "TCP R")
        {
            rightHandConversionFactor *= M_PI / 2;
        }
        else if (rightHandName != "Hand_R_EEF")
        {
            ARMARX_WARNING << "Right hand with name \"" << rightHandName << "\" is not supported.";
        }
 
        SingleTypeVariantListPtr rightHandPreshapeStrings =
            SingleTypeVariantListPtr::dynamicCast(rightHandUnitProxy->getShapeNames());
        QStringList rightHandList;
        int rightHandPreshapeCount = rightHandPreshapeStrings->getSize();
 
        for (int i = 0; i < rightHandPreshapeCount; ++i)
        {
            std::string shape = ((rightHandPreshapeStrings->getVariant(i))->get<std::string>());
            //            ARMARX_INFO << VAROUT(shape);
            rightHandList << QString::fromStdString(shape);
        }
 
        ui.comboRightHandPreshapes->clear();
        ui.comboRightHandPreshapes->addItems(rightHandList);
 
 
        //        ARMARX_INFO << "initGUIJointFrames";
        initGUIJointFrames();
        displayJointAngleUpdateTask =
            new PeriodicTask<HandUnitWidget>(this, &HandUnitWidget::updateJointValueTable, 50);
        displayJointAngleUpdateTask->start();
 
        updateInfoLabel();
    }
 
    void
    HandUnitWidget::onDisconnectComponent()
    {
        setLeftHandJointAngleUpdateTask->stop();
        setRightHandJointAngleUpdateTask->stop();
        displayJointAngleUpdateTask->stop();
        updateInfoTimer->stop();
    }
 
    void
    HandUnitWidget::onExitComponent()
    {
    }
 
    QPointer<QDialog>
    HandUnitWidget::getConfigDialog(QWidget* parent)
    {
        if (!dialog)
        {
            dialog = new HandUnitConfigDialog(parent);
            //dialog->ui->editHandUnitName->setText(QString::fromStdString(handUnitProxyName));
            //dialog->ui->editHandName->setText(QString::fromStdString(handName));
        }
 
        return qobject_cast<HandUnitConfigDialog*>(dialog);
    }
 
    void
    HandUnitWidget::configured()
    {
        leftHandUnitProxyName = dialog->proxyFinderLeftHand->getSelectedProxyName().toStdString();
        rightHandUnitProxyName = dialog->proxyFinderRightHand->getSelectedProxyName().toStdString();
    }
 
    void
    HandUnitWidget::preshapeLeftHand()
    {
        setLeftHandPreshape(ui.comboLeftHandPreshapes->currentText().toUtf8().data());
    }
 
    void
    HandUnitWidget::preshapeRightHand()
    {
        setRightHandPreshape(ui.comboRightHandPreshapes->currentText().toUtf8().data());
    }
 
    void
    HandUnitWidget::reloadPreshapes()
    {
        ARMARX_INFO << "Reloading preshapes in robot unit ...";
 
        // left hand unit
        {
            if (leftHandUnitProxy)
            {
                ARMARX_INFO << "Left hand unit: reloading preshapes.";
                leftHandUnitProxy->reloadPreshapes();
            }
        }
 
        // right hand unit
        {
            if (rightHandUnitProxy)
            {
                ARMARX_INFO << "Right hand unit: reloading preshapes.";
                rightHandUnitProxy->reloadPreshapes();
            }
        }
    }
 
    void
    HandUnitWidget::setLeftHandJointAngles()
    {
        //        ARMARX_INFO << "setLeftHandJointAngles";
        if (!leftHandUnitProxy)
        {
            ARMARX_WARNING << "invalid proxy";
            return;
        }
 
        if (!setLeftHandJointAnglesFlag)
        {
            return;
        }
 
        setLeftHandJointAnglesFlag = false;
 
        NameValueMap leftHandJa;
        NameValueMap currentLeftHandJointValues = leftHandUnitProxy->getCurrentJointValues();
        float value = 0;
        if (leftHandName == "Hand L" || leftHandName == "TCP L")
        {
            leftHandConversionFactor *= M_PI / 2;
        }
        else if (leftHandName != "Hand_L_EEF")
        {
            ARMARX_WARNING << "Left hand with name \"" << leftHandName << "\" is not supported.";
            return;
        }
 
        for (const auto& pair : currentLeftHandJointValues)
        {
            if (pair.first == currentLeftHandJoint)
            {
                value = static_cast<float>(ui.horizontalSliderLeftHandJointPos->value() *
                                           leftHandConversionFactor);
                //                ARMARX_INFO << VAROUT(value);
                leftHandJa[pair.first] = value;
                break;
            }
        }
 
        //        ARMARX_INFO << VAROUT(value / leftHandConversionFactor);
        ui.lcdNumberLeftHandJointValue->display(value / leftHandConversionFactor);
        leftHandUnitProxy->setJointAngles(leftHandJa);
    }
 
    void
    HandUnitWidget::setRightHandJointAngles()
    {
        //        ARMARX_INFO << "setRightHandJointAngles";
        if (!rightHandUnitProxy)
        {
            ARMARX_WARNING << "invalid proxy";
            return;
        }
 
        if (!setRightHandJointAnglesFlag)
        {
            return;
        }
 
        setRightHandJointAnglesFlag = false;
 
        NameValueMap rightHandJa;
        NameValueMap currentRightHandJointValues = rightHandUnitProxy->getCurrentJointValues();
        float value = 0;
        if (rightHandName == "Hand R" || rightHandName == "TCP R")
        {
            rightHandConversionFactor *= M_PI / 2;
        }
        else if (rightHandName != "Hand_R_EEF")
        {
            ARMARX_WARNING << "Right hand with name \"" << rightHandName << "\" is not supported.";
            return;
        }
 
        for (const auto& pair : currentRightHandJointValues)
        {
            if (pair.first == currentRightHandJoint)
            {
                value = static_cast<float>(ui.horizontalSliderRightHandJointPos->value() *
                                           rightHandConversionFactor);
                //                ARMARX_INFO << VAROUT(value);
                rightHandJa[pair.first] = value;
                break;
            }
        }
 
        //        ARMARX_INFO << VAROUT(value / rightHandConversionFactor);
        ui.lcdNumberRightHandJointValue->display(value / rightHandConversionFactor);
        rightHandUnitProxy->setJointAngles(rightHandJa);
    }
 
    void
    HandUnitWidget::requestSetLeftHandJointAngles()
    {
        setLeftHandJointAnglesFlag = true;
    }
 
    void
    HandUnitWidget::requestSetRightHandJointAngles()
    {
        setRightHandJointAnglesFlag = true;
    }
 
    void
    HandUnitWidget::openLeftHand()
    {
        setLeftHandPreshape("Open");
    }
 
    void
    HandUnitWidget::openRightHand()
    {
        setRightHandPreshape("Open");
    }
 
    void
    HandUnitWidget::closeLeftHand()
    {
        setLeftHandPreshape("Close");
    }
 
    void
    HandUnitWidget::closeRightHand()
    {
        setRightHandPreshape("Close");
    }
 
    void
    HandUnitWidget::relaxLeftHand()
    {
        setLeftHandPreshape("Relax");
    }
 
    void
    HandUnitWidget::relaxRightHand()
    {
        setRightHandPreshape("Relax");
    }
 
    void
    HandUnitWidget::updateInfoLabel()
    {
        ui.labelInfoLeftHand->setText(
            QString::fromStdString(leftHandUnitProxyName + " :: " + leftHandName +
                                   " State: " + leftHandUnitProxy->describeHandState()));
        ui.labelInfoRightHand->setText(
            QString::fromStdString(rightHandUnitProxyName + " :: " + rightHandName +
                                   " State: " + rightHandUnitProxy->describeHandState()));
    }
 
    void
    HandUnitWidget::updateJointValueTable()
    {
        NameValueMap currentLeftHandJointValues = leftHandUnitProxy->getCurrentJointValues();
        //        ARMARX_INFO << VAROUT(leftHandUnitProxy->getCurrentJointValues());
        int frameLeftHandRowIdx = 0;
 
        for (const auto& pair : currentLeftHandJointValues)
        {
            //            ARMARX_INFO << VAROUT(pair.first);
            QString name(pair.first.c_str());
            QTableWidgetItem* newItem = new QTableWidgetItem(QString::number(pair.second));
            ui.tableWidgetLeftHand->setItem(frameLeftHandRowIdx, 1, newItem);
            frameLeftHandRowIdx++;
        }
 
 
        NameValueMap currentRightHandJointValues = rightHandUnitProxy->getCurrentJointValues();
        //        ARMARX_INFO << VAROUT(rightHandUnitProxy->getCurrentJointValues());
        int frameRightHandRowIdx = 0;
 
        for (const auto& pair : currentRightHandJointValues)
        {
            //            ARMARX_INFO << VAROUT(pair.first);
            QString name(pair.first.c_str());
            QTableWidgetItem* newItem = new QTableWidgetItem(QString::number(pair.second));
            ui.tableWidgetRightHand->setItem(frameRightHandRowIdx, 1, newItem);
            frameRightHandRowIdx++;
        }
    }
 
    void
    HandUnitWidget::setExpertMode(side side, int state)
    {
        bool const isExpert = state == Qt::Checked;
 
        if (side == side::left)
        {
            ui.leftTare->setEnabled(isExpert);
            ui.leftReset->setEnabled(isExpert);
        }
        else if (side == side::right)
        {
            ui.rightTare->setEnabled(isExpert);
            ui.rightReset->setEnabled(isExpert);
        }
        else
        {
            ARMARX_WARNING << "Invalid side to set expert mode.";
        }
    }
 
    void
    HandUnitWidget::tareHand(side side)
    {
        if (side == side::left)
        {
            ARMARX_INFO << "Trigger taring left hand now...";
            leftHandUnitProxy->tare();
        }
        else if (side == side::right)
        {
            ARMARX_INFO << "Trigger taring right hand now...";
            rightHandUnitProxy->tare();
        }
        else
        {
            ARMARX_WARNING << "Invalid side to tare hand.";
        }
    }
 
    void
    HandUnitWidget::resetHand(side side)
    {
        if (side == side::left)
        {
            ARMARX_INFO << "Trigger resetting left hand firmware now...";
            leftHandUnitProxy->resetFirmware();
        }
        else if (side == side::right)
        {
            ARMARX_INFO << "Trigger resetting right hand firmware now...";
            rightHandUnitProxy->resetFirmware();
        }
        else
        {
            ARMARX_WARNING << "Invalid side to reset hand firmware.";
        }
    }
 
    void
    HandUnitWidget::selectLeftHandJoint(int i)
    {
        //        ARMARX_INFO << "selectLeftHandJoint " << i;
        NameValueMap currentLeftHandJointValues = leftHandUnitProxy->getCurrentJointValues();
        int idx = 0;
 
        for (const auto& pair : currentLeftHandJointValues)
        {
            if (idx == i)
            {
                currentLeftHandJoint = pair.first;
                int convertedValue = static_cast<int>(pair.second / leftHandConversionFactor);
                ui.horizontalSliderLeftHandJointPos->setSliderPosition(convertedValue);
                ui.lcdNumberLeftHandJointValue->display(convertedValue);
                //                ARMARX_INFO << "Found joint";
                break;
            }
            idx++;
        }
    }
 
    void
    HandUnitWidget::selectRightHandJoint(int i)
    {
        //        ARMARX_INFO << "selectRightHandJoint " << i;
        NameValueMap currentRightHandJointValues = rightHandUnitProxy->getCurrentJointValues();
        int idx = 0;
 
        for (const auto& pair : currentRightHandJointValues)
        {
            if (idx == i)
            {
                currentRightHandJoint = pair.first;
                int convertedValue = static_cast<int>(pair.second / rightHandConversionFactor);
                ui.horizontalSliderRightHandJointPos->setSliderPosition(convertedValue);
                ui.lcdNumberRightHandJointValue->display(convertedValue);
                //                ARMARX_INFO << "Found joint";
                break;
            }
            idx++;
        }
    }
 
    void
    HandUnitWidget::setLeftHandPreshape(std::string preshape)
    {
        //        ARMARX_INFO << "Setting new left hand shape: " << preshape;
        leftHandUnitProxy->setShape(preshape);
    }
 
    void
    HandUnitWidget::setRightHandPreshape(std::string preshape)
    {
        //        ARMARX_INFO << "Setting new right hand shape: " << preshape;
        rightHandUnitProxy->setShape(preshape);
    }
 
    void
    HandUnitWidget::loadSettings(QSettings* settings)
    {
        leftHandUnitProxyName =
            settings->value("leftHandUnitProxyName", QString::fromStdString(leftHandUnitProxyName))
                .toString()
                .toStdString();
        leftHandName = settings->value("leftHandName", QString::fromStdString(leftHandName))
                           .toString()
                           .toStdString();
        rightHandUnitProxyName =
            settings
                ->value("rightHandUnitProxyName", QString::fromStdString(rightHandUnitProxyName))
                .toString()
                .toStdString();
        rightHandName = settings->value("rightHandName", QString::fromStdString(rightHandName))
                            .toString()
                            .toStdString();
    }
 
    void
    HandUnitWidget::saveSettings(QSettings* settings)
    {
        settings->setValue("leftHandUnitProxyName", QString::fromStdString(leftHandUnitProxyName));
        settings->setValue("leftHandName", QString::fromStdString(leftHandName));
        settings->setValue("rightHandUnitProxyName",
                           QString::fromStdString(rightHandUnitProxyName));
        settings->setValue("rightHandName", QString::fromStdString(rightHandName));
    }
 
    void
    HandUnitWidget::initGUIJointFrames()
    {
        ARMARX_INFO << "initGUIJointFrames";
 
        NameValueMap currentLeftHandJointValues = leftHandUnitProxy->getCurrentJointValues();
        //        ARMARX_INFO << VAROUT(leftHandUnitProxy->getCurrentJointValues());
        int frameLeftHandRowIdx = 0;
 
        connect(ui.buttonPreshapeLeftHand,
                SIGNAL(clicked()),
                this,
                SLOT(preshapeLeftHand()),
                Qt::UniqueConnection);
        connect(ui.buttonOpenLeftHand,
                SIGNAL(clicked()),
                this,
                SLOT(openLeftHand()),
                Qt::UniqueConnection);
        connect(ui.buttonCloseLeftHand,
                SIGNAL(clicked()),
                this,
                SLOT(closeLeftHand()),
                Qt::UniqueConnection);
        connect(ui.buttonRelaxLeftHand,
                SIGNAL(clicked()),
                this,
                SLOT(relaxLeftHand()),
                Qt::UniqueConnection);
        ui.horizontalSliderLeftHandJointPos->setMaximum(100);
        ui.horizontalSliderLeftHandJointPos->setMinimum(0);
        connect(ui.horizontalSliderLeftHandJointPos,
                SIGNAL(sliderMoved(int)),
                this,
                SLOT(requestSetLeftHandJointAngles()),
                Qt::UniqueConnection);
        connect(ui.comboLeftHandJoints,
                SIGNAL(currentIndexChanged(int)),
                this,
                SLOT(selectLeftHandJoint(int)),
                Qt::UniqueConnection);
        connect(ui.leftExpertMode,
                &QCheckBox::stateChanged,
                [&](int state) { setExpertMode(side::left, state); });
        connect(ui.leftTare, &QPushButton::clicked, [&] { tareHand(side::left); });
        connect(ui.leftReset, &QPushButton::clicked, [&] { resetHand(side::left); });
 
        ui.tableWidgetLeftHand->setRowCount(currentLeftHandJointValues.size());
        ui.tableWidgetLeftHand->setColumnCount(2);
 
        ui.comboLeftHandJoints->clear();
 
        qRegisterMetaType<QVector<int>>("QVector<int>");
        for (const auto& pair : currentLeftHandJointValues)
        {
            //            ARMARX_INFO << VAROUT(pair.first);
            QString name(pair.first.c_str());
            QTableWidgetItem* newItem = new QTableWidgetItem(name);
            ui.tableWidgetLeftHand->setItem(frameLeftHandRowIdx, 0, newItem);
            ui.comboLeftHandJoints->addItem(name);
            frameLeftHandRowIdx++;
        }
 
        ui.comboLeftHandJoints->setCurrentIndex(-1);
 
 
        NameValueMap currentRightHandJointValues = rightHandUnitProxy->getCurrentJointValues();
        //        ARMARX_INFO << VAROUT(rightHandUnitProxy->getCurrentJointValues());
        int frameRightHandRowIdx = 0;
 
        connect(ui.buttonPreshapeRightHand,
                SIGNAL(clicked()),
                this,
                SLOT(preshapeRightHand()),
                Qt::UniqueConnection);
        connect(ui.buttonOpenRightHand,
                SIGNAL(clicked()),
                this,
                SLOT(openRightHand()),
                Qt::UniqueConnection);
        connect(ui.buttonCloseRightHand,
                SIGNAL(clicked()),
                this,
                SLOT(closeRightHand()),
                Qt::UniqueConnection);
        connect(ui.buttonRelaxRightHand,
                SIGNAL(clicked()),
                this,
                SLOT(relaxRightHand()),
                Qt::UniqueConnection);
        ui.horizontalSliderRightHandJointPos->setMaximum(100);
        ui.horizontalSliderRightHandJointPos->setMinimum(0);
        connect(ui.horizontalSliderRightHandJointPos,
                SIGNAL(sliderMoved(int)),
                this,
                SLOT(requestSetRightHandJointAngles()),
                Qt::UniqueConnection);
        connect(ui.comboRightHandJoints,
                SIGNAL(currentIndexChanged(int)),
                this,
                SLOT(selectRightHandJoint(int)),
                Qt::UniqueConnection);
        connect(ui.rightExpertMode,
                &QCheckBox::stateChanged,
                [&](int state) { setExpertMode(side::right, state); });
        connect(ui.rightTare, &QPushButton::clicked, [&] { tareHand(side::right); });
        connect(ui.rightReset, &QPushButton::clicked, [&] { resetHand(side::right); });
 
        ui.tableWidgetRightHand->setRowCount(currentRightHandJointValues.size());
        ui.tableWidgetRightHand->setColumnCount(2);
 
        ui.comboRightHandJoints->clear();
 
        qRegisterMetaType<QVector<int>>("QVector<int>");
        for (const auto& pair : currentRightHandJointValues)
        {
            //            ARMARX_INFO << VAROUT(pair.first);
            QString name(pair.first.c_str());
            QTableWidgetItem* newItem = new QTableWidgetItem(name);
            ui.tableWidgetRightHand->setItem(frameRightHandRowIdx, 0, newItem);
            ui.comboRightHandJoints->addItem(name);
            frameRightHandRowIdx++;
        }
 
        ui.comboRightHandJoints->setCurrentIndex(-1);
 
 
        // reloading of preshapes
        connect(ui.reloadPreshapesButton,
                SIGNAL(clicked()),
                this,
                SLOT(reloadPreshapes()),
                Qt::UniqueConnection);
    }
 
    void
    armarx::HandUnitWidget::reportHandShaped(const std::string& handName,
                                             const std::string& handShapeName,
                                             const Ice::Current&)
    {
        ARMARX_IMPORTANT << handName << ": " << handShapeName;
    }
 
    void
    armarx::HandUnitWidget::reportNewHandShapeName(const std::string& handName,
                                                   const std::string& handShapeName,
                                                   const Ice::Current&)
    {
        ARMARX_IMPORTANT << handName << ": " << handShapeName;
    }
 
    void
    armarx::HandUnitWidget::reportJointAngles(const armarx::NameValueMap& actualJointAngles,
                                              const Ice::Current& c)
    {
    }
 
    void
    armarx::HandUnitWidget::reportJointPressures(const armarx::NameValueMap& actualJointPressures,
                                                 const Ice::Current& c)
    {
    }
} // namespace armarx