Implement HandUnitGUI for controlling left hand having custom joint names

source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp

@@ -35,6 +35,10 @@
 #include <QLineEdit>
 #include <QMessageBox>
 #include <QTimer>
+#include <QtWidgets/QSlider>
+#include <QtWidgets/QGridLayout>
+#include <QtWidgets/QLabel>
+#include <QString>
 
 #include <cmath>
 
@@ -50,12 +54,13 @@ namespace armarx
         rightHandName("NOT SET YET"),
         leftHandUnitProxyName(""),
         rightHandUnitProxyName(""),
-        setJointAnglesFlag(false)
+        setLeftHandJointAnglesFlag(false)
     {
         // init gui
         ui.setupUi(getWidget());
 
-        jointAngleUpdateTask = new PeriodicTask<HandUnitWidget>(this, &HandUnitWidget::setJointAngles, 50);
+        leftHandJointAngleUpdateTask = new PeriodicTask<HandUnitWidget>(this, &HandUnitWidget::setLeftHandJointAngles, 50);
+        rightHandJointAngleUpdateTask = new PeriodicTask<HandUnitWidget>(this, &HandUnitWidget::setRightHandJointAngles, 50);
         updateInfoTimer = new QTimer(this);
     }
 
@@ -70,15 +75,16 @@ namespace armarx
 
     void HandUnitWidget::onConnectComponent()
     {
-        connectSlots();
-        jointAngleUpdateTask->start();
+        initGUIJointFrames();
         updateInfoTimer->start(50);
 
+        leftHandJointAngleUpdateTask->start();
+
         leftHandUnitProxy = getProxy<HandUnitInterfacePrx>(leftHandUnitProxyName);
         leftHandName = leftHandUnitProxy->getHandName();
 
         // @@@ In simulation hand is called 'Hand L'/'Hand R'. On 3b Hand is called 'TCP R'
-        if (leftHandName != "Hand L" && leftHandName != "Hand R" && leftHandName != "TCP L" && leftHandName != "TCP R")
+        if (leftHandName != "Hand L" && leftHandName != "TCP L")
         {
             //QMessageBox::warning(NULL, "Hand not supported", QString("Hand with name \"") + QString::fromStdString(handName) + " \" is not suppored.");
             ARMARX_WARNING << "Hand with name \"" << leftHandName << "\" is not supported.";
@@ -86,23 +92,50 @@ namespace armarx
 
         //ui.labelInfo->setText(QString::fromStdString(handUnitProxyName + " :: " + handName));
 
-        SingleTypeVariantListPtr preshapeStrings = SingleTypeVariantListPtr::dynamicCast(leftHandUnitProxy->getShapeNames());
-        QStringList list;
-        int preshapeCount = preshapeStrings->getSize();
+        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>());
+            leftHandList << QString::fromStdString(shape);
+        }
+
+        ui.comboLeftHandPreshapes->clear();
+        ui.comboLeftHandPreshapes->addItems(leftHandList);
+
+
+
+        rightHandJointAngleUpdateTask->start();
 
-        for (int i = 0; i < preshapeCount; ++i)
+        rightHandUnitProxy = getProxy<HandUnitInterfacePrx>(rightHandUnitProxyName);
+        rightHandName = rightHandUnitProxy->getHandName();
+
+        // @@@ In simulation hand is called 'Hand L'/'Hand R'. On 3b Hand is called 'TCP R'
+        if (rightHandName != "Hand R" && rightHandName != "TCP R")
+        {
+            ARMARX_WARNING << "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 = ((preshapeStrings->getVariant(i))->get<std::string>());
-            list << QString::fromStdString(shape);
+            std::string shape = ((rightHandPreshapeStrings->getVariant(i))->get<std::string>());
+            rightHandList << QString::fromStdString(shape);
         }
 
-        ui.comboPreshapes->clear();
-        ui.comboPreshapes->addItems(list);
+        ui.comboRightHandPreshapes->clear();
+        ui.comboRightHandPreshapes->addItems(rightHandList);
     }
 
     void HandUnitWidget::onDisconnectComponent()
     {
-        jointAngleUpdateTask->stop();
+        leftHandJointAngleUpdateTask->stop();
+        rightHandJointAngleUpdateTask->stop();
         updateInfoTimer->stop();
     }
 
