From ce0200ffe8492f17cf583243b32ceb4019bf93b0 Mon Sep 17 00:00:00 2001
From: Jan Hausberg <jan.hausberg@kit.edu>
Date: Fri, 17 Jun 2022 19:41:52 +0200
Subject: [PATCH] Implement HandUnitGUI for controlling left hand having custom
joint names
---
.../HandUnitPlugin/HandUnitGuiPlugin.cpp | 211 +++++++++++-------
.../HandUnitPlugin/HandUnitGuiPlugin.h | 30 ++-
2 files changed, 146 insertions(+), 95 deletions(-)
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
index e838c4e77..79979ddb4 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
+++ b/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()));
}
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.h
index 1b6420ccd..353156cff 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.h
+++ b/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
--
GitLab