BasicControllers.cpp

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
*/
 
#include "BasicControllers.h"
 
#include <algorithm>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/algorithm.h>
 
#include "util/CtrlUtil.h"
 
namespace armarx
{
    float
    velocityControlWithAccelerationBounds(float dt,
                                          float maxDt,
                                          const float currentV,
                                          float targetV,
                                          float maxV,
                                          float acceleration,
                                          float deceleration,
                                          float directSetVLimit)
    {
        dt = std::min(std::abs(dt), std::abs(maxDt));
        maxV = std::abs(maxV);
        acceleration = std::abs(acceleration);
        deceleration = std::abs(deceleration);
        targetV = std::clamp(targetV, -maxV, maxV);
 
        //we can have 3 cases:
        // 1. we directly set v and ignore acc/dec (if |curr - target| <= limit)
        // 2. we need to accelerate                (if curr and target v have same sign and |curr| < |target|)
        // 3. we need to decelerate               (other cases)
 
        //handle case 1
        const float curverror = targetV - currentV;
        if (std::abs(curverror) < directSetVLimit)
        {
            return targetV;
        }
 
        //handle case 2 + 3
        const bool accelerate = sign(targetV) == sign(currentV) && // v in same direction
                                std::abs(targetV) > std::abs(currentV); // currently to slow
 
        const float usedacc = accelerate ? acceleration : -deceleration;
        const float maxDeltaV = std::abs(usedacc * dt);
        if (maxDeltaV >= std::abs(curverror))
        {
            //we change our v too much with the used acceleration
            //but we could reach our target with a lower acceleration ->set the target
 
            return targetV;
        }
        const float deltaVel = std::clamp(sign(currentV) * usedacc * dt, -maxDeltaV, maxDeltaV);
        const float nextV = currentV + deltaVel;
        return nextV;
    }
 
