VelocityManipulatingTorque.cpp

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       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "VelocityManipulatingTorque.h"
 
#include "ArmarXCore/core/logging/Logging.h"
 
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
 
namespace armarx::control::joint_controller
{
 
    VelocityManipulatingTorqueController::VelocityManipulatingTorqueController(
        const VelocityManipulatingTorqueControllerConfigurationPtr& torqueConfigData) :
        pid(torqueConfigData->pid_proportional_gain,
            torqueConfigData->pid_integral_gain,
            torqueConfigData->pid_derivative_gain),
        pidTargetTorque(0, 1, 0),
        torqueGaussianFilter(30, 30),
        torqueMedianFilter(5),
        actualVelocityFilter(5),
        torqueConfigData(VelocityManipulatingTorqueControllerConfigurationPtr(
            new VelocityManipulatingTorqueControllerConfiguration(*torqueConfigData)))
    {
 
 
        pid.threadSafe = false;
        pid.setMaxControlValue(torqueConfigData->pid_max_value);
    }
 
    VelocityManipulatingTorqueControllerConfigurationPtr
    VelocityManipulatingTorqueControllerConfiguration::CreateTorqueConfigData(
        hardware_config::Config& config,
        bool limitless,
        float jointLimitLow,
        float jointLimitHigh)
    {
        VelocityManipulatingTorqueControllerConfiguration 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_max_value = config.getFloat("pid_max_value");
        configData.accelerationGain = config.getFloat("accelerationGain");
        configData.deadZone = config.getFloat("deadZone");
        configData.decay = config.getFloat("decay");
        configData.maxVelocity = config.getFloat("maxVelocity");
        configData.maxAcceleration = config.getFloat("maxAcceleration");
        configData.torqueToCurrent = config.getFloat("torqueToCurrent");
        configData.maxTargetTorque = config.getFloat("maxTargetTorque");
 
        auto v = config.getFloatList("firFilterImpulseResponse");
        for (auto& elem : v)
        {
            configData.firFilterImpulseResponse.push_back(elem);
        }
 
        configData.limitless = limitless;
        configData.jointLimitLow = jointLimitLow;
        configData.jointLimitHigh = jointLimitHigh;
 
        return std::make_shared<VelocityManipulatingTorqueControllerConfiguration>(configData);
    }
 
    VelocityManipulatingTorqueControllerConfiguration::
        VelocityManipulatingTorqueControllerConfiguration()
    {
    }
 
    double
    VelocityManipulatingTorqueController::calcJointLimitProtectionTorque(float actualPosition)
    {
        double jointLimitProtection = 0;
        if (!torqueConfigData->limitless)
        {
            double borderHigh = torqueConfigData->jointLimitHigh - torqueConfigData->pushbackMargin;
            double borderLow = torqueConfigData->jointLimitLow + torqueConfigData->pushbackMargin;
            double penetration = 0;
            if (actualPosition > borderHigh)
            {
                penetration = actualPosition - borderHigh;
            }
            else if (actualPosition < borderLow)
            {
                penetration = actualPosition - borderLow;
            }
            jointLimitProtection =
                penetration / torqueConfigData->pushbackMargin * torqueConfigData->pushBackTorque;
        }
        jointLimitProtection =
            math::MathUtils::LimitTo(jointLimitProtection, torqueConfigData->pushBackTorque);
        return jointLimitProtection;
    }
 
    double
    VelocityManipulatingTorqueController::getLastJerk() const
    {
        return lastJerk;
    }
 
    double
    VelocityManipulatingTorqueController::getLastAcceleration() const
    {
        return lastAcceleration;
    }
 
