PDController.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * 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    RobotAPI::RobotUnit
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#pragma once
 
#include <algorithm>
#include <cmath>
 
namespace armarx
{
    template <class FType>
    class PDController
    {
    public:
        using FloatingType = FType;
 
        PDController(FloatingType p = 0, FloatingType d = 0) : Kp{p}, Kd{d}
        {
        }
 
        // setters / getters
        /// @see Kp
        void setP(FloatingType p);
        /// @see Kp
        FloatingType getP() const;
 
        /// @see Kd
        void setD(FloatingType d);
        /// @see Kd
        FloatingType getD() const;
 
        /**
         * @brief set limits for the output value
         * (lo and hi limits are set to the same value)
         * @see limitOutHi
         * @see limitOutLo
         */
        void limitOut(FloatingType out);
        /**
         * @brief set limits for the output value
         * @see limitOutHi
         * @see limitOutLo
         */
        void limitOut(FloatingType min, FloatingType max);
        /// @see limitOutLo
        FloatingType getLoOutLimit() const;
        /// @see limitOutHi
        FloatingType getHiOutLimit() const;
 
        /// @see outRampDelta
        void setOutRampDelta(FloatingType delta);
        /// @see outRampDelta
        FloatingType getOutRampDelta() const;
 
        /// @see maxError
        void setMaxError(FloatingType error);
        /// @see maxError
        FloatingType getMaxError() const;
 
        /// @see targetRadius
        void setTargetRadius(FloatingType radius);
        /// @see targetRadius
        FloatingType getTargetRadius() const;
 
        /// @see lastOutInterpolation
        void setLastOutInterpolation(FloatingType lastOutFactor);
        /// @see lastOutInterpolation
        FloatingType getLastOutInterpolationFactor() const;
 
        //get controller state
        /// @see lastTarget
        FloatingType getLastTarget() const;
        /// @see lastInValue
        FloatingType getLastInValue() const;
        /// @see lastOut
        FloatingType getLastOutValue() const;
        /// @see iterationCount
        std::uint64_t getIterationCount() const;
 
        // updating functions
        /// @brief reset phe controller
        void reset();
        /**
         * @brief update the controller using the last input and target value
         * @return the output value for this control iteration
         */
        FloatingType update();
        /**
         * @brief update the controller using the last target value
         * @param inValue the current input value
         * @return the output value for this control iteration
         */
        FloatingType update(FloatingType inValue);
        /**
        * @brief update the controller
        * @param inValue the current input value
        * @param target the controller target
        * @return the output value for this control iteration
        */
        FloatingType update(FloatingType inValue, FloatingType target);
 
    private:
        /// @brief p portion of the controller
        FloatingType Kp{0};
        /// @brief d portion of the controller
        FloatingType Kd{0};
 
        /// @brief High limit for the output value
        FloatingType limitOutHi{0};
        /// @brief Low limit for the output value
        FloatingType limitOutLo{0};
 
        /// @brief The target's radius.
        /// if abs(error) <= targetRadius the error is 0,
        /// otherwise the errors magnitude is reduced by targetRadius
        FloatingType targetRadius{0};
 
        /// @brief The output can change maximally by this magnitude every time
        FloatingType outRampDelta{0};
 
        FloatingType lastOutInterpolation{0};
 
        /// @brief The error is clamped to -maxError, +maxError
        FloatingType maxError{0};
 
        //controller state
        /// @brief the number of control iterations
        std::uint64_t iterationCount{0};
        /// @brief the last control target
        FloatingType lastTarget{0};
        /// @brief the last control input value
        FloatingType lastInValue{0};
        /// @brief the last control output value
        FloatingType lastOutValue{0};
    };
} // namespace armarx
 
//functions
namespace armarx
{
    // setters / getters
    template <class FType>
    inline void
    PDController<FType>::setP(FType p)
    {
        Kp = std::abs(p);
    }
 
    template <class FType>
    inline FType
    PDController<FType>::getP() const
    {
        return Kp;
    }
 
    template <class FType>
    inline void
    PDController<FType>::setD(FType d)
    {
        Kd = std::abs(d);
    }
 
    template <class FType>
    inline FType
    PDController<FType>::getD() const
    {
        return Kd;
    }
 
