From 8d4fc131013c191167aac10cc6a2fa1f54315e76 Mon Sep 17 00:00:00 2001
From: Christoph Pohl <christoph.pohl@kit.edu>
Date: Tue, 8 Aug 2023 11:00:14 +0200
Subject: [PATCH] autoformatting
Signed-off-by: ARMAR-DE <>
---
.../KinematicUnitGuiPlugin.cpp | 2506 +++++++++--------
1 file changed, 1316 insertions(+), 1190 deletions(-)
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index dd14b34d3..fa8011d66 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -22,59 +22,59 @@
* GNU General Public License
*/
#include "KinematicUnitGuiPlugin.h"
-#include "KinematicUnitConfigDialog.h"
-#include <RobotAPI/gui-plugins/KinematicUnitPlugin/ui_KinematicUnitConfigDialog.h>
-#include <RobotAPI/interface/core/NameValueMap.h>
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <SimoxUtility/algorithm/string.h>
+#include <SimoxUtility/json.h>
+#include <VirtualRobot/XML/RobotIO.h>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/services/tasks/RunningTask.h"
#include "ArmarXCore/core/time/Metronome.h"
#include "ArmarXCore/core/time/forward_declarations.h"
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-#include <ArmarXCore/core/ArmarXManager.h>
-
-#include <ArmarXCore/util/json/JSONObject.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/util/algorithm.h>
+#include <ArmarXCore/util/json/JSONObject.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <SimoxUtility/json.h>
-#include <SimoxUtility/algorithm/string.h>
+#include <RobotAPI/gui-plugins/KinematicUnitPlugin/ui_KinematicUnitConfigDialog.h>
+#include <RobotAPI/interface/core/NameValueMap.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+
+#include "KinematicUnitConfigDialog.h"
// Qt headers
-#include <Qt>
-#include <QtGlobal>
-#include <QSpinBox>
-#include <QSlider>
+#include <QCheckBox>
+#include <QClipboard>
+#include <QInputDialog>
#include <QPushButton>
+#include <QSlider>
+#include <QSpinBox>
#include <QStringList>
#include <QTableView>
-#include <QCheckBox>
#include <QTableWidget>
-#include <QClipboard>
-#include <QInputDialog>
+#include <Qt>
+#include <QtGlobal>
#include <qtimer.h>
-#include <Inventor/SoDB.h>
-#include <Inventor/Qt/SoQt.h>
#include <ArmarXCore/observers/filters/MedianFilter.h>
+#include <Inventor/Qt/SoQt.h>
+#include <Inventor/SoDB.h>
+
// System
+#include <cmath>
#include <cstddef>
#include <cstdio>
-#include <memory>
-#include <string>
-#include <optional>
#include <cstdlib>
-#include <iostream>
-#include <cmath>
-
#include <filesystem>
+#include <iostream>
+#include <memory>
+#include <optional>
+#include <string>
//#define KINEMATIC_UNIT_FILE_DEFAULT std::string("RobotAPI/robots/Armar3/ArmarIII.xml")
@@ -84,268 +84,277 @@
#define SLIDER_POS_DEG_MULTIPLIER 5
#define SLIDER_POS_RAD_MULTIPLIER 100
-
namespace armarx
{
-KinematicUnitGuiPlugin::KinematicUnitGuiPlugin()
-{
-
- qRegisterMetaType<DebugInfo>("DebugInfo");
+ KinematicUnitGuiPlugin::KinematicUnitGuiPlugin()
+ {
- addWidget<KinematicUnitWidgetController>();
-}
+ qRegisterMetaType<DebugInfo>("DebugInfo");
-KinematicUnitWidgetController::KinematicUnitWidgetController() :
- kinematicUnitNode(nullptr),
- enableValueValidator(true),
- historyTime(100000), // 1/10 s
- currentValueMax(5.0f)
-{
- rootVisu = NULL;
- debugLayerVisu = NULL;
+ addWidget<KinematicUnitWidgetController>();
+ }
- // init gui
- ui.setupUi(getWidget());
- getWidget()->setEnabled(false);
+ KinematicUnitWidgetController::KinematicUnitWidgetController() :
+ kinematicUnitNode(nullptr),
+ enableValueValidator(true),
+ historyTime(100000), // 1/10 s
+ currentValueMax(5.0f)
+ {
+ rootVisu = NULL;
+ debugLayerVisu = NULL;
- ui.tableJointList->setItemDelegateForColumn(eTabelColumnAngleProgressbar, &delegate);
+ // init gui
+ ui.setupUi(getWidget());
+ getWidget()->setEnabled(false);
- ui.radioButtonUnknown->setHidden(true);
-}
+ ui.tableJointList->setItemDelegateForColumn(eTabelColumnAngleProgressbar, &delegate);
-void KinematicUnitWidgetController::onInitComponent()
-{
- ARMARX_INFO << flush;
- verbose = true;
+ ui.radioButtonUnknown->setHidden(true);
+ }
+ void
+ KinematicUnitWidgetController::onInitComponent()
+ {
+ ARMARX_INFO << flush;
+ verbose = true;
- rootVisu = new SoSeparator;
- rootVisu->ref();
- robotVisu = new SoSeparator;
- robotVisu->ref();
- rootVisu->addChild(robotVisu);
- // create the debugdrawer component
- std::string debugDrawerComponentName = "KinemticUnitGUIDebugDrawer_" + getName();
- ARMARX_INFO << "Creating component " << debugDrawerComponentName;
- debugDrawer = Component::create<DebugDrawerComponent>(getIceProperties(), debugDrawerComponentName);
- showVisuLayers(false);
+ rootVisu = new SoSeparator;
+ rootVisu->ref();
+ robotVisu = new SoSeparator;
+ robotVisu->ref();
+ rootVisu->addChild(robotVisu);
- if (mutex3D)
- {
- //ARMARX_IMPORTANT << "mutex3d:" << mutex3D.get();
- debugDrawer->setMutex(mutex3D);
- }
- else
- {
- ARMARX_ERROR << " No 3d mutex available...";
- }
+ // create the debugdrawer component
+ std::string debugDrawerComponentName = "KinemticUnitGUIDebugDrawer_" + getName();
+ ARMARX_INFO << "Creating component " << debugDrawerComponentName;
+ debugDrawer =
+ Component::create<DebugDrawerComponent>(getIceProperties(), debugDrawerComponentName);
+ showVisuLayers(false);
- ArmarXManagerPtr m = getArmarXManager();
- m->addObject(debugDrawer, false);
+ if (mutex3D)
+ {
+ //ARMARX_IMPORTANT << "mutex3d:" << mutex3D.get();
+ debugDrawer->setMutex(mutex3D);
+ }
+ else
+ {
+ ARMARX_ERROR << " No 3d mutex available...";
+ }
+ ArmarXManagerPtr m = getArmarXManager();
+ m->addObject(debugDrawer, false);
- {
- std::unique_lock lock(*mutex3D);
- debugLayerVisu = new SoSeparator();
- debugLayerVisu->ref();
- debugLayerVisu->addChild(debugDrawer->getVisualization());
- rootVisu->addChild(debugLayerVisu);
- }
- connectSlots();
+ {
+ std::unique_lock lock(*mutex3D);
+ debugLayerVisu = new SoSeparator();
+ debugLayerVisu->ref();
+ debugLayerVisu->addChild(debugDrawer->getVisualization());
+ rootVisu->addChild(debugLayerVisu);
+ }
- usingProxy(kinematicUnitName);
-}
+ connectSlots();
-void KinematicUnitWidgetController::onConnectComponent()
-{
- // ARMARX_INFO << "Kinematic Unit Gui :: onConnectComponent()";
- jointCurrentHistory.clear();
- jointCurrentHistory.set_capacity(5);
+ usingProxy(kinematicUnitName);
+ }
- // jointAnglesUpdateFrequency = new filters::MedianFilter(100);
- kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
+ void
+ KinematicUnitWidgetController::onConnectComponent()
+ {
+ // ARMARX_INFO << "Kinematic Unit Gui :: onConnectComponent()";
+ jointCurrentHistory.clear();
+ jointCurrentHistory.set_capacity(5);
- lastJointAngleUpdateTimestamp = Clock::Now();
- robotVisu->removeAllChildren();
+ // jointAnglesUpdateFrequency = new filters::MedianFilter(100);
+ kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
- robot.reset();
+ lastJointAngleUpdateTimestamp = Clock::Now();
+ robotVisu->removeAllChildren();
- std::string rfile;
- Ice::StringSeq includePaths;
+ robot.reset();
- // Get robot filename
- try
- {
- Ice::StringSeq packages = kinematicUnitInterfacePrx->getArmarXPackages();
- packages.push_back(Application::GetProjectName());
- ARMARX_VERBOSE << "ArmarX packages " << packages;
+ std::string rfile;
+ Ice::StringSeq includePaths;
- for (const std::string& projectName : packages)
+ // Get robot filename
+ try
{
- if (projectName.empty())
+ Ice::StringSeq packages = kinematicUnitInterfacePrx->getArmarXPackages();
+ packages.push_back(Application::GetProjectName());
+ ARMARX_VERBOSE << "ArmarX packages " << packages;
+
+ for (const std::string& projectName : packages)
{
- continue;
+ if (projectName.empty())
+ {
+ continue;
+ }
+
+ CMakePackageFinder project(projectName);
+ auto pathsString = project.getDataDir();
+ ARMARX_VERBOSE << "Data paths of ArmarX package " << projectName << ": "
+ << pathsString;
+ Ice::StringSeq projectIncludePaths = Split(pathsString, ";,", true, true);
+ ARMARX_VERBOSE << "Result: Data paths of ArmarX package " << projectName << ": "
+ << projectIncludePaths;
+ includePaths.insert(
+ includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
- CMakePackageFinder project(projectName);
- auto pathsString = project.getDataDir();
- ARMARX_VERBOSE << "Data paths of ArmarX package " << projectName << ": " << pathsString;
- Ice::StringSeq projectIncludePaths = Split(pathsString, ";,", true, true);
- ARMARX_VERBOSE << "Result: Data paths of ArmarX package " << projectName << ": " << projectIncludePaths;
- includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
- }
-
- rfile = kinematicUnitInterfacePrx->getRobotFilename();
- ARMARX_VERBOSE << "Relative robot file " << rfile;
- ArmarXDataPath::getAbsolutePath(rfile, rfile, includePaths);
- ARMARX_VERBOSE << "Absolute robot file " << rfile;
-
- robotNodeSetName = kinematicUnitInterfacePrx->getRobotNodeSetName();
- }
- catch (...)
- {
- ARMARX_ERROR << "Unable to retrieve robot filename.";
- }
+ rfile = kinematicUnitInterfacePrx->getRobotFilename();
+ ARMARX_VERBOSE << "Relative robot file " << rfile;
+ ArmarXDataPath::getAbsolutePath(rfile, rfile, includePaths);
+ ARMARX_VERBOSE << "Absolute robot file " << rfile;
- try
- {
- ARMARX_INFO << "Loading robot from file " << rfile;
- robot = loadRobotFile(rfile);
- }
- catch (const std::exception& e)
- {
- ARMARX_ERROR << "Failed to init robot: " << e.what();
- }
- catch (...)
- {
- ARMARX_ERROR << "Failed to init robot";
- }
-
- if (!robot || !robot->hasRobotNodeSet(robotNodeSetName))
- {
- getObjectScheduler()->terminate();
- if (getWidget()->parentWidget())
+ robotNodeSetName = kinematicUnitInterfacePrx->getRobotNodeSetName();
+ }
+ catch (...)
{
- getWidget()->parentWidget()->close();
+ ARMARX_ERROR << "Unable to retrieve robot filename.";
}
- return;
- }
- // Check robot name and disable setZero Button if necessary
- if (not simox::alg::starts_with(robot->getName(), "Armar3"))
- {
- ARMARX_VERBOSE << "Disable the SetZero button because the robot name is '" << robot->getName() << "'.";
- ui.pushButtonKinematicUnitPos1->setDisabled(true);
- }
+ try
+ {
+ ARMARX_INFO << "Loading robot from file " << rfile;
+ robot = loadRobotFile(rfile);
+ }
+ catch (const std::exception& e)
+ {
+ ARMARX_ERROR << "Failed to init robot: " << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Failed to init robot";
+ }
- kinematicUnitFile = rfile;
- robotNodeSet = robot->getRobotNodeSet(robotNodeSetName);
+ if (!robot || !robot->hasRobotNodeSet(robotNodeSetName))
+ {
+ getObjectScheduler()->terminate();
+ if (getWidget()->parentWidget())
+ {
+ getWidget()->parentWidget()->close();
+ }
+ return;
+ }
- kinematicUnitVisualization = getCoinVisualization(robot);
- kinematicUnitNode = kinematicUnitVisualization->getCoinVisualization();
- robotVisu->addChild(kinematicUnitNode);
+ // Check robot name and disable setZero Button if necessary
+ if (not simox::alg::starts_with(robot->getName(), "Armar3"))
+ {
+ ARMARX_VERBOSE << "Disable the SetZero button because the robot name is '"
+ << robot->getName() << "'.";
+ ui.pushButtonKinematicUnitPos1->setDisabled(true);
+ }
- // Fetch the current joint angles.
- synchronizeRobotJointAngles();
+ kinematicUnitFile = rfile;
+ robotNodeSet = robot->getRobotNodeSet(robotNodeSetName);
- initGUIComboBox(robotNodeSet); // init the pull down menu (QT: ComboBox)
- initGUIJointListTable(robotNodeSet);
+ kinematicUnitVisualization = getCoinVisualization(robot);
+ kinematicUnitNode = kinematicUnitVisualization->getCoinVisualization();
+ robotVisu->addChild(kinematicUnitNode);
- const auto initialDebugInfo = kinematicUnitInterfacePrx->getDebugInfo();
+ // Fetch the current joint angles.
+ synchronizeRobotJointAngles();
- initializeUi(initialDebugInfo);
+ initGUIComboBox(robotNodeSet); // init the pull down menu (QT: ComboBox)
+ initGUIJointListTable(robotNodeSet);
- QMetaObject::invokeMethod(this, "resetSlider");
- enableMainWidgetAsync(true);
+ const auto initialDebugInfo = kinematicUnitInterfacePrx->getDebugInfo();
- updateTask = new RunningTask<KinematicUnitWidgetController>(this, &KinematicUnitWidgetController::runUpdate);
- updateTask->start();
-}
+ initializeUi(initialDebugInfo);
-void KinematicUnitWidgetController::runUpdate()
-{
- Metronome metronome(Frequency::Hertz(10));
+ QMetaObject::invokeMethod(this, "resetSlider");
+ enableMainWidgetAsync(true);
- while(kinematicUnitInterfacePrx)
- {
- fetchData();
- metronome.waitForNextTick();
+ updateTask = new RunningTask<KinematicUnitWidgetController>(
+ this, &KinematicUnitWidgetController::runUpdate);
+ updateTask->start();
}
- ARMARX_INFO << "Connection to kinemetic unit lost. Update task terminates.";
-}
-
-void KinematicUnitWidgetController::onDisconnectComponent()
-{
- kinematicUnitInterfacePrx = nullptr;
-
- if(updateTask)
+ void
+ KinematicUnitWidgetController::runUpdate()
{
- updateTask->stop();
- updateTask->join();
- updateTask = nullptr;
- }
+ Metronome metronome(Frequency::Hertz(10));
- // killTimer(updateTimerId);
- enableMainWidgetAsync(false);
+ while (kinematicUnitInterfacePrx)
+ {
+ fetchData();
+ metronome.waitForNextTick();
+ }
- {
- std::unique_lock lock(mutexNodeSet);
- robot.reset();
- robotNodeSet.reset();
- currentNode.reset();
+ ARMARX_INFO << "Connection to kinemetic unit lost. Update task terminates.";
}
+ void
+ KinematicUnitWidgetController::onDisconnectComponent()
{
- std::unique_lock lock(*mutex3D);
- robotVisu->removeAllChildren();
- debugLayerVisu->removeAllChildren();
- }
-
-}
+ kinematicUnitInterfacePrx = nullptr;
-void KinematicUnitWidgetController::onExitComponent()
-{
- kinematicUnitInterfacePrx = nullptr;
-
- if(updateTask)
- {
- updateTask->stop();
- updateTask->join();
- updateTask = nullptr;
- }
+ if (updateTask)
+ {
+ updateTask->stop();
+ updateTask->join();
+ updateTask = nullptr;
+ }
- enableMainWidgetAsync(false);
+ // killTimer(updateTimerId);
+ enableMainWidgetAsync(false);
- {
- std::unique_lock lock(*mutex3D);
+ {
+ std::unique_lock lock(mutexNodeSet);
+ robot.reset();
+ robotNodeSet.reset();
+ currentNode.reset();
+ }
- if (robotVisu)
{
+ std::unique_lock lock(*mutex3D);
robotVisu->removeAllChildren();
- robotVisu->unref();
- robotVisu = NULL;
+ debugLayerVisu->removeAllChildren();
}
+ }
+
+ void
+ KinematicUnitWidgetController::onExitComponent()
+ {
+ kinematicUnitInterfacePrx = nullptr;
- if (debugLayerVisu)
+ if (updateTask)
{
- debugLayerVisu->removeAllChildren();
- debugLayerVisu->unref();
- debugLayerVisu = NULL;
+ updateTask->stop();
+ updateTask->join();
+ updateTask = nullptr;
}
- if (rootVisu)
+ enableMainWidgetAsync(false);
+
{
- rootVisu->removeAllChildren();
- rootVisu->unref();
- rootVisu = NULL;
+ std::unique_lock lock(*mutex3D);
+
+ if (robotVisu)
+ {
+ robotVisu->removeAllChildren();
+ robotVisu->unref();
+ robotVisu = NULL;
+ }
+
+ if (debugLayerVisu)
+ {
+ debugLayerVisu->removeAllChildren();
+ debugLayerVisu->unref();
+ debugLayerVisu = NULL;
+ }
+
+ if (rootVisu)
+ {
+ rootVisu->removeAllChildren();
+ rootVisu->unref();
+ rootVisu = NULL;
+ }
}
- }
- /*
+ /*
if (debugDrawer && debugDrawer->getObjectScheduler())
{
ARMARX_INFO << "Removing DebugDrawer component...";
@@ -353,757 +362,848 @@ void KinematicUnitWidgetController::onExitComponent()
ARMARX_INFO << "Removing DebugDrawer component...done";
}
*/
-}
-
-QPointer<QDialog> KinematicUnitWidgetController::getConfigDialog(QWidget* parent)
-{
- if (!dialog)
- {
- dialog = new KinematicUnitConfigDialog(parent);
- dialog->setName(dialog->getDefaultName());
}
- return qobject_cast<KinematicUnitConfigDialog*>(dialog);
-}
-
-void KinematicUnitWidgetController::configured()
-{
- ARMARX_VERBOSE << "KinematicUnitWidget::configured()";
- kinematicUnitName = dialog->proxyFinder->getSelectedProxyName().toStdString();
- enableValueValidator = dialog->ui->checkBox->isChecked();
- viewerEnabled = dialog->ui->checkBox3DViewerEnabled->isChecked();
- historyTime = dialog->ui->spinBoxHistory->value() * 1000;
- currentValueMax = dialog->ui->doubleSpinBoxMaxMinCurrent->value();
-}
-
-void KinematicUnitWidgetController::loadSettings(QSettings* settings)
-{
- kinematicUnitName = settings->value("kinematicUnitName", KINEMATIC_UNIT_NAME_DEFAULT).toString().toStdString();
- enableValueValidator = settings->value("enableValueValidator", true).toBool();
- viewerEnabled = settings->value("viewerEnabled", true).toBool();
- historyTime = settings->value("historyTime", 100).toInt() * 1000;
- currentValueMax = settings->value("currentValueMax", 5.0).toFloat();
-}
-
-void KinematicUnitWidgetController::saveSettings(QSettings* settings)
-{
- settings->setValue("kinematicUnitName", QString::fromStdString(kinematicUnitName));
- settings->setValue("enableValueValidator", enableValueValidator);
- settings->setValue("viewerEnabled", viewerEnabled);
- assert(historyTime % 1000 == 0);
- settings->setValue("historyTime", static_cast<int>(historyTime / 1000));
- settings->setValue("currentValueMax", currentValueMax);
-}
-
-
-void KinematicUnitWidgetController::showVisuLayers(bool show)
-{
- if (debugDrawer)
+ QPointer<QDialog>
+ KinematicUnitWidgetController::getConfigDialog(QWidget* parent)
{
- if (show)
+ if (!dialog)
{
- debugDrawer->enableAllLayers();
- }
- else
- {
- debugDrawer->disableAllLayers();
+ dialog = new KinematicUnitConfigDialog(parent);
+ dialog->setName(dialog->getDefaultName());
}
+
+ return qobject_cast<KinematicUnitConfigDialog*>(dialog);
}
-}
-void KinematicUnitWidgetController::copyToClipboard()
-{
- NameValueMap values;
+ void
+ KinematicUnitWidgetController::configured()
{
- std::unique_lock lock(mutexNodeSet);
-
- ARMARX_CHECK_NOT_NULL(kinematicUnitInterfacePrx);
- const auto debugInfo = kinematicUnitInterfacePrx->getDebugInfo();
-
- const auto selectedControlMode = getSelectedControlMode();
-
- if (selectedControlMode == ePositionControl)
- {
- values = debugInfo.jointAngles;
- }
- else if (selectedControlMode == eVelocityControl)
- {
- values = debugInfo.jointVelocities;
- }
+ ARMARX_VERBOSE << "KinematicUnitWidget::configured()";
+ kinematicUnitName = dialog->proxyFinder->getSelectedProxyName().toStdString();
+ enableValueValidator = dialog->ui->checkBox->isChecked();
+ viewerEnabled = dialog->ui->checkBox3DViewerEnabled->isChecked();
+ historyTime = dialog->ui->spinBoxHistory->value() * 1000;
+ currentValueMax = dialog->ui->doubleSpinBoxMaxMinCurrent->value();
}
- JSONObjectPtr serializer = new JSONObject();
- for (auto& kv : values)
+ void
+ KinematicUnitWidgetController::loadSettings(QSettings* settings)
{
- serializer->setFloat(kv.first, kv.second);
+ kinematicUnitName = settings->value("kinematicUnitName", KINEMATIC_UNIT_NAME_DEFAULT)
+ .toString()
+ .toStdString();
+ enableValueValidator = settings->value("enableValueValidator", true).toBool();
+ viewerEnabled = settings->value("viewerEnabled", true).toBool();
+ historyTime = settings->value("historyTime", 100).toInt() * 1000;
+ currentValueMax = settings->value("currentValueMax", 5.0).toFloat();
}
- const QString json = QString::fromStdString(serializer->asString(true));
- QClipboard* clipboard = QApplication::clipboard();
- clipboard->setText(json);
- QApplication::processEvents();
-}
-
-void KinematicUnitWidgetController::updateGuiElements()
-{
- // modelUpdateCB();
-}
-
-void KinematicUnitWidgetController::updateKinematicUnitListInDialog()
-{
-}
-
-void KinematicUnitWidgetController::modelUpdateCB()
-{
-}
-
-SoNode* KinematicUnitWidgetController::getScene()
-{
- if (viewerEnabled)
+ void
+ KinematicUnitWidgetController::saveSettings(QSettings* settings)
{
- ARMARX_INFO << "Returning scene ";
- return rootVisu;
+ settings->setValue("kinematicUnitName", QString::fromStdString(kinematicUnitName));
+ settings->setValue("enableValueValidator", enableValueValidator);
+ settings->setValue("viewerEnabled", viewerEnabled);
+ assert(historyTime % 1000 == 0);
+ settings->setValue("historyTime", static_cast<int>(historyTime / 1000));
+ settings->setValue("currentValueMax", currentValueMax);
}
- else
+
+ void
+ KinematicUnitWidgetController::showVisuLayers(bool show)
{
- ARMARX_INFO << "viewer disabled - returning null scene";
- return NULL;
+ if (debugDrawer)
+ {
+ if (show)
+ {
+ debugDrawer->enableAllLayers();
+ }
+ else
+ {
+ debugDrawer->disableAllLayers();
+ }
+ }
}
-}
-
-void KinematicUnitWidgetController::connectSlots()
-{
- connect(ui.pushButtonKinematicUnitPos1, SIGNAL(clicked()), this, SLOT(kinematicUnitZeroPosition()));
-
- connect(ui.nodeListComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectJoint(int)));
- connect(ui.horizontalSliderKinematicUnitPos, SIGNAL(valueChanged(int)), this, SLOT(sliderValueChanged(int)));
- connect(ui.horizontalSliderKinematicUnitPos, SIGNAL(sliderReleased()), this, SLOT(resetSliderToZeroPosition()));
-
- connect(ui.radioButtonPositionControl, SIGNAL(clicked(bool)), this, SLOT(setControlModePosition()));
- connect(ui.radioButtonVelocityControl, SIGNAL(clicked(bool)), this, SLOT(setControlModeVelocity()));
- connect(ui.radioButtonTorqueControl, SIGNAL(clicked(bool)), this, SLOT(setControlModeTorque()));
- connect(ui.pushButtonFromJson, SIGNAL(clicked()), this, SLOT(on_pushButtonFromJson_clicked()));
-
- connect(ui.copyToClipboard, SIGNAL(clicked()), this, SLOT(copyToClipboard()));
- connect(ui.showDebugLayer, SIGNAL(toggled(bool)), this, SLOT(showVisuLayers(bool)), Qt::QueuedConnection);
-
- connect(this, SIGNAL(jointAnglesReported()), this, SLOT(updateJointAnglesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointVelocitiesReported()), this, SLOT(updateJointVelocitiesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointTorquesReported()), this, SLOT(updateJointTorquesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointCurrentsReported()), this, SLOT(updateJointCurrentsTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointMotorTemperaturesReported()), this, SLOT(updateMotorTemperaturesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointControlModesReported()), this, SLOT(updateControlModesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointStatusesReported()), this, SLOT(updateJointStatusesTable()), Qt::QueuedConnection);
-
- connect(ui.tableJointList, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(selectJointFromTableWidget(int, int)), Qt::QueuedConnection);
-
- connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(resetSlider()), Qt::QueuedConnection);
- connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModePosition()));
- connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModeVelocity()));
- connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModeTorque()));
-
- connect(this, SIGNAL(onDebugInfoReceived(const DebugInfo&)), this, SLOT(debugInfoReceived(const DebugInfo&)));
-}
-
-void KinematicUnitWidgetController::initializeUi(const DebugInfo& debugInfo)
-{
- //signal clicked is not emitted if you call setDown(), setChecked() or toggle().
-
- // there is no default control mode
- setControlModeRadioButtonGroup(ControlMode::eUnknown);
+ void
+ KinematicUnitWidgetController::copyToClipboard()
+ {
+ NameValueMap values;
+ {
+ std::unique_lock lock(mutexNodeSet);
- ui.widgetSliderFactor->setVisible(false);
+ ARMARX_CHECK_NOT_NULL(kinematicUnitInterfacePrx);
+ const auto debugInfo = kinematicUnitInterfacePrx->getDebugInfo();
- fetchData();
-}
+ const auto selectedControlMode = getSelectedControlMode();
+ if (selectedControlMode == ePositionControl)
+ {
+ values = debugInfo.jointAngles;
+ }
+ else if (selectedControlMode == eVelocityControl)
+ {
+ values = debugInfo.jointVelocities;
+ }
+ }
-void KinematicUnitWidgetController::kinematicUnitZeroVelocity()
-{
- if (!robotNodeSet)
- {
- return;
+ JSONObjectPtr serializer = new JSONObject();
+ for (auto& kv : values)
+ {
+ serializer->setFloat(kv.first, kv.second);
+ }
+ const QString json = QString::fromStdString(serializer->asString(true));
+ QClipboard* clipboard = QApplication::clipboard();
+ clipboard->setText(json);
+ QApplication::processEvents();
}
- std::unique_lock lock(mutexNodeSet);
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- NameValueMap vels;
- NameControlModeMap jointModes;
-
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateGuiElements()
{
- jointModes[rn[i]->getName()] = eVelocityControl;
- vels[rn[i]->getName()] = 0.0f;
+ // modelUpdateCB();
}
- try
- {
- kinematicUnitInterfacePrx->switchControlMode(jointModes);
- kinematicUnitInterfacePrx->setJointVelocities(vels);
- }
- catch (...)
+ void
+ KinematicUnitWidgetController::updateKinematicUnitListInDialog()
{
}
- const auto selectedControlMode = getSelectedControlMode();
- if (selectedControlMode == eVelocityControl)
+ void
+ KinematicUnitWidgetController::modelUpdateCB()
{
- ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
}
-}
-
-void KinematicUnitWidgetController::resetSlider()
-{
- const auto selectedControlMode = getSelectedControlMode();
-
- if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
- {
- resetSliderToZeroPosition();
- }
- else if (selectedControlMode == ePositionControl)
+ SoNode*
+ KinematicUnitWidgetController::getScene()
{
- if (currentNode)
+ if (viewerEnabled)
{
- const bool isDeg = ui.checkBoxUseDegree->isChecked();
- const bool isRot = currentNode->isRotationalJoint();
- const auto factor = isRot && ! isDeg ? SLIDER_POS_RAD_MULTIPLIER : SLIDER_POS_DEG_MULTIPLIER;
- float conversionFactor = isRot && isDeg ? 180.0 / M_PI : 1.0f;
- float pos = currentNode->getJointValue() * conversionFactor;
-
- ui.lcdNumberKinematicUnitJointValue->display((int)pos);
- ui.horizontalSliderKinematicUnitPos->setSliderPosition((int)(pos * factor));
+ ARMARX_INFO << "Returning scene ";
+ return rootVisu;
+ }
+ else
+ {
+ ARMARX_INFO << "viewer disabled - returning null scene";
+ return NULL;
}
}
-}
-
-void KinematicUnitWidgetController::resetSliderToZeroPosition()
-{
- const auto selectedControlMode = getSelectedControlMode();
+ void
+ KinematicUnitWidgetController::connectSlots()
+ {
+ connect(ui.pushButtonKinematicUnitPos1,
+ SIGNAL(clicked()),
+ this,
+ SLOT(kinematicUnitZeroPosition()));
+
+ connect(
+ ui.nodeListComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectJoint(int)));
+ connect(ui.horizontalSliderKinematicUnitPos,
+ SIGNAL(valueChanged(int)),
+ this,
+ SLOT(sliderValueChanged(int)));
+
+ connect(ui.horizontalSliderKinematicUnitPos,
+ SIGNAL(sliderReleased()),
+ this,
+ SLOT(resetSliderToZeroPosition()));
+
+ connect(ui.radioButtonPositionControl,
+ SIGNAL(clicked(bool)),
+ this,
+ SLOT(setControlModePosition()));
+ connect(ui.radioButtonVelocityControl,
+ SIGNAL(clicked(bool)),
+ this,
+ SLOT(setControlModeVelocity()));
+ connect(
+ ui.radioButtonTorqueControl, SIGNAL(clicked(bool)), this, SLOT(setControlModeTorque()));
+ connect(
+ ui.pushButtonFromJson, SIGNAL(clicked()), this, SLOT(on_pushButtonFromJson_clicked()));
+
+ connect(ui.copyToClipboard, SIGNAL(clicked()), this, SLOT(copyToClipboard()));
+ connect(ui.showDebugLayer,
+ SIGNAL(toggled(bool)),
+ this,
+ SLOT(showVisuLayers(bool)),
+ Qt::QueuedConnection);
+
+ connect(this,
+ SIGNAL(jointAnglesReported()),
+ this,
+ SLOT(updateJointAnglesTable()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(jointVelocitiesReported()),
+ this,
+ SLOT(updateJointVelocitiesTable()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(jointTorquesReported()),
+ this,
+ SLOT(updateJointTorquesTable()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(jointCurrentsReported()),
+ this,
+ SLOT(updateJointCurrentsTable()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(jointMotorTemperaturesReported()),
+ this,
+ SLOT(updateMotorTemperaturesTable()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(jointControlModesReported()),
+ this,
+ SLOT(updateControlModesTable()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(jointStatusesReported()),
+ this,
+ SLOT(updateJointStatusesTable()),
+ Qt::QueuedConnection);
+
+ connect(ui.tableJointList,
+ SIGNAL(cellDoubleClicked(int, int)),
+ this,
+ SLOT(selectJointFromTableWidget(int, int)),
+ Qt::QueuedConnection);
+
+ connect(ui.checkBoxUseDegree,
+ SIGNAL(clicked()),
+ this,
+ SLOT(resetSlider()),
+ Qt::QueuedConnection);
+ connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModePosition()));
+ connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModeVelocity()));
+ connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModeTorque()));
+
+ connect(this,
+ SIGNAL(onDebugInfoReceived(const DebugInfo&)),
+ this,
+ SLOT(debugInfoReceived(const DebugInfo&)));
+ }
+
+ void
+ KinematicUnitWidgetController::initializeUi(const DebugInfo& debugInfo)
+ {
+ //signal clicked is not emitted if you call setDown(), setChecked() or toggle().
+
+ // there is no default control mode
+ setControlModeRadioButtonGroup(ControlMode::eUnknown);
+
+ ui.widgetSliderFactor->setVisible(false);
- if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
- {
- ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
- ui.lcdNumberKinematicUnitJointValue->display(SLIDER_ZERO_POSITION);
- }
-}
-
-void KinematicUnitWidgetController::setControlModeRadioButtonGroup(const ControlMode& controlMode)
-{
- ARMARX_VERBOSE << "Setting control mode of radio button group to " << controlMode;
-
- switch(controlMode)
- {
- case eDisabled:
- case eUnknown:
- case ePositionVelocityControl:
- ui.radioButtonUnknown->setChecked(true);
- break;
- case ePositionControl:
- ui.radioButtonPositionControl->setChecked(true);
- break;
- case eVelocityControl:
- ui.radioButtonVelocityControl->setChecked(true);
- break;
- case eTorqueControl:
- ui.radioButtonTorqueControl->setChecked(true);
- break;
+ fetchData();
}
-}
-
-void KinematicUnitWidgetController::setControlModePosition()
-{
- if (!ui.radioButtonPositionControl->isChecked())
+ void
+ KinematicUnitWidgetController::kinematicUnitZeroVelocity()
{
- return;
- }
- NameControlModeMap jointModes;
- // selectedControlMode = ePositionControl;
- ui.widgetSliderFactor->setVisible(false);
+ if (!robotNodeSet)
+ {
+ return;
+ }
- // FIXME currentNode should be passed to this function!
+ std::unique_lock lock(mutexNodeSet);
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+ NameValueMap vels;
+ NameControlModeMap jointModes;
- if (currentNode)
- {
- QString unit = currentNode->isRotationalJoint()
- ? (ui.checkBoxUseDegree->isChecked() ? "deg" : "rad")
- : "mm";
- ui.labelUnit->setText(unit);
- const bool isDeg = ui.checkBoxUseDegree->isChecked();
- const bool isRot = currentNode->isRotationalJoint();
- const auto factor = isRot && ! isDeg ? SLIDER_POS_RAD_MULTIPLIER : SLIDER_POS_DEG_MULTIPLIER;
- float conversionFactor = isRot && isDeg ? 180.0 / M_PI : 1.0f;
- jointModes[currentNode->getName()] = ePositionControl;
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ jointModes[rn[i]->getName()] = eVelocityControl;
+ vels[rn[i]->getName()] = 0.0f;
+ }
- if (kinematicUnitInterfacePrx)
+ try
{
kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ kinematicUnitInterfacePrx->setJointVelocities(vels);
}
-
- float lo = currentNode->getJointLimitLo() * conversionFactor;
- float hi = currentNode->getJointLimitHi() * conversionFactor;
-
- if (hi - lo <= 0.0f)
+ catch (...)
{
- return;
}
+ const auto selectedControlMode = getSelectedControlMode();
+ if (selectedControlMode == eVelocityControl)
{
- // currentNode->getJointValue() can we wrong after we re-connected to the robot unit.
- // E.g., it can be 0 although the torso joint was at -365 before the unit disconnected.
- // Therefore, we first have to fetch the actual joint values and use that one.
- // However, this should actually not be necessary, as the robot model should be updated
- // via the topics.
- synchronizeRobotJointAngles();
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
}
+ }
- float pos = currentNode->getJointValue() * conversionFactor;
- ARMARX_INFO << "Setting position control for current node "
- << "(name '" << currentNode->getName() << "' with current value " << pos << ")";
+ void
+ KinematicUnitWidgetController::resetSlider()
+ {
+ const auto selectedControlMode = getSelectedControlMode();
- // Setting the slider position to pos will set the position to the slider tick closest to pos
- // This will initially send a position target with a small delta to the joint.
- ui.horizontalSliderKinematicUnitPos->blockSignals(true);
+ if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
+ {
+ resetSliderToZeroPosition();
+ }
+ else if (selectedControlMode == ePositionControl)
+ {
+ if (currentNode)
+ {
+ const bool isDeg = ui.checkBoxUseDegree->isChecked();
+ const bool isRot = currentNode->isRotationalJoint();
+ const auto factor =
+ isRot && !isDeg ? SLIDER_POS_RAD_MULTIPLIER : SLIDER_POS_DEG_MULTIPLIER;
+ float conversionFactor = isRot && isDeg ? 180.0 / M_PI : 1.0f;
+ float pos = currentNode->getJointValue() * conversionFactor;
+
+ ui.lcdNumberKinematicUnitJointValue->display((int)pos);
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition((int)(pos * factor));
+ }
+ }
+ }
- ui.horizontalSliderKinematicUnitPos->setMaximum(hi * factor);
- ui.horizontalSliderKinematicUnitPos->setMinimum(lo * factor);
- ui.lcdNumberKinematicUnitJointValue->display(pos);
+ void
+ KinematicUnitWidgetController::resetSliderToZeroPosition()
+ {
+ const auto selectedControlMode = getSelectedControlMode();
- ui.horizontalSliderKinematicUnitPos->blockSignals(false);
- resetSlider();
+ if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
+ {
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
+ ui.lcdNumberKinematicUnitJointValue->display(SLIDER_ZERO_POSITION);
+ }
}
-}
-void KinematicUnitWidgetController::setControlModeVelocity()
-{
- if (!ui.radioButtonVelocityControl->isChecked())
+ void
+ KinematicUnitWidgetController::setControlModeRadioButtonGroup(const ControlMode& controlMode)
{
- return;
+ ARMARX_VERBOSE << "Setting control mode of radio button group to " << controlMode;
+
+ switch (controlMode)
+ {
+ case eDisabled:
+ case eUnknown:
+ case ePositionVelocityControl:
+ ui.radioButtonUnknown->setChecked(true);
+ break;
+ case ePositionControl:
+ ui.radioButtonPositionControl->setChecked(true);
+ break;
+ case eVelocityControl:
+ ui.radioButtonVelocityControl->setChecked(true);
+ break;
+ case eTorqueControl:
+ ui.radioButtonTorqueControl->setChecked(true);
+ break;
+ }
}
- NameControlModeMap jointModes;
- NameValueMap jointVelocities;
- if (currentNode)
+ void
+ KinematicUnitWidgetController::setControlModePosition()
{
- jointModes[currentNode->getName()] = eVelocityControl;
-
- // set the velocity to zero to stop any previous controller (e.g. torque controller)
- jointVelocities[currentNode->getName()] = 0;
+ if (!ui.radioButtonPositionControl->isChecked())
+ {
+ return;
+ }
+ NameControlModeMap jointModes;
+ // selectedControlMode = ePositionControl;
+ ui.widgetSliderFactor->setVisible(false);
- const bool isDeg = ui.checkBoxUseDegree->isChecked();
- const bool isRot = currentNode->isRotationalJoint();
- QString unit = isRot ?
- (isDeg ? "deg/s" : "rad/(100*s)") :
- "mm/s";
- ui.labelUnit->setText(unit);
- ARMARX_INFO << "setting velocity control for current Node Name: " << currentNode->getName() << flush;
- float lo = isRot ? (isDeg ? -90 : -M_PI * 100) : -1000;
- float hi = isRot ? (isDeg ? +90 : +M_PI * 100) : 1000;
+ // FIXME currentNode should be passed to this function!
- try
+ if (currentNode)
{
+ QString unit = currentNode->isRotationalJoint()
+ ? (ui.checkBoxUseDegree->isChecked() ? "deg" : "rad")
+ : "mm";
+ ui.labelUnit->setText(unit);
+ const bool isDeg = ui.checkBoxUseDegree->isChecked();
+ const bool isRot = currentNode->isRotationalJoint();
+ const auto factor =
+ isRot && !isDeg ? SLIDER_POS_RAD_MULTIPLIER : SLIDER_POS_DEG_MULTIPLIER;
+ float conversionFactor = isRot && isDeg ? 180.0 / M_PI : 1.0f;
+ jointModes[currentNode->getName()] = ePositionControl;
+
if (kinematicUnitInterfacePrx)
{
kinematicUnitInterfacePrx->switchControlMode(jointModes);
- kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
}
- }
- catch (...)
- {
- }
-
- ui.widgetSliderFactor->setVisible(true);
+ float lo = currentNode->getJointLimitLo() * conversionFactor;
+ float hi = currentNode->getJointLimitHi() * conversionFactor;
- ui.horizontalSliderKinematicUnitPos->blockSignals(true);
- ui.horizontalSliderKinematicUnitPos->setMaximum(hi);
- ui.horizontalSliderKinematicUnitPos->setMinimum(lo);
- ui.horizontalSliderKinematicUnitPos->blockSignals(false);
- resetSlider();
- }
-}
-
-ControlMode KinematicUnitWidgetController::getSelectedControlMode() const
-{
- if(ui.radioButtonPositionControl->isChecked())
- {
- return ControlMode::ePositionControl;
- }
-
- if (ui.radioButtonVelocityControl->isChecked())
- {
- return ControlMode::eVelocityControl;
- }
+ if (hi - lo <= 0.0f)
+ {
+ return;
+ }
- if (ui.radioButtonTorqueControl->isChecked())
- {
- return ControlMode::eTorqueControl;
- }
+ {
+ // currentNode->getJointValue() can we wrong after we re-connected to the robot unit.
+ // E.g., it can be 0 although the torso joint was at -365 before the unit disconnected.
+ // Therefore, we first have to fetch the actual joint values and use that one.
+ // However, this should actually not be necessary, as the robot model should be updated
+ // via the topics.
+ synchronizeRobotJointAngles();
+ }
- // if no button is checked, then the joint is likely initialized but no controller has been loaded yet
- // (well, the no movement controller should be active)
- return ControlMode::eUnknown;
+ float pos = currentNode->getJointValue() * conversionFactor;
+ ARMARX_INFO << "Setting position control for current node "
+ << "(name '" << currentNode->getName() << "' with current value " << pos
+ << ")";
+ // Setting the slider position to pos will set the position to the slider tick closest to pos
+ // This will initially send a position target with a small delta to the joint.
+ ui.horizontalSliderKinematicUnitPos->blockSignals(true);
-}
+ ui.horizontalSliderKinematicUnitPos->setMaximum(hi * factor);
+ ui.horizontalSliderKinematicUnitPos->setMinimum(lo * factor);
+ ui.lcdNumberKinematicUnitJointValue->display(pos);
-void KinematicUnitWidgetController::setControlModeTorque()
-{
- if (!ui.radioButtonTorqueControl->isChecked())
- {
- return;
+ ui.horizontalSliderKinematicUnitPos->blockSignals(false);
+ resetSlider();
+ }
}
- NameControlModeMap jointModes;
- if (currentNode)
+ void
+ KinematicUnitWidgetController::setControlModeVelocity()
{
- jointModes[currentNode->getName()] = eTorqueControl;
- ui.labelUnit->setText("Ncm");
- ARMARX_INFO << "setting torque control for current Node Name: " << currentNode->getName() << flush;
+ if (!ui.radioButtonVelocityControl->isChecked())
+ {
+ return;
+ }
+ NameControlModeMap jointModes;
+ NameValueMap jointVelocities;
- if (kinematicUnitInterfacePrx)
+ if (currentNode)
{
+ jointModes[currentNode->getName()] = eVelocityControl;
+
+ // set the velocity to zero to stop any previous controller (e.g. torque controller)
+ jointVelocities[currentNode->getName()] = 0;
+
+ const bool isDeg = ui.checkBoxUseDegree->isChecked();
+ const bool isRot = currentNode->isRotationalJoint();
+ QString unit = isRot ? (isDeg ? "deg/s" : "rad/(100*s)") : "mm/s";
+ ui.labelUnit->setText(unit);
+ ARMARX_INFO << "setting velocity control for current Node Name: "
+ << currentNode->getName() << flush;
+ float lo = isRot ? (isDeg ? -90 : -M_PI * 100) : -1000;
+ float hi = isRot ? (isDeg ? +90 : +M_PI * 100) : 1000;
+
try
{
- kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ if (kinematicUnitInterfacePrx)
+ {
+ kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
+ }
}
catch (...)
{
-
}
- }
-
- ui.horizontalSliderKinematicUnitPos->blockSignals(true);
- ui.horizontalSliderKinematicUnitPos->setMaximum(20000.0);
- ui.horizontalSliderKinematicUnitPos->setMinimum(-20000.0);
- ui.widgetSliderFactor->setVisible(true);
+ ui.widgetSliderFactor->setVisible(true);
- ui.horizontalSliderKinematicUnitPos->blockSignals(false);
- resetSlider();
+ ui.horizontalSliderKinematicUnitPos->blockSignals(true);
+ ui.horizontalSliderKinematicUnitPos->setMaximum(hi);
+ ui.horizontalSliderKinematicUnitPos->setMinimum(lo);
+ ui.horizontalSliderKinematicUnitPos->blockSignals(false);
+ resetSlider();
+ }
}
-}
-VirtualRobot::RobotPtr KinematicUnitWidgetController::loadRobotFile(std::string fileName)
-{
- VirtualRobot::RobotPtr robot;
-
- if (verbose)
+ ControlMode
+ KinematicUnitWidgetController::getSelectedControlMode() const
{
- ARMARX_INFO << "Loading KinematicUnit " << kinematicUnitName << " from " << kinematicUnitFile << " ..." << flush;
+ if (ui.radioButtonPositionControl->isChecked())
+ {
+ return ControlMode::ePositionControl;
+ }
+
+ if (ui.radioButtonVelocityControl->isChecked())
+ {
+ return ControlMode::eVelocityControl;
+ }
+
+ if (ui.radioButtonTorqueControl->isChecked())
+ {
+ return ControlMode::eTorqueControl;
+ }
+
+ // if no button is checked, then the joint is likely initialized but no controller has been loaded yet
+ // (well, the no movement controller should be active)
+ return ControlMode::eUnknown;
}
- if (!ArmarXDataPath::getAbsolutePath(fileName, fileName))
+ void
+ KinematicUnitWidgetController::setControlModeTorque()
{
- ARMARX_INFO << "Could not find Robot XML file with name " << fileName << flush;
- }
+ if (!ui.radioButtonTorqueControl->isChecked())
+ {
+ return;
+ }
+ NameControlModeMap jointModes;
- robot = VirtualRobot::RobotIO::loadRobot(fileName);
+ if (currentNode)
+ {
+ jointModes[currentNode->getName()] = eTorqueControl;
+ ui.labelUnit->setText("Ncm");
+ ARMARX_INFO << "setting torque control for current Node Name: "
+ << currentNode->getName() << flush;
- if (!robot)
- {
- ARMARX_INFO << "Could not find Robot XML file with name " << fileName << "(" << kinematicUnitName << ")" << flush;
- }
+ if (kinematicUnitInterfacePrx)
+ {
+ try
+ {
+ kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ }
+ catch (...)
+ {
+ }
+ }
- return robot;
-}
+ ui.horizontalSliderKinematicUnitPos->blockSignals(true);
+ ui.horizontalSliderKinematicUnitPos->setMaximum(20000.0);
+ ui.horizontalSliderKinematicUnitPos->setMinimum(-20000.0);
-VirtualRobot::CoinVisualizationPtr KinematicUnitWidgetController::getCoinVisualization(VirtualRobot::RobotPtr robot)
-{
- VirtualRobot::CoinVisualizationPtr coinVisualization;
+ ui.widgetSliderFactor->setVisible(true);
+
+ ui.horizontalSliderKinematicUnitPos->blockSignals(false);
+ resetSlider();
+ }
+ }
- if (robot != NULL)
+ VirtualRobot::RobotPtr
+ KinematicUnitWidgetController::loadRobotFile(std::string fileName)
{
- ARMARX_VERBOSE << "getting coin visualization" << flush;
- coinVisualization = robot->getVisualization<VirtualRobot::CoinVisualization>();
+ VirtualRobot::RobotPtr robot;
- if (!coinVisualization || !coinVisualization->getCoinVisualization())
+ if (verbose)
{
- ARMARX_INFO << "could not get coin visualization" << flush;
+ ARMARX_INFO << "Loading KinematicUnit " << kinematicUnitName << " from "
+ << kinematicUnitFile << " ..." << flush;
}
- }
- return coinVisualization;
-}
+ if (!ArmarXDataPath::getAbsolutePath(fileName, fileName))
+ {
+ ARMARX_INFO << "Could not find Robot XML file with name " << fileName << flush;
+ }
-VirtualRobot::RobotNodeSetPtr KinematicUnitWidgetController::getRobotNodeSet(VirtualRobot::RobotPtr robot, std::string nodeSetName)
-{
- VirtualRobot::RobotNodeSetPtr nodeSetPtr;
+ robot = VirtualRobot::RobotIO::loadRobot(fileName);
+
+ if (!robot)
+ {
+ ARMARX_INFO << "Could not find Robot XML file with name " << fileName << "("
+ << kinematicUnitName << ")" << flush;
+ }
- if (robot)
+ return robot;
+ }
+
+ VirtualRobot::CoinVisualizationPtr
+ KinematicUnitWidgetController::getCoinVisualization(VirtualRobot::RobotPtr robot)
{
- nodeSetPtr = robot->getRobotNodeSet(nodeSetName);
+ VirtualRobot::CoinVisualizationPtr coinVisualization;
- if (!nodeSetPtr)
+ if (robot != NULL)
{
- ARMARX_INFO << "RobotNodeSet with name " << nodeSetName << " is not defined" << flush;
+ ARMARX_VERBOSE << "getting coin visualization" << flush;
+ coinVisualization = robot->getVisualization<VirtualRobot::CoinVisualization>();
+ if (!coinVisualization || !coinVisualization->getCoinVisualization())
+ {
+ ARMARX_INFO << "could not get coin visualization" << flush;
+ }
}
+
+ return coinVisualization;
}
- return nodeSetPtr;
-}
+ VirtualRobot::RobotNodeSetPtr
+ KinematicUnitWidgetController::getRobotNodeSet(VirtualRobot::RobotPtr robot,
+ std::string nodeSetName)
+ {
+ VirtualRobot::RobotNodeSetPtr nodeSetPtr;
+
+ if (robot)
+ {
+ nodeSetPtr = robot->getRobotNodeSet(nodeSetName);
+ if (!nodeSetPtr)
+ {
+ ARMARX_INFO << "RobotNodeSet with name " << nodeSetName << " is not defined"
+ << flush;
+ }
+ }
-bool KinematicUnitWidgetController::initGUIComboBox(VirtualRobot::RobotNodeSetPtr robotNodeSet)
-{
- ui.nodeListComboBox->clear();
+ return nodeSetPtr;
+ }
- if (robotNodeSet)
+ bool
+ KinematicUnitWidgetController::initGUIComboBox(VirtualRobot::RobotNodeSetPtr robotNodeSet)
{
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
+ ui.nodeListComboBox->clear();
- for (unsigned int i = 0; i < rn.size(); i++)
+ if (robotNodeSet)
{
- // ARMARX_INFO << "adding item to joint combo box" << rn[i]->getName() << flush;
- QString name(rn[i]->getName().c_str());
- ui.nodeListComboBox->addItem(name);
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ // ARMARX_INFO << "adding item to joint combo box" << rn[i]->getName() << flush;
+ QString name(rn[i]->getName().c_str());
+ ui.nodeListComboBox->addItem(name);
+ }
+ ui.nodeListComboBox->setCurrentIndex(-1);
+ return true;
}
- ui.nodeListComboBox->setCurrentIndex(-1);
- return true;
+ return false;
}
- return false;
-}
+ bool
+ KinematicUnitWidgetController::initGUIJointListTable(VirtualRobot::RobotNodeSetPtr robotNodeSet)
+ {
+ uint numberOfColumns = 10;
-bool KinematicUnitWidgetController::initGUIJointListTable(VirtualRobot::RobotNodeSetPtr robotNodeSet)
-{
- uint numberOfColumns = 10;
-
- //dont use clear! It is not required here and somehow causes the tabel to have
- //numberOfColumns additional empty columns and rn.size() additional empty rows.
- //Somehow columncount (rowcount) stay at numberOfColumns (rn.size())
- //ui.tableJointList->clear();
+ //dont use clear! It is not required here and somehow causes the tabel to have
+ //numberOfColumns additional empty columns and rn.size() additional empty rows.
+ //Somehow columncount (rowcount) stay at numberOfColumns (rn.size())
+ //ui.tableJointList->clear();
- if (robotNodeSet)
- {
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
-
- //set dimension of table
- //ui.tableJointList->setColumnWidth(0,110);
-
- //ui.tableJointList->horizontalHeader()->setResizeMode(QHeaderView::ResizeToContents);
- ui.tableJointList->setRowCount(rn.size());
- ui.tableJointList->setColumnCount(eTabelColumnCount);
-
-
- //ui.tableJointList->setSizePolicy(QSizePolicy::Expanding,QSizePolicy::Expanding);
-
- // set table header
- // if the order is changed dont forget to update the order in the enum JointTabelColumnIndex
- // in theheader file
- QStringList s;
- s << "Joint Name"
- << "Control Mode"
- << "Angle [deg]/Position [mm]"
- << "Velocity [deg/s]/[mm/s]"
- << "Torque [Nm] / PWM"
- << "Current [A]"
- << "Temperature [C]"
- << "Operation"
- << "Error"
- << "Enabled"
- << "Emergency Stop";
- ui.tableJointList->setHorizontalHeaderLabels(s);
- ARMARX_CHECK_EXPRESSION(ui.tableJointList->columnCount() == eTabelColumnCount) << "Current table size: " << ui.tableJointList->columnCount();
-
-
- // fill in joint names
- for (unsigned int i = 0; i < rn.size(); i++)
+ if (robotNodeSet)
{
- // ARMARX_INFO << "adding item to joint table" << rn[i]->getName() << flush;
- QString name(rn[i]->getName().c_str());
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+
+ //set dimension of table
+ //ui.tableJointList->setColumnWidth(0,110);
+
+ //ui.tableJointList->horizontalHeader()->setResizeMode(QHeaderView::ResizeToContents);
+ ui.tableJointList->setRowCount(rn.size());
+ ui.tableJointList->setColumnCount(eTabelColumnCount);
+
+
+ //ui.tableJointList->setSizePolicy(QSizePolicy::Expanding,QSizePolicy::Expanding);
+
+ // set table header
+ // if the order is changed dont forget to update the order in the enum JointTabelColumnIndex
+ // in theheader file
+ QStringList s;
+ s << "Joint Name"
+ << "Control Mode"
+ << "Angle [deg]/Position [mm]"
+ << "Velocity [deg/s]/[mm/s]"
+ << "Torque [Nm] / PWM"
+ << "Current [A]"
+ << "Temperature [C]"
+ << "Operation"
+ << "Error"
+ << "Enabled"
+ << "Emergency Stop";
+ ui.tableJointList->setHorizontalHeaderLabels(s);
+ ARMARX_CHECK_EXPRESSION(ui.tableJointList->columnCount() == eTabelColumnCount)
+ << "Current table size: " << ui.tableJointList->columnCount();
+
+
+ // fill in joint names
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ // ARMARX_INFO << "adding item to joint table" << rn[i]->getName() << flush;
+ QString name(rn[i]->getName().c_str());
- QTableWidgetItem* newItem = new QTableWidgetItem(name);
- ui.tableJointList->setItem(i, eTabelColumnName, newItem);
- }
+ QTableWidgetItem* newItem = new QTableWidgetItem(name);
+ ui.tableJointList->setItem(i, eTabelColumnName, newItem);
+ }
- // init missing table fields with default values
- for (unsigned int i = 0; i < rn.size(); i++)
- {
- for (unsigned int j = 1; j < numberOfColumns; j++)
+ // init missing table fields with default values
+ for (unsigned int i = 0; i < rn.size(); i++)
{
- QString state = "--";
- QTableWidgetItem* newItem = new QTableWidgetItem(state);
- ui.tableJointList->setItem(i, j, newItem);
+ for (unsigned int j = 1; j < numberOfColumns; j++)
+ {
+ QString state = "--";
+ QTableWidgetItem* newItem = new QTableWidgetItem(state);
+ ui.tableJointList->setItem(i, j, newItem);
+ }
}
- }
- //hide columns Operation, Error, Enabled and Emergency Stop
- //they will be shown when changes occur
- ui.tableJointList->setColumnHidden(eTabelColumnTemperature, true);
- ui.tableJointList->setColumnHidden(eTabelColumnOperation, true);
- ui.tableJointList->setColumnHidden(eTabelColumnError, true);
- ui.tableJointList->setColumnHidden(eTabelColumnEnabled, true);
- ui.tableJointList->setColumnHidden(eTabelColumnEmergencyStop, true);
+ //hide columns Operation, Error, Enabled and Emergency Stop
+ //they will be shown when changes occur
+ ui.tableJointList->setColumnHidden(eTabelColumnTemperature, true);
+ ui.tableJointList->setColumnHidden(eTabelColumnOperation, true);
+ ui.tableJointList->setColumnHidden(eTabelColumnError, true);
+ ui.tableJointList->setColumnHidden(eTabelColumnEnabled, true);
+ ui.tableJointList->setColumnHidden(eTabelColumnEmergencyStop, true);
+
+ return true;
+ }
- return true;
+ return false;
}
- return false;
-}
+ void
+ KinematicUnitWidgetController::selectJoint(int i)
+ {
+ std::unique_lock lock(mutexNodeSet);
+ ARMARX_INFO << "Selected index: " << ui.nodeListComboBox->currentIndex();
-void KinematicUnitWidgetController::selectJoint(int i)
-{
- std::unique_lock lock(mutexNodeSet);
+ if (!robotNodeSet || i < 0 || i >= static_cast<int>(robotNodeSet->getSize()))
+ {
+ return;
+ }
- ARMARX_INFO << "Selected index: " << ui.nodeListComboBox->currentIndex();
+ currentNode = robotNodeSet->getAllRobotNodes()[i];
+ ARMARX_IMPORTANT << "Selected joint is `" << currentNode->getName() << "`.";
- if (!robotNodeSet || i < 0 || i >= static_cast<int>(robotNodeSet->getSize()))
- {
- return;
- }
+ const auto controlModes = kinematicUnitInterfacePrx->getControlModes();
+ if (controlModes.count(currentNode->getName()) == 0)
+ {
+ ARMARX_ERROR << "Could not retrieve control mode for joint `" << currentNode->getName()
+ << "` from kinematic unit!";
+ return;
+ }
- currentNode = robotNodeSet->getAllRobotNodes()[i];
- ARMARX_IMPORTANT << "Selected joint is `" << currentNode->getName() << "`.";
+ const auto controlMode = controlModes.at(currentNode->getName());
+ setControlModeRadioButtonGroup(controlMode);
- const auto controlModes = kinematicUnitInterfacePrx->getControlModes();
- if(controlModes.count(currentNode->getName()) == 0)
- {
- ARMARX_ERROR << "Could not retrieve control mode for joint `" << currentNode->getName() << "` from kinematic unit!";
- return;
+ if (controlMode == ePositionControl)
+ {
+ setControlModePosition();
+ }
+ else if (controlMode == eVelocityControl)
+ {
+ setControlModeVelocity();
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
+ }
+ else if (controlMode == eTorqueControl)
+ {
+ setControlModeTorque();
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
+ }
}
- const auto controlMode = controlModes.at(currentNode->getName());
- setControlModeRadioButtonGroup(controlMode);
-
- if (controlMode == ePositionControl)
- {
- setControlModePosition();
- }
- else if (controlMode == eVelocityControl)
+ void
+ KinematicUnitWidgetController::selectJointFromTableWidget(int row, int column)
{
- setControlModeVelocity();
- ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
- }
- else if (controlMode == eTorqueControl)
- {
- setControlModeTorque();
- ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
+ if (column == eTabelColumnName)
+ {
+ ui.nodeListComboBox->setCurrentIndex(row);
+ // selectJoint(row);
+ }
}
-}
-void KinematicUnitWidgetController::selectJointFromTableWidget(int row, int column)
-{
- if (column == eTabelColumnName)
+ void
+ KinematicUnitWidgetController::sliderValueChanged(int pos)
{
- ui.nodeListComboBox->setCurrentIndex(row);
- // selectJoint(row);
- }
-}
-
-void KinematicUnitWidgetController::sliderValueChanged(int pos)
-{
- std::unique_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
- if (!currentNode)
- {
- return;
- }
+ if (!currentNode)
+ {
+ return;
+ }
- const float value = static_cast<float>(ui.horizontalSliderKinematicUnitPos->value());
+ const float value = static_cast<float>(ui.horizontalSliderKinematicUnitPos->value());
- const ControlMode currentControlMode = getSelectedControlMode();
+ const ControlMode currentControlMode = getSelectedControlMode();
- const bool isDeg = ui.checkBoxUseDegree->isChecked();
- const bool isRot = currentNode->isRotationalJoint();
+ const bool isDeg = ui.checkBoxUseDegree->isChecked();
+ const bool isRot = currentNode->isRotationalJoint();
- if (currentControlMode == ePositionControl)
- {
- const auto factor = isRot && ! isDeg ? SLIDER_POS_RAD_MULTIPLIER : SLIDER_POS_DEG_MULTIPLIER;
- float conversionFactor = isRot && isDeg ? 180.0 / M_PI : 1.0f;
+ if (currentControlMode == ePositionControl)
+ {
+ const auto factor =
+ isRot && !isDeg ? SLIDER_POS_RAD_MULTIPLIER : SLIDER_POS_DEG_MULTIPLIER;
+ float conversionFactor = isRot && isDeg ? 180.0 / M_PI : 1.0f;
- NameValueMap jointAngles;
+ NameValueMap jointAngles;
- jointAngles[currentNode->getName()] = value / conversionFactor / factor;
- ui.lcdNumberKinematicUnitJointValue->display(value / factor);
- if (kinematicUnitInterfacePrx)
- {
- try
- {
- kinematicUnitInterfacePrx->setJointAngles(jointAngles);
- }
- catch (...)
+ jointAngles[currentNode->getName()] = value / conversionFactor / factor;
+ ui.lcdNumberKinematicUnitJointValue->display(value / factor);
+ if (kinematicUnitInterfacePrx)
{
+ try
+ {
+ kinematicUnitInterfacePrx->setJointAngles(jointAngles);
+ }
+ catch (...)
+ {
+ }
}
}
- }
- else if (currentControlMode == eVelocityControl)
- {
- float conversionFactor = isRot ? (isDeg ? 180.0 / M_PI : 100.f) : 1.0f;
- NameValueMap jointVelocities;
- jointVelocities[currentNode->getName()] = value / conversionFactor * static_cast<float>(ui.doubleSpinBoxKinematicUnitPosFactor->value());
- ui.lcdNumberKinematicUnitJointValue->display(value);
-
- if (kinematicUnitInterfacePrx)
+ else if (currentControlMode == eVelocityControl)
{
- try
- {
- kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
- }
- catch (...)
- {
- }
- }
- }
- else if (currentControlMode == eTorqueControl)
- {
- NameValueMap jointTorques;
- float torqueTargetValue = value / 100.0f * static_cast<float>(ui.doubleSpinBoxKinematicUnitPosFactor->value());
- jointTorques[currentNode->getName()] = torqueTargetValue;
- ui.lcdNumberKinematicUnitJointValue->display(torqueTargetValue);
+ float conversionFactor = isRot ? (isDeg ? 180.0 / M_PI : 100.f) : 1.0f;
+ NameValueMap jointVelocities;
+ jointVelocities[currentNode->getName()] =
+ value / conversionFactor *
+ static_cast<float>(ui.doubleSpinBoxKinematicUnitPosFactor->value());
+ ui.lcdNumberKinematicUnitJointValue->display(value);
- if (kinematicUnitInterfacePrx)
- {
- try
+ if (kinematicUnitInterfacePrx)
{
- kinematicUnitInterfacePrx->setJointTorques(jointTorques);
+ try
+ {
+ kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
+ }
+ catch (...)
+ {
+ }
}
- catch (...)
+ }
+ else if (currentControlMode == eTorqueControl)
+ {
+ NameValueMap jointTorques;
+ float torqueTargetValue =
+ value / 100.0f *
+ static_cast<float>(ui.doubleSpinBoxKinematicUnitPosFactor->value());
+ jointTorques[currentNode->getName()] = torqueTargetValue;
+ ui.lcdNumberKinematicUnitJointValue->display(torqueTargetValue);
+
+ if (kinematicUnitInterfacePrx)
{
+ try
+ {
+ kinematicUnitInterfacePrx->setJointTorques(jointTorques);
+ }
+ catch (...)
+ {
+ }
}
}
- }
- else
- {
- ARMARX_INFO << "current ControlModes unknown" << flush;
- }
-}
-
-
-
-void KinematicUnitWidgetController::updateControlModesTable(const NameControlModeMap& reportedJointControlModes)
-{
- if (!getWidget() || !robotNodeSet)
- {
- return;
+ else
+ {
+ ARMARX_INFO << "current ControlModes unknown" << flush;
+ }
}
- std::unique_lock lock(mutexNodeSet);
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
-
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateControlModesTable(
+ const NameControlModeMap& reportedJointControlModes)
{
- NameControlModeMap::const_iterator it;
- it = reportedJointControlModes.find(rn[i]->getName());
- QString state;
-
- if (it == reportedJointControlModes.end())
+ if (!getWidget() || !robotNodeSet)
{
- state = "unknown";
+ return;
}
- else
- {
- ControlMode currentMode = it->second;
+ std::unique_lock lock(mutexNodeSet);
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ NameControlModeMap::const_iterator it;
+ it = reportedJointControlModes.find(rn[i]->getName());
+ QString state;
- switch (currentMode)
+ if (it == reportedJointControlModes.end())
+ {
+ state = "unknown";
+ }
+ else
{
- /*case eNoMode:
+ ControlMode currentMode = it->second;
+
+
+ switch (currentMode)
+ {
+ /*case eNoMode:
state = "None";
break;
@@ -1111,537 +1211,563 @@ void KinematicUnitWidgetController::updateControlModesTable(const NameControlMod
state = "Unknown";
break;
*/
- case eDisabled:
- state = "Disabled";
- break;
+ case eDisabled:
+ state = "Disabled";
+ break;
- case eUnknown:
- state = "Unknown";
- break;
+ case eUnknown:
+ state = "Unknown";
+ break;
- case ePositionControl:
- state = "Position";
- break;
+ case ePositionControl:
+ state = "Position";
+ break;
- case eVelocityControl:
- state = "Velocity";
- break;
+ case eVelocityControl:
+ state = "Velocity";
+ break;
- case eTorqueControl:
- state = "Torque";
- break;
+ case eTorqueControl:
+ state = "Torque";
+ break;
- case ePositionVelocityControl:
- state = "Position + Velocity";
- break;
+ case ePositionVelocityControl:
+ state = "Position + Velocity";
+ break;
- default:
- //show the value of the mode so it can be implemented
- state = QString("<nyi Mode: %1>").arg(static_cast<int>(currentMode));
- break;
+ default:
+ //show the value of the mode so it can be implemented
+ state = QString("<nyi Mode: %1>").arg(static_cast<int>(currentMode));
+ break;
+ }
}
- }
-
- QTableWidgetItem* newItem = new QTableWidgetItem(state);
- ui.tableJointList->setItem(i, eTabelColumnControlMode, newItem);
- }
-}
-void KinematicUnitWidgetController::updateJointStatusesTable(const NameStatusMap& reportedJointStatuses)
-{
- if (!getWidget() || !robotNodeSet || reportedJointStatuses.empty())
- {
- return;
+ QTableWidgetItem* newItem = new QTableWidgetItem(state);
+ ui.tableJointList->setItem(i, eTabelColumnControlMode, newItem);
+ }
}
- std::unique_lock lock(mutexNodeSet);
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
-
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateJointStatusesTable(
+ const NameStatusMap& reportedJointStatuses)
{
-
- auto it = reportedJointStatuses.find(rn[i]->getName());
- if (it == reportedJointStatuses.end())
+ if (!getWidget() || !robotNodeSet || reportedJointStatuses.empty())
{
- ARMARX_WARNING << deactivateSpam(5) << "Joint Status for " <<
- rn[i]->getName() << " was not reported!";
- continue;
+ return;
}
- JointStatus currentStatus = it->second;
- QString state = translateStatus(currentStatus.operation);
- QTableWidgetItem* newItem = new QTableWidgetItem(state);
- ui.tableJointList->setItem(i, eTabelColumnOperation, newItem);
-
- state = translateStatus(currentStatus.error);
- newItem = new QTableWidgetItem(state);
- ui.tableJointList->setItem(i, eTabelColumnError, newItem);
+ std::unique_lock lock(mutexNodeSet);
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
- state = currentStatus.enabled ? "X" : "-";
- newItem = new QTableWidgetItem(state);
- ui.tableJointList->setItem(i, eTabelColumnEnabled, newItem);
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
- state = currentStatus.emergencyStop ? "X" : "-";
- newItem = new QTableWidgetItem(state);
- ui.tableJointList->setItem(i, eTabelColumnEmergencyStop, newItem);
- }
+ auto it = reportedJointStatuses.find(rn[i]->getName());
+ if (it == reportedJointStatuses.end())
+ {
+ ARMARX_WARNING << deactivateSpam(5) << "Joint Status for " << rn[i]->getName()
+ << " was not reported!";
+ continue;
+ }
+ JointStatus currentStatus = it->second;
- //show columns
- ui.tableJointList->setColumnHidden(eTabelColumnOperation, false);
- ui.tableJointList->setColumnHidden(eTabelColumnError, false);
- ui.tableJointList->setColumnHidden(eTabelColumnEnabled, false);
- ui.tableJointList->setColumnHidden(eTabelColumnEmergencyStop, false);
-}
+ QString state = translateStatus(currentStatus.operation);
+ QTableWidgetItem* newItem = new QTableWidgetItem(state);
+ ui.tableJointList->setItem(i, eTabelColumnOperation, newItem);
-QString KinematicUnitWidgetController::translateStatus(OperationStatus status)
-{
- switch (status)
- {
- case eOffline:
- return "Offline";
+ state = translateStatus(currentStatus.error);
+ newItem = new QTableWidgetItem(state);
+ ui.tableJointList->setItem(i, eTabelColumnError, newItem);
- case eOnline:
- return "Online";
+ state = currentStatus.enabled ? "X" : "-";
+ newItem = new QTableWidgetItem(state);
+ ui.tableJointList->setItem(i, eTabelColumnEnabled, newItem);
- case eInitialized:
- return "Initialized";
+ state = currentStatus.emergencyStop ? "X" : "-";
+ newItem = new QTableWidgetItem(state);
+ ui.tableJointList->setItem(i, eTabelColumnEmergencyStop, newItem);
+ }
- default:
- return "?";
+ //show columns
+ ui.tableJointList->setColumnHidden(eTabelColumnOperation, false);
+ ui.tableJointList->setColumnHidden(eTabelColumnError, false);
+ ui.tableJointList->setColumnHidden(eTabelColumnEnabled, false);
+ ui.tableJointList->setColumnHidden(eTabelColumnEmergencyStop, false);
}
-}
-QString KinematicUnitWidgetController::translateStatus(ErrorStatus status)
-{
- switch (status)
+ QString
+ KinematicUnitWidgetController::translateStatus(OperationStatus status)
{
- case eOk:
- return "Ok";
+ switch (status)
+ {
+ case eOffline:
+ return "Offline";
- case eWarning:
- return "Wr";
+ case eOnline:
+ return "Online";
- case eError:
- return "Er";
+ case eInitialized:
+ return "Initialized";
- default:
- return "?";
+ default:
+ return "?";
+ }
}
-}
-void KinematicUnitWidgetController::updateJointAnglesTable(const NameValueMap& reportedJointAngles)
-{
- std::unique_lock lock(mutexNodeSet);
-
- if (!robotNodeSet)
+ QString
+ KinematicUnitWidgetController::translateStatus(ErrorStatus status)
{
- return;
- }
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
+ switch (status)
+ {
+ case eOk:
+ return "Ok";
+ case eWarning:
+ return "Wr";
- for (unsigned int i = 0; i < rn.size(); i++)
+ case eError:
+ return "Er";
+
+ default:
+ return "?";
+ }
+ }
+
+ void
+ KinematicUnitWidgetController::updateJointAnglesTable(const NameValueMap& reportedJointAngles)
{
- NameValueMap::const_iterator it;
- VirtualRobot::RobotNodePtr node = rn[i];
- it = reportedJointAngles.find(node->getName());
+ std::unique_lock lock(mutexNodeSet);
- if (it == reportedJointAngles.end())
+ if (!robotNodeSet)
{
- continue;
+ return;
}
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
- const float currentValue = it->second;
- QModelIndex index = ui.tableJointList->model()->index(i, eTabelColumnAngleProgressbar);
- float conversionFactor = ui.checkBoxUseDegree->isChecked() &&
- node->isRotationalJoint() ? 180.0 / M_PI : 1;
- ui.tableJointList->model()->setData(index, (int)(cutJitter(currentValue * conversionFactor) * 100) / 100.0f, eJointAngleRole);
- ui.tableJointList->model()->setData(index, node->getJointLimitHigh() * conversionFactor, eJointHiRole);
- ui.tableJointList->model()->setData(index, node->getJointLimitLow() * conversionFactor, eJointLoRole);
- }
-}
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ NameValueMap::const_iterator it;
+ VirtualRobot::RobotNodePtr node = rn[i];
+ it = reportedJointAngles.find(node->getName());
-void KinematicUnitWidgetController::updateJointVelocitiesTable(const NameValueMap& reportedJointVelocities)
-{
- if (!getWidget())
- {
- return;
- }
+ if (it == reportedJointAngles.end())
+ {
+ continue;
+ }
- std::unique_lock lock(mutexNodeSet);
- if (!robotNodeSet)
- {
- return;
+ const float currentValue = it->second;
+
+ QModelIndex index = ui.tableJointList->model()->index(i, eTabelColumnAngleProgressbar);
+ float conversionFactor =
+ ui.checkBoxUseDegree->isChecked() && node->isRotationalJoint() ? 180.0 / M_PI : 1;
+ ui.tableJointList->model()->setData(
+ index,
+ (int)(cutJitter(currentValue * conversionFactor) * 100) / 100.0f,
+ eJointAngleRole);
+ ui.tableJointList->model()->setData(
+ index, node->getJointLimitHigh() * conversionFactor, eJointHiRole);
+ ui.tableJointList->model()->setData(
+ index, node->getJointLimitLow() * conversionFactor, eJointLoRole);
+ }
}
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- QTableWidgetItem* newItem;
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateJointVelocitiesTable(
+ const NameValueMap& reportedJointVelocities)
{
- NameValueMap::const_iterator it;
- it = reportedJointVelocities.find(rn[i]->getName());
-
- if (it == reportedJointVelocities.end())
+ if (!getWidget())
{
- continue;
+ return;
}
- float currentValue = it->second;
- if (ui.checkBoxUseDegree->isChecked() && rn[i]->isRotationalJoint())
+ std::unique_lock lock(mutexNodeSet);
+ if (!robotNodeSet)
{
- currentValue *= 180.0 / M_PI;
+ return;
}
- const QString Text = QString::number(cutJitter(currentValue), 'g', 2);
- newItem = new QTableWidgetItem(Text);
- ui.tableJointList->setItem(i, eTabelColumnVelocity, newItem);
- }
-}
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+ QTableWidgetItem* newItem;
-void KinematicUnitWidgetController::updateJointTorquesTable(const NameValueMap& reportedJointTorques)
-{
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ NameValueMap::const_iterator it;
+ it = reportedJointVelocities.find(rn[i]->getName());
+ if (it == reportedJointVelocities.end())
+ {
+ continue;
+ }
- std::unique_lock lock(mutexNodeSet);
- if (!getWidget() || !robotNodeSet)
- {
- return;
+ float currentValue = it->second;
+ if (ui.checkBoxUseDegree->isChecked() && rn[i]->isRotationalJoint())
+ {
+ currentValue *= 180.0 / M_PI;
+ }
+ const QString Text = QString::number(cutJitter(currentValue), 'g', 2);
+ newItem = new QTableWidgetItem(Text);
+ ui.tableJointList->setItem(i, eTabelColumnVelocity, newItem);
+ }
}
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- QTableWidgetItem* newItem;
- NameValueMap::const_iterator it;
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateJointTorquesTable(const NameValueMap& reportedJointTorques)
{
- it = reportedJointTorques.find(rn[i]->getName());
- if (it == reportedJointTorques.end())
+
+ std::unique_lock lock(mutexNodeSet);
+ if (!getWidget() || !robotNodeSet)
{
- continue;
+ return;
}
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+ QTableWidgetItem* newItem;
+ NameValueMap::const_iterator it;
- const float currentValue = it->second;
- newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
- ui.tableJointList->setItem(i, eTabelColumnTorque, newItem);
- }
-}
-
-void KinematicUnitWidgetController::updateJointCurrentsTable(const NameValueMap& reportedJointCurrents, const NameStatusMap& reportedJointStatuses)
-{
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ it = reportedJointTorques.find(rn[i]->getName());
+ if (it == reportedJointTorques.end())
+ {
+ continue;
+ }
- std::unique_lock lock(mutexNodeSet);
- if (!getWidget() || !robotNodeSet || jointCurrentHistory.size() == 0)
- {
- return;
+ const float currentValue = it->second;
+ newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
+ ui.tableJointList->setItem(i, eTabelColumnTorque, newItem);
+ }
}
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- QTableWidgetItem* newItem;
- // FIXME history!
- // NameValueMap reportedJointCurrents = jointCurrentHistory.back().second;
- NameValueMap::const_iterator it;
-
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateJointCurrentsTable(
+ const NameValueMap& reportedJointCurrents,
+ const NameStatusMap& reportedJointStatuses)
{
- it = reportedJointCurrents.find(rn[i]->getName());
- if (it == reportedJointCurrents.end())
+
+ std::unique_lock lock(mutexNodeSet);
+ if (!getWidget() || !robotNodeSet || jointCurrentHistory.size() == 0)
{
- continue;
+ return;
}
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+ QTableWidgetItem* newItem;
- const float currentValue = it->second;
- newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
- ui.tableJointList->setItem(i, eTabelColumnCurrent, newItem);
- }
+ // FIXME history!
+ // NameValueMap reportedJointCurrents = jointCurrentHistory.back().second;
+ NameValueMap::const_iterator it;
- highlightCriticalValues(reportedJointStatuses);
-}
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ it = reportedJointCurrents.find(rn[i]->getName());
-void KinematicUnitWidgetController::updateMotorTemperaturesTable(const NameValueMap& reportedJointTemperatures)
-{
+ if (it == reportedJointCurrents.end())
+ {
+ continue;
+ }
+ const float currentValue = it->second;
+ newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
+ ui.tableJointList->setItem(i, eTabelColumnCurrent, newItem);
+ }
- std::unique_lock lock(mutexNodeSet);
- if (!getWidget() || !robotNodeSet || reportedJointTemperatures.empty())
- {
- return;
+ highlightCriticalValues(reportedJointStatuses);
}
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- QTableWidgetItem* newItem;
- NameValueMap::const_iterator it;
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateMotorTemperaturesTable(
+ const NameValueMap& reportedJointTemperatures)
{
- it = reportedJointTemperatures.find(rn[i]->getName());
- if (it == reportedJointTemperatures.end())
+
+ std::unique_lock lock(mutexNodeSet);
+ if (!getWidget() || !robotNodeSet || reportedJointTemperatures.empty())
{
- continue;
+ return;
}
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+ QTableWidgetItem* newItem;
+ NameValueMap::const_iterator it;
- const float currentValue = it->second;
- newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
- ui.tableJointList->setItem(i, eTabelColumnTemperature, newItem);
- }
- ui.tableJointList->setColumnHidden(eTabelColumnTemperature, false);
-
-}
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ it = reportedJointTemperatures.find(rn[i]->getName());
+ if (it == reportedJointTemperatures.end())
+ {
+ continue;
+ }
-void KinematicUnitWidgetController::updateModel(const NameValueMap& reportedJointAngles)
-{
- // ARMARX_INFO << "updateModel()" << flush;
- std::unique_lock lock(mutexNodeSet);
- if (!robotNodeSet)
- {
- return;
+ const float currentValue = it->second;
+ newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
+ ui.tableJointList->setItem(i, eTabelColumnTemperature, newItem);
+ }
+ ui.tableJointList->setColumnHidden(eTabelColumnTemperature, false);
}
- robot->setJointValues(reportedJointAngles);
-}
-std::optional<float> mean(const boost::circular_buffer<NameValueMap>& buffer, const std::string& key)
-{
- float sum = 0;
- std::size_t count = 0;
-
- for(const auto& element: buffer)
+ void
+ KinematicUnitWidgetController::updateModel(const NameValueMap& reportedJointAngles)
{
- if(element.count(key) > 0)
+ // ARMARX_INFO << "updateModel()" << flush;
+ std::unique_lock lock(mutexNodeSet);
+ if (!robotNodeSet)
{
- sum += element.at(key);
+ return;
}
+ robot->setJointValues(reportedJointAngles);
}
- if(count == 0)
+ std::optional<float>
+ mean(const boost::circular_buffer<NameValueMap>& buffer, const std::string& key)
{
- return std::nullopt;
- }
-
- return sum / static_cast<float>(count);
-}
-
-void KinematicUnitWidgetController::highlightCriticalValues(const NameStatusMap& reportedJointStatuses)
-{
- if (!enableValueValidator)
- {
- return;
- }
+ float sum = 0;
+ std::size_t count = 0;
- std::unique_lock lock(mutexNodeSet);
-
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
+ for (const auto& element : buffer)
+ {
+ if (element.count(key) > 0)
+ {
+ sum += element.at(key);
+ }
+ }
- // get standard line colors
- static std::vector<QBrush> standardColors;
- if (standardColors.size() == 0)
- {
- for (unsigned int i = 0; i < rn.size(); i++)
+ if (count == 0)
{
- // all cells of a row have the same color
- standardColors.push_back(ui.tableJointList->item(i, eTabelColumnCurrent)->background());
+ return std::nullopt;
}
+
+ return sum / static_cast<float>(count);
}
- // check robot current value of nodes
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::highlightCriticalValues(
+ const NameStatusMap& reportedJointStatuses)
{
- const auto& jointName = rn[i]->getName();
-
- const auto currentSmoothValOpt = mean(jointCurrentHistory, jointName);
- if(not currentSmoothValOpt.has_value())
+ if (!enableValueValidator)
{
- continue;
+ return;
}
- const float smoothValue = std::fabs(currentSmoothValOpt.value());
+ std::unique_lock lock(mutexNodeSet);
+
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
- if(jointCurrentHistory.front().count(jointName) == 0)
+ // get standard line colors
+ static std::vector<QBrush> standardColors;
+ if (standardColors.size() == 0)
{
- continue;
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ // all cells of a row have the same color
+ standardColors.push_back(
+ ui.tableJointList->item(i, eTabelColumnCurrent)->background());
+ }
}
- const float startValue = jointCurrentHistory.front().at(jointName);
- const bool isStatic = (smoothValue == startValue);
+ // check robot current value of nodes
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ const auto& jointName = rn[i]->getName();
+
+ const auto currentSmoothValOpt = mean(jointCurrentHistory, jointName);
+ if (not currentSmoothValOpt.has_value())
+ {
+ continue;
+ }
+
+ const float smoothValue = std::fabs(currentSmoothValOpt.value());
+
+ if (jointCurrentHistory.front().count(jointName) == 0)
+ {
+ continue;
+ }
- NameStatusMap::const_iterator it;
- it = reportedJointStatuses.find(rn[i]->getName());
- JointStatus currentStatus = it->second;
+ const float startValue = jointCurrentHistory.front().at(jointName);
+ const bool isStatic = (smoothValue == startValue);
- if (isStatic)
- {
- if (currentStatus.operation != eOffline)
+ NameStatusMap::const_iterator it;
+ it = reportedJointStatuses.find(rn[i]->getName());
+ JointStatus currentStatus = it->second;
+
+ if (isStatic)
+ {
+ if (currentStatus.operation != eOffline)
+ {
+ // current value is zero, but joint is not offline
+ ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::yellow);
+ }
+ }
+ else if (std::abs(smoothValue) > currentValueMax)
+ {
+ // current value is too high
+ ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::red);
+ }
+ else
{
- // current value is zero, but joint is not offline
- ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::yellow);
+ // everything seems to work as expected
+ ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(standardColors[i]);
}
+ }
+ }
+
+ void
+ KinematicUnitWidgetController::setMutex3D(RecursiveMutexPtr const& mutex3D)
+ {
+ this->mutex3D = mutex3D;
+ if (debugDrawer)
+ {
+ debugDrawer->setMutex(mutex3D);
}
- else if (std::abs(smoothValue) > currentValueMax)
+ }
+
+ QPointer<QWidget>
+ KinematicUnitWidgetController::getCustomTitlebarWidget(QWidget* parent)
+ {
+ if (customToolbar)
{
- // current value is too high
- ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::red);
+ customToolbar->setParent(parent);
}
else
{
- // everything seems to work as expected
- ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(standardColors[i]);
+ customToolbar = new QToolBar(parent);
+ customToolbar->addAction("ZeroVelocity", this, SLOT(kinematicUnitZeroVelocity()));
}
+ return customToolbar.data();
}
-}
-
-void KinematicUnitWidgetController::setMutex3D(RecursiveMutexPtr const& mutex3D)
-{
- this->mutex3D = mutex3D;
- if (debugDrawer)
+ float
+ KinematicUnitWidgetController::cutJitter(float value)
{
- debugDrawer->setMutex(mutex3D);
+ return (abs(value) < static_cast<float>(ui.jitterThresholdSpinBox->value())) ? 0 : value;
}
-}
-QPointer<QWidget> KinematicUnitWidgetController::getCustomTitlebarWidget(QWidget* parent)
-{
- if (customToolbar)
+ void
+ KinematicUnitWidgetController::fetchData()
{
- customToolbar->setParent(parent);
- }
- else
- {
- customToolbar = new QToolBar(parent);
- customToolbar->addAction("ZeroVelocity", this, SLOT(kinematicUnitZeroVelocity()));
- }
- return customToolbar.data();
-}
+ ARMARX_DEBUG << "updateGui";
-float KinematicUnitWidgetController::cutJitter(float value)
-{
- return (abs(value) < static_cast<float>(ui.jitterThresholdSpinBox->value())) ? 0 : value;
-}
+ if (not kinematicUnitInterfacePrx)
+ {
+ ARMARX_WARNING << "KinematicUnit is not available!";
+ return;
+ }
-void KinematicUnitWidgetController::fetchData()
-{
- ARMARX_DEBUG << "updateGui";
+ const auto debugInfo = kinematicUnitInterfacePrx->getDebugInfo();
- if(not kinematicUnitInterfacePrx)
- {
- ARMARX_WARNING << "KinematicUnit is not available!";
- return;
+ emit onDebugInfoReceived(debugInfo);
}
- const auto debugInfo = kinematicUnitInterfacePrx->getDebugInfo();
-
- emit onDebugInfoReceived(debugInfo);
-}
-
-void KinematicUnitWidgetController::debugInfoReceived(const DebugInfo& debugInfo)
-{
- ARMARX_DEBUG << "debug info received";
-
- updateModel(debugInfo.jointAngles);
+ void
+ KinematicUnitWidgetController::debugInfoReceived(const DebugInfo& debugInfo)
+ {
+ ARMARX_DEBUG << "debug info received";
- updateJointAnglesTable(debugInfo.jointAngles);
- updateJointVelocitiesTable(debugInfo.jointVelocities);
- updateJointTorquesTable(debugInfo.jointTorques);
- updateJointCurrentsTable(debugInfo.jointCurrents, debugInfo.jointStatus);
- updateControlModesTable(debugInfo.jointModes);
- updateJointStatusesTable(debugInfo.jointStatus);
- updateMotorTemperaturesTable(debugInfo.jointMotorTemperatures);
+ updateModel(debugInfo.jointAngles);
-}
+ updateJointAnglesTable(debugInfo.jointAngles);
+ updateJointVelocitiesTable(debugInfo.jointVelocities);
+ updateJointTorquesTable(debugInfo.jointTorques);
+ updateJointCurrentsTable(debugInfo.jointCurrents, debugInfo.jointStatus);
+ updateControlModesTable(debugInfo.jointModes);
+ updateJointStatusesTable(debugInfo.jointStatus);
+ updateMotorTemperaturesTable(debugInfo.jointMotorTemperatures);
+ }
-void RangeValueDelegate::paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const
-{
- if (index.column() == KinematicUnitWidgetController::eTabelColumnAngleProgressbar)
+ void
+ RangeValueDelegate::paint(QPainter* painter,
+ const QStyleOptionViewItem& option,
+ const QModelIndex& index) const
{
- float jointValue = index.data(KinematicUnitWidgetController:: eJointAngleRole).toFloat();
- float loDeg = index.data(KinematicUnitWidgetController::eJointLoRole).toFloat();
- float hiDeg = index.data(KinematicUnitWidgetController::eJointHiRole).toFloat();
+ if (index.column() == KinematicUnitWidgetController::eTabelColumnAngleProgressbar)
+ {
+ float jointValue = index.data(KinematicUnitWidgetController::eJointAngleRole).toFloat();
+ float loDeg = index.data(KinematicUnitWidgetController::eJointLoRole).toFloat();
+ float hiDeg = index.data(KinematicUnitWidgetController::eJointHiRole).toFloat();
+
+ if (hiDeg - loDeg <= 0)
+ {
+ QStyledItemDelegate::paint(painter, option, index);
+ return;
+ }
- if (hiDeg - loDeg <= 0)
+ QStyleOptionProgressBar progressBarOption;
+ progressBarOption.rect = option.rect;
+ progressBarOption.minimum = loDeg;
+ progressBarOption.maximum = hiDeg;
+ progressBarOption.progress = jointValue;
+ progressBarOption.text = QString::number(jointValue);
+ progressBarOption.textVisible = true;
+ QPalette pal;
+ pal.setColor(QPalette::Background, Qt::red);
+ progressBarOption.palette = pal;
+ QApplication::style()->drawControl(QStyle::CE_ProgressBar, &progressBarOption, painter);
+ }
+ else
{
QStyledItemDelegate::paint(painter, option, index);
- return;
}
-
- QStyleOptionProgressBar progressBarOption;
- progressBarOption.rect = option.rect;
- progressBarOption.minimum = loDeg;
- progressBarOption.maximum = hiDeg;
- progressBarOption.progress = jointValue;
- progressBarOption.text = QString::number(jointValue);
- progressBarOption.textVisible = true;
- QPalette pal;
- pal.setColor(QPalette::Background, Qt::red);
- progressBarOption.palette = pal;
- QApplication::style()->drawControl(QStyle::CE_ProgressBar,
- &progressBarOption, painter);
-
}
- else
+
+ KinematicUnitWidgetController::~KinematicUnitWidgetController()
{
- QStyledItemDelegate::paint(painter, option, index);
- }
-}
+ kinematicUnitInterfacePrx = nullptr;
-KinematicUnitWidgetController::~KinematicUnitWidgetController()
-{
- kinematicUnitInterfacePrx = nullptr;
+ if (updateTask)
+ {
+ updateTask->stop();
+ updateTask->join();
+ updateTask = nullptr;
+ }
+ }
- if(updateTask)
+ void
+ KinematicUnitWidgetController::on_pushButtonFromJson_clicked()
{
- updateTask->stop();
- updateTask->join();
- updateTask = nullptr;
- }
-}
+ bool ok;
+ const auto text = QInputDialog::getMultiLineText(
+ __widget, tr("JSON Joint values"), tr("Json:"), "{\n}", &ok)
+ .toStdString();
-void KinematicUnitWidgetController::on_pushButtonFromJson_clicked()
-{
- bool ok;
- const auto text = QInputDialog::getMultiLineText(
- __widget,
- tr("JSON Joint values"),
- tr("Json:"), "{\n}", &ok).toStdString();
+ if (!ok || text.empty())
+ {
+ return;
+ }
- if (!ok || text.empty())
- {
- return;
- }
+ NameValueMap jointAngles;
+ try
+ {
+ jointAngles = simox::json::json2NameValueMap(text);
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "invalid json";
+ }
- NameValueMap jointAngles;
- try
- {
- jointAngles = simox::json::json2NameValueMap(text);
- }
- catch (...)
- {
- ARMARX_ERROR << "invalid json";
- }
+ NameControlModeMap jointModes;
+ for (const auto& [key, _] : jointAngles)
+ {
+ jointModes[key] = ePositionControl;
+ }
- NameControlModeMap jointModes;
- for (const auto& [key, _] : jointAngles)
- {
- jointModes[key] = ePositionControl;
+ try
+ {
+ kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ kinematicUnitInterfacePrx->setJointAngles(jointAngles);
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "failed to switch mode or set angles";
+ }
}
- try
+ void
+ KinematicUnitWidgetController::synchronizeRobotJointAngles()
{
- kinematicUnitInterfacePrx->switchControlMode(jointModes);
- kinematicUnitInterfacePrx->setJointAngles(jointAngles);
+ const NameValueMap currentJointAngles = kinematicUnitInterfacePrx->getJointAngles();
+ robot->setJointValues(currentJointAngles);
}
- catch (...)
- {
- ARMARX_ERROR << "failed to switch mode or set angles";
- }
-}
-
-void KinematicUnitWidgetController::synchronizeRobotJointAngles()
-{
- const NameValueMap currentJointAngles = kinematicUnitInterfacePrx->getJointAngles();
- robot->setJointValues(currentJointAngles);
-}
-}
+} // namespace armarx
--
GitLab