Torque.h

Go to the documentation of this file.

#pragma once
 
 
// control
#include <armarx/control/joint_controller/ControllerConfiguration.h>
#include <armarx/control/rt_filters/AverageFilter.h>
#include <armarx/control/rt_filters/FirFilter.h>
 
 
namespace armarx::control::joint_controller
{
 
    class TorqueControllerConfiguration;
    using TorqueControllerConfigurationPtr = std::shared_ptr<class TorqueControllerConfiguration>;
 
 
    class TorqueControllerConfiguration
    {
    public:
        TorqueControllerConfiguration();
        static TorqueControllerConfigurationPtr
        CreateTorqueConfigData(hardware_config::Config& config,
                               bool limitless,
                               float jointLimitLow,
                               float jointLimitHigh);
        double pid_proportional_gain;
        double pid_integral_gain;
        double pid_derivative_gain;
        double pid_windup_guard;
        double pid_max_value;
        double pid_min_value;
        double pid_dis;
        double Kd;
        double inertia;
        double scalePI;
        double scaleTorque;
        double maxTorque;
        // double torqueToCurrent = 0.0;  // for joint limit avoidance
        int actuatorType;
        bool limitless = true;
        double jointLimitHigh = 0.0, jointLimitLow = 0.0;
        std::vector<float> firFilterImpulseResponse;
    };
 
 
    class TorquePID
    {
    public:
        TorquePID();
        void zeroize();
        void findAcc(double Vel, double dt);
        void update(double curr_error, double dt);
        void updatePhi(double curr_pos_error, double dt);
        void AbsVel(double curr_angle_err, double dt);
        //void define_pid(PID* pid);
        double windup_guard = 0;
        double proportional_gain = 0;
        double integral_gain = 0;
        double derivative_gain = 0;
        double prev_error = 0;
        double prev_prev_error = 0;
        double int_error = 0;
        double control = 0;
        double max_value = 0;
        double min_value = 0;
        double phi = 0;
        double old_pos = 0;
        double dis = 0;
        double abs_vel = 0;
        double prev_angle = 0;
        double if_angle = 0;
        double angle_offset = 0;
        double CurrError = 0;
        double SollMoment = 0;
        double ii = 0;
        double prev_vel = 0;
        double acc = 0;
        double Kd = 0;
        double inertia = 0;
        double scalePI = 0;
        double scaleTorque = 0;
        double maxTorque = 0;
        int actuatorType;
        bool status = false;
    };
 
 
    class TorqueController
    {
    public:
        TorqueController(const TorqueControllerConfigurationPtr& torqueConfigData);
        float update(const IceUtil::Time& timeSinceLastIteration,
                     const std::string& jointName,
                     float gravity,
                     float actualTorque,
                     float targetTorque,
                     float actualVelocity,
                     float actualPosition);
        const armarx::control::rt_filters::FirFilter& getFilter() const;
        double calcJointLimitProtectionTorque(float actualPosition);
 
    private:
        TorquePID pid;
        armarx::control::rt_filters::FirFilter filter;
        armarx::control::rt_filters::RtAverageFilter targetFilter;
        TorqueControllerConfigurationPtr configData;
    };
 
} // namespace armarx::control::joint_controller