Improve armar7 platform movement

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

@@ -45,28 +45,18 @@ namespace armarx
         velocitySensor = sensor->asA<SensorValueHolonomicPlatformVelocity>();
         ARMARX_CHECK_EXPRESSION(velocitySensor)
             << "Sensor value for " << cfg->platformName << " has invalid type";
-
-        //        ramp.setMaxPositionAcceleration(cfg->maxPositionAcceleration);
-        //        ramp.setMaxOrientationAcceleration(cfg->maxOrientationAcceleration);
     }
 
     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));
-
-        NJointHolonomicPlatformVelocityControllerControlData data;
-        data.velocityX = velX;
-        data.velocityY = velY;
-        data.velocityRotation = velRot;
-        data.commandTimestamp = IceUtil::Time::now();
-        setControlStruct(data);
-        //        reinitTripleBuffer(data);
+        ramp.init(activationVelocity);
     }
 
     void
@@ -88,24 +78,22 @@ namespace armarx
         }
         else
         {
-            Eigen::Vector3f x(rtGetControlStruct().velocityX,
-                              rtGetControlStruct().velocityY,
-                              rtGetControlStruct().velocityRotation);
-
-            if (x == Eigen::Vector3f::Zero())
+            Eigen::Vector3f result;
+            if (activationTime > rtGetControlStruct().commandTimestamp)
             {
-                target->velocityX = 0;
-                target->velocityY = 0;
-                target->velocityRotation = 0;
+                result = ramp.update(activationVelocity, timeSinceLastIteration.toSecondsDouble());
             }
             else
             {
-
-                Eigen::VectorXf result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
-                target->velocityX = result(0);
-                target->velocityY = result(1);
-                target->velocityRotation = result(2);
+                Eigen::Vector3f x(rtGetControlStruct().velocityX,
+                                  rtGetControlStruct().velocityY,
+                                  rtGetControlStruct().velocityRotation);
+                result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
             }
+
+            target->velocityX = result(0);
+            target->velocityY = result(1);
+            target->velocityRotation = result(2);
         }
     }
 
@@ -122,19 +110,21 @@ namespace armarx
         registrationNJointHolonomicPlatformVelocityControllerWithRamp(
             "NJointHolonomicPlatformVelocityControllerWithRamp");
 
-    RtRamp::RtRamp(float maxPos, float maxRot) :
-        maxPositionAcceleration(maxPos), maxOrientationAcceleration(maxRot)
+    Cartesian2DimVelocityRamp ::Cartesian2DimVelocityRamp(float maxPositionAcceleration,
+                                                          float maxOrientationAcceleration) :
+        maxPositionAcceleration(maxPositionAcceleration),
+        maxOrientationAcceleration(maxOrientationAcceleration)
     {
     }
 
     void
-    RtRamp::init(const Eigen::Vector3f& state)
+    Cartesian2DimVelocityRamp::init(const Eigen::Vector3f& state)
     {
         this->state = state;
     }
 
     Eigen::Vector3f
-    RtRamp::update(const Eigen::Vector3f& target, float dt)
+    Cartesian2DimVelocityRamp::update(const Eigen::Vector3f& target, float dt)
     {
         if (dt <= 0)
         {
@@ -150,45 +140,18 @@ namespace armarx
         float oriFactor = std::abs(delta(2)) / maxOrientationAcceleration / dt;
         factor = std::max(factor, oriFactor);
 
-        if (factor < 1)
-        {
-            throw LocalException("Error factor negative!");
-        }
-
-        Eigen::Vector3f oldState = state;
         state += delta / factor;
-
-
-        if (target == Eigen::Vector3f::Zero() && state != Eigen::Vector3f::Zero())
-        {
-            //            if (state.norm() >= oldState.norm())
-            //            {
-            ARMARX_RT_LOGF_IMPORTANT("Target %.04f %.04f %.04f", target(0), target(1), target(2));
-            ARMARX_RT_LOGF_IMPORTANT(
-                "OStte  %.04f %.04f %.04f", oldState(0), oldState(1), oldState(2));
-            ARMARX_RT_LOGF_IMPORTANT("State  %.04f %.04f %.04f", state(0), state(1), state(2));
-            ARMARX_RT_LOGF_IMPORTANT("Delta  %.04f %.04f %.04f", delta(0), delta(1), delta(2));
-            ARMARX_RT_LOGF_IMPORTANT("Factor %.04f", factor);
-            if (state.norm() > oldState.norm())
-            {
-                ARMARX_RT_LOGF_IMPORTANT("!!!!NORM INVALID!!!!");
-            }
-
-            //                throw LocalException("haeee");
-            //            }
-        }
-
         return state;
     }
 
     void
-    RtRamp::setMaxPositionAcceleration(float maxPositionAcceleration)
+    Cartesian2DimVelocityRamp::setMaxPositionAcceleration(float maxPositionAcceleration)
     {
         this->maxPositionAcceleration = maxPositionAcceleration;
     }
 
     void
-    RtRamp::setMaxOrientationAcceleration(float maxOrientationAcceleration)
+    Cartesian2DimVelocityRamp::setMaxOrientationAcceleration(float maxOrientationAcceleration)
     {
         this->maxOrientationAcceleration = maxOrientationAcceleration;
     }