made multidimpidcontroller heap allocation free

source/RobotAPI/libraries/core/CMakeLists.txt

@@ -51,6 +51,7 @@ set(LIB_FILES
 set(LIB_HEADERS
     EigenStl.h
     PIDController.h
+    MultiDimPIDController.h
     Trajectory.h
     TrajectoryController.h
     VectorHelpers.h

source/RobotAPI/libraries/core/MultiDimPIDController.h

+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    ArmarX
+ * @author     Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+#pragma once
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <Eigen/Core>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
+
+namespace armarx
+{
+    template <int dimensions = Eigen::Dynamic>
+    class MultiDimPIDControllerTemplate :
+        public Logging
+    {
+    public:
+        typedef Eigen::Matrix<float, dimensions, 1> PIDVectorX;
+
+        MultiDimPIDControllerTemplate(float Kp,
+                                      float Ki,
+                                      float Kd,
+                                      double maxControlValue = std::numeric_limits<double>::max(),
+                                      double maxDerivation = std::numeric_limits<double>::max(),
+                                      bool threadSafe = true,
+                                      std::vector<bool> limitless = {}) :
+            Kp(Kp),
+            Ki(Ki),
+            Kd(Kd),
+            integral(0),
+            derivative(0),
+            previousError(0),
+            maxControlValue(maxControlValue),
+            maxDerivation(maxDerivation),
+            threadSafe(threadSafe),
+            limitless(limitless)
+        {
+            reset();
+        }
+
+        void preallocate(size_t size)
+        {
+            stackAllocations.zeroVec = PIDVectorX::Zero(size);
+            stackAllocations.errorVec = stackAllocations.zeroVec;
+            stackAllocations.direction = stackAllocations.zeroVec;
+            stackAllocations.oldControlValue = stackAllocations.zeroVec;
+        }
+
+        ~MultiDimPIDControllerTemplate() {}
+        void update(const double deltaSec, const PIDVectorX& measuredValue, const PIDVectorX& targetValue)
+        {
+            ScopedRecursiveLockPtr lock = getLock();
+            processValue = measuredValue;
+            target = targetValue;
+
+            stackAllocations.errorVec = target - processValue;
+
+            if (limitless.size() != 0)
+            {
+                ARMARX_CHECK_EQUAL(limitless.size(), (size_t)stackAllocations.errorVec.rows());
+                for (size_t i = 0; i < limitless.size(); i++)
+                {
+                    if (limitless.at(i))
+                    {
+                        stackAllocations.errorVec(i) = math::MathUtils::angleModPI(stackAllocations.errorVec(i));
+                    }
+                }
+            }
+
+
+            double error = stackAllocations.errorVec.norm();
+
+            //double dt = (now - lastUpdateTime).toSecondsDouble();
+            //    ARMARX_INFO << deactivateSpam() << VAROUT(dt);
+            if (!firstRun)
+            {
+                integral += error * deltaSec;
+
+                if (deltaSec > 0.0)
+                {
+                    derivative = (error - previousError) / deltaSec;
+                }
+            }
+
+            firstRun = false;
+            stackAllocations.direction = targetValue; // copy size
+
+            if (error > 0)
+            {
+                stackAllocations.direction = stackAllocations.errorVec.normalized();
+            }
+            else
+            {
+                stackAllocations.direction.setZero();
+            }
+
+            if (controlValue.rows() > 0)
+            {
+                stackAllocations.oldControlValue = controlValue;
+            }
+            else
+            {
+                stackAllocations.oldControlValue = stackAllocations.zeroVec;
+            }
+            controlValue = stackAllocations.direction * (Kp * error + Ki * integral + Kd * derivative);
+
+            if (deltaSec > 0.0)
+            {
+                PIDVectorX accVec = (controlValue - stackAllocations.oldControlValue) / deltaSec;
+                float maxNewJointAcc = accVec.maxCoeff();
+                float minNewJointAcc = accVec.minCoeff();
+                maxNewJointAcc = std::max<float>(fabs(minNewJointAcc), fabs(maxNewJointAcc));
+                if (maxNewJointAcc > maxDerivation)
+                {
+                    auto newValue = stackAllocations.oldControlValue + accVec * maxDerivation / maxNewJointAcc * deltaSec;
+                    ARMARX_DEBUG << deactivateSpam(0.5) << VAROUT(maxDerivation) << VAROUT(maxNewJointAcc) << VAROUT(controlValue)  << VAROUT(stackAllocations.oldControlValue) << VAROUT(newValue);
+                    controlValue = newValue;
+                }
+            }
+
+
+            float max = controlValue.maxCoeff();
+            float min = controlValue.minCoeff();
+            max = std::max<float>(fabs(min), fabs(max));
+
+
+
+            if (max > maxControlValue)
+            {
+                auto newValue = controlValue  * maxControlValue / max;
+                ARMARX_DEBUG << deactivateSpam(0.5) << " Control value to big: " << controlValue << " max value: " << maxControlValue << " new value: " << newValue;
+                controlValue = newValue;
+            }
+            ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) <<  " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
+
+            previousError = error;
+            lastUpdateTime += IceUtil::Time::seconds(deltaSec);
+
+        }
+        void update(const PIDVectorX& measuredValue, const PIDVectorX& targetValue)
+        {
+            ScopedRecursiveLockPtr lock = getLock();
+            IceUtil::Time now = TimeUtil::GetTime();
+
+            if (firstRun)
+            {
+                lastUpdateTime = TimeUtil::GetTime();
+            }
+
+            double dt = (now - lastUpdateTime).toSecondsDouble();
+            update(dt, measuredValue, targetValue);
+            lastUpdateTime = now;
+        }
+        const PIDVectorX&
+        getControlValue() const
+        {
+            return controlValue;
+        }
+        void setMaxControlValue(double value)
+        {
+            ScopedRecursiveLockPtr lock = getLock();
+            maxControlValue = value;
+        }
+
+        void reset()
+        {
+            ScopedRecursiveLockPtr lock = getLock();
+            firstRun = true;
+            previousError = 0;
+            integral = 0;
+            lastUpdateTime = TimeUtil::GetTime();
+            //    controlValue.setZero();
+            //    processValue.setZero();
+            //    target.setZero();
+
+
+        }
+        //    protected:
+        float Kp, Ki, Kd;
+        double integral;
+        double derivative;
+        double previousError;
+        PIDVectorX processValue;
+        PIDVectorX target;
+        IceUtil::Time lastUpdateTime;
+        PIDVectorX controlValue;
+        double maxControlValue;
+        double maxDerivation;
+        bool firstRun;
+        mutable  RecursiveMutex mutex;
+        bool threadSafe = true;
+        std::vector<bool> limitless;
+    private:
+
+        struct StackAllocationHelper
+        {
+            PIDVectorX errorVec;
+            PIDVectorX direction;
+            PIDVectorX oldControlValue;
+            PIDVectorX zeroVec;
+        } stackAllocations;
+
+        ScopedRecursiveLockPtr getLock() const
+        {
+            if (threadSafe)
+            {
+                return ScopedRecursiveLockPtr(new ScopedRecursiveLock(mutex));
+            }
+            else
+            {
+                return ScopedRecursiveLockPtr();
+            }
+        }
+    };
+    typedef MultiDimPIDControllerTemplate<> MultiDimPIDController;
+    typedef boost::shared_ptr<MultiDimPIDControllerTemplate<>> MultiDimPIDControllerPtr;
+}

source/RobotAPI/libraries/core/PIDController.cpp

@@ -25,7 +25,6 @@
 #include "PIDController.h"
 #include <ArmarXCore/core/time/TimeUtil.h>
 #include <RobotAPI/libraries/core/math/MathUtils.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
 using namespace armarx;
@@ -162,158 +161,16 @@ double PIDController::getControlValue() const
 }
 
 
