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 saturatedOnYawXYAxis = false;
 
    bool saturatedOnDepthRollPitchAxis = false;
 
    void reset() override;
 
    void resetAllExceptDepthIntegral();
 
    Vector6d getForce(const State& goal, const State& curr, double dt) override;
 
    void save_integral();
 
    void load_integral();
 
    void ramp_integral(double ratio);
 
    Vector6d getIntegralError() const;
 
    Vector6d getPoseError() const;
 
    Vector6d getTwistError() const;
 
    Vector6d getIntegralForce() const;
 
    void setIntegral(const Vector6d& i);
};
 
#endif  // CONTROLS_CONTROLLER_FEEDBACK_CONTROLLER_H