diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index fa8011d66e9fb45de290395eab222e9299888620..7c7cfae4d2fba28158d7bbe6cb2a3cb89fd5c195 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -74,6 +74,7 @@
#include <iostream>
#include <memory>
#include <optional>
+#include <stdexcept>
#include <string>
@@ -81,8 +82,10 @@
//#define KINEMATIC_UNIT_FILE_DEFAULT_PACKAGE std::string("RobotAPI")
#define KINEMATIC_UNIT_NAME_DEFAULT "Robot"
//#define TOPIC_NAME_DEFAULT "RobotState"
-#define SLIDER_POS_DEG_MULTIPLIER 5
-#define SLIDER_POS_RAD_MULTIPLIER 100
+
+constexpr float SLIDER_POS_DEG_MULTIPLIER = 5;
+constexpr float SLIDER_POS_RAD_MULTIPLIER = 100;
+constexpr float SLIDER_POS_HEMI_MULTIPLIER = 100;
namespace armarx
{
@@ -648,15 +651,26 @@ namespace armarx
{
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));
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint())
+ {
+ const bool isDeg = ui.checkBoxUseDegree->isChecked();
+ const auto factor =
+ isDeg ? SLIDER_POS_DEG_MULTIPLIER : SLIDER_POS_RAD_MULTIPLIER;
+ const float conversionFactor = isDeg ? 180.0 / M_PI : 1.0f;
+ const float pos = currentNode->getJointValue() * conversionFactor;
+
+ ui.lcdNumberKinematicUnitJointValue->display((int)pos);
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition((int)(pos * factor));
+ }
+
+ if (currentNode->isTranslationalJoint())
+ {
+ const auto factor = SLIDER_POS_DEG_MULTIPLIER;
+ const float pos = currentNode->getJointValue();
+
+ ui.lcdNumberKinematicUnitJointValue->display((int)pos);
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition((int)(pos * factor));
+ }
}
}
}
@@ -712,15 +726,49 @@ namespace armarx
if (currentNode)
{
- QString unit = currentNode->isRotationalJoint()
- ? (ui.checkBoxUseDegree->isChecked() ? "deg" : "rad")
- : "mm";
+ const QString unit = [&]() -> QString
+ {
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint())
+ {
+ if (ui.checkBoxUseDegree->isChecked())
+ {
+ return "deg";
+ }
+
+ return "rad";
+ }
+
+ if (currentNode->isTranslationalJoint())
+ {
+ return "mm";
+ }
+
+ throw std::invalid_argument("unknown/unsupported joint type");
+ }();
+
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;
+
+
+ const auto [factor, conversionFactor] = [&]() -> std::pair<float, float>
+ {
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint())
+ {
+ const bool isDeg = ui.checkBoxUseDegree->isChecked();
+ if (isDeg)
+ {
+ return {SLIDER_POS_DEG_MULTIPLIER, 180.0 / M_PI};
+ }
+ return {SLIDER_POS_RAD_MULTIPLIER, 1};
+ }
+
+ if (currentNode->isTranslationalJoint())
+ {
+ return {SLIDER_POS_DEG_MULTIPLIER, 1};
+ }
+
+ throw std::invalid_argument("unknown/unsupported joint type");
+ }();
+
jointModes[currentNode->getName()] = ePositionControl;
if (kinematicUnitInterfacePrx)
@@ -728,8 +776,8 @@ namespace armarx
kinematicUnitInterfacePrx->switchControlMode(jointModes);
}
- float lo = currentNode->getJointLimitLo() * conversionFactor;
- float hi = currentNode->getJointLimitHi() * conversionFactor;
+ const float lo = currentNode->getJointLimitLo() * conversionFactor;
+ const float hi = currentNode->getJointLimitHi() * conversionFactor;
if (hi - lo <= 0.0f)
{
@@ -745,7 +793,7 @@ namespace armarx
synchronizeRobotJointAngles();
}
- float pos = currentNode->getJointValue() * conversionFactor;
+ const float pos = currentNode->getJointValue() * conversionFactor;
ARMARX_INFO << "Setting position control for current node "
<< "(name '" << currentNode->getName() << "' with current value " << pos
<< ")";
@@ -754,8 +802,18 @@ namespace armarx
// 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);
+ const float sliderMax = hi * factor;
+ const float sliderMin = lo * factor;
+
+ ui.horizontalSliderKinematicUnitPos->setMaximum(sliderMax);
+ ui.horizontalSliderKinematicUnitPos->setMinimum(sliderMin);
+
+ const std::size_t desiredNumberOfTicks = 1'000;
+
+ const float tickInterval = (sliderMax - sliderMin) / desiredNumberOfTicks;
+ ARMARX_INFO << VAROUT(tickInterval);
+
+ ui.horizontalSliderKinematicUnitPos->setTickInterval(tickInterval);
ui.lcdNumberKinematicUnitJointValue->display(pos);
ui.horizontalSliderKinematicUnitPos->blockSignals(false);
@@ -780,14 +838,37 @@ namespace armarx
// 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";
+
+ const QString unit = [&]() -> QString
+ {
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint())
+ {
+ if (ui.checkBoxUseDegree->isChecked())
+ {
+ return "deg/s";
+ }
+
+ return "rad/(100*s)";
+ }
+
+ if (currentNode->isTranslationalJoint())
+ {
+ return "mm/s";
+ }
+
+ throw std::invalid_argument("unknown/unsupported joint type");
+ }();
+
+
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;
+
+ const bool isDeg = ui.checkBoxUseDegree->isChecked();
+ const bool isRot = currentNode->isRotationalJoint() or currentNode->isHemisphereJoint();
+
+ const float lo = isRot ? (isDeg ? -90 : -M_PI * 100) : -1000;
+ const float hi = isRot ? (isDeg ? +90 : +M_PI * 100) : 1000;
try
{
@@ -1105,7 +1186,7 @@ namespace armarx
const ControlMode currentControlMode = getSelectedControlMode();
const bool isDeg = ui.checkBoxUseDegree->isChecked();
- const bool isRot = currentNode->isRotationalJoint();
+ const bool isRot = currentNode->isRotationalJoint() or currentNode->isHemisphereJoint();
if (currentControlMode == ePositionControl)
{
@@ -1361,7 +1442,10 @@ namespace armarx
QModelIndex index = ui.tableJointList->model()->index(i, eTabelColumnAngleProgressbar);
float conversionFactor =
- ui.checkBoxUseDegree->isChecked() && node->isRotationalJoint() ? 180.0 / M_PI : 1;
+ ui.checkBoxUseDegree->isChecked() &&
+ (node->isRotationalJoint() or node->isHemisphereJoint())
+ ? 180.0 / M_PI
+ : 1;
ui.tableJointList->model()->setData(
index,
(int)(cutJitter(currentValue * conversionFactor) * 100) / 100.0f,
@@ -1401,7 +1485,8 @@ namespace armarx
}
float currentValue = it->second;
- if (ui.checkBoxUseDegree->isChecked() && rn[i]->isRotationalJoint())
+ if (ui.checkBoxUseDegree->isChecked() &&
+ (rn[i]->isRotationalJoint() or rn[i]->isHemisphereJoint()))
{
currentValue *= 180.0 / M_PI;
}