Torque.cpp

Go to the documentation of this file.

#include "Torque.h"
 
 
// armarx
#include <RobotAPI/libraries/core/math/MathUtils.h>
 
 
namespace armarx::control::joint_controller
{
 
    TorqueController::TorqueController(const TorqueControllerConfigurationPtr& torqueConfigData) :
        pid(),
        targetFilter(100),
        configData(
            TorqueControllerConfigurationPtr(new TorqueControllerConfiguration(*torqueConfigData)))
    {
        pid.proportional_gain = torqueConfigData->pid_proportional_gain;
        pid.integral_gain = torqueConfigData->pid_integral_gain;
        pid.derivative_gain = torqueConfigData->pid_derivative_gain;
        pid.windup_guard = torqueConfigData->pid_windup_guard;
        pid.max_value = torqueConfigData->pid_max_value;
        pid.min_value = torqueConfigData->pid_min_value;
        pid.dis = torqueConfigData->pid_dis;
        pid.Kd = torqueConfigData->Kd;
        pid.inertia = torqueConfigData->inertia;
        pid.scalePI = torqueConfigData->scalePI;
        pid.scaleTorque = torqueConfigData->scaleTorque;
        pid.maxTorque = torqueConfigData->maxTorque;
        pid.actuatorType = torqueConfigData->actuatorType;
        filter.setImpulseResponse({// lowpass FIR-filter
                                   0.1744f, 0.0205f, 0.0215f, 0.0225f, 0.0235f, 0.0244f, 0.0252f,
                                   0.0259f, 0.0265f, 0.0270f, 0.0274f, 0.0276f, 0.0278f, 0.0282f,
                                   0.0282f, 0.0278f, 0.0276f, 0.0274f, 0.0270f, 0.0265f, 0.0259f,
                                   0.0252f, 0.0244f, 0.0235f, 0.0225f, 0.0215f, 0.0205f, 0.1744f});
    }
 
 
    TorquePID::TorquePID()
    {
        zeroize();
    }
 
 
    void
    TorquePID::zeroize()
    {
        prev_error = 0;
        prev_prev_error = 0;
        int_error = 0;
        phi = 0;
        old_pos = 0;
        abs_vel = 0;
        prev_angle = 0;
        acc = 0;
    }
 
 
    void
    TorquePID::findAcc(double Vel, double dt)
    {
        acc = (Vel - prev_vel) / dt;
        prev_vel = Vel;
    }
 
 
    void
    TorquePID::update(double curr_error, double dt)
    {
        // ==================================================== Digital Controller
        // PID-Controller
        double Kp = proportional_gain;
        double Ki = integral_gain;
        double Kd = derivative_gain;
        double er0 = (Kp + (dt * Ki) + (Kd / dt)) * curr_error;
        double er1 = (-Kp - ((2 * Kd) / dt)) * prev_error;
        double er2 = ((Kd) / dt) * prev_prev_error;
        double delta = (er0 + er1 + er2);
        control = control + delta;
        // Limit the output of the Controller/Current
        if (control < (min_value))
        {
            control = (min_value);
        }
        else if (control > max_value)
        {
            control = max_value;
        }
        prev_prev_error = prev_error;
        prev_error = curr_error;
    }
 
 
    TorqueControllerConfigurationPtr
    TorqueControllerConfiguration::CreateTorqueConfigData(hardware_config::Config& config,
                                                          bool limitless,
                                                          float jointLimitLow,
                                                          float jointLimitHigh)
    {
        TorqueControllerConfiguration configData;
        configData.pid_proportional_gain = config.getFloat("pid_proportional_gain");
        configData.pid_integral_gain = config.getFloat("pid_integral_gain");
        configData.pid_derivative_gain = config.getFloat("pid_derivative_gain");
        configData.pid_windup_guard = config.getFloat("pid_windup_guard");
        configData.pid_max_value = config.getFloat("pid_max_value");
        configData.pid_min_value = config.getFloat("pid_min_value");
        configData.pid_dis = config.getFloat("pid_dis");
        configData.Kd = config.getFloat("Kd");
        configData.inertia = config.getFloat("inertia");
        configData.scalePI = config.getFloat("scalePI");
        configData.scaleTorque = config.getFloat("scaleTorque");
        configData.maxTorque = config.getFloat("maxTorque");
        configData.actuatorType = config.getInt("actuatorType");
        auto v = config.getFloatList("firFilterImpulseResponse");
        for (auto& elem : v)
        {
            configData.firFilterImpulseResponse.push_back(elem);
        }
 
        configData.limitless = limitless;
        configData.jointLimitLow = jointLimitLow;
        configData.jointLimitHigh = jointLimitHigh;
        // configData.torqueToCurrent = config.getFloat("joint_lim_avoid_torque_to_current_factor");
 
        return std::make_shared<TorqueControllerConfiguration>(configData);
    }
 
 
    TorqueControllerConfiguration::TorqueControllerConfiguration()
    {
    }
 
 
    float
    TorqueController::update(const IceUtil::Time& timeSinceLastIteration,
                             const std::string& jointName,
                             float gravity,
                             float actualTorque,
                             float targetTorque,
                             float actualVelocity,
                             float actualPosition)
    {
        double Kd = pid.Kd; // define default damping constant
        double scalePI = pid.scalePI; // define scale factor for I-Term of PID-Controller
        double dt = timeSinceLastIteration.toSecondsDouble(); // sampling time of torque values
        pid.proportional_gain = 0; // the value will be re-defined later
        pid.integral_gain = 0; // the value will be re-defined later
        float omega = 0.55; // max. rotational speed in [rad/sec]
        float actualTorqueF =
            filter.update(actualTorque); // torque value will be filtered with low pass FIR-Filter
        Kd = (-gravity + targetTorque + actualTorqueF) /
             omega; // calculation of the damping constant
        if (Kd < 0) // need to be done for the backd-rivability
        {
            Kd = -Kd;
        }
        if ((pid.actuatorType == 1) && (Kd < 6)) // define min. damping for large aktuators
        {
            Kd = 6;
        }
        if (Kd > 80) // define max. damping for all aktuators
        {
            Kd = 80;
        }
        if (pid.actuatorType == 1) // define I-Term for large aktuators
        {
            pid.integral_gain =
                (-0.003721 * std::pow(Kd, 2.0) + 0.43429 * Kd - 1.4408) * (scalePI * 0.45);
        }
        else
        {
            pid.integral_gain = 1.2 * scalePI; // define I-Term for midle and small aktuators
            pid.proportional_gain = 0.03; // define P-Term for midle and small aktuators
        }
        if (pid.integral_gain < 0) // define min. I-Term for all aktuators
        {
            pid.integral_gain = 0.5;
        }
        pid.findAcc(actualVelocity, dt);
        //        double protectionTorque = calcJointLimitProtectionTorque(actualPosition);
//         // TODO testing joint lim avoidance
//         double torqueToCurrentFactor = configData->torqueToCurrent;
// //        auto jointLimitProtectionCurrent =
// //                calcJointLimitProtectionTorque(actualPosition) *
// //                (torqueToCurrentFactor == 0.0 ? 0.05 : torqueToCurrentFactor) * 1000;
//         // -- end testing
 
        float setTorque =
            (-gravity + targetTorque - Kd * actualVelocity); // calculate desired Torque
 
 
        double CurrError =
            setTorque +
            actualTorqueF; // calculate error between desired and actual(measured and filtered) torque
        // send error to PID-Controller
        pid.update(CurrError, dt);
        // The Controller calculate the required current. The current will be converted to Elmo current value
        float current = -(pid.control) * 1000.0;
        return current;
        // TODO testing joint lim avoidance
//        return current + jointLimitProtectionCurrent;
    }
 
 
    double
    TorqueController::calcJointLimitProtectionTorque(float actualPosition)
    {
        double pushbackMargin = 5.0 / 180. * M_PI;
        double pushBackTorque = 100;
 
        double jointLimitProtection = 0;
        if (!configData->limitless)
        {
            double borderHigh = configData->jointLimitHigh - pushbackMargin;
            double borderLow = configData->jointLimitLow + pushbackMargin;
            double penetration = 0;
            if (actualPosition > borderHigh)
            {
                penetration = actualPosition - borderHigh;
            }
            else if (actualPosition < borderLow)
            {
                penetration = actualPosition - borderLow;
            }
            jointLimitProtection = penetration / pushbackMargin * pushBackTorque;
        }
        jointLimitProtection = math::MathUtils::LimitTo(jointLimitProtection, pushBackTorque);
        return jointLimitProtection;
    }
 
 
    const armarx::control::rt_filters::FirFilter&
    TorqueController::getFilter() const
    {
        return filter;
    }
 
} // namespace armarx::control::joint_controller