Improve armar7 platform movement

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

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::NJointHolonomicPlatformUnitVelocityPassThroughController
+ * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "NJointHolonomicPlatformVelocityControllerWithRamp.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
+namespace armarx
+{
+    NJointHolonomicPlatformVelocityControllerWithRamp::
+        NJointHolonomicPlatformVelocityControllerWithRamp(RobotUnit* prov,
+                                                          ConfigPtrT cfg,
+                                                          const VirtualRobot::RobotPtr&) :
+        ramp(cfg->maxPositionAcceleration, cfg->maxOrientationAcceleration)
+    {
+        target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
+                     ->asA<ControlTargetHolonomicPlatformVelocity>();
+        ARMARX_CHECK_EXPRESSION(target)
+            << "The actuator " << cfg->platformName << " has no control mode "
+            << ControlModes::HolonomicPlatformVelocity;
+
+        const auto sensor = useSensorValue(cfg->platformName);
+        ARMARX_CHECK_EXPRESSION(sensor) << "No sensor value for " << cfg->platformName;
+        velocitySensor = sensor->asA<SensorValueHolonomicPlatformVelocity>();
+        ARMARX_CHECK_EXPRESSION(velocitySensor)
+            << "Sensor value for " << cfg->platformName << " has invalid type";
+    }
+
+    void
+    NJointHolonomicPlatformVelocityControllerWithRamp::rtPreActivateController()
+    {
+        activationVelocity(0) = velocitySensor->velocityX;
+        activationVelocity(1) = velocitySensor->velocityY;
+        activationVelocity(2) = velocitySensor->velocityRotation;
+        activationTime = IceUtil::Time::now();
+
+        // init velocity ramp
+        ramp.init(activationVelocity);
+    }
+
+    void
+    NJointHolonomicPlatformVelocityControllerWithRamp::rtRun(
+        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::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());
+            }
+
+            target->velocityX = result(0);
+            target->velocityY = result(1);
+            target->velocityRotation = result(2);
+        }
+    }
+
+    void
+    NJointHolonomicPlatformVelocityControllerWithRamp::setMaxAccelerations(
+        float maxPositionAcceleration,
+        float maxOrientationAcceleration)
+    {
+        ramp.setMaxPositionAcceleration(maxPositionAcceleration);
+        ramp.setMaxOrientationAcceleration(maxOrientationAcceleration);
+    }
+
+    NJointControllerRegistration<NJointHolonomicPlatformVelocityControllerWithRamp>
+        registrationNJointHolonomicPlatformVelocityControllerWithRamp(
+            "NJointHolonomicPlatformVelocityControllerWithRamp");
+
+    Cartesian2DimVelocityRamp ::Cartesian2DimVelocityRamp(float maxPositionAcceleration,
+                                                          float maxOrientationAcceleration) :
+        maxPositionAcceleration(maxPositionAcceleration),
+        maxOrientationAcceleration(maxOrientationAcceleration)
+    {
+    }
+
+    void
+    Cartesian2DimVelocityRamp::init(const Eigen::Vector3f& state)
+    {
+        this->state = state;
+    }
+
+    Eigen::Vector3f
+    Cartesian2DimVelocityRamp::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);
+
+        state += delta / factor;
+        return state;
+    }
+
+    void
+    Cartesian2DimVelocityRamp::setMaxPositionAcceleration(float maxPositionAcceleration)
+    {
+        this->maxPositionAcceleration = maxPositionAcceleration;
+    }
+
+    void
+    Cartesian2DimVelocityRamp::setMaxOrientationAcceleration(float maxOrientationAcceleration)
+    {
+        this->maxOrientationAcceleration = maxOrientationAcceleration;
+    }
+
+} // namespace armarx