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>();
@@ -39,22 +40,24 @@ namespace armarx
             << "The actuator " << cfg->platformName << " has no control mode "
             << ControlModes::HolonomicPlatformVelocity;
 
-        velocitySensor =
-            useSensorValue(cfg->platformName + "asdf")->asA<SensorValueHolonomicPlatformVelocity>();
-        ARMARX_CHECK_EXPRESSION(velocitySensor) << "No sensor value for " << cfg->platformName;
-
-        ramp.setMaxPositionAcceleration(cfg->maxPositionAcceleration);
-        ramp.setMaxOrientationAcceleration(cfg->maxOrientationAcceleration);
+        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()
     {
+        const float velX = velocitySensor->velocityX;
+        const float velY = velocitySensor->velocityY;
+        const float velRot = velocitySensor->velocityRotation;
+
         // init velocity ramp
-        Eigen::VectorXf state(6);
-        state << velocitySensor->velocityX, velocitySensor->velocityY, 0, 0, 0,
-            velocitySensor->velocityRotation;
-        ramp.setState(state, VirtualRobot::IKSolver::CartesianSelection::All);
+        ramp.init(Eigen::Vector3f(velX, velY, velRot));
+
+        setVelocites(velX, velY, velRot);
     }
 
     void
@@ -76,14 +79,14 @@ namespace armarx
         }
         else
         {
-            Eigen::VectorXf x(6);
-            x << rtGetControlStruct().velocityX, rtGetControlStruct().velocityY, 0, 0, 0,
-                rtGetControlStruct().velocityRotation;
+            Eigen::Vector3f x(rtGetControlStruct().velocityX,
+                              rtGetControlStruct().velocityY,
+                              rtGetControlStruct().velocityRotation);
 
             Eigen::VectorXf result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
             target->velocityX = result(0);
             target->velocityY = result(1);
-            target->velocityRotation = result(5);
+            target->velocityRotation = result(2);
         }
     }
 
@@ -99,4 +102,51 @@ namespace armarx
     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