-MultiDimPIDController::MultiDimPIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation, bool threadSafe, std::vector<bool> limitless) :
-    Kp(Kp),
-    Ki(Ki),
-    Kd(Kd),
-    integral(0),
-    derivative(0),
-    previousError(0),
-    maxControlValue(maxControlValue),
-    maxDerivation(maxDerivation),
-    threadSafe(threadSafe),
-    limitless(limitless)
-{
-    reset();
-}
 
-void MultiDimPIDController::update(const double deltaSec, const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue)
-{
-    ScopedRecursiveLockPtr lock = getLock();
-    processValue = measuredValue;
-    target = targetValue;
 
-    Eigen::VectorXf errorVec = target - processValue;
 
-    if (limitless.size() != 0)
-    {
-        ARMARX_CHECK_EQUAL(limitless.size(), (size_t)errorVec.rows());
-        for (size_t i = 0; i < limitless.size(); i++)
-        {
-            if (limitless.at(i))
-            {
-                errorVec(i) = math::MathUtils::angleModPI(errorVec(i));
-            }
-        }
-    }
 
 
-    double error = errorVec.norm();
 
-    //double dt = (now - lastUpdateTime).toSecondsDouble();
-    //    ARMARX_INFO << deactivateSpam() << VAROUT(dt);
-    if (!firstRun)
-    {
-        integral += error * deltaSec;
 
-        if (deltaSec > 0.0)
-        {
-            derivative = (error - previousError) / deltaSec;
-        }
-    }
-
-    firstRun = false;
-    Eigen::VectorXf direction = targetValue; // copy size
-
-    if (error > 0)
-    {
-        direction = errorVec.normalized();
-    }
-    else
-    {
-        direction.setZero();
-    }
-    Eigen::VectorXf oldControlValue;
-    if (controlValue.rows() > 0)
-    {
-        oldControlValue = controlValue;
-    }
-    else
-    {
-        oldControlValue = Eigen::VectorXf::Zero(measuredValue.rows());
-    }
-    controlValue = direction * (Kp * error + Ki * integral + Kd * derivative);
 
-    if (deltaSec > 0.0)
-    {
-        Eigen::VectorXf accVec = (controlValue - oldControlValue) / deltaSec;
-        float maxNewJointAcc = accVec.maxCoeff();
-        float minNewJointAcc = accVec.minCoeff();
-        maxNewJointAcc = std::max<float>(fabs(minNewJointAcc), fabs(maxNewJointAcc));
-        if (maxNewJointAcc > maxDerivation)
-        {
-            auto newValue = oldControlValue + accVec * maxDerivation / maxNewJointAcc * deltaSec;
-            ARMARX_DEBUG << deactivateSpam(0.5) << VAROUT(maxDerivation) << VAROUT(maxNewJointAcc) << VAROUT(controlValue)  << VAROUT(oldControlValue) << VAROUT(newValue);
-            controlValue = newValue;
-        }
-    }
 
 
-    float max = controlValue.maxCoeff();
-    float min = controlValue.minCoeff();
-    max = std::max<float>(fabs(min), fabs(max));
 
 
 
-    if (max > maxControlValue)
-    {
-        auto newValue = controlValue  * maxControlValue / max;
-        ARMARX_DEBUG << deactivateSpam(0.5) << " Control value to big: " << controlValue << " max value: " << maxControlValue << " new value: " << newValue;
-        controlValue = newValue;
-    }
-    ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) <<  " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
-
-    previousError = error;
-    lastUpdateTime += IceUtil::Time::seconds(deltaSec);
-
-}
-
-void MultiDimPIDController::update(const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue)
-{
-    ScopedRecursiveLockPtr lock = getLock();
-    IceUtil::Time now = TimeUtil::GetTime();
-
-    if (firstRun)
-    {
-        lastUpdateTime = TimeUtil::GetTime();
-    }
-
-    double dt = (now - lastUpdateTime).toSecondsDouble();
-    update(dt, measuredValue, targetValue);
-    lastUpdateTime = now;
-}
-
-const Eigen::VectorXf& MultiDimPIDController::getControlValue() const
-{
-    return controlValue;
-}
-
-void MultiDimPIDController::reset()
-{
-    ScopedRecursiveLockPtr lock = getLock();
-    firstRun = true;
-    previousError = 0;
-    integral = 0;
-    lastUpdateTime = TimeUtil::GetTime();
-    //    controlValue.setZero();
-    //    processValue.setZero();
-    //    target.setZero();
-}
-
-ScopedRecursiveLockPtr MultiDimPIDController::getLock() const
-{
-    if (threadSafe)
-    {
-        return ScopedRecursiveLockPtr(new ScopedRecursiveLock(mutex));
-    }
-    else
-    {
-        return ScopedRecursiveLockPtr();
-    }
-}
-
-void MultiDimPIDController::setMaxControlValue(double value)
-{
-    ScopedRecursiveLockPtr lock = getLock();
-    maxControlValue = value;
-}

