Improve armar7 platform movement

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

@@ -31,7 +31,8 @@ namespace armarx
     NJointHolonomicPlatformVelocityControllerWithRamp::
         NJointHolonomicPlatformVelocityControllerWithRamp(RobotUnit* prov,
                                                           ConfigPtrT cfg,
-                                                          const VirtualRobot::RobotPtr&)
+                                                          const VirtualRobot::RobotPtr&) :
+        ramp(cfg->maxPositionAcceleration, cfg->maxOrientationAcceleration)
     {
         target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
                      ->asA<ControlTargetHolonomicPlatformVelocity>();
@@ -45,8 +46,8 @@ namespace armarx
         ARMARX_CHECK_EXPRESSION(velocitySensor)
             << "Sensor value for " << cfg->platformName << " has invalid type";
 
-        ramp.setMaxPositionAcceleration(cfg->maxPositionAcceleration);
-        ramp.setMaxOrientationAcceleration(cfg->maxOrientationAcceleration);
+        //        ramp.setMaxPositionAcceleration(cfg->maxPositionAcceleration);
+        //        ramp.setMaxOrientationAcceleration(cfg->maxOrientationAcceleration);
     }
 
     void
@@ -57,15 +58,15 @@ namespace armarx
         const float velRot = velocitySensor->velocityRotation;
 
         // init velocity ramp
-        Eigen::VectorXf state(6);
-        state << velX, velY, 0, 0, 0, velRot;
-        ramp.setState(state, VirtualRobot::IKSolver::CartesianSelection::All);
+        ramp.init(Eigen::Vector3f(velX, velY, velRot));
 
         NJointHolonomicPlatformVelocityControllerControlData data;
         data.velocityX = velX;
         data.velocityY = velY;
         data.velocityRotation = velRot;
-        reinitTripleBuffer(data);
+        data.commandTimestamp = IceUtil::Time::now();
+        setControlStruct(data);
+        //        reinitTripleBuffer(data);
     }
 
     void
@@ -73,29 +74,39 @@ namespace armarx
         const IceUtil::Time& sensorValuesTimestamp,
         const IceUtil::Time& timeSinceLastIteration)
     {
-        //        auto commandAge = sensorValuesTimestamp - rtGetControlStruct().commandTimestamp;
-
-        //        if (commandAge > maxCommandDelay && // command must be recent
-        //            (rtGetControlStruct().velocityX != 0.0f || rtGetControlStruct().velocityY != 0.0f ||
-        //             rtGetControlStruct().velocityRotation !=
-        //                 0.0f)) // only throw error if any command is not zero
-        //        {
-        //            throw LocalException(
-        //                "Platform target velocity was not set for a too long time: delay: ")
-        //                << commandAge.toSecondsDouble()
-        //                << " s, max allowed delay: " << maxCommandDelay.toSecondsDouble() << " s";
-        //        }
-        //        else
-        //        {
-        Eigen::VectorXf x(6);
-        x << rtGetControlStruct().velocityX, rtGetControlStruct().velocityY, 0, 0, 0,
-            rtGetControlStruct().velocityRotation;
-
-        Eigen::VectorXf result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
-        target->velocityX = result(0);
-        target->velocityY = result(1);
-        target->velocityRotation = result(5);
-        //        }
+        auto commandAge = sensorValuesTimestamp - rtGetControlStruct().commandTimestamp;
+
+        if (commandAge > maxCommandDelay && // command must be recent
+            (rtGetControlStruct().velocityX != 0.0f || rtGetControlStruct().velocityY != 0.0f ||
+             rtGetControlStruct().velocityRotation !=
+                 0.0f)) // only throw error if any command is not zero
+        {
+            throw LocalException(
+                "Platform target velocity was not set for a too long time: delay: ")
+                << commandAge.toSecondsDouble()
+                << " s, max allowed delay: " << maxCommandDelay.toSecondsDouble() << " s";
+        }
+        else
+        {
+            Eigen::Vector3f x(rtGetControlStruct().velocityX,
+                              rtGetControlStruct().velocityY,
+                              rtGetControlStruct().velocityRotation);
+
+            if (x == Eigen::Vector3f::Zero())
+            {
+                target->velocityX = 0;
+                target->velocityY = 0;
+                target->velocityRotation = 0;
+            }
+            else
+            {
+
+                Eigen::VectorXf result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
+                target->velocityX = result(0);
+                target->velocityY = result(1);
+                target->velocityRotation = result(2);
+            }
+        }
     }
 
     void
@@ -110,4 +121,76 @@ namespace armarx
     NJointControllerRegistration<NJointHolonomicPlatformVelocityControllerWithRamp>
         registrationNJointHolonomicPlatformVelocityControllerWithRamp(
             "NJointHolonomicPlatformVelocityControllerWithRamp");
+
+    RtRamp::RtRamp(float maxPos, float maxRot) :
+        maxPositionAcceleration(maxPos), maxOrientationAcceleration(maxRot)
+    {
+    }
+
+    void
+    RtRamp::init(const Eigen::Vector3f& state)
+    {
+        this->state = state;
+    }
+
+    Eigen::Vector3f
+    RtRamp::update(const Eigen::Vector3f& target, float dt)
+    {
+        if (dt <= 0)
+        {
+            return state;
+        }
+        Eigen::Vector3f delta = target - state;
+        float factor = 1;
+
+        Eigen::Vector2f posDelta = delta.block<2, 1>(0, 0);
+        float posFactor = posDelta.norm() / maxPositionAcceleration / dt;
+        factor = std::max(factor, posFactor);
+
+        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)
+    {
+        this->maxPositionAcceleration = maxPositionAcceleration;
+    }
+
+    void
+    RtRamp::setMaxOrientationAcceleration(float maxOrientationAcceleration)
+    {
+        this->maxOrientationAcceleration = maxOrientationAcceleration;
+    }
+
 } // namespace armarx