kinematic sim: velocity is reported and aValueChanged flag corrected

source/RobotAPI/components/units/KinematicUnitSimulation.cpp

@@ -119,15 +119,21 @@ void KinematicUnitSimulation::simulationFunction()
         {
 
             float newAngle = 0.0f;
+            float newVelocity = 0.0f;
             KinematicUnitSimulationJointInfo& jointInfo = jointInfos[it->first];
             // calculate joint positions if joint is in velocity control mode
             if (it->second.controlMode == eVelocityControl)
             {
                 double change = (it->second.velocity * timeDeltaInSeconds);
-
+                newVelocity = it->second.velocity;
                 double randomNoiseValue = distribution(generator);
                 change *= 1 + randomNoiseValue;
                 newAngle = it->second.angle + change;
+                // check if velocities have changed
+                if (fabs(previousJointStates[it->first].velocity - newVelocity) > 0.00000001)
+                {
+                    aVelValueChanged = true;
+                }
             }
             else if (it->second.controlMode == ePositionControl)
             {
@@ -142,11 +148,16 @@ void KinematicUnitSimulation::simulationFunction()
                     {
                         float velValue = (it->second.velocity != 0.0f ? signedMin(pid->getControlValue(), it->second.velocity) : pid->getControlValue()); //restrain to max velocity
                         velValue *= 1 + distribution(generator);
+
                         pid->update(it->second.angle);
                         newAngle = it->second.angle +
                                    velValue *
                                    timeDeltaInSeconds;
-
+                        if (fabs(previousJointStates[it->first].velocityActual - velValue) > 0.00000001)
+                        {
+                            aVelValueChanged = true;
+                        }
+                        it->second.velocityActual = newVelocity = velValue;
                     }
                 }
             }
@@ -177,11 +188,7 @@ void KinematicUnitSimulation::simulationFunction()
                 it->second.angle = newAngle;
             }
 
-            // check if velocities have changed
-            if (fabs(previousJointStates[it->first].velocity - it->second.velocity) > 0.00000001)
-            {
-                aVelValueChanged = true;
-            }
+
 
             if (jointInfo.continuousJoint())
             {
@@ -197,7 +204,7 @@ void KinematicUnitSimulation::simulationFunction()
             }
 
             actualJointAngles[it->first] = newAngle;
-            actualJointVelocities[it->first] = it->second.velocity;
+            actualJointVelocities[it->first] = newVelocity;
         }
 
         previousJointStates = jointStates;

source/RobotAPI/components/units/KinematicUnitSimulation.h

@@ -57,6 +57,7 @@ namespace armarx
             controlMode = eDisabled;
             angle = 0.0f;
             velocity = 0.0f;
+            velocityActual = 0.0f;
             torque = 0.0f;
             temperature = 0.0f;
         }
@@ -64,6 +65,7 @@ namespace armarx
         ControlMode controlMode;
         float angle;
         float velocity;
+        float velocityActual;
         float torque;
         float temperature;
     };