Add warning on high current value

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

@@ -77,6 +77,7 @@ KinematicUnitWidgetController::KinematicUnitWidgetController() :
     kinematicUnitNode(nullptr),
     enableValueValidator(true),
     historyTime(100000), // 1/10 s
+    currentValueMax(5.0f),
     selectedControlMode(ePositionControl)
 {
     rootVisu = NULL;
@@ -1241,8 +1242,9 @@ void KinematicUnitWidgetController::highlightCriticalValues()
 
     boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
 
-    // show error on current value
     std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
+
+    // get standard line colors
     static std::vector<QBrush> standardColors;
     if (standardColors.size() == 0)
     {
@@ -1253,9 +1255,12 @@ void KinematicUnitWidgetController::highlightCriticalValues()
         }
     }
 
+    // check robot current value of nodes
     for (unsigned int i = 0; i < rn.size(); i++)
     {
         bool isZero = true;
+        float smoothedValue = 0;
+
         for (auto historyIt = jointCurrentHistory.begin(); historyIt != jointCurrentHistory.end(); historyIt++)
         {
             NameValueMap reportedJointCurrents = historyIt->second;
@@ -1268,10 +1273,11 @@ void KinematicUnitWidgetController::highlightCriticalValues()
 
             const float currentValue = it->second;
 
+            smoothedValue = 0.5f*currentValue + 0.5f*smoothedValue;
+
             if (currentValue != 0)
             {
                 isZero = false;
-                break;
             }
         }
 
@@ -1281,10 +1287,17 @@ void KinematicUnitWidgetController::highlightCriticalValues()
 
         if (isZero && currentStatus.enabled)
         {
+            // current value is zero, but joint is enabled
+            ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::darkYellow);
+        }
+        else if (smoothedValue > currentValueMax)
+        {
+            // current value is too high
             ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::red);
         }
         else
         {
+            // everything seems to work as expected
             ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(standardColors[i]);
         }
     }

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

@@ -272,6 +272,8 @@ namespace armarx
         bool enableValueValidator;
         Ice::Long historyTime;
 
+        float currentValueMax;
+
         NameValueMap reportedJointAngles;
         NameValueMap reportedJointVelocities;
         NameControlModeMap reportedJointControlModes;