    float
    VelocityManipulatingTorqueController::update(const IceUtil::Time& sensorValuesTimestamp,
                                                 const IceUtil::Time& timeSinceLastIteration,
                                                 float gravity,
                                                 float actualTorque,
                                                 float targetTorque,
                                                 float actualVelocity,
                                                 float actualPosition)
    {
        ARMARX_CHECK_EXPRESSION(torqueConfigData);
        // pidParameterMutex.lock();
        pid.Kp = torqueConfigData->pid_proportional_gain;
        pid.Ki = torqueConfigData->pid_integral_gain;
        pid.Kd = torqueConfigData->pid_derivative_gain;
 
        pid.maxDerivation = 30; // avoids rapid oscillation of pid controller
        pid.setMaxControlValue(torqueConfigData->pid_max_value);
        //        ARMARX_INFO << "PID max value: " << torqueConfigData->pid_max_value;
 
        targetTorque = math::MathUtils::LimitTo(targetTorque, torqueConfigData->maxTargetTorque);
        double decay = torqueConfigData->decay;
        double accelerationGain = torqueConfigData->accelerationGain;
        double maxAcceleration = torqueConfigData->maxAcceleration;
        double maxJerk = torqueConfigData->maxJerk;
        double deadZone = torqueConfigData->deadZone;
        double maxVelocity = torqueConfigData->maxVelocity;
        double torqueToCurrentFactor = torqueConfigData->torqueToCurrent;
        // pidParameterMutex.unlock();
 
 
        double dt = timeSinceLastIteration.toSecondsDouble(); // sampling time of torque values
        //        double actualTorqueF = filter.update(actualTorque);
        // actualTorque = torqueMedianFilter.update(actualTorque);
        actualVelocity = actualVelocityFilter.update(actualVelocity);
        double gravityCompensatedTorque = -gravity + actualTorque;
 
        // apply a deadzone due to model/sensor error
        //        if (std::abs(targetTorque) < 0.1)
        {
            gravityCompensatedTorque =
                math::MathUtils::Sign(gravityCompensatedTorque) *
                std::max<float>(0.0f, (std::abs(gravityCompensatedTorque) - deadZone));
        }
 
 
        double jointLimitProtectionTorque = calcJointLimitProtectionTorque(actualPosition);
        if (std::abs(jointLimitProtectionTorque) > 0.0)
        {
            pid.integral *= 0.9; // we dont want an high integral if we are out of the joint limits
        }
        jointLimitProtectionTorque = math::MathUtils::LimitTo(
            jointLimitProtectionTorque, std::abs(targetTorque + gravityCompensatedTorque) * 1.00);
 
        double acceleration =
            (targetTorque + gravityCompensatedTorque - jointLimitProtectionTorque) *
            accelerationGain; // torque is an acceleration
        acceleration = math::MathUtils::LimitTo(
            acceleration,
            maxAcceleration); // joints cannot/should not accelerate faster than this by zero torque control
 
        acceleration = math::MathUtils::LimitMinMax(
            lastAcceleration - maxJerk * dt, lastAcceleration + maxJerk * dt, acceleration);
        if (math::MathUtils::Sign(currentTargetVelocity) == math::MathUtils::Sign(acceleration))
        {
            decay /= std::max(
                std::abs(gravityCompensatedTorque),
                1.0); // less decay when external torque is applied for easier zero torque control
        }
        currentTargetVelocity += acceleration * dt;
        lastJerk = (acceleration - lastAcceleration) / dt;
        lastAcceleration = acceleration;
 
        decay /= math::MathUtils::LimitMinMax(
            1.0,
            15.0,
            std::abs(
                gravityCompensatedTorque *
                gravityCompensatedTorque)); // if external torque is applied, the decay is reduced to have a lighter torque control
 
        decay = math::MathUtils::LimitMinMax(0.0, 1.0, decay);
        currentTargetVelocity *=
            1.0 - decay; // damping - slows down joint motion (important if no torque is applied)
        currentTargetVelocity = math::MathUtils::LimitTo(
            currentTargetVelocity,
            maxVelocity); // some joints should not be moved over a certain velocity
        pid.update(dt, actualVelocity, currentTargetVelocity);
        double resultingTargetCurrent = -pid.getControlValue() * 1000.0;
 
 
        double directControlCurrent = 0.; //gravity * torqueToCurrentFactor * 1000;
 
        //               ARMARX_RT_LOGF_INFO("direct-control: %.3f, currentTargetVelocity: %.3f, current Vel: %.3f, g-torque: %.2f, actual torque: %.2f, "
        //                                   "torqueDiff: %.3f, target torque: %.2f, acc: %.3f, target current: %.3f, Kp: %.1f, Ki: %.1f, cur Integral: %.4f, "
        //                                   "Kd: %0.2f, pid MaxVal: %.2f, a-gain: %.2f, current jerk: %.2f, deadZone: %.1f, decay: %.4f"
        //                                   ,
        //                                   directControlCurrent, currentTargetVelocity, actualVelocity, gravity, actualTorque,
        //                                   gravityCompensatedTorque, targetTorque, acceleration,
        //                                   resultingTargetCurrent, pid.Kp, pid.Ki, pid.integral, pid.Kd, pid.maxControlValue,
        //                                   accelerationGain, lastJerk, deadZone, decay).deactivateSpam(0.3);
 
        // calculate a current that protects the joint limits
        auto jointLimitProtectionCurrent =
            calcJointLimitProtectionTorque(actualPosition) *
            (torqueToCurrentFactor == 0.0 ? 0.05 : torqueToCurrentFactor) * 1000;
        //        ARMARX_RT_LOGF_INFO("targetCurrent: %.3f, protection torque: %.3f",
        //                            resultingTargetCurrent, jointLimitProtectionTorque).deactivateSpam(0.3);
        return resultingTargetCurrent + directControlCurrent + jointLimitProtectionCurrent;
    }
 
    void
    VelocityManipulatingTorqueController::reset()
    {
        pid.reset();
        currentTargetVelocity = 0.0;
        lastAcceleration = 0.0;
        lastJerk = 0.0;
    }
 
    double
    VelocityManipulatingTorqueController::getCurrentTargetVelocity() const
    {
        return currentTargetVelocity;
    }
 
    VelocityManipulatingTorqueControllerConfiguration
    VelocityManipulatingTorqueController::getTorqueConfigData() const
    {
        return *torqueConfigData;
    }
 
    bool
    VelocityManipulatingTorqueController::setTorqueConfigData(
        VelocityManipulatingTorqueControllerConfiguration configData)
    {
        // if (pidParameterMutex.try_lock())
        // {
        torqueConfigData->pid_proportional_gain = configData.pid_proportional_gain;
        torqueConfigData->pid_integral_gain = configData.pid_integral_gain;
        torqueConfigData->pid_derivative_gain = configData.pid_derivative_gain;
 
        torqueConfigData->decay = configData.decay;
        torqueConfigData->deadZone = configData.deadZone;
 
        //            ARMARX_INFO << "Updated control parameters";
 
 
        // }
 
        return true;
    }
 
    const PIDController&
    VelocityManipulatingTorqueController::getPid()
    {
        return pid;
    }
 
} // namespace armarx::control::joint_controller