source/RobotAPI/libraries/core/PIDController.h

@@ -26,6 +26,7 @@
 
 #include <ArmarXCore/core/logging/Logging.h>
 #include <Eigen/Core>
+#include "MultiDimPIDController.h"
 
 namespace armarx
 {
@@ -69,41 +70,6 @@ namespace armarx
     };
     typedef boost::shared_ptr<PIDController> PIDControllerPtr;
 
-    class MultiDimPIDController :
-        public Logging
-    {
-    public:
-        MultiDimPIDController(float Kp,
-                              float Ki,
-                              float Kd,
-                              double maxControlValue = std::numeric_limits<double>::max(),
-                              double maxDerivation = std::numeric_limits<double>::max(),
-                              bool threadSafe = true,
-                              std::vector<bool> limitless = {});
-        void update(const double deltaSec, const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue);
-        void update(const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue);
-        const Eigen::VectorXf& getControlValue() const;
-        void setMaxControlValue(double value);
 
-        void reset();
-        //    protected:
-        float Kp, Ki, Kd;
-        double integral;
-        double derivative;
-        double previousError;
-        Eigen::VectorXf processValue;
-        Eigen::VectorXf target;
-        IceUtil::Time lastUpdateTime;
-        Eigen::VectorXf controlValue;
-        double maxControlValue;
-        double maxDerivation;
-        bool firstRun;
-        mutable  RecursiveMutex mutex;
-        bool threadSafe = true;
-        std::vector<bool> limitless;
-    private:
-        ScopedRecursiveLockPtr getLock() const;
-    };
-    typedef boost::shared_ptr<MultiDimPIDController> MultiDimPIDControllerPtr;
 }