made kinematic platform unit work

source/RobotAPI/components/units/PlatformUnitSimulation.cpp

@@ -25,20 +25,24 @@
 
 #include "PlatformUnitSimulation.h"
 
+#include <Eigen/Geometry>
+#include <VirtualRobot/MathTools.h>
+
 #include <cmath>
 
 using namespace armarx;
 
 void PlatformUnitSimulation::onInitPlatformUnit()
 {
+    platformMode = eUndefined;
     referenceFrame = getProperty<std::string>("ReferenceFrame").getValue();
     targetPositionX = currentPositionX = getProperty<float>("InitialPosition.x").getValue();
     targetPositionY = currentPositionY = getProperty<float>("InitialPosition.y").getValue();
     targetRotation = 0.0;
     targetRotation = currentRotation = getProperty<float>("InitialRotation").getValue();
 
-    linearVelocity = getProperty<float>("LinearVelocity").getValue();
-    angularVelocity = getProperty<float>("AngularVelocity").getValue();
+    maxLinearVelocity = getProperty<float>("LinearVelocity").getValue();
+    maxAngularVelocity = getProperty<float>("AngularVelocity").getValue();
 
     positionalAccuracy = 0.01;
 
@@ -79,44 +83,78 @@ void PlatformUnitSimulation::simulationFunction()
     std::vector<float> vel(3, 0.0f);
     {
         ScopedLock lock(currentPoseMutex);
-        float diff = fabs(targetPositionX - currentPositionX);
-
-        if (diff > positionalAccuracy)
-        {
-            int sign = copysignf(1.0f, (targetPositionX - currentPositionX));
-            currentPositionX += sign * std::min<float>(linearVelocity * timeDeltaInSeconds, diff);
-            vel[0] = linearVelocity;
-        }
-
-        diff = fabs(targetPositionY - currentPositionY);
-
-        if (diff > positionalAccuracy)
-        {
-            int sign = copysignf(1.0f, (targetPositionY - currentPositionY));
-            currentPositionY +=  sign * std::min<float>(linearVelocity * timeDeltaInSeconds, diff);
-            vel[1] = linearVelocity;
-        }
-
-        diff = fabs(targetRotation - currentRotation);
-
-        if (diff > orientationalAccuracy)
+        switch (platformMode)
         {
-            int sign = copysignf(1.0f, (targetRotation - currentRotation));
-            currentRotation += sign * std::min<float>(angularVelocity * timeDeltaInSeconds, diff);
-
-            // stay between +/- M_2_PI
-            if (currentRotation > 2 * M_PI)
+            case ePositionControl:
             {
-                currentRotation -= 2 * M_PI;
+                float diff = fabs(targetPositionX - currentPositionX);
+                ARMARX_INFO << deactivateSpam(0.5) << "Distance to goal X: " << diff;
+
+                if (diff > positionalAccuracy)
+                {
+                    int sign = copysignf(1.0f, (targetPositionX - currentPositionX));
+                    currentPositionX += sign * std::min<float>(maxLinearVelocity * timeDeltaInSeconds, diff);
+                    vel[0] = linearVelocityX;
+                }
+
+                diff = fabs(targetPositionY - currentPositionY);
+
+                if (diff > positionalAccuracy)
+                {
+                    int sign = copysignf(1.0f, (targetPositionY - currentPositionY));
+                    currentPositionY +=  sign * std::min<float>(maxLinearVelocity * timeDeltaInSeconds, diff);
+                    vel[1] = linearVelocityY;
+                }
+
+                diff = fabs(targetRotation - currentRotation);
+
+                if (diff > orientationalAccuracy)
+                {
+                    int sign = copysignf(1.0f, (targetRotation - currentRotation));
+                    currentRotation += sign * std::min<float>(maxAngularVelocity * timeDeltaInSeconds, diff);
+
+                    // stay between +/- M_2_PI
+                    if (currentRotation > 2 * M_PI)
+                    {
+                        currentRotation -= 2 * M_PI;
+                    }
+
+                    if (currentRotation < -2 * M_PI)
+                    {
+                        currentRotation += 2 * M_PI;
+                    }
+
+                    vel[2] = angularVelocity;
+
+                }
             }
-
-            if (currentRotation < -2 * M_PI)
+            break;
+            case eVelocityControl:
             {
-                currentRotation += 2 * M_PI;
+                Eigen::Vector2f targetVel(linearVelocityX, linearVelocityY);
+                Eigen::Rotation2Df rot(currentRotation);
+                targetVel = rot * targetVel;
+                currentPositionX += timeDeltaInSeconds * targetVel[0];
+                currentPositionY += timeDeltaInSeconds * targetVel[1];
+                currentRotation += angularVelocity * timeDeltaInSeconds;
+
+                // stay between +/- M_2_PI
+                if (currentRotation > 2 * M_PI)
+                {
+                    currentRotation -= 2 * M_PI;
+                }
+
+                if (currentRotation < -2 * M_PI)
+                {
+                    currentRotation += 2 * M_PI;
+                }
+                vel[0] = linearVelocityY;
+                vel[1] = linearVelocityY;
+                vel[2] = angularVelocity;
             }
-
-            vel[2] = angularVelocity;
-
+            break;
+            default:
+                break;
         }
     }
     listenerPrx->reportPlatformPose(currentPositionX, currentPositionY, currentRotation);
@@ -125,8 +163,10 @@ void PlatformUnitSimulation::simulationFunction()
 
 void PlatformUnitSimulation::moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c)
 {
+    ARMARX_INFO << "new target pose : " << targetPlatformPositionX << ", " << targetPlatformPositionY << " with angle " << targetPlatformRotation;
     {
         ScopedLock lock(currentPoseMutex);
+        platformMode = ePositionControl;
         targetPositionX = targetPlatformPositionX;
         targetPositionY = targetPlatformPositionY;
         targetRotation = targetPlatformRotation;
@@ -146,7 +186,13 @@ PropertyDefinitionsPtr PlatformUnitSimulation::createPropertyDefinitions()
 
 void armarx::PlatformUnitSimulation::move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current& c)
 {
-    throw LocalException("NYI");
+    ARMARX_INFO << deactivateSpam(1) << "New velocity: " << targetPlatformVelocityX << ", " << targetPlatformVelocityY << " with angular velocity: " << targetPlatformVelocityRotation;
+    ScopedLock lock(currentPoseMutex);
+    platformMode = eVelocityControl;
+    linearVelocityX = std::min(maxLinearVelocity, targetPlatformVelocityX);
+    linearVelocityY = std::min(maxLinearVelocity, targetPlatformVelocityY);
+    angularVelocity = std::min(maxAngularVelocity, targetPlatformVelocityRotation);
+
 }
 
 void PlatformUnitSimulation::moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy, const Ice::Current& c)