    ////////////////////////////
    //wip?
    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)
    {
        if (currentPosition <= positionLimitLoHard || currentPosition >= positionLimitHiHard)
        {
            return std::nanf("1");
        }
 
        float softLimitViolation = 0;
        if (currentPosition <= positionLimitLoSoft)
        {
            softLimitViolation = -1;
        }
        if (currentPosition >= positionLimitHiSoft)
        {
            softLimitViolation = 1;
        }
 
        const float upperDt = std::max(std::abs(dt), std::abs(maxDt));
        dt = std::min(std::abs(dt), std::abs(maxDt));
        maxV = std::abs(maxV);
 
        //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.g. if we violated them initially)
        // 2. we directly set v and ignore acc/dec (if |curr - target| <= limit)
        // 3. we need to accelerate                (if curr and target v have same sign and |curr| < |target|)
        // 4. we need to decelerate               (other cases)
        float nextv;
        //handle case 1
        const float vsquared = currentV * currentV;
        const float brakingDist =
            sign(currentV) * vsquared / 2.f /
            std::abs(deceleration); //the braking distance points in the direction of the velocity
 
 
        const float posIfBrakingNow = currentPosition + brakingDist;
        if (posIfBrakingNow <= positionLimitLoSoft || posIfBrakingNow >= positionLimitHiSoft)
        {
            //case 1. -> brake now! (we try to have v=0 at the limit)
            const auto limit =
                posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
            const float wayToGo = limit - currentPosition;
 
            //decelerate!
            // s = v²/(2a)  <=>  a = v²/(2s)
            const float dec = std::abs(vsquared / 2.f / wayToGo);
            const float vel = currentV - sign(currentV) * dec * upperDt;
            nextv = std::clamp(vel, -maxV, maxV);
            if (sign(currentV) != sign(nextv))
            {
                //stop now
                nextv = 0;
            }
        }
        else
        {
            //handle 2-3
            nextv = velocityControlWithAccelerationBounds(
                dt, maxDt, currentV, targetV, maxV, acceleration, deceleration, directSetVLimit);
        }
        if (softLimitViolation == sign(nextv))
        {
            //the area between soft and hard limits is sticky
            //the controller can only move out of it (not further in)
            return 0;
        }
 
        //the next velocity will not violate the pos bounds harder than they are already
        return nextv;
    }
 
    float
    positionThroughVelocityControlWithAccelerationBounds(float dt,
                                                         float maxDt,
                                                         float currentV,
                                                         float maxV,
                                                         float acceleration,
                                                         float deceleration,
                                                         float currentPosition,
                                                         float targetPosition,
                                                         float p)
    {
        dt = std::min(std::abs(dt), std::abs(maxDt));
        maxV = std::abs(maxV);
        acceleration = std::abs(acceleration);
        deceleration = std::abs(deceleration);
        const float signV = sign(currentV);
        //we can have 3 cases:
        // 1. we use a p controller and ignore acc/dec (if velocity target from p controller < velocity target from ramp controller)
        // 2. we need to accelerate (or hold vel)      (if e = (targetPosition - currentPosition)
        //                                                 the brakingDistance have the same sign and brakingDistance < e
        //                                                 and currentVel <= maxV)
        // 3. we need to decelerate                   (other cases)
 
        //handle case 1
        const float positionError = targetPosition - currentPosition;
        float newTargetVelPController = positionError * p;
 
        //handle case 2-3
        const float brakingDistance =
            signV * currentV * currentV / 2.f /
            deceleration; //the braking distance points in the direction of the velocity
        const float posIfBrakingNow = currentPosition + brakingDistance;
        const float posErrorIfBrakingNow = targetPosition - posIfBrakingNow;
 
        const bool decelerate =
            std::abs(currentV) > maxV || // we need to slow down (to stay in [-maxV,maxV]
            //            std::abs(posIfBrakingNow - targetPosition) <= pControlPosErrorLimit || // we want to hit the target
            sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
        const float usedacc = decelerate ? -deceleration : acceleration;
        const float maxDeltaV = std::abs(usedacc * dt);
        const float deltaVel = std::clamp(signV * usedacc * dt, -maxDeltaV, maxDeltaV);
 
        float newTargetVel = std::clamp(currentV + deltaVel, -maxV, maxV);
        bool usePID = std::abs(newTargetVelPController) < std::abs(newTargetVel);
        //        ARMARX_INFO << deactivateSpam(0.01) << VAROUT(usePID) << VAROUT(dt) << VAROUT(decelerate) << VAROUT(usedacc) << VAROUT(maxDeltaV) << VAROUT(deltaVel) << VAROUT(newTargetVel) << VAROUT(newTargetVelPController)
        //                    << VAROUT(currentPosition) << VAROUT(targetPosition);
        if (usePID)
        {
            return newTargetVelPController;
        }
        else
        {
            return newTargetVel;
        }
    }
 
    float
    positionThroughVelocityControlWithAccelerationAndPositionBounds(float dt,
                                                                    float maxDt,
                                                                    float currentV,
                                                                    float maxV,
                                                                    float acceleration,
                                                                    float deceleration,
                                                                    float currentPosition,
                                                                    float targetPosition,
                                                                    float p,
                                                                    float positionLimitLo,
                                                                    float positionLimitHi
 
    )
    {
        dt = std::min(std::abs(dt), std::abs(maxDt));
        maxV = std::abs(maxV);
        acceleration = std::abs(acceleration);
        deceleration = std::abs(deceleration);
        const float signV = sign(currentV);
        //we can have 4 cases:
        // 1. we need to decelerate now or we will violate the position limits (maybe we still will violate them)
        // 2. we use a p controller and ignore acc/dec (if |currentPosition - targetPosition| <= pControlPosErrorLimit AND |currentPosition - targetPosition|*p < maxV)
        // 4. we need to accelerate (or hold vel)      (if e = (targetPosition - currentPosition)
        //                                                 and the brakingDistance have the same sign and brakingDistance < e
        //                                                 and currentVel <= maxV)
        // 5. we need to decelerate                   (other cases)
 
        //handle case 1
        const float vsquared = currentV * currentV;
        const float brakingDistance =
            signV * vsquared / 2.f /
            deceleration; //the braking distance points in the direction of the velocity
        const float posIfBrakingNow = currentPosition + brakingDistance;
        if (posIfBrakingNow <= positionLimitLo || posIfBrakingNow >= positionLimitHi)
        {
            //case 1. -> brake now! (we try to have v=0 at the limit)
            const auto limit = brakingDistance > 0 ? positionLimitHi : positionLimitLo;
            const float wayToGo = std::abs(limit - currentPosition);
            // s = v²/(2a)  <=>  a = v²/(2s)
            const float dec = std::abs(vsquared / 2.f / wayToGo);
            const float vel = currentV - signV * dec * dt;
            return vel;
        }
 
        //handle case 2-3
        return positionThroughVelocityControlWithAccelerationBounds(
            dt,
            maxDt,
            currentV,
            maxV,
            acceleration,
            deceleration,
            currentPosition,
            std::clamp(targetPosition, positionLimitLo, positionLimitHi),
            p);
    }
 
    ////////////////////////////
    //wip
 
 
    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)
    {
        currentPosition = periodicClamp(currentPosition, positionPeriodLo, positionPeriodHi);
        targetPosition = periodicClamp(targetPosition, positionPeriodLo, positionPeriodHi);
 
        const float brakingDist = brakingDistance(currentV, deceleration);
        const float posIfBrakingNow = currentPosition + brakingDist;
        const float posIfBrakingNowError = targetPosition - posIfBrakingNow;
        const float posError = targetPosition - currentPosition;
        if (std::abs(posIfBrakingNowError) <= pControlPosErrorLimit ||
            std::abs(posError) <= pControlPosErrorLimit)
        {
            //this allows slight overshooting (in the limits of the p controller)
            return positionThroughVelocityControlWithAccelerationBounds(dt,
                                                                        maxDt,
                                                                        currentV,
                                                                        maxV,
                                                                        acceleration,
                                                                        deceleration,
                                                                        currentPosition,
                                                                        targetPosition,
                                                                        p);
        }
 
        //we transform the problem with periodic bounds into
        //a problem with position bounds
        const float positionPeriodLength = std::abs(positionPeriodHi - positionPeriodLo);
 
        //we shift the positions such that 0 == target
        currentPosition =
            periodicClamp(currentPosition - targetPosition, 0.f, positionPeriodLength);
        //how many times will we go ovet the target if we simply bake now?
        const float overshoot = std::trunc((currentPosition + brakingDist) / positionPeriodLength);
 
        if (true || direction == 0)
        {
            //determine the direction to go (1 == pos vel, -1 == neg vel)
            direction = (periodicClamp(currentPosition + brakingDist, 0.f, positionPeriodLength) >=
                         positionPeriodLength / 2)
                            ? 1
                            : -1;
        }
        //shift the target away from 0
        targetPosition = (overshoot - std::min(0.f, -direction)) *
                         positionPeriodLength; // - direction * pControlPosErrorLimit;
 
        //move
        return positionThroughVelocityControlWithAccelerationBounds(dt,
                                                                    maxDt,
                                                                    currentV,
                                                                    maxV,
                                                                    acceleration,
                                                                    deceleration,
                                                                    currentPosition,
                                                                    targetPosition,
                                                                    p);
    }
 
    bool
    VelocityControllerWithAccelerationBounds::validParameters() const
    {
        return maxV > 0 && acceleration > 0 && deceleration > 0 && targetV <= maxV &&
               targetV >= -maxV;
    }
 
    float
    VelocityControllerWithAccelerationBounds::run() const
    {
        const float useddt = std::min(std::abs(dt), std::abs(maxDt));
 
        //we can have 3 cases:
        // 1. we directly set v and ignore acc/dec (if |curr - target| <= limit)
        // 2. we need to accelerate                (if curr and target v have same sign and |curr| < |target|)
        // 3. we need to decelerate               (other cases)
        //handle case 1
        const float curverror = targetV - currentV;
        if (std::abs(curverror) < directSetVLimit)
        {
            return targetV;
        }
 
        //handle case 2 + 3
        const bool accelerate = sign(targetV) == sign(currentV) && // v in same direction
                                std::abs(targetV) > std::abs(currentV); // currently to slow
 
        const float usedacc = accelerate ? acceleration : -deceleration;
        const float maxDeltaV = std::abs(usedacc * useddt);
        if (maxDeltaV >= std::abs(curverror))
        {
            //we change our v too much with the used acceleration
            //but we could reach our target with a lower acceleration ->set the target
 
            return targetV;
        }
        const float deltaVel = std::clamp(sign(currentV) * usedacc * useddt, -maxDeltaV, maxDeltaV);
        const float nextV = currentV + deltaVel;
        return nextV;
    }
 
    bool
    VelocityControllerWithRampedAcceleration::validParameters() const
    {
        return maxV > 0 && jerk > 0 && targetV <= maxV && targetV >= -maxV;
    }
 
    VelocityControllerWithRampedAcceleration::Output
    VelocityControllerWithRampedAcceleration::run() const
    {
        const double useddt = std::min(std::abs(dt), std::abs(maxDt));
 
        //we can have 3 cases:
        // 1. we directly set v and ignore acc/dec (if |curr - target| <= limit)
        // 2. we need to accelerate                (if curr and target v have same sign and |curr| < |target|)
        // 3. we need to decelerate               (other cases)
        //handle case 1
        const double clampedTargetV = math::MathUtils::LimitTo(targetV, maxV);
        const double curverror = clampedTargetV - currentV;
        if (std::abs(curverror) < directSetVLimit)
        {
            //            double nextAcc = (clampedTargetV - currentV) / useddt; // calculated acc is unstable!!!
            //            double nextJerk = (nextAcc - currentAcc) / useddt;
            Output result{clampedTargetV, 0, 0};
 
            return result;
        }
 
        //handle case 2 + 3
        //        const bool accelerate = sign(clampedTargetV) == sign(currentV) &&     // v in same direction
        //                                std::abs(clampedTargetV) > std::abs(currentV); // currently to slow
        const auto goalDir = math::MathUtils::Sign(clampedTargetV - currentV);
        // calculate if we need to increase or decrease acc
        bool increaseAcc = true;
        if (goalDir == math::MathUtils::Sign(currentAcc))
        {
            //            double t_to_v = ctrlutil::t_to_v(clampedTargetV - currentV, currentAcc, goalDir * jerk);
            //            double acc_at_t = ctrlutil::a(t_to_v, std::abs(currentAcc), -jerk);
            //            increaseJerk = acc_at_t < 0.0;
 
            double t_to_zero_acc = std::abs(currentAcc) / jerk;
            double v_at_t = ctrlutil::v(t_to_zero_acc, currentV, currentAcc, -goalDir * jerk);
            increaseAcc = math::MathUtils::Sign(v_at_t - clampedTargetV) != goalDir;
            //            ARMARX_INFO << VAROUT(t_to_zero_acc) << VAROUT(v_at_t) << VAROUT(increaseAcc);
        }
        // v = a*a/(2j)
        const double adjustedJerk =
            std::abs(currentAcc * currentAcc / (2 * (clampedTargetV - currentV)));
        double usedJerk = increaseAcc
                              ? goalDir * jerk
                              : -goalDir * ((std::abs(currentV) > 0.01 && !std::isnan(adjustedJerk))
                                                ? adjustedJerk
                                                : jerk);
        //        double t_to_target_v = ctrlutil::t_to_v(clampedTargetV - currentV, currentAcc, usedJerk);
        //        double v_at_t2 = ctrlutil::v(t_to_target_v, currentV, currentAcc, usedJerk);
        //        ctrlutil::t_to_v(currentV, currentAcc, jerk);
        //        const double usedacc = currentAcc + useddt * usedJerk;
        //        const double maxDeltaVSimple = usedacc * useddt;
 
        const double maxDeltaV = std::abs(ctrlutil::v(useddt, 0, currentAcc, usedJerk));
        if (maxDeltaV >= std::abs(curverror))
        {
            //we change our v too much with the used acceleration
            //but we could reach our target with a lower acceleration ->set the target
            double nextAcc = (clampedTargetV - currentV) / useddt;
            double nextJerk = (nextAcc - currentAcc) / useddt;
            //            ARMARX_INFO << "overshooting! " << VAROUT(clampedTargetV) << VAROUT(nextAcc) << VAROUT(nextJerk);
            Output result{clampedTargetV, nextAcc, nextJerk};
            return result;
        }
        //        const double deltaVel = boost::algorithm::clamp(sign(currentV) * usedacc * useddt, -maxDeltaV, maxDeltaV);
        const double nextV = ctrlutil::v(useddt, currentV, currentAcc, usedJerk);
        const double nextAcc = ctrlutil::a(useddt, currentAcc, usedJerk);
        //        ARMARX_INFO << VAROUT(clampedTargetV) << VAROUT(nextV) << VAROUT(nextAcc) << VAROUT(usedJerk);
        Output result{nextV, nextAcc, usedJerk};
        return result;
    }
 
    float
    VelocityControllerWithAccelerationBounds::estimateTime() const
    {
        const float curverror = std::abs(targetV - currentV);
        if (curverror < directSetVLimit)
        {
            return maxDt;
        }
        return curverror / ((targetV > currentV) ? acceleration : deceleration);
    }
 
    bool
    VelocityControllerWithAccelerationAndPositionBounds::validParameters() const
    {
        return VelocityControllerWithAccelerationBounds::validParameters() &&
               //                positionLimitLoHard < positionLimitLoSoft &&
               positionLimitLoSoft < positionLimitHiSoft;
        //                positionLimitHiSoft < positionLimitHiHard;
    }
 
    float
    VelocityControllerWithAccelerationAndPositionBounds::run() const
    {
        //        if (currentPosition <= positionLimitLoHard || currentPosition >= positionLimitHiHard)
        //        {
        //            ARMARX_INFO << deactivateSpam(1) << "Hard limit violation. " << VAROUT(currentPosition) << VAROUT(positionLimitLoHard) << VAROUT(positionLimitHiHard);
        //            return std::nanf("1");
        //        }
 
        float softLimitViolation = 0;
        if (currentPosition <= positionLimitLoSoft)
        {
            softLimitViolation = -1;
        }
        if (currentPosition >= positionLimitHiSoft)
        {
            softLimitViolation = 1;
        }
 
        const float upperDt = std::max(std::abs(dt), std::abs(maxDt));
 
        //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.g. if we violated them initially)
        // 2. we directly set v and ignore acc/dec (if |curr - target| <= limit)
        // 3. we need to accelerate                (if curr and target v have same sign and |curr| < |target|)
        // 4. we need to decelerate               (other cases)
        float nextv;
        //handle case 1
        const float vsquared = currentV * currentV;
        const float brakingDist =
            sign(currentV) * vsquared / 2.f /
            std::abs(deceleration); //the braking distance points in the direction of the velocity
 
        const float posIfBrakingNow = currentPosition + brakingDist;
        //        ARMARX_INFO << deactivateSpam(1) << VAROUT(currentPosition) << VAROUT(positionLimitLoSoft) << VAROUT(positionLimitHiSoft) << VAROUT(posIfBrakingNow);
        if ((posIfBrakingNow <= positionLimitLoSoft && currentV < 0) ||
            (posIfBrakingNow >= positionLimitHiSoft && currentV > 0))
        {
            //case 1. -> brake now! (we try to have v=0 at the limit)
            const auto limit =
                posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
            const float wayToGo = limit - currentPosition;
 
            //decelerate!
            // s = v²/(2a)  <=>  a = v²/(2s)
            const float dec = std::abs(vsquared / 2.f / wayToGo);
            const float vel = currentV - sign(currentV) * dec * upperDt;
            nextv = std::clamp(vel, -maxV, maxV);
            //            ARMARX_INFO << deactivateSpam(1) << "clamped new Vel: " << VAROUT(nextv);
            if (sign(currentV) != sign(nextv))
            {
                //                ARMARX_INFO << deactivateSpam(1) << "wrong sign: stopping"; //stop now
                nextv = 0;
            }
        }
        else
        {
            //handle 2-3
            nextv = VelocityControllerWithAccelerationBounds::run();
            //            ARMARX_INFO << deactivateSpam(1) << "Desired target Vel: " << targetV << " " << VAROUT(nextv);
        }
        if (softLimitViolation == sign(nextv) && nextv != 0)
        {
            //the area between soft and hard limits is sticky
            //the controller can only move out of it (not further in)
            //            ARMARX_DEBUG << deactivateSpam(1) << "Soft limit violation. " << softLimitViolation << VAROUT(nextv);
            return 0;
        }
 
        //the next velocity will not violate the pos bounds harder than they are already
        return nextv;
    }
 
    PositionThroughVelocityControllerWithAccelerationBounds::
        PositionThroughVelocityControllerWithAccelerationBounds()
    {
#ifdef DEBUG_POS_CTRL
        buffer = boost::circular_buffer<HelpStruct>(20);
#endif
        pid.reset(new PIDController(1, 0, 0));
        pid->threadSafe = false;
    }
 
    float
    PositionThroughVelocityControllerWithAccelerationBounds::calculateProportionalGain() const
    {
        /*            s = v_0*v_0/(2*a) -> sqrt(s*2*a) = v_0;
 
                    K_p * error = v_0; -> v_0/error = K_p;
                    */
        auto v_0 = std::sqrt(pControlPosErrorLimit * 2 * deceleration);
        return v_0 / pControlPosErrorLimit;
    }
 
    bool
    PositionThroughVelocityControllerWithAccelerationBounds::validParameters() const
    {
        return maxV > 0 && acceleration > 0 && deceleration > 0 &&
               //                pControlPosErrorLimit > 0 &&
               //                pControlVelLimit > 0 &&
               pid->Kp > 0;
    }
 
    float
    PositionThroughVelocityControllerWithAccelerationBounds::run() const
    {
        const float useddt = std::min(std::abs(dt), std::abs(maxDt));
        const float signV = sign(currentV);
        //we can have 3 cases:
        // 1. we use a p controller and ignore acc/dec (if |currentPosition - targetPosition| <= pControlPosErrorLimit AND |currentV| < pControlVelLimit)
        // 2. we need to accelerate (or hold vel)      (if e = (targetPosition - currentPosition)
        //                                                 the brakingDistance have the same sign and brakingDistance < e
        //                                                 and currentVel <= maxV)
        // 3. we need to decelerate                   (other cases)
 
        //handle case 1
        const float positionError = targetPosition - currentPosition;
        pid->update((double)useddt, (double)currentPosition, (double)targetPosition);
        float newTargetVelPController = pid->getControlValue();
 
        //handle case 2-3
        const float brakingDistance =
            signV * currentV * currentV / 2.f /
            deceleration; //the braking distance points in the direction of the velocity
        const float posIfBrakingNow = currentPosition + brakingDistance;
        const float posErrorIfBrakingNow = targetPosition - posIfBrakingNow;
        const bool hardBrakingNeeded = std::abs(brakingDistance) > std::abs(positionError);
        const float safePositionError =
            (std::abs(positionError) < 0.0001) ? (sign(positionError) * 0.0001) : positionError;
        const float usedDeceleration = hardBrakingNeeded
                                           ? std::abs(currentV * currentV / 2.f / safePositionError)
                                           : deceleration;
 
        const bool decelerate =
            std::abs(currentV) > maxV || // we need to slow down (to stay in [-maxV,maxV]
            hardBrakingNeeded ||
            sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
 
        const float usedacc = decelerate ? -usedDeceleration : acceleration;
        const float deltaVel = signV * usedacc * useddt;
        float newTargetVelRampCtrl = std::clamp(currentV + deltaVel, -maxV, maxV);
        bool PIDActive = /*std::abs(newTargetVelPController) < std::abs(newTargetVelRampCtrl)
                             || std::abs(newTargetVelPController) < pControlVelLimit ||*/
            std::abs(positionError) < pControlPosErrorLimit;
        //        if (currentlyPIDActive != PIDActive && PIDActive)
        //        {
        //            ARMARX_INFO << "Switching to PID mode: " << VAROUT(positionError) << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl);
        //        }
        this->currentlyPIDActive = PIDActive;
        float finalTargetVel =
            (currentlyPIDActive) ? newTargetVelPController : newTargetVelRampCtrl;
        if (std::abs(positionError) < accuracy)
        {
            return 0.0; // if close to target set zero velocity to avoid oscillating around target
        }
        //        if (hardBrakingNeeded)
        //        {
        //            ARMARX_INFO /*<< deactivateSpam(0.1)*/ << "Hard braking! " << VAROUT(positionError) << VAROUT(brakingDistance) << VAROUT(finalTargetVel) << VAROUT(currentV);
        //        }
        //        if (decelerate)
        //        {
        //            ARMARX_INFO /*<< deactivateSpam(0.1)*/ << "Decelerating! " << VAROUT(targetPosition) << VAROUT(currentPosition) << VAROUT(positionError) << VAROUT(brakingDistance) << VAROUT(finalTargetVel) << VAROUT(currentV) <<
        //                                                   VAROUT(usedacc) << VAROUT(deltaVel) << VAROUT(useddt);
        //        }
#ifdef DEBUG_POS_CTRL
        buffer.push_back({currentPosition,
                          newTargetVelPController,
                          newTargetVelRampCtrl,
                          currentV,
                          positionError,
                          IceUtil::Time::now().toMicroSeconds()});
 
        //        if (PIDModeActive != usePID)
        if (buffer.size() > 0 &&
            sign(positionError) * sign(buffer[buffer.size() - 2].currentError) < 0 &&
            eventHappeningCounter < 0)
        {
            eventHappeningCounter = 10;
            ARMARX_IMPORTANT << "HIGH VELOCITY  DETECTED";
        }
        if (eventHappeningCounter == 0)
        {
            ARMARX_IMPORTANT << "BUFFER CONTENT";
            for (auto& elem : buffer)
            {
                ARMARX_INFO << VAROUT(elem.currentPosition) << VAROUT(elem.targetVelocityPID)
                            << VAROUT(elem.targetVelocityRAMP) << VAROUT(elem.currentV)
                            << VAROUT(elem.currentError) << VAROUT(elem.timestamp);
            }
            ARMARX_IMPORTANT << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl)
                             << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError);
        }
        if (eventHappeningCounter >= 0)
        {
            eventHappeningCounter--;
        }
 
        ARMARX_INFO << deactivateSpam(5) << VAROUT(newTargetVelPController)
                    << VAROUT(newTargetVelRampCtrl) << VAROUT(currentPosition) << VAROUT(usePID)
                    << VAROUT(positionError) << VAROUT(targetPosition) << VAROUT(currentV)
                    << VAROUT(deltaVel) << VAROUT(useddt) << VAROUT(usedacc);
        PIDModeActive = usePID;
