Implement methods for the right hand in HandUnitGuiPlugin

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

@@ -211,26 +211,86 @@ namespace armarx
         leftHandUnitProxy->setJointAngles(leftHandJa);
     }
 
+    void HandUnitWidget::setRightHandJointAngles()
+    {
+        if (!rightHandUnitProxy)
+        {
+            ARMARX_WARNING << "invalid proxy";
+            return;
+        }
+
+        if (!setRightHandJointAnglesFlag)
+        {
+            return;
+        }
+
+        setRightHandJointAnglesFlag = false;
+
+        NameValueMap rightHandJa;
+        NameValueMap currentRightHandJointValues = rightHandUnitProxy->getCurrentJointValues();
+
+        if (rightHandName == "Hand L EEF")
+        {
+            for (const auto& pair : currentRightHandJointValues)
+            {
+                rightHandJa[pair.first] = ui.frameRightHand->findChild<QSlider *>(pair.first.c_str())->value();
+            }
+        }
+        else if (rightHandName == "Hand L" || rightHandName == "TCP L")
+        {
+            for (const auto& pair : currentRightHandJointValues)
+            {
+                rightHandJa[pair.first] = ui.frameRightHand->findChild<QSlider *>(pair.first.c_str())->value() * M_PI / 2;
+            }
+        }
+        else
+        {
+            ARMARX_WARNING << "Right hand with name \"" << rightHandName << "\" is not supported.";
+        }
+
+        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()));
@@ -243,6 +303,12 @@ namespace armarx
         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();
@@ -263,7 +329,7 @@ namespace armarx
     void HandUnitWidget::initGUIJointFrames()
     {
         NameValueMap currentLeftHandJointValues = leftHandUnitProxy->getCurrentJointValues();
-        QGridLayout* gridLayout = new QGridLayout(ui.frameLeftHand);
+        QGridLayout* gridLayoutLeftHand = new QGridLayout(ui.frameLeftHand);
         int frameLeftHandRowIdx = 0;
 
         connect(ui.buttonPreshapeLeftHand, SIGNAL(clicked()), this, SLOT(preshapeLeftHand()), Qt::UniqueConnection);
@@ -276,23 +342,49 @@ namespace armarx
         {
             QLabel* label = new QLabel(ui.frameLeftHand);
             label->setText(QString::fromUtf8(pair.first.c_str()));
-            gridLayout->addWidget(label, frameLeftHandRowIdx, 0, 1, 1);
+            gridLayoutLeftHand->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);
+            gridLayoutLeftHand->addWidget(horizontalSlider, frameLeftHandRowIdx, 1, 1, 2);
 
             frameLeftHandRowIdx++;
         }
 
-        ui.frameLeftHand->setLayout(gridLayout);
+        ui.frameLeftHand->setLayout(gridLayoutLeftHand);
+
+
 
+        NameValueMap currentRightHandJointValues = rightHandUnitProxy->getCurrentJointValues();
+        QGridLayout* gridLayoutRightHand = new QGridLayout(ui.frameRightHand);
+        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);
+        connect(ui.buttonSetRightHandJointAngles, SIGNAL(clicked()), this, SLOT(requestSetRightHandJointAngles()), Qt::UniqueConnection);
+
+        for (const auto& pair : currentRightHandJointValues)
+        {
+            QLabel* label = new QLabel(ui.frameRightHand);
+            label->setText(QString::fromUtf8(pair.first.c_str()));
+            gridLayoutRightHand->addWidget(label, frameRightHandRowIdx, 0, 1, 1);
+
+            QSlider* horizontalSlider = new QSlider(ui.frameRightHand);
+            horizontalSlider->setObjectName("horizontalSlider" + QString::fromUtf8(pair.first.c_str()));
+            horizontalSlider->setMaximum(1);
+            horizontalSlider->setOrientation(Qt::Horizontal);
+            connect(horizontalSlider, SIGNAL(sliderMoved(int)), this, SLOT(requestSetRightHandJointAngles()), Qt::UniqueConnection);
+            gridLayoutRightHand->addWidget(horizontalSlider, frameRightHandRowIdx, 1, 1, 2);
+
+            frameRightHandRowIdx++;
+        }
 
-        // TODO: right hand
+        ui.frameRightHand->setLayout(gridLayoutRightHand);