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 <RobotAPI/libraries/core/math/MathUtils.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <Eigen/Core>
 
#include <mutex>
 
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<>>;
}