From 6e791e10a0bcbde3c73ac97b042572fd42afe695 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 2 May 2022 18:24:07 +0200
Subject: [PATCH] Fix bug: After a reconnect to robot/kinematic unit, local
 joint values can be wrong. Need to fetch the real joint values first to avoid
 large jumps when selecting the joint.

---
 .../KinematicUnitGuiPlugin.cpp                | 20 ++++++++++++++++++-
 1 file changed, 19 insertions(+), 1 deletion(-)

diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 144dacf08..4107eaf1c 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -638,8 +638,26 @@ 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;
+        }
+
         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.
-- 
GitLab