#endif
 
        return finalTargetVel;
    }
 
    float
    PositionThroughVelocityControllerWithAccelerationBounds::estimateTime() const
    {
        //            const float brakingDistance = sign(currentV) * currentV * currentV / 2.f / deceleration;
        const float posError = targetPosition - currentPosition;
        const float posIfBrakingNow = currentPosition + brakingDistance(currentV, deceleration);
        //we are near the goal if we break now
        //        if (std::abs(posIfBrakingNow - targetPosition) <= pControlPosErrorLimit  && currentV <= pControlVelLimit)
        //        {
        //            return std::sqrt(2 * std::abs(brakingDistance(currentV, deceleration)) / deceleration);
        //        }
        const float posErrorIfBrakingNow = targetPosition - posIfBrakingNow;
 
        float t = 0;
        float curVel = currentV;
        float curPos = currentPosition;
        auto curPosEr = [&] { return targetPosition - curPos; };
        //            auto curBrake = [&] {return std::abs(brakingDistance(curVel, deceleration));};
 
        //check for overshooting of target
        if (sign(posError) != sign(posErrorIfBrakingNow))
        {
            t = currentV / deceleration;
            curVel = 0;
            curPos = posIfBrakingNow;
        }
        //        //check for to high v
        //        if (std::abs(curVel) > maxV)
        //        {
        //            const float dv = curVel - maxV;
        //            float dt = dv / deceleration;
        //            t += dt;
        //            //            curPos += 0.5f / deceleration * dv * dv * sign(curVel);
        //            curPos += 0.5 * deceleration * dt * dt + curVel * dt * sign(curVel);
        //            curVel = maxV;
        //        }
        //curBrake()<=curPosEr() && curVel <= maxV
        //        auto tr = trapeze(curVel, acceleration, maxV, deceleration, 0, curPosEr());
        auto tr = trapeze(curVel,
                          std::abs(curVel) > maxV ? deceleration : acceleration,
                          maxV,
                          deceleration,
                          0,
                          curPosEr());
 
        return t + tr.at(0).dt + tr.at(1).dt + tr.at(2).dt;
    }
 
    bool
    PositionThroughVelocityControllerWithAccelerationAndPositionBounds::validParameters() const
    {
        return PositionThroughVelocityControllerWithAccelerationBounds::validParameters() &&
               !std::isnan(positionLimitLo) && !std::isnan(positionLimitHi) &&
               positionLimitLo < positionLimitHi && targetPosition <= positionLimitHi &&
               positionLimitLo <= targetPosition;
    }
 
    float
    PositionThroughVelocityControllerWithAccelerationAndPositionBounds::run() const
    {
        const float useddt = std::min(std::abs(dt), std::abs(maxDt));
        const float signV = sign(currentV);
        //we can have 4 cases:
        // 1. we need to decelerate now or we will violate the position limits (maybe we still will violate them)
        // 2. we use a p controller and ignore acc/dec (if |currentPosition - targetPosition| <= pControlPosErrorLimit AND |currentPosition - targetPosition|*p < maxV)
        // 4. we need to accelerate (or hold vel)      (if e = (targetPosition - currentPosition)
        //                                                 and the brakingDistance have the same sign and brakingDistance < e
        //                                                 and currentVel <= maxV)
        // 5. we need to decelerate                   (other cases)
        //handle case 1
        const float vsquared = currentV * currentV;
        const float brakingDistance =
            signV * vsquared / 2.f /
            deceleration; //the braking distance points in the direction of the velocity
        const float posIfBrakingNow = currentPosition + brakingDistance;
        float direction = targetPosition < currentPosition ? -1 : 1;
        //        ARMARX_INFO << deactivateSpam(0.5) << VAROUT(direction) << VAROUT(brakingDistance) << VAROUT(currentPosition) << VAROUT(targetPosition);
        if ((posIfBrakingNow <= positionLimitLo && direction < 1) ||
            (posIfBrakingNow >= positionLimitHi && direction > 1))
        {
            //case 1. -> brake now! (we try to have v=0 at the limit)
            const auto limit = brakingDistance > 0 ? positionLimitHi : positionLimitLo;
            const float wayToGo = std::abs(limit - currentPosition);
            // s = v²/(2a)  <=>  a = v²/(2s)
            const float dec = std::abs(vsquared / 2.f / wayToGo);
            const float vel = currentV - signV * dec * useddt;
            //            ARMARX_INFO << /*deactivateSpam(0.1) <<*/ "Braking now! " << VAROUT(vel) << VAROUT(wayToGo) << VAROUT(limit);
            return vel;
        }
 
        //handle case 2-3
        return PositionThroughVelocityControllerWithAccelerationBounds::run();
    }
 
    std::array<deltas, 3>
    trapeze(float v0, float acc, float vMax, float dec, float vt, float dx)
    {
        acc = std::abs(acc);
        vMax = std::abs(vMax);
        dec = std::abs(dec);
        auto calc = [&](float vmax)
        {
            const deltas dacc = accelerateToVelocity(v0, acc, vmax);
            const deltas ddec = accelerateToVelocity(vmax, dec, vt);
            const float dxMax = dacc.dx + ddec.dx;
            if (sign(dxMax) == sign(dx))
            {
                if (std::abs(dxMax) <= dx)
                {
                    deltas mid;
                    mid.dx = dxMax - dx;
                    mid.dv = vMax;
                    mid.dt = std::abs(mid.dx / mid.dv);
                    return std::make_pair(true, std::array<deltas, 3>{dacc, mid, ddec});
                }
                //we need a lower vMax (vx)
                //in all following formulas # elem {0,t}
                //a0=acc , at = dec
                //vx=v0+dv0 = vt + dvt -> dv# = vx-v#
                //dx=dx0+dxt
                //dx#=dv#^2/2/a#+dv#/a#*v#
                //dx#=(vx-v#)^2/2/a#+(vx-v#)/a#*v#
                //dx#=vx^2*(1/2/a#) + vx*(v#/2 - v#/a#) + (v#^2/2/a# - v#^2/2)
                //dx=vx^2*(1/2/a0+1/2/at) + vx*(v0/2 - v0/a0 + vt/2 - vt/at) + (v0^2/2/a0 - v0^2/2 +vt^2/2/at - vt^2/2)
                //0 =vx^2*(1/2/a0+1/2/at) + vx*(v0/2 - v0/a0 + vt/2 - vt/at) + (v0^2/2/a0 - v0^2/2 +vt^2/2/at - vt^2/2 -dx)
                //div = (1/2/a0             + 1/2/at                )
                //p   = (v0/2 - v0/a0       + vt/2 - vt/at          )/div
                //q   = (v0^2/2/a0 - v0^2/2 + vt^2/2/at - vt^2/2 -dx)/div
                // -> vx1/2 = pq(p,q)
                const float a0 = acc;
                const float at = dec;
                const float div = (1.f / 2.f / a0 + 1.f / 2.f / at);
                const float p = (v0 / 2.f - v0 / a0 + vt / 2.f - vt / at) / div;
                const float q =
                    (v0 * v0 / 2.f * (1.f / a0 - 1.f) + vt * vt / 2.f * (1.f / at - 1.f) - dx) /
                    div;
                const auto vxs = pq(p, q);
                //one or both of the results may be invalid
                bool vx1Valid = std::isfinite(vxs.first) && std::abs(vxs.first) <= std::abs(vmax);
                bool vx2Valid = std::isfinite(vxs.second) && std::abs(vxs.second) <= std::abs(vmax);
                switch (vx1Valid + vx2Valid)
                {
                    case 0:
                        return std::make_pair(false, std::array<deltas, 3>());
                    case 1:
                    {
                        float vx = vx1Valid ? vxs.first : vxs.second;
                        const deltas daccvx = accelerateToVelocity(v0, acc, vx);
                        const deltas ddecvx = accelerateToVelocity(vx, dec, vt);
                        deltas mid;
                        mid.dx = 0;
                        mid.dv = vx;
                        mid.dt = 0;
                        return std::make_pair(true, std::array<deltas, 3>{daccvx, mid, ddecvx});
                    }
                    case 2:
                    {
                        const deltas daccvx1 = accelerateToVelocity(v0, acc, vxs.first);
                        const deltas ddecvx1 = accelerateToVelocity(vxs.first, dec, vt);
                        const deltas daccvx2 = accelerateToVelocity(v0, acc, vxs.second);
                        const deltas ddecvx2 = accelerateToVelocity(vxs.second, dec, vt);
                        deltas mid;
                        mid.dx = 0;
                        mid.dt = 0;
                        if (daccvx1.dt + ddecvx1.dt < daccvx2.dt + ddecvx2.dt)
                        {
                            mid.dv = vxs.first;
                            return std::make_pair(true,
                                                  std::array<deltas, 3>{daccvx1, mid, ddecvx1});
                        }
                        mid.dv = vxs.second;
                        return std::make_pair(true, std::array<deltas, 3>{daccvx2, mid, ddecvx2});
                    }
                    default:
                        throw std::logic_error{"unreachable code (bool + bool neither 0,1,2)"};
                }
            }
            return std::make_pair(false, std::array<deltas, 3>());
        };
 
        const auto plusVMax = calc(vMax);
        const auto negVMax = calc(-vMax);
        switch (plusVMax.first + negVMax.first)
        {
            case 0:
                throw std::invalid_argument("could not find a trapez to reach the goal");
            case 1:
                return plusVMax.first ? plusVMax.second : negVMax.second;
            case 2:
            {
                const float dt1 =
                    plusVMax.second.at(0).dt + plusVMax.second.at(1).dt + plusVMax.second.at(2).dt;
                const float dt2 =
                    negVMax.second.at(0).dt + negVMax.second.at(1).dt + negVMax.second.at(2).dt;
                return dt1 < dt2 ? plusVMax.second : negVMax.second;
            }
            default:
                throw std::logic_error{"unreachable code (bool + bool neither 0,1,2)"};
        }
    }
 
    float
    PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition::run() const
    {
        //run should be static, but we need to adapt the parabs for the subcontroler -> have the subcontroler as value;
        PositionThroughVelocityControllerWithAccelerationBounds sub =
            *this; // slice parent class from this
        sub.currentPosition = periodicClamp(currentPosition, positionPeriodLo, positionPeriodHi);
        //        ARMARX_INFO << deactivateSpam(1) << VAROUT(currentPosition) << VAROUT(sub.currentPosition) << VAROUT(positionPeriodLo) << VAROUT(positionPeriodHi);
        sub.targetPosition = periodicClamp(targetPosition, positionPeriodLo, positionPeriodHi);
 
        const float brakingDist = brakingDistance(currentV, deceleration);
        //        const float posIfBrakingNow = currentPosition + brakingDist;
        //        const float posIfBrakingNowError = targetPosition - posIfBrakingNow;
        //        const float posError = targetPosition - currentPosition;
        //        if (
        //            std::abs(posIfBrakingNowError) <= pControlPosErrorLimit ||
        //            std::abs(posError) <= pControlPosErrorLimit
        //        )
        //        {
        //            //this allows slight overshooting (in the limits of the p controller)
        //            return PositionThroughVelocityControllerWithAccelerationBounds::run();
        //        }
 
        //we transform the problem with periodic bounds into
        //a problem with position bounds
        const float positionPeriodLength = std::abs(positionPeriodHi - positionPeriodLo);
 
        //we shift the positions such that 0 == target
        sub.currentPosition = periodicClamp(currentPosition - targetPosition,
                                            -positionPeriodLength * 0.5f,
                                            positionPeriodLength * 0.5f);
        //how many times will we go over the target if we simply brake now?
        const float overshoot = std::trunc((currentPosition + brakingDist) / positionPeriodLength);
 
        ///TODO check if there may be a case requiring the direction
        float direction = 0;
        if (direction == 0)
        {
            //determine the direction to go (1 == pos vel, -1 == neg vel)
            direction = (periodicClamp(currentPosition + brakingDist,
                                       -positionPeriodLength * 0.5f,
                                       positionPeriodLength * 0.5f) <= 0)
                            ? 1
                            : -1;
        }
        //shift the target away from 0
        sub.targetPosition =
            (overshoot * -direction) * positionPeriodLength; // - direction * pControlPosErrorLimit;
 
        //move
        return sub.run();
    }
 
    double
    PositionThroughVelocityControllerWithAccelerationRamps::getTargetPosition() const
    {
        return targetPosition;
    }
 
    bool
    PositionThroughVelocityControllerWithAccelerationBounds::getCurrentlyPIDActive() const
    {
        return currentlyPIDActive;
    }
 
    void
    PositionThroughVelocityControllerWithAccelerationRamps::setTargetPosition(double value)
    {
        if (std::abs(value - targetPosition) > 0.0001)
        {
            state = State::Unknown;
        }
        targetPosition = value;
    }
 
    PositionThroughVelocityControllerWithAccelerationRamps::
        PositionThroughVelocityControllerWithAccelerationRamps()
    {
    }
 
    bool
    PositionThroughVelocityControllerWithAccelerationRamps::validParameters() const
    {
        return maxV > 0 && acceleration > 0 && deceleration > 0 && jerk > 0 &&
               //                pControlPosErrorLimit > 0 &&
               //                pControlVelLimit > 0 &&
               p > 0;
    }
 
    PositionThroughVelocityControllerWithAccelerationRamps::Output
    PositionThroughVelocityControllerWithAccelerationRamps::run()
    {
        PositionThroughVelocityControllerWithAccelerationRamps::Output result;
        const double useddt = std::min(std::abs(dt), std::abs(maxDt));
        const double signV = sign(currentV);
        //we can have 3 cases:
        // 1. we use a p controller and ignore acc/dec (if |currentPosition - targetPosition| <= pControlPosErrorLimit AND |currentV| < pControlVelLimit)
        // 2. we need to accelerate (or hold vel)      (if e = (targetPosition - currentPosition)
        //                                                 the brakingDistance have the same sign and brakingDistance < e
        //                                                 and currentVel <= maxV)
        // 3. we need to decelerate                   (other cases)
 
        //handle case 1
        const double positionError = targetPosition - currentPosition;
        double newTargetVelPController = (positionError * p) * 0.5 + currentV * 0.5;
 
        //handle case 2-3
        auto brData = ctrlutil::braking_distance_with_wedge(currentV, currentAcc, jerk);
        if (brData.dt2 < 0)
        {
            brData = ctrlutil::braking_distance_with_wedge(currentV, currentAcc, jerk * 10);
        }
        const auto goalDir = math::MathUtils::Sign(targetPosition - currentPosition);
        //        State currentState;
        double usedAbsJerk;
        State newState;
        Output output;
        std::tie(newState, output) = calcState();
        usedAbsJerk = output.jerk;
        //        double newJerk = output.jerk;
        //        double newAcceleration = output.acceleration;
        //        ARMARX_INFO << "state: " << (int)state << " new state: " << (int)newState;
        state = newState;
        const double brakingDistance =
            brData.dt2 >= 0
                ? brData.s_total
                : signV * currentV * currentV / 2.f /
                      deceleration; //the braking distance points in the direction of the velocity
        const double posIfBrakingNow = currentPosition + brakingDistance;
        const double posErrorIfBrakingNow = targetPosition - posIfBrakingNow;
        const bool hardBrakingNeeded = std::abs(brakingDistance) > std::abs(positionError);
        //        const double usedJerk = jerk;//currentState==State::DecAccDecJerk?std::abs(ctrlutil::jerk(ctrlutil::t_to_v(currentV, -signV*currentAcc, signV*jerk), positionError, currentV, currentAcc))
        //                           :jerk;
        const double jerkDir =
            (newState == State::DecAccIncJerk || newState == State::IncAccDecJerk) ? -1.0f : 1.0f;
        const double usedJerk = goalDir * jerkDir * usedAbsJerk;
        const double deltaAcc = usedJerk * useddt;
        const double newAcceleration =
            (newState == State::Keep)
                ? currentAcc
                : (newState == State::DecAccDecJerk ? output.acceleration : currentAcc + deltaAcc);
        result.acceleration = newAcceleration;
        result.jerk = usedJerk;
 
        const double usedDeceleration =
            hardBrakingNeeded ? -std::abs(currentV * currentV / 2.f /
                                          math::MathUtils::LimitTo(positionError, 0.0001))
                              : newAcceleration;
        (void)usedDeceleration;
 
        const bool decelerate =
            std::abs(currentV) > maxV || // we need to slow down (to stay in [-maxV,maxV]
            hardBrakingNeeded ||
            sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
        (void)decelerate;
 
        const double deltaVel =
            ctrlutil::v(useddt, 0, newAcceleration, usedJerk); //signV * newAcceleration * useddt;
        (void)deltaVel;
 
        double newTargetVelRampCtrl =
            ctrlutil::v(useddt,
                        currentV,
                        currentAcc,
                        usedJerk); //boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV);
        newTargetVelRampCtrl = std::clamp(newTargetVelRampCtrl, -maxV, maxV);
        bool usePID = std::abs(newTargetVelPController) < std::abs(newTargetVelRampCtrl); //||
        //                      std::abs(newTargetVelPController) < pControlVelLimit &&
        //std::abs(positionError) < pControlPosErrorLimit;
        usePID |= state == State::PCtrl;
        usePID &= usePIDAtEnd;
        if (usePID)
        {
            state = State::PCtrl;
        }
        double finalTargetVel = usePID ? newTargetVelPController : newTargetVelRampCtrl;
        ARMARX_INFO << VAROUT(usePID) << VAROUT(result.jerk) << VAROUT(newTargetVelPController)
                    << VAROUT(currentV);
        //        ARMARX_INFO << VAROUT(currentV) << VAROUT(currentAcc) << VAROUT(usedJerk) << VAROUT(newAcceleration) << VAROUT(deltaVel) << VAROUT(finalTargetVel) << " new distance: " << (targetPosition - ctrlutil::s(dt, currentPosition, currentV, currentAcc, usedJerk));
        if (std::abs(positionError) < accuracy)
        {
            finalTargetVel =
                0.0f; // if close to target set zero velocity to avoid oscillating around target
        }
 
#ifdef DEBUG_POS_CTRL
        buffer.push_back({currentPosition,
                          newTargetVelPController,
                          newTargetVelRampCtrl,
                          currentV,
                          positionError,
                          IceUtil::Time::now().toMicroSeconds()});
 
        //        if (PIDModeActive != usePID)
        if (buffer.size() > 0 &&
            sign(positionError) * sign(buffer[buffer.size() - 2].currentError) < 0 &&
            eventHappeningCounter < 0)
        {
            eventHappeningCounter = 10;
            ARMARX_IMPORTANT << "HIGH VELOCITY  DETECTED";
        }
        if (eventHappeningCounter == 0)
        {
            ARMARX_IMPORTANT << "BUFFER CONTENT";
            for (auto& elem : buffer)
            {
                ARMARX_INFO << VAROUT(elem.currentPosition) << VAROUT(elem.targetVelocityPID)
                            << VAROUT(elem.targetVelocityRAMP) << VAROUT(elem.currentV)
                            << VAROUT(elem.currentError) << VAROUT(elem.timestamp);
            }
            ARMARX_IMPORTANT << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl)
                             << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError);
        }
        if (eventHappeningCounter >= 0)
        {
            eventHappeningCounter--;
        }
 
        ARMARX_INFO << deactivateSpam(5) << VAROUT(newTargetVelPController)
                    << VAROUT(newTargetVelRampCtrl) << VAROUT(currentPosition) << VAROUT(usePID)
                    << VAROUT(positionError) << VAROUT(targetPosition) << VAROUT(currentV)
                    << VAROUT(deltaVel) << VAROUT(useddt) << VAROUT(usedacc);
        PIDModeActive = usePID;
