MultiDimPIDController.h

Go to the documentation of this file.

/*
 * 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 <mutex>
 
#include <Eigen/Core>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.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:
        using PIDVectorX = Eigen::Matrix<float, dimensions, 1>;
 
        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();
            if (stackAllocations.zeroVec.rows() == 0)
            {
                preallocate(measuredValue.rows());
            }
            ARMARX_CHECK_EQUAL(measuredValue.rows(), targetValue.rows());
            ARMARX_CHECK_EQUAL(measuredValue.rows(), stackAllocations.zeroVec.rows());
            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;
                integral = std::min(integral, maxIntegral);
                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();
            //reset control
            controlValue.setZero();
            processValue.setZero();
            target.setZero();
        }
 
        //    protected:
        float Kp, Ki, Kd;
        double integral;
        double maxIntegral = std::numeric_limits<double>::max();
        double derivative;
        double previousError;
        PIDVectorX processValue;
        PIDVectorX target;
        IceUtil::Time lastUpdateTime;
        PIDVectorX controlValue;
        double maxControlValue;
        double maxDerivation;
        bool firstRun;
        mutable std::recursive_mutex mutex;
        bool threadSafe = true;
        std::vector<bool> limitless;
 
    private:
        struct StackAllocationHelper
        {
            PIDVectorX errorVec;
            PIDVectorX direction;
            PIDVectorX oldControlValue;
            PIDVectorX zeroVec;
        } stackAllocations;
 
        using ScopedRecursiveLock = std::unique_lock<std::recursive_mutex>;
        using ScopedRecursiveLockPtr = std::unique_ptr<ScopedRecursiveLock>;
 
        ScopedRecursiveLockPtr
        getLock() const
        {
            if (threadSafe)
            {
                return ScopedRecursiveLockPtr(new ScopedRecursiveLock(mutex));
            }
            else
            {
                return ScopedRecursiveLockPtr();
            }
        }
    };
 
    using MultiDimPIDController = MultiDimPIDControllerTemplate<>;
    using MultiDimPIDControllerPtr = std::shared_ptr<MultiDimPIDControllerTemplate<>>;
} // namespace armarx