Merge branch...

Merge branch '73-fix-bug-in-kinematicunitgui-controlling-torso-joint-of-armar-de-to-0-when-reconnecting' into 'master'

Resolve "Fix bug in KinematicUnitGui controlling Torso Joint of ARMAR-DE to 0 when reconnecting"

Closes #73

See merge request ArmarX/RobotAPI!241

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);
+}

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;