@@ -125,134 +158,144 @@ namespace armarx
     void HandUnitWidget::configured()
     {
         leftHandUnitProxyName = dialog->proxyFinder->getSelectedProxyName().toStdString();
+        rightHandUnitProxyName = dialog->proxyFinder->getSelectedProxyName().toStdString();
     }
 
-    void HandUnitWidget::preshapeHand()
+    void HandUnitWidget::preshapeLeftHand()
     {
-        setPreshape(ui.comboPreshapes->currentText().toUtf8().data());
+        setLeftHandPreshape(ui.comboLeftHandPreshapes->currentText().toUtf8().data());
     }
 
-    void HandUnitWidget::setJointAngles()
+    void HandUnitWidget::preshapeRightHand()
     {
+        setRightHandPreshape(ui.comboRightHandPreshapes->currentText().toUtf8().data());
+    }
+
+    void HandUnitWidget::setLeftHandJointAngles()
+    {    
         if (!leftHandUnitProxy)
         {
             ARMARX_WARNING << "invalid proxy";
             return;
         }
 
-        if (!setJointAnglesFlag)
+        if (!setLeftHandJointAnglesFlag)
         {
             return;
         }
 
-        setJointAnglesFlag = false;
+        setLeftHandJointAnglesFlag = false;
 
-        NameValueMap ja;
+        NameValueMap leftHandJa;
+        NameValueMap currentLeftHandJointValues = leftHandUnitProxy->getCurrentJointValues();
 
-        if (leftHandName == "Hand L" || leftHandName == "TCP L")
+        if (leftHandName == "Hand L EEF")
         {
-            ja["Hand Palm 2 L"] = ui.horizontalSliderPalm->value() * M_PI / 180;
-            ja["Index L J0"] = ui.horizontalSliderIndexJ0->value() * M_PI / 180;
-            ja["Index L J1"] = ui.horizontalSliderIndexJ1->value() * M_PI / 180;
-            ja["Middle L J0"] = ui.horizontalSliderMiddleJ0->value() * M_PI / 180;
-            ja["Middle L J1"] = ui.horizontalSliderMiddleJ1->value() * M_PI / 180;
-            ja["Thumb L J0"] = ui.horizontalSliderThumbJ0->value() * M_PI / 180;
-            ja["Thumb L J1"] = ui.horizontalSliderThumbJ1->value() * M_PI / 180;
-            float rinkyValue = ui.horizontalSliderRinky->value() * M_PI / 180;
-            ja["Ring L J0"] = rinkyValue;
-            ja["Ring L J1"] = rinkyValue;
-            ja["Pinky L J0"] = rinkyValue;
-            ja["Pinky L J1"] = rinkyValue;
+            for (const auto& pair : currentLeftHandJointValues)
+            {
+                leftHandJa[pair.first] = ui.frameLeftHand->findChild<QSlider *>(pair.first.c_str())->value();
+            }
         }
-        else if (leftHandName == "Hand R" || leftHandName == "TCP R")
+        else if (leftHandName == "Hand L" || leftHandName == "TCP L")
         {
-            ja["Hand Palm 2 R"] = ui.horizontalSliderPalm->value() * M_PI / 180;
-            ja["Index R J0"] = ui.horizontalSliderIndexJ0->value() * M_PI / 180;
-            ja["Index R J1"] = ui.horizontalSliderIndexJ1->value() * M_PI / 180;
-            ja["Middle R J0"] = ui.horizontalSliderMiddleJ0->value() * M_PI / 180;
-            ja["Middle R J1"] = ui.horizontalSliderMiddleJ1->value() * M_PI / 180;
-            ja["Thumb R J0"] = ui.horizontalSliderThumbJ0->value() * M_PI / 180;
-            ja["Thumb R J1"] = ui.horizontalSliderThumbJ1->value() * M_PI / 180;
-            float rinkyValue = ui.horizontalSliderRinky->value() * M_PI / 180;
-            ja["Ring R J0"] = rinkyValue;
-            ja["Ring R J1"] = rinkyValue;
-            ja["Pinky R J0"] = rinkyValue;
-            ja["Pinky R J1"] = rinkyValue;
+            for (const auto& pair : currentLeftHandJointValues)
+            {
+                leftHandJa[pair.first] = ui.frameLeftHand->findChild<QSlider *>(pair.first.c_str())->value() * M_PI / 2;
+            }
         }
         else
         {
-            ARMARX_WARNING << "Hand with name \"" << leftHandName << "\" is not supported.";
+            ARMARX_WARNING << "Left hand with name \"" << leftHandName << "\" is not supported.";
         }
 
-        leftHandUnitProxy->setJointAngles(ja);
-    }
-
-    void HandUnitWidget::requestSetJointAngles()
-    {
-        setJointAnglesFlag = true;
+        leftHandUnitProxy->setJointAngles(leftHandJa);
     }
 
