Set complete joint angles

source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp

@@ -638,21 +638,15 @@ 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.
-        const NameValueMap currentJointAngles = kinematicUnitInterfacePrx->getJointAngles();
-        if (auto it = currentJointAngles.find(currentNode->getName()); it != currentJointAngles.end())
         {
-            currentNode->setJointValue(it->second);
-        }
-        else
-        {
-            // Put this into else clause so the message is only constructed when necessary.
-            ARMARX_CHECK(false)
-                    << "Expected joint angles to contain '" << currentNode->getName() << ", "
-                    << "but contained: " << simox::alg::get_keys(currentJointAngles);
-            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.
+
+            const NameValueMap currentJointAngles = kinematicUnitInterfacePrx->getJointAngles();
+            robot->setJointValues(currentJointAngles);
         }
 
         float pos = currentNode->getJointValue() * conversionFactor;
@@ -713,10 +707,7 @@ void KinematicUnitWidgetController::setControlModeVelocity()
         ui.horizontalSliderKinematicUnitPos->setMinimum(lo);
         ui.horizontalSliderKinematicUnitPos->blockSignals(false);
         resetSlider();
-
-
     }
-
 }
 
 void KinematicUnitWidgetController::setControlModeTorque()