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::
+        NJointHolonomicPlatformUnitVelocityPassThroughController(
+            RobotUnit* prov,
+            NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr cfg,
+            const VirtualRobot::RobotPtr&) :
+        maxCommandDelay(IceUtil::Time::milliSeconds(500))
+    {
+        target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
+                     ->asA<ControlTargetHolonomicPlatformVelocity>();
+        ARMARX_CHECK_EXPRESSION(target)
+            << "The actuator " << cfg->platformName << " has no control mode "
+            << ControlModes::HolonomicPlatformVelocity;
+
+        initialSettings.velocityX = cfg->initialVelocityX;
+        initialSettings.velocityY = cfg->initialVelocityY;
+        initialSettings.velocityRotation = cfg->initialVelocityRotation;
+        reinitTripleBuffer(initialSettings);
+    }
+
+    void
+    NJointHolonomicPlatformVelocityControllerWithRamp::rtRun(
+        const IceUtil::Time& sensorValuesTimestamp,
+        const IceUtil::Time&)
+    {
+        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
+        {
+            target->velocityX = rtGetControlStruct().velocityX;
+            target->velocityY = rtGetControlStruct().velocityY;
+            target->velocityRotation = rtGetControlStruct().velocityRotation;
+        }
+    }
+
+    void
+    NJointHolonomicPlatformVelocityControllerWithRamp::setVelocites(float velocityX,
+                                                                    float velocityY,
+                                                                    float velocityRotation)
+    {
+        LockGuardType guard{controlDataMutex};
+        getWriterControlStruct().velocityX = velocityX;
+        getWriterControlStruct().velocityY = velocityY;
+        getWriterControlStruct().velocityRotation = velocityRotation;
+        getWriterControlStruct().commandTimestamp = IceUtil::Time::now();
+        writeControlStruct();
+    }
+
+    IceUtil::Time
+    NJointHolonomicPlatformVelocityControllerWithRamp::getMaxCommandDelay() const
+    {
+        return maxCommandDelay;
+    }
+
+    void
+    NJointHolonomicPlatformVelocityControllerWithRamp::setMaxCommandDelay(
+        const IceUtil::Time& value)
+    {
+        maxCommandDelay = value;
+    }
+
+    NJointControllerRegistration<NJointHolonomicPlatformVelocityControllerWithRamp>
+        registrationNJointHolonomicPlatformUnitVelocityPassThroughController(
+            "NJointHolonomicPlatformVelocityControllerWithRamp");
+} // namespace armarx