Improve armar7 platform movement

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

@@ -45,9 +45,6 @@ namespace armarx
         velocitySensor = sensor->asA<SensorValueHolonomicPlatformVelocity>();
         ARMARX_CHECK_EXPRESSION(velocitySensor)
             << "Sensor value for " << cfg->platformName << " has invalid type";
-
-        //        ramp.setMaxPositionAcceleration(cfg->maxPositionAcceleration);
-        //        ramp.setMaxOrientationAcceleration(cfg->maxOrientationAcceleration);
     }
 
     void
@@ -60,12 +57,7 @@ namespace armarx
         // init velocity ramp
         ramp.init(Eigen::Vector3f(velX, velY, velRot));
 
-        NJointHolonomicPlatformVelocityControllerControlData data;
-        data.velocityX = velX;
-        data.velocityY = velY;
-        data.velocityRotation = velRot;
-        setControlStruct(data);
-        //        reinitTripleBuffer(data);
+        setVelocites(velX, velY, velRot);
     }
 
     void
@@ -111,19 +103,21 @@ namespace armarx
         registrationNJointHolonomicPlatformVelocityControllerWithRamp(
             "NJointHolonomicPlatformVelocityControllerWithRamp");
 
-    RtRamp::RtRamp(float maxPos, float maxRot) :
-        maxPositionAcceleration(maxPos), maxOrientationAcceleration(maxRot)
+    Cartesian2DimVelocityRamp ::Cartesian2DimVelocityRamp(float maxPositionAcceleration,
+                                                          float maxOrientationAcceleration) :
+        maxPositionAcceleration(maxPositionAcceleration),
+        maxOrientationAcceleration(maxOrientationAcceleration)
     {
     }
 
     void
-    RtRamp::init(const Eigen::Vector3f& state)
+    Cartesian2DimVelocityRamp::init(const Eigen::Vector3f& state)
     {
         this->state = state;
     }
 
     Eigen::Vector3f
-    RtRamp::update(const Eigen::Vector3f& target, float dt)
+    Cartesian2DimVelocityRamp::update(const Eigen::Vector3f& target, float dt)
     {
         if (dt <= 0)
         {
@@ -136,7 +130,7 @@ namespace armarx
         float posFactor = posDelta.norm() / maxPositionAcceleration / dt;
         factor = std::max(factor, posFactor);
 
-        float oriFactor = std::abs(delta(3)) / maxOrientationAcceleration / dt;
+        float oriFactor = std::abs(delta(2)) / maxOrientationAcceleration / dt;
         factor = std::max(factor, oriFactor);
 
         state += delta / factor;
@@ -144,13 +138,13 @@ namespace armarx
     }
 
     void
-    RtRamp::setMaxPositionAcceleration(float maxPositionAcceleration)
+    Cartesian2DimVelocityRamp::setMaxPositionAcceleration(float maxPositionAcceleration)
     {
         this->maxPositionAcceleration = maxPositionAcceleration;
     }
 
     void
-    RtRamp::setMaxOrientationAcceleration(float maxOrientationAcceleration)
+    Cartesian2DimVelocityRamp::setMaxOrientationAcceleration(float maxOrientationAcceleration)
     {
         this->maxOrientationAcceleration = maxOrientationAcceleration;
     }