    template <class FType>
    inline void
    PDController<FType>::limitOut(FType out)
    {
        limitOut(-out, out);
    }
 
    template <class FType>
    inline void
    PDController<FType>::limitOut(FType min, FType max)
    {
        std::tie(limitOutLo, limitOutHi) = std::minmax(min, max);
    }
 
    template <class FType>
    inline FType
    PDController<FType>::getLoOutLimit() const
    {
        return limitOutLo;
    }
 
    template <class FType>
    inline FType
    PDController<FType>::getHiOutLimit() const
    {
        return limitOutHi;
    }
 
    template <class FType>
    inline void
    PDController<FType>::setOutRampDelta(FType delta)
    {
        outRampDelta = delta;
    }
 
    template <class FType>
    inline FType
    PDController<FType>::getOutRampDelta() const
    {
        return outRampDelta;
    }
 
    template <class FType>
    inline void
    PDController<FType>::setMaxError(FType error)
    {
        maxError = std::abs(error);
    }
 
    template <class FType>
    inline FType
    PDController<FType>::getMaxError() const
    {
        return maxError;
    }
 
    template <class FType>
    inline void
    PDController<FType>::setTargetRadius(FType radius)
    {
        targetRadius = std::abs(radius);
    }
 
    template <class FType>
    inline FType
    PDController<FType>::getTargetRadius() const
    {
        return targetRadius;
    }
 
    template <class FType>
    inline void
    PDController<FType>::setLastOutInterpolation(FType lastOutFactor)
    {
        if (!(0 >= lastOutFactor && lastOutFactor < 1))
        {
            throw std::invalid_argument{"the interpolation factor has to be in [0, 1)! factor = " +
                                        to_string(lastOutFactor)};
        }
        lastOutInterpolation = lastOutFactor;
    }
 
    template <class FType>
    inline FType
    PDController<FType>::getLastOutInterpolationFactor() const
    {
        return lastOutInterpolation;
    }
 
    //get controller state
    template <class FType>
    inline FType
    PDController<FType>::getLastTarget() const
    {
        return lastTarget;
    }
 
    template <class FType>
    inline FType
    PDController<FType>::getLastInValue() const
    {
        return lastInValue;
    }
 
    template <class FType>
    inline FType
    PDController<FType>::getLastOutValue() const
    {
        return lastOutValue;
    }
 
    template <class FType>
    inline uint64_t
    PDController<FType>::getIterationCount() const
    {
        return iterationCount;
    }
 
    // updating functions
    template <class FType>
    inline void
    PDController<FType>::reset()
    {
        iterationCount = 0;
    }
 
    template <class FType>
    inline FType
    PDController<FType>::update()
    {
        return update(lastInValue, lastTarget);
    }
 
    template <class FType>
    inline FType
    PDController<FType>::update(FType inValue)
    {
        return update(inValue, lastTarget);
    }
 
    template <class FType>
    inline FType
    PDController<FType>::update(FType inValue, FType target)
    {
        lastTarget = target;
 
        //calculate error
        FloatingType error = (target - inValue);
        if (std::abs(error) < targetRadius)
        {
            error = 0;
        }
        else
        {
            error = sign(error) * (std::abs(error) - targetRadius);
        }
        if (maxError != 0)
        {
            error = std::clamp(error, -maxError, +maxError);
        }
 
        const FloatingType pOut = Kp * error;
 
        if (!iterationCount)
        {
            lastInValue = inValue;
            lastOutValue = pOut;
        }
        ++iterationCount;
        const FloatingType dOut = -Kd * (inValue - lastInValue);
        lastInValue = inValue;
 
        FloatingType out = pOut + dOut;
 
        if (outRampDelta != 0)
        {
            out = std::clamp(out, lastOutValue - outRampDelta, lastOutValue + outRampDelta);
        }
        if (limitOutLo != limitOutHi)
        {
            out = std::clamp(out, limitOutLo, limitOutHi);
        }
        if (lastOutInterpolation != 0)
        {
            out = lastOutValue * lastOutInterpolation + out * (1 - lastOutInterpolation);
        }
 
        lastOutValue = out;
        return out;
    }
} // namespace armarx