Fix set current velocity in controller pre activate

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp

@@ -50,14 +50,13 @@ namespace armarx
     void
     NJointHolonomicPlatformVelocityControllerWithRamp::rtPreActivateController()
     {
-        const float velX = velocitySensor->velocityX;
-        const float velY = velocitySensor->velocityY;
-        const float velRot = velocitySensor->velocityRotation;
+        activationVelocity(0) = velocitySensor->velocityX;
+        activationVelocity(1) = velocitySensor->velocityY;
+        activationVelocity(2) = velocitySensor->velocityRotation;
+        activationTime = IceUtil::Time::now();
 
         // init velocity ramp
-        ramp.init(Eigen::Vector3f(velX, velY, velRot));
-
-        setVelocites(velX, velY, velRot);
+        ramp.init(activationVelocity);
     }
 
     void
@@ -79,11 +78,19 @@ namespace armarx
         }
         else
         {
-            Eigen::Vector3f x(rtGetControlStruct().velocityX,
-                              rtGetControlStruct().velocityY,
-                              rtGetControlStruct().velocityRotation);
+            Eigen::Vector3f result;
+            if (activationTime > rtGetControlStruct().commandTimestamp)
+            {
+                result = ramp.update(activationVelocity, timeSinceLastIteration.toSecondsDouble());
+            }
+            else
+            {
+                Eigen::Vector3f x(rtGetControlStruct().velocityX,
+                                  rtGetControlStruct().velocityY,
+                                  rtGetControlStruct().velocityRotation);
+                result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
+            }
 
-            Eigen::VectorXf result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
             target->velocityX = result(0);
             target->velocityY = result(1);
             target->velocityRotation = result(2);

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h

@@ -93,6 +93,9 @@ namespace armarx
     protected:
         Cartesian2DimVelocityRamp ramp;
 
+        Eigen::Vector3f activationVelocity;
+        IceUtil::Time activationTime;
+
         ControlTargetHolonomicPlatformVelocity* target;
         const SensorValueHolonomicPlatformVelocity* velocitySensor;
     };