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 "HandUnitConfigDialog.h"
#include <RobotAPI/gui-plugins/HandUnitPlugin/ui_HandUnitConfigDialog.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
 
// Qt headers
#include <Qt>
#include <QtGlobal>
#include <QPushButton>
#include <QLabel>
#include <QLineEdit>
#include <QMessageBox>
#include <QTimer>
#include <QtWidgets/QSlider>
#include <QtWidgets/QTableWidgetItem>
#include <QString>
 
#include <cmath>
 
namespace armarx
{
    HandUnitGuiPlugin::HandUnitGuiPlugin()
    {
        addWidget<HandUnitWidget>();
    }
 
    HandUnitWidget::HandUnitWidget() :
        leftHandName("NOT SET YET"),
        rightHandName("NOT SET YET"),
        leftHandUnitProxyName(""),
        rightHandUnitProxyName(""),
        setLeftHandJointAnglesFlag(false),
        setRightHandJointAnglesFlag(false)
    {
        // init gui
        ui.setupUi(getWidget());
 
        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();
    }
 
    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::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::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()
    {
        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);
 
        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);
 
        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);
    }
 
 
 
 
 
    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)
    {
 
    }
}