BasicControllers.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
 * @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 <cmath>
#include <type_traits>
 
#include <ArmarXCore/core/util/algorithm.h>
 
#include <RobotAPI/libraries/core/PIDController.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
// #define DEBUG_POS_CTRL
#ifdef DEBUG_POS_CTRL
#include <boost/circular_buffer.hpp>
#endif
 
 
namespace armarx
{
    template <class T, class = typename std::enable_if<std::is_floating_point<T>::value>::type>
    T
    periodicClamp(T value, T periodLo, T periodHi)
    {
        float dist = periodHi - periodLo;
        return std::fmod(value - periodLo + dist, dist) + periodLo;
    }
 
    template <class T, class = typename std::enable_if<std::is_floating_point<T>::value>::type>
    std::pair<T, T>
    pq(T p, T q)
    {
        T a = -p / 2;
        T b = std::sqrt(std::pow(p / 2, 2) - q);
        return {a + b, a - b};
    }
 
    struct deltas
    {
        float dv;
        float dx;
        float dt;
    };
 
    inline deltas
    accelerateToVelocity(float v0, float acc, float vt)
    {
        acc = std::abs(acc);
        deltas d;
        d.dv = vt - v0;
        d.dt = std::abs(d.dv / acc);
        d.dx = sign(d.dv) * d.dv * d.dv / 2.f / acc + v0 * d.dt;
        return d;
    }
 
    std::array<deltas, 3> trapeze(float v0, float acc, float vMax, float dec, float vt, float dx);
 
    inline float
    trapezeArea(float v0, float vmax, float dt1, float dt2, float dt3)
    {
        return (v0 + vmax) / 2 * dt1 + dt2 * vmax + dt3 * vmax * 0.5;
    }
 
    inline void
    findVelocityAndAccelerationForTimeAndDistance(float distance,
                                                  float v0,
                                                  float vmax,
                                                  float dec,
                                                  std::array<deltas, 3> trapeze,
                                                  float newDt,
                                                  float& newV,
                                                  float& newAcc,
                                                  float& newDec)
    {
 
 
        float oldDt = trapeze.at(0).dt + trapeze.at(1).dt + trapeze.at(2).dt;
        float dt0 = trapeze.at(0).dt;
        float dt1 = trapeze.at(1).dt;
        float dt2 = trapeze.at(2).dt;
        float area = trapezeArea(v0, vmax, dt0, dt1, dt2);
        dt1 += newDt - oldDt;
        newV = (-dt0 * v0 + 2 * area) / (dt0 + 2 * dt1 + dt2);
        newAcc = (newV - v0) / dt0;
        newDec = newV / dt2;
 
        if (newV < std::abs(v0))
        {
            dt0 = v0 / dec;
            dt1 = newDt - dt0;
            deltas d = accelerateToVelocity(v0, dec, 0);
            newV = (distance - d.dx) / dt1;
            newAcc = dec;
            newDec = dec;
 
            //            //            float oldDt = trapeze.at(0).dt + trapeze.at(1).dt + trapeze.at(2).dt;
            //            //            dt0 =  trapeze.at(0).dt;
            //            //            dt1 =  trapeze.at(1).dt;
            //            //            dt2 =  trapeze.at(2).dt;
            //            //            area = trapezeArea(v0, newV, dt0, dt1, dt2);
            //            //            auto v_pair = pq(dt1*dec, -(area*dec+v0*v0*0.5f));
            //            auto v_pair = pq(-newDt * dec - v0, area * dec - v0 * v0 * 0.5f);
            //            if (std::isfinite(v_pair.first) && std::isfinite(v_pair.second))
            //            {
            //                newV = std::abs(v_pair.first) < std::abs(v_pair.second) ? v_pair.first : v_pair.second;
            //            }
            //            else if (std::isfinite(v_pair.first))
            //            {
            //                newV = v_pair.first;
            //            }
            //            else if (std::isfinite(v_pair.second))
            //            {
            //                newV = v_pair.second;
            //            }
            //            //            float f = (v0+newV)/2*((newV-v0)/dec) + dt1*newV + (newV/dec)*newV*0.5;
            //            dt0 = std::abs(newV - v0) / dec;
            //            dt1 = (newDt - std::abs(newV - v0) / dec - (newV / dec));
            //            dt2 = (newV / dec);
 
            //            float f = (v0 + newV) / 2 * (std::abs(newV - v0) / dec);
            //            f += (newDt - std::abs(newV - v0) / dec - (newV / dec)) * newV;
            //            f += (newV / dec) * newV * 0.5;
 
            //            newAcc = dec;
            //            newDec = dec;
        }
    }
 
    std::array<deltas, 3>
    trapezeWithDt(float v0, float acc, float vMax, float dec, float vt, float dx, float dt);
 