@@ -164,7 +210,12 @@ void PlatformUnitSimulation::moveRelative(float targetPlatformOffsetX, float tar
 void PlatformUnitSimulation::setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current& c)
 {
     ScopedLock lock(currentPoseMutex);
-    linearVelocity = positionalVelocity;
-    angularVelocity = orientaionalVelocity;
+    maxLinearVelocity = positionalVelocity;
+    maxAngularVelocity = orientaionalVelocity;
+}
+
+void PlatformUnitSimulation::stopPlatform(const Ice::Current& c)
+{
+    move(0, 0, 0);
 }
 

source/RobotAPI/components/units/PlatformUnitSimulation.h

@@ -89,7 +89,7 @@ namespace armarx
 
         void moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy, const Ice::Current& c = Ice::Current());
         void setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current& c = Ice::Current());
-
+        void stopPlatform(const Ice::Current& c = Ice::Current());
         /**
          * \see PropertyUser::createPropertyDefinitions()
          */
@@ -100,6 +100,14 @@ namespace armarx
         IceUtil::Time lastExecutionTime;
         int intervalMs;
 
+        enum PlatformMode
+        {
+            eUndefined,
+            ePositionControl,
+            eVelocityControl
+        }
+        platformMode;
+
         ::Ice::Float targetPositionX;
         ::Ice::Float targetPositionY;
         ::Ice::Float currentPositionX;
@@ -107,8 +115,11 @@ namespace armarx
         ::Ice::Float targetRotation;
         ::Ice::Float currentRotation;
 
-        ::Ice::Float linearVelocity;
+        ::Ice::Float linearVelocityX;
+        ::Ice::Float linearVelocityY;
+        ::Ice::Float maxLinearVelocity;
         ::Ice::Float angularVelocity;
+        ::Ice::Float maxAngularVelocity;
 
         ::Ice::Float positionalAccuracy;
         ::Ice::Float orientationalAccuracy;