CtrlUtil.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http{//www.gnu.org/licenses/>.
 *
 * @package    ArmarX
 * @author     Mirko Waechter( mirko.waechter at kit dot edu)
 * @date       2019
 * @copyright  http{//www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
//pragma once
#include <cmath>
#include <iostream>
 
#include <RobotAPI/libraries/core/math/MathUtils.h>
 
namespace armarx::ctrlutil
{
    double
    s(double t, double s0, double v0, double a0, double j)
    {
        return s0 + v0 * t + 0.5 * a0 * t * t + 1.0 / 6.0 * j * t * t * t;
    }
 
    double
    v(double t, double v0, double a0, double j)
    {
        return v0 + a0 * t + 0.5 * j * t * t;
    }
 
    double
    a(double t, double a0, double j)
    {
        return a0 + j * t;
    }
 
    double
    t_to_v(double v, double a, double j)
    {
        // time to accelerate to v with jerk j starting at acceleration a0
        //return 2*math.sqrt(a0**2+2*j*(v/2))-a0/j
        // 0 = (-v + a0*t) /(0.5*j) + t*t
        // 0 = -2v/j + 2a0t/j + t*t
        // --> p = 2a0/j; q = -2v/j
        // t = - a0/j +- sqrt((a0/j)**2 +2v/j)
        double tmp = a / j;
        return -a / j + std::sqrt(tmp * tmp + 2 * v / j);
    }
 
    double
    t_to_v_with_wedge_acc(double v, double a0, double j)
    {
        // ramp up and down of acceleration (i.e. a0=at); returns t to achieve delta v
        return 2 * t_to_v(v / 2.0, a0, j);
    }
 
    struct BrakingData
    {
        double s_total, v1, v2, v3, dt1, dt2, dt3;
    };
 
    double
    brakingDistance(double v0, double a0, double j)
    {
        auto signV = math::MathUtils::Sign(v0);
        auto t = t_to_v(v0, -signV * a0, signV * j);
        return s(t, 0, v0, a0, -signV * j);
    }
 
    BrakingData
    brakingDistance(double goal,
                    double s0,
                    double v0,
                    double a0,
                    double v_max,
                    double a_max,
                    double j,
                    double dec_max)
    {
        double d = math::MathUtils::Sign(goal - s0); // if v0 == 0.0 else abs(v0)/a_max
        dec_max *= -d;
        //        std::cout << "dec max: " << (dec_max) << " d: " << d << std::endl;
        double dt1 = std::abs((dec_max - a0) / (-j * d));
        //        double dt1 = t_to_v(v_max-v0, a0, j);
        //        double a1 = a(dt1, a0, -d * j);
        double v1 = v(dt1, v0, a0, -d * j);
        double acc_duration = std::abs(dec_max) / j;
        double v2 =
            v(acc_duration, 0, 0, d * j); //  # inverse of time to accelerate from 0 to max v
        v2 = math::MathUtils::LimitTo(v2, v_max);
        // v2 = v1 + dv2
        double dt2 = d * ((v1 - v2) / dec_max);
        //        double a2 = a(dt2, a1, 0);
 
        double dt3 = t_to_v(std::abs(v2), 0, j);
        double s1 = s(dt1, 0, v0, a0, d * j);
        double s2 = s(dt2, 0, v1, dec_max, 0);
        double s3 = s(dt3, 0, v2, dec_max, d * j);
        double v3 = v(dt3, v2, dec_max, d * j);
        double s_total = s1 + s2 + s3;
 
        if (dt2 < 0) // # we dont have a phase with a=a_max and need to reduce the a_max
        {
            double excess_time = t_to_v_with_wedge_acc(std::abs(v1), std::abs(dec_max), j);
            double delta_a = a(excess_time / 2, 0, d * j);
            a_max -= d * delta_a;
            dec_max = std::abs(dec_max) - std::abs(delta_a);
            dec_max *= -d;
            return brakingDistance(goal, s0, v0, a0, v_max, a_max, j, dec_max);
        }
 
        return {s_total * d, v1, v2, v3, dt1, dt2, dt3};
        //        return (s_total, v1, v2, v3, dt1, dt2, dt3);
    }
 
    struct WedgeBrakingData
    {
        double s_total, s1, s2, v1, v2, a1, a2, t, dt1, dt2;
    };
 
    bool
    isPossibleToBrake(double v0, double a0, double j)
    {
        return j * v0 - a0 * a0 / 2 > 0.0f;
    }
 
    double
    jerk(double t, double s0, double v0, double a0)
    {
        return (6 * s0 - 3 * t * (a0 * t + 2 * v0)) / (t * t * t);
    }
 
    std::tuple<double, double, double>
    calcAccAndJerk(double s0, double v0)
    {
        s0 *= -1;
        double t = -(3 * s0) / v0;
        double a0 = -(2 * v0) / t;
        double j = 2 * v0 / (t * t);
        return std::make_tuple(t, a0, j);
    }
 
    WedgeBrakingData
    braking_distance_with_wedge(double v0, double a0, double j)
    {
        //        # v0 = v1 + v2
        //        # v1 = t1 * a0 + 0.5*j*t1**2
        //        # v2 = 0.5*j*t2**2
        //        # a1 = t2 * j ^ a1 = a0 - t1 * j -> t2 * j = a0 - t1 * j
        //        # t2 = (a0 - t1 * j) / j
        //        # t2 = a0/j - t1
        //        # t1_1 = -(math.sqrt(2*j**2 * (a0**2 + 2*j*v0)) + 2*a0*j)/(2*j**2)
        //        # t1_2 = (math.sqrt(2*j**2 * (a0**2 + 2*j*v0)) - 2*a0*j)/(2*j**2)
        //        # t1 = t1_2
        double d = math::MathUtils::Sign(v0);
        v0 *= d;
        a0 *= d;
        //        j *= d;
        double part = j * v0 - a0 * a0 / 2.0;
        if (part < 0)
        {
            WedgeBrakingData r;
            r.s_total = -1;
            r.dt2 = -1;
            return r;
            throw std::runtime_error("unsuitable parameters! : j: " + std::to_string(j) +
                                     " a0: " + std::to_string(a0) + " v0: " + std::to_string(v0));
        }
        double t1 = std::sqrt(part) / j;
        double t2 = (a0 / j) + t1;
        double v1 = v(t1, v0, a0, -j);
        //        double dv1 = v(t1, 0, a0, -j);
        //        double diff_v1 = v0 - v1;
        double a1 = a(t1, -a0, -j);
        //        double a1_2 = a(t2, 0, j);
        double v2 = v(t2, v1, a1, j);
        //        double dv2 = v(t2, 0, 0, j);
        //        double v_sum = abs(dv1)+dv2;
        double a2 = a(t2, a1, j);
        //        assert(abs(v_sum - v0) < 0.001);
        //        assert(abs(v2) < 0.0001);
        double s1 = s(t1, 0, v0, a0, -j);
        double s2 = s(t2, 0, v1, a1, j);
        double s_total = s1 + s2;
        return {s_total, s1, s2, d * v1, d * v2, d * a1, d * a2, t1 + t2, t1, t2};
    }
} // namespace armarx::ctrlutil