diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp index 144dacf08d6f3f2fdbb33aab1e73d1d67052b1b1..4107eaf1cc4c6529188ff0b8041ce1fa07dab9f5 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.