diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 144dacf08d6f3f2fdbb33aab1e73d1d67052b1b1..1973976129415be09d640896b3bd2ec68736e60e 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -147,7 +147,7 @@ void KinematicUnitWidgetController::onInitComponent()
void KinematicUnitWidgetController::onConnectComponent()
{
- ARMARX_INFO << "kinematic unit gui :: onConnectComponent()";
+ // ARMARX_INFO << "Kinematic Unit Gui :: onConnectComponent()";
reportedJointAngles.clear();
reportedJointVelocities.clear();
reportedJointControlModes.clear();
@@ -156,10 +156,11 @@ void KinematicUnitWidgetController::onConnectComponent()
reportedJointStatuses.clear();
reportedJointTemperatures.clear();
-
jointAnglesUpdateFrequency = new filters::MedianFilter(100);
kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
topicName = kinematicUnitInterfacePrx->getReportTopicName();
+ usingTopic(topicName);
+
lastJointAngleUpdateTimestamp = TimeUtil::GetTime().toSecondsDouble();
robotVisu->removeAllChildren();
@@ -168,10 +169,9 @@ void KinematicUnitWidgetController::onConnectComponent()
std::string rfile;
Ice::StringSeq includePaths;
- // get robot filename
+ // Get robot filename
try
{
-
Ice::StringSeq packages = kinematicUnitInterfacePrx->getArmarXPackages();
packages.push_back(Application::GetProjectName());
ARMARX_VERBOSE << "ArmarX packages " << packages;
@@ -200,7 +200,7 @@ void KinematicUnitWidgetController::onConnectComponent()
}
catch (...)
{
- ARMARX_ERROR << "Unable to retrieve robot filename";
+ ARMARX_ERROR << "Unable to retrieve robot filename.";
}
try
@@ -208,6 +208,10 @@ void KinematicUnitWidgetController::onConnectComponent()
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";
@@ -216,35 +220,34 @@ void KinematicUnitWidgetController::onConnectComponent()
if (!robot || !robot->hasRobotNodeSet(robotNodeSetName))
{
getObjectScheduler()->terminate();
-
if (getWidget()->parentWidget())
{
getWidget()->parentWidget()->close();
}
-
- std::cout << "returning" << std::endl;
return;
}
- // check robot name and disable setZero Button if necessary
+ // Check robot name and disable setZero Button if necessary
if (not simox::alg::starts_with(robot->getName(), "Armar3"))
{
- ARMARX_IMPORTANT << "Disable the SetZero button because the robot name is " << robot->getName();
+ ARMARX_IMPORTANT << "Disable the SetZero button because the robot name is '" << robot->getName() << "'.";
ui.pushButtonKinematicUnitPos1->setDisabled(true);
}
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);
- // init control mode map
+ // Fetch the current joint angles.
+ synchronizeRobotJointAngles();
+
+ initGUIComboBox(robotNodeSet); // init the pull down menu (QT: ComboBox)
+ initGUIJointListTable(robotNodeSet);
+
+ // Init control mode map
try
{
reportedJointControlModes = kinematicUnitInterfacePrx->getControlModes();
@@ -255,7 +258,6 @@ void KinematicUnitWidgetController::onConnectComponent()
initializeUi();
- usingTopic(topicName);
QMetaObject::invokeMethod(this, "resetSlider");
enableMainWidgetAsync(true);
updateTimerId = startTimer(1000); // slow timer that only ensures that skipped updates are shown at all
@@ -355,13 +357,11 @@ void KinematicUnitWidgetController::saveSettings(QSettings* settings)
settings->setValue("enableValueValidator", enableValueValidator);
settings->setValue("viewerEnabled", viewerEnabled);
assert(historyTime % 1000 == 0);
- settings->setValue("historyTime", (int)(historyTime / 1000));
+ settings->setValue("historyTime", static_cast<int>(historyTime / 1000));
settings->setValue("currentValueMax", currentValueMax);
}
-
-
void KinematicUnitWidgetController::showVisuLayers(bool show)
{
if (debugDrawer)
@@ -410,7 +410,9 @@ void KinematicUnitWidgetController::updateGuiElements()
// modelUpdateCB();
}
-void KinematicUnitWidgetController::updateKinematicUnitListInDialog() {}
+void KinematicUnitWidgetController::updateKinematicUnitListInDialog()
+{
+}
void KinematicUnitWidgetController::modelUpdateCB()
{
@@ -418,7 +420,6 @@ void KinematicUnitWidgetController::modelUpdateCB()
SoNode* KinematicUnitWidgetController::getScene()
{
-
if (viewerEnabled)
{
ARMARX_INFO << "Returning scene ";
@@ -500,7 +501,7 @@ void KinematicUnitWidgetController::kinematicUnitZeroPosition()
{
}
- //set slider to 0 if position control mode is used
+ // Set slider to 0 if position control mode is used.
if (selectedControlMode == ePositionControl)
{
ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
@@ -615,9 +616,9 @@ void KinematicUnitWidgetController::setControlModePosition()
if (currentNode)
{
- QString unit = currentNode->isRotationalJoint() ?
- (ui.checkBoxUseDegree->isChecked() ? "deg" : "rad") :
- "mm";
+ 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();
@@ -638,8 +639,18 @@ void KinematicUnitWidgetController::setControlModePosition()
return;
}
+ {
+ // 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();
+ }
+
float pos = currentNode->getJointValue() * conversionFactor;
- ARMARX_INFO << "setting position control for current Node Name: " << currentNode->getName() << " with current value: " << pos;
+ 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.
@@ -695,10 +706,7 @@ void KinematicUnitWidgetController::setControlModeVelocity()
ui.horizontalSliderKinematicUnitPos->setMinimum(lo);
ui.horizontalSliderKinematicUnitPos->blockSignals(false);
resetSlider();
-
-
}
-
}
void KinematicUnitWidgetController::setControlModeTorque()
@@ -736,10 +744,7 @@ void KinematicUnitWidgetController::setControlModeTorque()
ui.horizontalSliderKinematicUnitPos->blockSignals(false);
resetSlider();
-
}
-
-
}
VirtualRobot::RobotPtr KinematicUnitWidgetController::loadRobotFile(std::string fileName)
@@ -1720,3 +1725,9 @@ void KinematicUnitWidgetController::on_pushButtonFromJson_clicked()
ARMARX_ERROR << "failed to switch mode or set angles";
}
}
+
+void KinematicUnitWidgetController::synchronizeRobotJointAngles()
+{
+ const NameValueMap currentJointAngles = kinematicUnitInterfacePrx->getJointAngles();
+ robot->setJointValues(currentJointAngles);
+}
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
index 8287176d247aebc0fbebac9b5a6b91d20df22c25..d0a98a0a1a57efb6958b103b81732e022afbedf4 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
@@ -231,7 +231,6 @@ namespace armarx
// ice proxies
KinematicUnitInterfacePrx kinematicUnitInterfacePrx;// send commands to kinematic unit
- KinematicUnitListenerPrx kinematicUnitListenerPrx; // receive reports from kinematic unit
bool verbose;
@@ -269,9 +268,13 @@ namespace armarx
void highlightCriticalValues();
protected slots:
+
void showVisuLayers(bool show);
void copyToClipboard();
void on_pushButtonFromJson_clicked();
+
+ void synchronizeRobotJointAngles();
+
private:
std::recursive_mutex mutexNodeSet;