#endif
        result.velocity = finalTargetVel;
        return result;
    }
 
    double
    PositionThroughVelocityControllerWithAccelerationRamps::estimateTime() const
    {
        throw LocalException("NYI");
        return 0;
    }
 
    double
    PositionThroughVelocityControllerWithAccelerationRamps::calculateProportionalGain() const
    {
        /*            s = v_0*v_0/(2*a) -> sqrt(s*2*a) = v_0;
 
                        K_p * error = v_0; -> v_0/error = K_p;
                        */
        auto v_0 = std::sqrt(pControlPosErrorLimit * 2 * deceleration);
        return v_0 / pControlPosErrorLimit;
    }
 
    std::pair<PositionThroughVelocityControllerWithAccelerationRamps::State,
              PositionThroughVelocityControllerWithAccelerationRamps::Output>
    PositionThroughVelocityControllerWithAccelerationRamps::calcState() const
    {
        //        auto integratedChange = [](v0)
 
        //        double newJerk = this->jerk;
        //        double newAcc = currentAcc;
        //        double newVel = currentV;
        State newState = state;
        double a0 = currentAcc, newJerk = jerk, t;
        Output result = {currentV, a0, newJerk};
 
        const auto brData = ctrlutil::braking_distance_with_wedge(currentV, currentAcc, newJerk);
        const auto goalDir = math::MathUtils::Sign(targetPosition - currentPosition);
        const auto curVDir = math::MathUtils::Sign(currentV);
        const auto straightBrakingDistance =
            ctrlutil::brakingDistance(currentV, currentAcc, newJerk);
        const double brakingDistance = brData.dt2 < 0 ? straightBrakingDistance : brData.s_total;
        const double distance = std::abs(targetPosition - currentPosition);
        const double t_to_stop =
            ctrlutil::t_to_v(currentV, -curVDir * currentAcc, curVDir * newJerk);
        const auto calculatedJerk =
            std::abs(ctrlutil::jerk(t_to_stop, distance, currentV, currentAcc));
        double tmp_t, tmp_acc, tmp_jerk;
        //        std::tie( tmp_t, tmp_acc, tmp_jerk) = ctrlutil::calcAccAndJerk(targetPosition-(currentPosition+brData.s1), brData.v1);
        std::tie(tmp_t, tmp_acc, tmp_jerk) =
            ctrlutil::calcAccAndJerk(targetPosition - currentPosition, currentV);
        const double posWhenBrakingWithCustomJerk =
            ctrlutil::s(tmp_t, currentPosition + brData.s1, brData.v1, tmp_acc, curVDir * tmp_jerk);
        //        const double brakingDistanceWithCustomJerk = ctrlutil::s(tmp_t, 0, currentV, tmp_acc, curVDir * tmp_jerk);
        const double posWhenBrakingWithFixedJerk = ctrlutil::s(
            brData.dt2, currentPosition + brData.s1, brData.v1, brData.a1, curVDir * jerk);
        (void)calculatedJerk, (void)posWhenBrakingWithCustomJerk, (void)posWhenBrakingWithFixedJerk;
 
        ARMARX_INFO
            << VAROUT((int)state) << VAROUT(distance)
            << VAROUT(
                   brakingDistance); //VAROUT(straightBrakingDistance) << VAROUT(brData.dt1) << VAROUT(brData.dt2) << VAROUT(brData.s_total) << VAROUT(calculatedJerk);
 
        //        if(brData.dt2 < 0)
        //        {
        //        ARMARX_INFO << "distance with fixed jerk: " << ctrlutil::brakingDistance(currentV, currentAcc, newJerk)
        //                       << " distance with custom jerk: " << ctrlutil::brakingDistance(currentV, currentAcc, calculatedJerk)
        //                        << " vel after: " << ctrlutil::v(t_to_stop, currentV, currentAcc, -curVDir * newJerk) << " vel after next step: " << ctrlutil::v(dt, currentV, currentAcc, -curVDir * newJerk);
        ////                        << " acc at stop: " << ctrlutil::a(t_to_stop, currentAcc, -curVDir *newJerk);
        //            jerk = 0.95*calculatedJerk;
        //            state = State::DecAccDecJerk;
        //            return;
        //        }
        if (state < State::DecAccIncJerk &&
            ((distance > brakingDistance && (math::MathUtils::Sign(currentAcc) == curVDir ||
                                             std::abs(currentAcc) < 0.1)) // we are accelerating
             || distance > brakingDistance *
                               2 // we are decelerating -> dont switch to accelerating to easily
             || curVDir != goalDir)) // we are moving away from goal
        {
            if (std::abs(maxV - currentV) < 0.1 && std::abs(currentAcc) < 0.1)
            {
                newState = State::Keep;
                newJerk = 0;
                return std::make_pair(newState, result);
            }
            // calculate if we need to increase or decrease acc
            double t_to_max_v =
                ctrlutil::t_to_v(goalDir * maxV - currentV, currentAcc, goalDir * newJerk);
            double acc_at_t = ctrlutil::a(t_to_max_v, std::abs(currentAcc), -newJerk);
            if (acc_at_t < 0.1)
            {
                newState = State::IncAccIncJerk;
                return std::make_pair(newState, result);
            }
            else
            {
                newState = State::IncAccDecJerk;
                return std::make_pair(newState, result);
            }
        }
        else
        {
            // calculate if we need to increase or decrease acc
            double t_to_0 = ctrlutil::t_to_v(currentV, -curVDir * currentAcc, curVDir * newJerk);
            double tmpV = ctrlutil::v(t_to_0, 0, currentAcc, -curVDir * newJerk);
            double vAfterBraking = ctrlutil::v(t_to_0, currentV, currentAcc, -curVDir * newJerk);
            double accAfterBraking = ctrlutil::a(t_to_0, currentAcc, curVDir * newJerk);
            const double posWhenBrakingNow =
                ctrlutil::s(t_to_0, currentPosition, currentV, currentAcc, -curVDir * newJerk);
            const double simpleBrakingDistance =
                ctrlutil::s(t_to_0, 0, currentV, currentAcc, -curVDir * newJerk);
            (void)tmpV, (void)vAfterBraking, (void)accAfterBraking, (void)posWhenBrakingNow,
                (void)simpleBrakingDistance;
            double acc_at_t = ctrlutil::a(t_to_0, std::abs(currentAcc), -newJerk);
 
            std::tie(t, a0, newJerk) =
                ctrlutil::calcAccAndJerk(targetPosition - currentPosition, currentV);
            double at = ctrlutil::a(t, a0, newJerk);
            double vt = ctrlutil::v(t, currentV, a0, newJerk);
            double st = ctrlutil::s(t, targetPosition - currentPosition, currentV, a0, newJerk);
            (void)at, (void)vt, (void)st;
 
            if (this->state <= State::Keep)
            {
                newState = State::DecAccIncJerk;
                return std::make_pair(newState, result);
            }
            else if ((state == State::DecAccIncJerk && acc_at_t > 1) ||
                     state == State::DecAccDecJerk)
            {
                result.jerk = newJerk;
                result.acceleration = currentAcc + (a0 - currentAcc) * 0.5 + dt * newJerk;
                newState = State::DecAccDecJerk;
                return std::make_pair(newState, result);
            }
            else
            {
                return std::make_pair(newState, result);
            }
        }
        throw LocalException();
    }
 
    void
    MinJerkPositionController::reset()
    {
        currentTime = 0;
 
        fixedMinJerkState.reset();
        //        pid.reset();
        currentP_Phase2 = -1;
        currentP_Phase3 = -1;
    }
 
    double
    MinJerkPositionController::getTargetPosition() const
    {
        return targetPosition;
    }
 
    //    MinJerkPositionController::Output MinJerkPositionController::run()
    //    {
    //        auto min_jerk_calc_jerk = [&](double tf, double s0, double v0, double a0, double xf = 0, double t0 = 0.0)
    //        {
    //            double D = tf - t0;
    //            return 60.0 / (D * D * D) * (xf - s0) - 36.0 / (D * D) * v0 - 9.0 / D * a0;
    //        };
 
 
    //        auto min_jerk = [&](double t, double tf, double s0, double v0, double a0, double xf = 0, double t0 = 0.0)
    //        {
    //            double D = tf - t0;
    //            BOOST_ASSERT(D != 0.0);
    //            double D2 = D * D;
    //            double tau = (t - t0) / D;
    //            double b0 = s0;
    //            double b1 = D * v0;
    //            double b2 = D2 * a0 / 2;
 
    //            double b3 = (-3 * D2) / 2 * a0 - 6 * D * v0 + 10 * (xf - s0);
    //            double b4 = (3 * D2) / 2 * a0 + 8 * D * v0 - 15 * (xf - s0);
    //            double b5 = (-D2) / 2 * a0 - 3 * D * v0 + 6 * (xf - s0);
 
    //            double tau2 = tau * tau;
    //            double tau3 = tau2 * tau;
    //            double tau4 = tau3 * tau;
    //            double tau5 = tau4 * tau;
    //            double st = b0 + b1 * tau + b2 * tau2 + b3 * tau3 + b4 * tau4 + b5 * tau5;
    //            double vt = b1 / D + 2 * b2 / D * tau + 3 * b3 / D * tau2 + 4 * b4 / D * tau3 + 5 * b5 / D * tau4;
    //            double at = 2 * b2 / D2 + 6 * b3 / D2 * tau + 12 * b4 / D2 * tau2 + 20 * b5 / D2 * tau3;
    //            return State{t, st, vt, at};
    //        };
 
    //        double remainingTime = finishTime - currentTime;
    //        double signedDistance = targetPosition - currentPosition;
    //        if (remainingTime <= 0.0)
    //        {
    //            //            if(!pid)
    //            //            {
    //            //                double Kd = (currentV - distance * p) /(-currentV);
    //            //                ARMARX_INFO << VAROUT(Kd);
    //            //                pid.reset(new PIDController(1, 0, Kd));
    //            //            }
    //            if (currentP_Phase3 < 0)
    //            {
    //                currentP_Phase3 = std::abs(currentV / signedDistance);
    //                ARMARX_DEBUG << "optimal P: " << currentP_Phase3;
    //            }
 
 
    //            currentP_Phase3 = currentP_Phase3 * p_adjust_ratio + p * (1.0 - p_adjust_ratio);
    //            double newVel = signedDistance * currentP_Phase3;
    //            double newAcc = (newVel - currentV) / dt;
    //            double newJerk = (newAcc - currentAcc) / dt;
    //            double newPos = currentPosition + newVel * dt;
    //            Output result {newPos, newVel, newAcc, newJerk};
    //            currentTime += dt;
    //            //            State s = min_jerk(currentTime, finishTime, fixedMinJerkState->s0, fixedMinJerkState->v0, fixedMinJerkState->a0, targetPosition, fixedMinJerkState->t0);
    //            ARMARX_DEBUG << VAROUT(remainingTime) << VAROUT(signedDistance) << VAROUT(signedDistance * currentP_Phase3) << VAROUT(currentP_Phase3);// << VAROUT(s.s) << VAROUT(s.v) << VAROUT(s.a);
    //            return result;
    //        }
 
    //        if (std::abs(signedDistance) < phase2SwitchDistance && remainingTime < phase2SwitchMaxRemainingTime)
    //        {
    //            if (!fixedMinJerkState)
    //            {
    //                fixedMinJerkState.reset(new FixedMinJerkState{currentTime, currentPosition, currentV, currentAcc});
    //                currentP_Phase2 = 0;
    //            }
    //            //            std::vector<State> states;
    //            //            double t = currentTime;
    //            //            while(t < finishTime)
    //            //            {
    //            //                State s = min_jerk(t, finishTime, fixedMinJerkState->s0, fixedMinJerkState->v0, fixedMinJerkState->a0, targetPosition, fixedMinJerkState->t0);
    //            //                t += dt;
    //            //                states.push_back(s);
    //            //            }
    //            currentTime += dt;
    //            State s = min_jerk(currentTime, finishTime, fixedMinJerkState->s0, fixedMinJerkState->v0, fixedMinJerkState->a0, targetPosition, fixedMinJerkState->t0);
    //            currentP_Phase2 = currentP_Phase2 * p_adjust_ratio + p * (1.0 - p_adjust_ratio);
    //            double newVel = (s.s - currentPosition) * currentP_Phase2 + s.v;
    //            ARMARX_DEBUG << VAROUT(s.s) << VAROUT(s.v) << VAROUT(newVel);
    //            double newAcc = (newVel - currentV) / dt;
    //            double newJerk = (newAcc - currentAcc) / dt;
    //            double newPos = currentPosition + newVel * dt;
    //            Output result {newPos, newVel, newAcc, newJerk};
    //            return result;
    //        }
 
 
    //        double jerk = min_jerk_calc_jerk(remainingTime, currentPosition, currentV, currentAcc, targetPosition);
    //        double newAcc = currentAcc + jerk * dt;
    //        double newVelocity = ctrlutil::v(dt, currentV, currentAcc, jerk);
    //        double accAtEnd = ctrlutil::a(remainingTime, currentAcc, jerk);
 
    //        std::vector<State> states;
    //        std::vector<State> states2;
    //        //#define debug
    //#ifdef  debug
    //        {
    //            double dt = 0.001;
    //            double t = 1;
    //            double simS = currentPosition, simV = currentV, simAcc = currentAcc;
    //            while (t > 0)
    //            {
    //                double jerk = min_jerk_calc_jerk(t, simS, simV, simAcc, targetPosition);
    //                simAcc = simAcc + jerk * dt;
    //                simV = ctrlutil::v(dt, simV, simAcc, 0);
    //                simS += dt * simV;
    //                t -= dt;
    //                states.push_back(State{t, simS, simV, simAcc, jerk});
    //            }
    //        }
    //        {
    //            double dt = 0.001;
 
    //            double t = 1;
    //            double simS = currentPosition, simV = currentV, simAcc = currentAcc;
    //            while (t > 0)
    //            {
    //                double jerk = min_jerk_calc_jerk(t, simS, simV, simAcc, targetPosition);
    //                simS += ctrlutil::s(dt, 0, simV, simAcc, jerk);
    //                simV = ctrlutil::v(dt, simV, simAcc, jerk);
    //                simAcc = simAcc + jerk * dt;
    //                t -= dt;
    //                states2.push_back(State{t, simS, simV   , simAcc, jerk});
    //            }
    //        }
    //#endif
    //        ARMARX_DEBUG << VAROUT(currentTime) << VAROUT(remainingTime) << VAROUT(jerk) << VAROUT(accAtEnd) << VAROUT(signedDistance) << " delta acc: " << jerk* dt;
    //        currentTime += dt;
 
 
    //        double newPos = ctrlutil::s(dt, currentPosition, currentV, currentAcc, jerk);
    //        Output result { newPos, newVelocity, newAcc, jerk};
    //        return result;
 
    //    }
 
    bool
    VelocityControllerWithRampedAccelerationAndPositionBounds::validParameters() const
    {
        return VelocityControllerWithRampedAcceleration::validParameters() &&
               positionLimitHiSoft > positionLimitLoSoft && deceleration > 0;
    }
 
    VelocityControllerWithRampedAcceleration::Output
    VelocityControllerWithRampedAccelerationAndPositionBounds::run() const
    {
        const double useddt = std::min(std::abs(dt), std::abs(maxDt));
        (void)useddt;
 
        //        if (currentPosition <= positionLimitLoHard || currentPosition >= positionLimitHiHard)
        //        {
        //            ARMARX_INFO << deactivateSpam(1) << "Hard limit violation. " << VAROUT(currentPosition) << VAROUT(positionLimitLoHard) << VAROUT(positionLimitHiHard);
        //            return std::nanf("1");
        //        }
 
        double softLimitViolation = 0;
        if (currentPosition <= positionLimitLoSoft)
        {
            softLimitViolation = -1;
        }
        if (currentPosition >= positionLimitHiSoft)
        {
            softLimitViolation = 1;
        }
 
        const double upperDt = std::max(std::abs(dt), std::abs(maxDt));
 
        //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.g. if we violated them initially)
        // 2. we directly set v and ignore acc/dec (if |curr - target| <= limit)
        // 3. we need to accelerate                (if curr and target v have same sign and |curr| < |target|)
        // 4. we need to decelerate               (other cases)
        double nextv;
        //handle case 1
        const double vsquared = currentV * currentV;
        const double brakingDist =
            sign(currentV) * vsquared / 2.f /
            std::abs(deceleration); //the braking distance points in the direction of the velocity
 
        const double posIfBrakingNow = currentPosition + brakingDist;
        //        ARMARX_INFO << deactivateSpam(0.1) << VAROUT(currentPosition) << VAROUT(positionLimitLoSoft) << VAROUT(positionLimitHiSoft) << VAROUT(posIfBrakingNow) << VAROUT(currentV);
        if ((posIfBrakingNow <= positionLimitLoSoft && currentV < 0) ||
            (posIfBrakingNow >= positionLimitHiSoft && currentV > 0))
        {
            //case 1. -> brake now! (we try to have v=0 at the limit)
            const auto limit =
                posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
            const double wayToGo = limit - currentPosition;
 
            //decelerate!
            // s = v²/(2a)  <=>  a = v²/(2s)
            const double dec = std::abs(vsquared / 2.0 / wayToGo);
            const double vel = currentV - sign(currentV) * dec * upperDt;
            nextv = std::clamp<double>(vel, -maxV, maxV);
            //            ARMARX_INFO /*<< deactivateSpam(0.1)*/ << "clamped new Vel: " << VAROUT(nextv) << VAROUT(currentV);
            if (sign(currentV) != sign(nextv))
            {
                //                ARMARX_INFO << deactivateSpam(1) << "wrong sign: stopping"; //stop now
                nextv = 0;
            }
        }
        else
        {
            //handle 2-3
            return VelocityControllerWithRampedAcceleration::run();
            //            ARMARX_INFO << deactivateSpam(1) << "Desired target Vel: " << targetV << " " << VAROUT(nextv);
        }
        if (softLimitViolation == sign(nextv) && nextv != 0)
        {
            //the area between soft and hard limits is sticky
            //the controller can only move out of it (not further in)
            //            ARMARX_INFO << deactivateSpam(1) << "Soft limit violation. " << softLimitViolation << VAROUT(nextv);
            nextv = 0;
        }
        //        double nextAcc = (nextv - currentV) / useddt; // doesnt work with calculated acc!!
        //        double nextJerk = (nextAcc - currentAcc) / useddt;
        //the next velocity will not violate the pos bounds harder than they are already
        return Output{nextv, 0, 0};
    }
 
    void
    MinJerkPositionController::setTargetPosition(double newTargetPosition)
    {
        if (std::abs(targetPosition - newTargetPosition) > 0.0001)
        {
            reset();
            double distance = newTargetPosition - currentPosition;
            double rawFinishTime = 0.0;
            double decelerationTime = std::abs(currentV / desiredDeceleration);
            // calculate the time it should take to reach the goal (ToDo: find exact time)
            if (math::MathUtils::Sign(currentV) == math::MathUtils::Sign(distance) &&
                std::abs(currentV) > 0.0 &&
                ctrlutil::s(decelerationTime, 0, std::abs(currentV), -desiredDeceleration, 0) >
                    std::abs(distance))
            //                    && ctrlutil::brakingDistance(currentV, currentAcc, desiredJerk) < distance)
            {
                //                ARMARX_INFO << "branch 1: " << VAROUT(currentV) << VAROUT(desiredDeceleration) << VAROUT(distance);
                rawFinishTime = decelerationTime;
            }
            else
            {
                //                ARMARX_INFO << "branch 2: " << VAROUT(currentV) << VAROUT(desiredDeceleration) << VAROUT(distance);
                double accelerationTime = std::abs(
                    (math::MathUtils::Sign(distance) * maxV - currentV) / desiredDeceleration);
                double decelerationTime = std::abs(maxV / desiredDeceleration);
                rawFinishTime = std::max(decelerationTime + accelerationTime,
                                         std::abs(distance / (maxV * 0.75)));
            }
            double estimatedTime = ctrlutil::t_to_v_with_wedge_acc(
                std::abs(distance), std::abs(currentV), std::abs(desiredDeceleration));
            double minTime = 0.8;
            if (!std::isnan(estimatedTime))
            {
                finishTime = std::max(estimatedTime, minTime);
            }
            else
            {
                finishTime = std::max(rawFinishTime, minTime);
            }
            targetPosition = newTargetPosition;
            //            ARMARX_INFO << "New finish time: " << finishTime << " " << VAROUT(rawFinishTime) << VAROUT(estimatedTime);
        }
    }
 
    MinJerkPositionController::Output
    MinJerkPositionController::run()
    {
        auto min_jerk_calc_jerk =
            [&](double tf, double s0, double v0, double a0, double xf = 0, double t0 = 0.0)
        {
            double D = tf - t0;
            return 60.0 / (D * D * D) * (xf - s0) - 36.0 / (D * D) * v0 - 9.0 / D * a0;
        };
 
 
        auto min_jerk = [&](double t,
                            double tf,
                            double s0,
                            double v0,
                            double a0,
                            double xf = 0,
                            double t0 = 0.0)
        {
            double D = tf - t0;
            ARMARX_CHECK(D != 0.0);
            double D2 = D * D;
            double tau = (t - t0) / D;
            double b0 = s0;
            double b1 = D * v0;
            double b2 = D2 * a0 / 2;
 
            double b3 = (-3 * D2) / 2 * a0 - 6 * D * v0 + 10 * (xf - s0);
            double b4 = (3 * D2) / 2 * a0 + 8 * D * v0 - 15 * (xf - s0);
            double b5 = (-D2) / 2 * a0 - 3 * D * v0 + 6 * (xf - s0);
 
            double tau2 = tau * tau;
            double tau3 = tau2 * tau;
            double tau4 = tau3 * tau;
            double tau5 = tau4 * tau;
            double st = b0 + b1 * tau + b2 * tau2 + b3 * tau3 + b4 * tau4 + b5 * tau5;
            double vt = b1 / D + 2 * b2 / D * tau + 3 * b3 / D * tau2 + 4 * b4 / D * tau3 +
                        5 * b5 / D * tau4;
            double at = 2 * b2 / D2 + 6 * b3 / D2 * tau + 12 * b4 / D2 * tau2 + 20 * b5 / D2 * tau3;
            return State{t, st, vt, at};
        };
 
        double remainingTime = finishTime - currentTime;
        double signedDistance = targetPosition - currentPosition;
        if (remainingTime <= 0.0)
        {
            //            if(!pid)
            //            {
            //                double Kd = (currentV - distance * p) /(-currentV);
            //                ARMARX_INFO << VAROUT(Kd);
            //                pid.reset(new PIDController(1, 0, Kd));
            //            }
            if (currentP_Phase3 < 0)
            {
                if (signedDistance == 0.0f)
                {
                    currentP_Phase3 = p;
                }
                else
                {
                    currentP_Phase3 = std::abs(currentV / signedDistance);
                    currentP_Phase3 = math::MathUtils::LimitTo(currentP_Phase3, p * 10);
                }
                //                ARMARX_INFO << deactivateSpam(0.1) << "optimal P: " << currentP_Phase3 << VAROUT(currentV) << VAROUT(signedDistance);
            }
 
 
            currentP_Phase3 = currentP_Phase3 * p_adjust_ratio + p * (1.0 - p_adjust_ratio);
            double newVel = signedDistance * currentP_Phase3;
            double newAcc = (newVel - currentV) / dt;
            double newJerk = (newAcc - currentAcc) / dt;
            double newPos = currentPosition + newVel * dt;
 
            Output result{newPos, newVel, newAcc, newJerk};
            currentTime += dt;
            //            State s = min_jerk(currentTime, finishTime, fixedMinJerkState->s0, fixedMinJerkState->v0, fixedMinJerkState->a0, targetPosition, fixedMinJerkState->t0);
            //            ARMARX_INFO << deactivateSpam(0.1) << VAROUT(remainingTime) << VAROUT(signedDistance) << VAROUT(signedDistance * currentP_Phase3) << VAROUT(currentP_Phase3);// << VAROUT(s.s) << VAROUT(s.v) << VAROUT(s.a);
            return result;
        }
 
        if (std::abs(signedDistance) < phase2SwitchDistance &&
            remainingTime < phase2SwitchMaxRemainingTime)
        {
            if (!fixedMinJerkState)
            {
                fixedMinJerkState.reset(
                    new FixedMinJerkState{currentTime, currentPosition, currentV, currentAcc});
                currentP_Phase2 = 0;
            }
            //            std::vector<State> states;
            //            double t = currentTime;
            //            while(t < finishTime)
            //            {
            //                State s = min_jerk(t, finishTime, fixedMinJerkState->s0, fixedMinJerkState->v0, fixedMinJerkState->a0, targetPosition, fixedMinJerkState->t0);
            //                t += dt;
            //                states.push_back(s);
            //            }
            currentTime += dt;
            State s = min_jerk(currentTime,
                               finishTime,
                               fixedMinJerkState->s0,
                               fixedMinJerkState->v0,
                               fixedMinJerkState->a0,
                               targetPosition,
                               fixedMinJerkState->t0);
            double newVelocity = (s.s - (currentPosition + s.v * dt)) * currentP_Phase2 + s.v;
            double newAcc = (newVelocity - currentV) / dt;
            double newJerk = (newAcc - currentAcc) / dt;
            double newPos = ctrlutil::s(dt,
                                        currentPosition,
                                        currentV,
                                        currentAcc,
                                        newJerk); //currentPosition + newVelocity * dt;
            //            ARMARX_INFO /*<< deactivateSpam(0.1) */ << VAROUT(currentP_Phase2) << VAROUT(currentPosition) << VAROUT(signedDistance) << VAROUT(s.s) << VAROUT(s.v) << VAROUT(newVelocity) << VAROUT(newAcc) << VAROUT(newJerk) << VAROUT(s.a);
            currentP_Phase2 = currentP_Phase2 * p_adjust_ratio + p * (1.0 - p_adjust_ratio);
            Output result{newPos, newVelocity, newAcc, newJerk};
            return result;
        }
 
 
        double jerk = min_jerk_calc_jerk(
            remainingTime, currentPosition, currentV, currentAcc, targetPosition);
        double newAcc = currentAcc + jerk * dt;
        double newVelocity = ctrlutil::v(dt, currentV, currentAcc, jerk);
        double accAtEnd =
            ctrlutil::a(remainingTime,
                        currentAcc,
                        jerk); // assumes that fixed jerk would be used, which is not the case here
        (void)accAtEnd;
 
        std::vector<State> states;
        std::vector<State> states2;
        //#define debug
#ifdef debug
        {
            double dt = 0.001;
            double t = 1;
            double simS = currentPosition, simV = currentV, simAcc = currentAcc;
            while (t > 0)
            {
                double jerk = min_jerk_calc_jerk(t, simS, simV, simAcc, targetPosition);
                simAcc = simAcc + jerk * dt;
                simV = ctrlutil::v(dt, simV, simAcc, 0);
                simS += dt * simV;
                t -= dt;
                states.push_back(State{t, simS, simV, simAcc, jerk});
            }
        }
        {
            double dt = 0.001;
 
            double t = 1;
            double simS = currentPosition, simV = currentV, simAcc = currentAcc;
            while (t > 0)
            {
                double jerk = min_jerk_calc_jerk(t, simS, simV, simAcc, targetPosition);
                simS += ctrlutil::s(dt, 0, simV, simAcc, jerk);
                simV = ctrlutil::v(dt, simV, simAcc, jerk);
                simAcc = simAcc + jerk * dt;
                t -= dt;
                states2.push_back(State{t, simS, simV, simAcc, jerk});
            }
        }
#endif
        //        ARMARX_INFO << /*deactivateSpam(0.1) <<*/ VAROUT(currentTime) << VAROUT(remainingTime) << VAROUT(jerk) << VAROUT(accAtEnd) << VAROUT(signedDistance) << " new acc: " << newAcc << VAROUT(currentV);
        currentTime += dt;
 
 
        double newPos = ctrlutil::s(dt, currentPosition, currentV, currentAcc, jerk);
        Output result{newPos, newVelocity, newAcc, jerk};
        return result;
    }
} // namespace armarx