StopMovementController as 0-VelocityController in head_board::armar7de

source/devices/ethercat/head_board/armar7de/PWMVelocity.cpp

@@ -76,6 +76,40 @@ namespace devices::ethercat::head_board::armar7de
         return targetPWM;
     }
 
+    PWMVelocityControllerConfiguration::PWMVelocityControllerConfiguration(
+            float maxVelocityRad,
+            float maxDecelerationRad,
+            float jerk,
+            float maxDt,
+            float directSetVLimit,
+            float p,
+            float i,
+            float d,
+            float maxIntegral,
+            float feedforwardVelocityToPWMFactor,
+            float feedforwardTorqueToPWMFactor,
+            float PWMDeadzone,
+            float velocityUpdatePercent,
+            float conditionalIntegralErrorTreshold,
+            bool feedForwardMode) :
+        p(p),
+        i(i),
+        d(d),
+        maxIntegral(maxIntegral),
+        feedforwardVelocityToPWMFactor(feedforwardVelocityToPWMFactor),
+        feedforwardTorqueToPWMFactor(feedforwardTorqueToPWMFactor),
+        PWMDeadzone(PWMDeadzone),
+        velocityUpdatePercent(velocityUpdatePercent),
+        conditionalIntegralErrorTreshold(conditionalIntegralErrorTreshold),
+        feedForwardMode(feedForwardMode)
+    {
+        this->maxVelocityRad = maxVelocityRad;
+        this->maxDecelerationRad = maxDecelerationRad;
+        this->jerk = jerk;
+        this->maxDt = maxDt;
+        this->directSetVLimit = directSetVLimit;
+    }
+
 
     void
     PWMVelocityController::reset(double currentVelocity)

source/devices/ethercat/head_board/armar7de/PWMVelocity.h

@@ -46,6 +46,22 @@ namespace devices::ethercat::head_board::armar7de
 
     public:
         PWMVelocityControllerConfiguration() = default;
+        PWMVelocityControllerConfiguration(
+                float maxVelocityRad,
+                float maxDecelerationRad,
+                float jerk,
+                float maxDt,
+                float directSetVLimit,
+                float p,
+                float i,
+                float d,
+                float maxIntegral,
+                float feedforwardVelocityToPWMFactor,
+                float feedforwardTorqueToPWMFactor,
+                float PWMDeadzone,
+                float velocityUpdatePercent,
+                float conditionalIntegralErrorTreshold,
+                bool feedForwardMode);
         static PWMVelocityControllerConfigurationCPtr
         CreatePWMVelocityControllerConfigData(armarx::control::hardware_config::Config& hwConfig);
         float p;

source/devices/ethercat/head_board/armar7de/joint_controller/StopMovement.cpp

 #include "StopMovement.h"
 
 
+// armarx
+#include <ArmarXCore/core/ManagedIceObject.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+
 // robot_devices
 #include <devices/ethercat/head_board/armar7de/Data.h>
 
@@ -8,31 +12,73 @@
 namespace devices::ethercat::head_board::armar7de::joint_controller
 {
 
-    StopMovementController::StopMovementController(DataPtr dataPtr) : dataPtr(dataPtr)
+    StopMovementController::StopMovementController(DataPtr dataPtr) :
+        JointController(),
+        config(new PWMVelocityControllerConfiguration(6, 2.5, 30, 0.003, 0.005, 300, 7000, 0, 0.1, 600, 0, 60, 0.5, 1000000, false)),
+        controller(config),
+        dataPtr(dataPtr)
     {
-        pid.reset(new armarx::PIDController(0, 5000, 0));
-        pid->maxIntegral = 0.1;
+        //    velController.acceleration = velocityControllerConfigDataPtr->maxAccelerationRad;
+        velController.deceleration = config->maxDecelerationRad;
+        velController.jerk = config->jerk;
+        velController.maxDt = config->maxDt;
+        velController.maxV = config->maxVelocityRad;
+        velController.directSetVLimit = config->directSetVLimit;
+    }
+
+
+    StopMovementController::~StopMovementController() noexcept(true)
+    {
+        stopRequested = true;
+        try
+        {
+            threadHandle.join();
+        }
+        catch (...)
+        {
+        }
     }
 
 
     void
     StopMovementController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                   const IceUtil::Time& timeSinceLastIteration)
