FeedbackController.h

Go to the documentation of this file.

#ifndef CONTROLS_CONTROLLER_FEEDBACK_CONTROLLER_H
#define CONTROLS_CONTROLLER_FEEDBACK_CONTROLLER_H
 
#include <controller/BaseControllerInterface.h>
 
class FeedbackController : public BaseControllerInterface {
    static constexpr double MAX_DT                = 0.1;
    static constexpr double EXPLODE_THRESHOLD     = 1e4;
    static constexpr double INTEGRAL_CLAMP        = 5.0;
    static constexpr double INTEGRAL_DECAY_FACTOR = 0.5;
 
private:
    double error_saturation_limit_ = 1;
 
    Vector18d e_ = Vector18d::Zero();
 
    Vector6d body_integral_error_ = Vector6d::Zero();
    Vector6d body_pose_error_     = Vector6d::Zero();
    Vector6d body_twist_error_    = Vector6d::Zero();
 
    Vector6d integral_ = Vector6d::Zero();
 
    Vector6d saved_integral_ = Vector6d::Zero();
 
    Vector6d force_i_ = Vector6d::Zero();
    Vector6d force_p_ = Vector6d::Zero();
    Vector6d force_d_ = Vector6d::Zero();
 
public:
    using BaseControllerInterface::BaseControllerInterface;
 
    bool saturated_on_yaw_xy_axis_ = false;
 
    bool saturated_on_depth_roll_pitch_axis_ = false;
 
    void reset() override;
 
    void reset_all_except_depth_integral();
 
    Vector6d get_force(const State& goal, const State& curr, double dt) override;
 
    void save_integral();
 
    void load_integral();
 
    void ramp_integral(double ratio);
 
    Vector6d get_integral_error() const;
 
    Vector6d get_pose_error() const;
 
    Vector6d get_twist_error() const;
 
    Vector6d get_integral_force() const;
 
    void set_integral(const Vector6d& i);
};
 
#endif  // CONTROLS_CONTROLLER_FEEDBACK_CONTROLLER_H