    /**
     * @param v0 The initial velocity.
     * @param deceleration The deceleration.
     * @return The braking distance given the parameters.
     */
    inline float
    brakingDistance(float v0, float deceleration)
    {
        return accelerateToVelocity(std::abs(v0), std::abs(deceleration), 0).dx;
    }
 
    inline float
    angleDistance(float angle1, float angle2)
    {
        return M_PI - std::fabs(std::fmod(std::fabs(angle1 - angle2), M_PI * 2) - M_PI);
    }
 
    struct VelocityControllerWithAccelerationBounds
    {
        float dt;
        float maxDt;
        float currentV;
        float targetV;
        float maxV;
        float acceleration;
        float deceleration;
        float directSetVLimit;
 
        bool validParameters() const;
        float run() const;
        float estimateTime() const;
    };
 
    struct VelocityControllerWithRampedAcceleration
    {
 
        struct Output
        {
            double velocity;
            double acceleration;
            double jerk;
        };
 
        // config
        float maxDt;
        float maxV;
        float jerk;
        float directSetVLimit;
 
        // state
        float dt;
        float currentV;
        float currentAcc;
        float targetV;
 
 
        bool validParameters() const;
        Output run() const;
    };
 
    typedef std::shared_ptr<class RampedAccelerationVelocityControllerConfiguration>
        RampedAccelerationVelocityControllerConfigurationPtr;
    typedef std::shared_ptr<const RampedAccelerationVelocityControllerConfiguration>
        RampedAccelerationVelocityControllerConfigurationCPtr;
 
    class RampedAccelerationVelocityControllerConfiguration
    {
    public:
        RampedAccelerationVelocityControllerConfiguration() = default;
 
        float maxVelocityRad;
        float maxDecelerationRad;
        float jerk;
        float maxDt;
        float directSetVLimit;
    };
 
    struct VelocityControllerWithRampedAccelerationAndPositionBounds :
        VelocityControllerWithRampedAcceleration
    {
 
        double currentPosition;
        double positionLimitLoSoft;
        double positionLimitHiSoft;
        //        double positionLimitLoHard;
        //        double positionLimitHiHard;
        double deceleration;
        bool validParameters() const;
        Output run() const;
    };
 
    float velocityControlWithAccelerationBounds(float dt,
                                                float maxDt,
                                                const float currentV,
                                                float targetV,
                                                float maxV,
                                                float acceleration,
                                                float deceleration,
                                                float directSetVLimit);
 
    /**
     * @brief Performs velocity control while staying in positional bounds, obeying acceleration / deceleration and staying below a velocity bound.
     *
     * we can have 4 cases:
     * 1. we need to decelerate now or we will violate the position limits (maybe we still will violate them. e.f. if we already violate them or the robot can't brake fast enough)
     * 2. we directly set v and ignore acc/dec (if |currentV - targetV| <= directSetVLimit)
     * 3. we need to accelerate                (if currentV and targetV have same sign and |currentV| < |currentV|)
     * 4. we need to decelerate               (other cases)
     * @param dt The time in seconds until the next call is made. (use the time since the last call as an approximate)
     * @param maxDt Limits dt in case the given approximation has a sudden high value.
     * @param currentV
     * @param targetV
     * @param maxV
     * @param acceleration
     * @param deceleration
     * @param directSetVLimit In case |currentV - targetV| <= directSetVLimit this function will return targetV (if the position bounds are not violated)
     * @param currentPosition
     * @param positionLimitLo
     * @param positionLimitHi
     * @return The next velocity.
     */
    struct VelocityControllerWithAccelerationAndPositionBounds :
        VelocityControllerWithAccelerationBounds
    {
 
        float currentPosition;
        float positionLimitLoSoft;
        float positionLimitHiSoft;
        //        float positionLimitLoHard;
        //        float positionLimitHiHard;
 
        bool validParameters() const;
        float run() const;
    };
 
    float velocityControlWithAccelerationAndPositionBounds(float dt,
                                                           float maxDt,
                                                           float currentV,
                                                           float targetV,
                                                           float maxV,
                                                           float acceleration,
                                                           float deceleration,
                                                           float directSetVLimit,
                                                           float currentPosition,
                                                           float positionLimitLoSoft,
                                                           float positionLimitHiSoft,
                                                           float positionLimitLoHard,
                                                           float positionLimitHiHard);
 
    struct PositionThroughVelocityControllerWithAccelerationBounds
    {
        float dt;
        float maxDt;
        float currentV;
        float maxV;
        float acceleration;
        float deceleration;
        float currentPosition;
        float targetPosition;
        float pControlPosErrorLimit = 0.01;
        float pControlVelLimit =
            0.002; // if target is below this threshold, PID controller will be used
        float accuracy = 0.001;
        std::shared_ptr<PIDController> pid;
        //        float p;
        PositionThroughVelocityControllerWithAccelerationBounds();
        float calculateProportionalGain() const;
        bool validParameters() const;
        float run() const;
        float estimateTime() const;
#ifdef DEBUG_POS_CTRL
        mutable bool PIDModeActive = false;
 