-    void HandUnitWidget::openHand()
+    void HandUnitWidget::requestSetLeftHandJointAngles()
     {
-        setPreshape("Open");
+        setLeftHandJointAnglesFlag = true;
     }
 
-    void HandUnitWidget::closeHand()
+    void HandUnitWidget::openLeftHand()
     {
-        setPreshape("Close");
+        setLeftHandPreshape("Open");
     }
 
-    void HandUnitWidget::closeThumb()
+    void HandUnitWidget::closeLeftHand()
     {
-        setPreshape("Thumb");
+        setLeftHandPreshape("Close");
     }
 
-    void HandUnitWidget::relaxHand()
+    void HandUnitWidget::relaxLeftHand()
     {
-        setPreshape("Relax");
+        setLeftHandPreshape("Relax");
     }
 
     void HandUnitWidget::updateInfoLabel()
     {
-        ui.labelInfo->setText(QString::fromStdString(leftHandUnitProxyName + " :: " + leftHandName + " State: " + leftHandUnitProxy->describeHandState()));
+        ui.labelInfoLeftHand->setText(QString::fromStdString(leftHandUnitProxyName + " :: " + leftHandName + " State: " + leftHandUnitProxy->describeHandState()));
+        ui.labelInfoRightHand->setText(QString::fromStdString(rightHandUnitProxyName + " :: " + rightHandName + " State: " + rightHandUnitProxy->describeHandState()));
     }
 