-    {
-        pid->update(timeSinceLastIteration.toSecondsDouble(), dataPtr->getVelocity(), 0.0);
-        float targetPwm = pid->getControlValue();
+    {   
+        auto currentPosition = dataPtr->getAbsoluteEncoderAngleInRad();
 
-        if (std::isnan(targetPwm))
+        velController.currentPosition = currentPosition;
+
+        velController.currentV = lastTargetVelocity;
+        velController.currentAcc = lastTargetAcceleration;
+        velController.dt = timeSinceLastIteration.toSecondsDouble();
+        velController.targetV = 0;
+        auto r = velController.run();
+        double newVel = r.velocity;
+        double newAcc = r.acceleration;
+
+        //            ARMARX_INFO << deactivateSpam(1) << VAROUT(newVel) << VAROUT(target.velocity);
+        if (std::isnan(newVel))
         {
-            targetPwm = 0.0f;
+            newVel = 0;
+            newAcc = 0;
         }
 
-        // ARMARX_INFO << "StopMovement";
-//        ARMARX_INFO << VAROUT(dataPtr->getVelocity());
-//        ARMARX_INFO << VAROUT(targetPwm);
-        targetPwm = 0.0f;
+        auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration,
+                                                         dataPtr->getVelocity(),
+                                                         newVel,
+                                                         0));
+//            ARMARX_RT_LOGF_INFO("target pwm: %d", targetPWM);
+//            if (std::abs(targetPWM) > 300)
+//                targetPWM = 0;
+        dataPtr->setMotorPwmValue(targetPWM);
+
+        lastTargetVelocity = newVel;
+        lastTargetAcceleration = newAcc;
 
-        dataPtr->setMotorPwmValue(targetPwm);
+        //            ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
+        //                                target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
+//            ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, velController acc: %.3f",
+//                                target.velocity, dataPtr->getVelocity(), targetPWM, controller.pid->Kp, controller.pid->Ki, controller.pid->Kd, velController.currentAcc).deactivateSpam(1);
     }
 
 
@@ -46,7 +92,9 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
     void
     StopMovementController::rtPreActivateController()
     {
-        pid->reset();
+        lastTargetVelocity = dataPtr->getVelocity();
+        lastTargetAcceleration = dataPtr->getAcceleration();
+        controller.reset(static_cast<double>(dataPtr->getVelocity()));
     }
 
 } // namespace devices::ethercat::head_board::armar7de::joint_controller

source/devices/ethercat/head_board/armar7de/joint_controller/StopMovement.h

@@ -2,16 +2,29 @@
 
 
 // armarx
+#include <ArmarXCore/core/services/tasks/ThreadPool.h>
+#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
 #include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/libraries/core/PIDController.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+
+// robot_devices
+#include <devices/ethercat/head_board/armar7de/PWMVelocity.h>
+
+
+namespace armarx
+{
+    using PIDControllerPtr = std::shared_ptr<class PIDController>;
+}
 
 
 namespace devices::ethercat::head_board::armar7de
 {
     using DataPtr = std::shared_ptr<class Data>;
+    using PWMVelocityControllerConfigurationCPtr =
+        std::shared_ptr<const class PWMVelocityControllerConfiguration>;
 }
 
-
 namespace devices::ethercat::head_board::armar7de::joint_controller
 {
 
@@ -23,6 +36,7 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
 
     public:
         StopMovementController(DataPtr dataPtr);
+        ~StopMovementController() noexcept(true);
 
     private:
         void rtRun(const IceUtil::Time& sensorValuesTimestamp,
@@ -37,9 +51,16 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
         void rtPreActivateController() override;
 
     private:
+        PWMVelocityControllerConfigurationCPtr config;
+        devices::ethercat::head_board::armar7de::PWMVelocityController controller;
+        armarx::VelocityControllerWithRampedAccelerationAndPositionBounds velController;
+
+        std::atomic<float> lastTargetVelocity, lastTargetAcceleration;
+
         armarx::DummyControlTargetStopMovement target;
         DataPtr dataPtr;
-        armarx::PIDControllerPtr pid;
+        bool stopRequested = false;
+        mutable armarx::ThreadPool::Handle threadHandle;
     };
 
 } // namespace devices::ethercat::head_board::armar7de::joint_controller