CartesianPosPDController.hpp

Go to the documentation of this file.

#ifndef WBC_CONTROLLERS_CARTPOSPDCONTROLLER_HPP
#define WBC_CONTROLLERS_CARTPOSPDCONTROLLER_HPP
 
#include "../types/Pose.hpp"
#include "../types/Twist.hpp"
#include "../types/SpatialAcceleration.hpp"
#include <memory>
 
namespace wbc {

class CartesianPosPDController {
protected:
    uint dim_controller;
    types::Twist control_out_vel;
    types::SpatialAcceleration control_out_acc;
    Eigen::VectorXd u;
    Eigen::VectorXd u_max;
    Eigen::VectorXd p_gain;
    Eigen::VectorXd i_gain;
    Eigen::VectorXd d_gain;
    Eigen::VectorXd ff_gain;
    Eigen::VectorXd windup_term;
    Eigen::VectorXd error_accumulated;
    Eigen::VectorXd x;
    Eigen::VectorXd v;
    Eigen::VectorXd rx;
    Eigen::VectorXd rv;
    Eigen::VectorXd ra;
    types::Twist pose_diff;
 
public:
    CartesianPosPDController();
    const types::Twist& update(const types::Pose& ref_pose,
                               const types::Twist& ref_twist,
                               const types::Pose& pose);
    const types::SpatialAcceleration& update(const types::Pose& ref_pose,
                                             const types::Twist& ref_twist,
                                             const types::SpatialAcceleration& ref_acc,
                                             const types::Pose& pose,
                                             const types::Twist& twist);
    void setPGain(const Eigen::VectorXd &gain);
    const Eigen::VectorXd& pGain(){return p_gain;}   
    void setIGain(const Eigen::VectorXd &gain);
    const Eigen::VectorXd& iGain(){return i_gain;}
    void setDGain(const Eigen::VectorXd &gain);
    const Eigen::VectorXd& dGain(){return d_gain;}
    void setFFGain(const Eigen::VectorXd &gain);
    const Eigen::VectorXd& ffGain(){return ff_gain;}
    void setMaxCtrlOutput(const Eigen::VectorXd &max_ctrl_out);
    const Eigen::VectorXd& maxCtrlOutput(){return u_max;}
    void setWindup(const Eigen::VectorXd &windup_terms);
    const Eigen::VectorXd& windup(){return windup_term;}

    void applySaturation(const Eigen::VectorXd& in, const Eigen::VectorXd& max, Eigen::VectorXd &out);
};
using CartesianPosPDControllerPtr = std::shared_ptr<CartesianPosPDController>;
 
}
#endif