-    void HandUnitWidget::setPreshape(std::string preshape)
+    void HandUnitWidget::setLeftHandPreshape(std::string preshape)
     {
-        ARMARX_INFO << "Setting new hand shape: " << preshape;
+        ARMARX_INFO << "Setting new left hand shape: " << preshape;
         leftHandUnitProxy->setShape(preshape);
     }
 
     void HandUnitWidget::loadSettings(QSettings* settings)
     {
-        leftHandUnitProxyName = settings->value("handUnitProxyName", QString::fromStdString(leftHandUnitProxyName)).toString().toStdString();
-        leftHandName = settings->value("handName", QString::fromStdString(leftHandName)).toString().toStdString();
+        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("handUnitProxyName", QString::fromStdString(leftHandUnitProxyName));
-        settings->setValue("handName", QString::fromStdString(leftHandName));
+        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::connectSlots()
+    void HandUnitWidget::initGUIJointFrames()
     {
-        connect(ui.buttonPreshape, SIGNAL(clicked()), this, SLOT(preshapeHand()), Qt::UniqueConnection);
-        connect(ui.buttonOpenHand, SIGNAL(clicked()), this, SLOT(openHand()), Qt::UniqueConnection);
-        connect(ui.buttonCloseHand, SIGNAL(clicked()), this, SLOT(closeHand()), Qt::UniqueConnection);
-        connect(ui.buttonCloseThumb, SIGNAL(clicked()), this, SLOT(closeThumb()), Qt::UniqueConnection);
-        connect(ui.buttonRelaxHand, SIGNAL(clicked()), this, SLOT(relaxHand()), Qt::UniqueConnection);
-        //connect(ui.comboPreshapes, SIGNAL(currentIndexChanged(const QString&)), this, SLOT(selectPreshape(const QString&)), Qt::UniqueConnection);
-        connect(ui.buttonSetJointAngles, SIGNAL(clicked()), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
-        connect(ui.horizontalSliderIndexJ0, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
-        connect(ui.horizontalSliderIndexJ1, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
-        connect(ui.horizontalSliderMiddleJ0, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
-        connect(ui.horizontalSliderMiddleJ1, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
-        connect(ui.horizontalSliderRinky, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
-        connect(ui.horizontalSliderPalm, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
-        connect(ui.horizontalSliderThumbJ0, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
-        connect(ui.horizontalSliderThumbJ1, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()), Qt::UniqueConnection);
+        NameValueMap currentLeftHandJointValues = leftHandUnitProxy->getCurrentJointValues();
+        QGridLayout* gridLayout = new QGridLayout(ui.frameLeftHand);
+        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);
+        connect(ui.buttonSetLeftHandJointAngles, SIGNAL(clicked()), this, SLOT(requestSetLeftHandJointAngles()), Qt::UniqueConnection);
+
+        for (const auto& pair : currentLeftHandJointValues)
+        {
+            QLabel* label = new QLabel(ui.frameLeftHand);
+            label->setText(QString::fromUtf8(pair.first.c_str()));
+            gridLayout->addWidget(label, frameLeftHandRowIdx, 0, 1, 1);
+
+            QSlider* horizontalSlider = new QSlider(ui.frameLeftHand);
+            horizontalSlider->setObjectName("horizontalSlider" + QString::fromUtf8(pair.first.c_str()));
+            horizontalSlider->setMaximum(1);
+            horizontalSlider->setOrientation(Qt::Horizontal);
+            connect(horizontalSlider, SIGNAL(sliderMoved(int)), this, SLOT(requestSetLeftHandJointAngles()), Qt::UniqueConnection);
+            gridLayout->addWidget(horizontalSlider, frameLeftHandRowIdx, 1, 1, 2);
+
+            frameLeftHandRowIdx++;
+        }
+
+        ui.frameLeftHand->setLayout(gridLayout);
+
+
+
+        // TODO: right hand
+
+
+
         connect(updateInfoTimer, SIGNAL(timeout()), this, SLOT(updateInfoLabel()));
     }
 

source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.h

@@ -112,20 +112,26 @@ namespace armarx
 
     public slots:
 
-        void preshapeHand();
-        void setJointAngles();
-        void requestSetJointAngles();
-        void openHand();
-        void closeHand();
-        void closeThumb();
-        void relaxHand();
+        void preshapeLeftHand();
+        void preshapeRightHand();
+        void setLeftHandJointAngles();
+        void setRightHandJointAngles();
+        void requestSetLeftHandJointAngles();
+        void requestSetRightHandJointAngles();
+        void openLeftHand();
+        void openRightHand();
+        void closeLeftHand();
+        void closeRightHand();
+        void relaxLeftHand();
+        void relaxRightHand();
         void updateInfoLabel();
 
     private:
-        void setPreshape(std::string preshape);
+        void setLeftHandPreshape(std::string preshape);
+        void setRightHandPreshape(std::string preshape);
 
     protected:
-        void connectSlots();
+        void initGUIJointFrames();
 
         Ui::HandUnitGuiPlugin ui;
 
@@ -142,9 +148,11 @@ namespace armarx
         //QPointer<QWidget> __widget;
         QPointer<HandUnitConfigDialog> dialog;
 
-        PeriodicTask<HandUnitWidget>::pointer_type jointAngleUpdateTask;
+        PeriodicTask<HandUnitWidget>::pointer_type leftHandJointAngleUpdateTask;
+        PeriodicTask<HandUnitWidget>::pointer_type rightHandJointAngleUpdateTask;
         QTimer* updateInfoTimer;
-        bool setJointAnglesFlag;
+        bool setLeftHandJointAnglesFlag;
+        bool setRightHandJointAnglesFlag;
 
 
         // HandUnitListener interface