Implement controller

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

@@ -29,10 +29,9 @@
 namespace armarx
 {
     NJointHolonomicPlatformVelocityControllerWithRamp::
-        NJointHolonomicPlatformUnitVelocityPassThroughController(
-            RobotUnit* prov,
-            NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr cfg,
-            const VirtualRobot::RobotPtr&) :
+        NJointHolonomicPlatformVelocityControllerWithRamp(RobotUnit* prov,
+                                                          ConfigPtrT cfg,
+                                                          const VirtualRobot::RobotPtr&) :
         maxCommandDelay(IceUtil::Time::milliSeconds(500))
     {
         target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
@@ -41,16 +40,26 @@ namespace armarx
             << "The actuator " << cfg->platformName << " has no control mode "
             << ControlModes::HolonomicPlatformVelocity;
 
+        NJointHolonomicPlatformVelocityControllerWithRampControlData initialSettings;
         initialSettings.velocityX = cfg->initialVelocityX;
         initialSettings.velocityY = cfg->initialVelocityY;
         initialSettings.velocityRotation = cfg->initialVelocityRotation;
         reinitTripleBuffer(initialSettings);
+
+        // init velocity ramp
+        Eigen::VectorXf state(6);
+        state << initialSettings.velocityX, initialSettings.velocityY, 0, 0, 0,
+            initialSettings.velocityRotation;
+        ramp.setState(state, VirtualRobot::IKSolver::CartesianSelection::All);
+
+        ramp.setMaxPositionAcceleration(cfg->maxPositionAcceleration);
+        ramp.setMaxOrientationAcceleration(cfg->maxOrientationAcceleration);
     }
 
     void
     NJointHolonomicPlatformVelocityControllerWithRamp::rtRun(
         const IceUtil::Time& sensorValuesTimestamp,
-        const IceUtil::Time&)
+        const IceUtil::Time& timeSinceLastIteration)
     {
         auto commandAge = sensorValuesTimestamp - rtGetControlStruct().commandTimestamp;
 
@@ -66,9 +75,14 @@ namespace armarx
         }
         else
         {
-            target->velocityX = rtGetControlStruct().velocityX;
-            target->velocityY = rtGetControlStruct().velocityY;
-            target->velocityRotation = rtGetControlStruct().velocityRotation;
+            Eigen::VectorXf x(6);
+            x << rtGetControlStruct().velocityX, rtGetControlStruct().velocityY, 0, 0, 0,
+                rtGetControlStruct().velocityRotation;
+
+            Eigen::VectorXf result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
+            target->velocityX = result(0);
+            target->velocityY = result(1);
+            target->velocityRotation = result(5);
         }
     }
 
@@ -85,6 +99,15 @@ namespace armarx
         writeControlStruct();
     }
 
+    void
+    NJointHolonomicPlatformVelocityControllerWithRamp::setMaxAccelerations(
+        float maxPositionAcceleration,
+        float maxOrientationAcceleration)
+    {
+        ramp.setMaxPositionAcceleration(maxPositionAcceleration);
+        ramp.setMaxOrientationAcceleration(maxOrientationAcceleration);
+    }
+
     IceUtil::Time
     NJointHolonomicPlatformVelocityControllerWithRamp::getMaxCommandDelay() const
     {
@@ -99,6 +122,6 @@ namespace armarx
     }
 
     NJointControllerRegistration<NJointHolonomicPlatformVelocityControllerWithRamp>
-        registrationNJointHolonomicPlatformUnitVelocityPassThroughController(
+        registrationNJointHolonomicPlatformVelocityControllerWithRamp(
             "NJointHolonomicPlatformVelocityControllerWithRamp");
 } // namespace armarx

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

@@ -43,6 +43,9 @@ namespace armarx
         float initialVelocityX;
         float initialVelocityY;
         float initialVelocityRotation;
+
+        float maxPositionAcceleration;
+        float maxOrientationAcceleration;
     };
 
     TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformVelocityControllerWithRampControlData);
@@ -65,15 +68,18 @@ namespace armarx
     public:
         using ConfigPtrT = NJointHolonomicPlatformVelocityControllerWithRampConfigPtr;
 
-        NJointHolonomicPlatformVelocityControllerWithRampConfig(RobotUnit* prov,
-                                                                ConfigPtrT config,
-                                                                const VirtualRobot::RobotPtr&);
+        NJointHolonomicPlatformVelocityControllerWithRamp(RobotUnit* prov,
+                                                          ConfigPtrT config,
+                                                          const VirtualRobot::RobotPtr&);
 
-        void rtRun(const IceUtil::Time&, const IceUtil::Time&) override;
+        void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+                   const IceUtil::Time& timeSinceLastIteration) override;
 
         //for the platform unit
         void setVelocites(float velocityX, float velocityY, float velocityRotation);
 
+        void setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration);
+
         //ice interface
         std::string
         getClassName(const Ice::Current& = Ice::emptyCurrent) const override
@@ -84,12 +90,12 @@ namespace armarx
         IceUtil::Time getMaxCommandDelay() const;
         void setMaxCommandDelay(const IceUtil::Time& value);
 
+
     protected:
         IceUtil::Time maxCommandDelay;
 
         CartesianVelocityRamp ramp;
 
         ControlTargetHolonomicPlatformVelocity* target;
-        NJointHolonomicPlatformVelocityControllerWithRampControlData initialSettings;
     };
 } // namespace armarx