        struct HelpStruct
        {
            float currentPosition;
            float targetVelocityPID;
            float targetVelocityRAMP;
            float currentV;
            float currentError;
            long timestamp;
        };
 
        mutable boost::circular_buffer<HelpStruct> buffer;
        mutable int eventHappeningCounter = -1;
#endif
 
    public:
        bool getCurrentlyPIDActive() const;
 
    private:
        mutable bool currentlyPIDActive = false;
    };
 
    struct PositionThroughVelocityControllerWithAccelerationRamps
    {
        enum class State
        {
            Unknown = -1,
            IncAccIncJerk,
            IncAccDecJerk,
            Keep,
            DecAccIncJerk,
            DecAccDecJerk,
            PCtrl
        };
 
        struct Output
        {
            double velocity;
            double acceleration;
            double jerk;
        };
 
        double dt;
        double maxDt;
        double currentV;
        double currentAcc;
        double maxV;
        double acceleration;
        double deceleration;
        double jerk;
        double currentPosition;
 
    protected:
        double targetPosition;
 
    public:
        double getTargetPosition() const;
        void setTargetPosition(double value);
 
        double pControlPosErrorLimit = 0.01;
        double pControlVelLimit =
            0.002; // if target is below this threshold, PID controller will be used
        double accuracy = 0.001;
        double p;
        bool usePIDAtEnd = true;
        mutable State state = State::Unknown;
        PositionThroughVelocityControllerWithAccelerationRamps();
        bool validParameters() const;
        Output run();
        double estimateTime() const;
        double calculateProportionalGain() const;
 
 
        std::pair<State, Output> calcState() const;
#ifdef DEBUG_POS_CTRL
        mutable bool PIDModeActive = false;
 
        struct HelpStruct
        {
            double currentPosition;
            double targetVelocityPID;
            double targetVelocityRAMP;
            double currentV;
            double currentError;
            long timestamp;
        };
 
        mutable boost::circular_buffer<HelpStruct> buffer;
        mutable int eventHappeningCounter = -1;
#endif
    };
 
    float positionThroughVelocityControlWithAccelerationBounds(float dt,
                                                               float maxDt,
                                                               float currentV,
                                                               float maxV,
                                                               float acceleration,
                                                               float deceleration,
                                                               float currentPosition,
                                                               float targetPosition,
                                                               float p);
 
    struct PositionThroughVelocityControllerWithAccelerationAndPositionBounds :
        PositionThroughVelocityControllerWithAccelerationBounds
    {
        float positionLimitLo;
        float positionLimitHi;
 
        bool validParameters() const;
        float run() const;
    };
 
    float positionThroughVelocityControlWithAccelerationAndPositionBounds(float dt,
                                                                          float maxDt,
                                                                          float currentV,
                                                                          float maxV,
                                                                          float acceleration,
                                                                          float deceleration,
                                                                          float currentPosition,
                                                                          float targetPosition,
                                                                          float p,
                                                                          float positionLimitLo,
                                                                          float positionLimitHi);
 
    struct PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition :
        PositionThroughVelocityControllerWithAccelerationBounds
    {
        //        float direction;
        float positionPeriodLo;
        float positionPeriodHi;
        float run() const;
    };
 
    float positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition(
        float dt,
        float maxDt,
        float currentV,
        float maxV,
        float acceleration,
        float deceleration,
        float currentPosition,
        float targetPosition,
        float pControlPosErrorLimit,
        float p,
        float& direction,
        float positionPeriodLo,
        float positionPeriodHi);
 
    class MinJerkPositionController
    {
    public:
        struct State
        {
            double t, s, v, a;
        };
 
        struct FixedMinJerkState
        {
            double t0, s0, v0, a0;
        };
 
        struct Output
        {
            double position;
            double velocity;
            double acceleration;
            double jerk;
        };
 
        double dt;
        double maxDt;
        double currentPosition;
        double currentV;
        double currentAcc;
        double maxV;
        double desiredDeceleration;
        double desiredJerk;
        double p = 1.0;
        double phase2SwitchDistance = 0.1;
        double phase2SwitchMaxRemainingTime = 0.2; // seconds
        double p_adjust_ratio = 0.8;
        //        PIDControllerPtr pid;
    protected:
        double currentP_Phase3 = -1;
        double currentP_Phase2 = -1;
        double targetPosition;
        double finishTime = 0;
        double currentTime = 0;
        std::unique_ptr<FixedMinJerkState> fixedMinJerkState;
 
    public:
        void reset();
        double getTargetPosition() const;
        void setTargetPosition(double value);
 
        Output run();
    };
} // namespace armarx