inverse_dynamics_controller.h

#ifndef INVERSE_DYNAMICS_H
#define INVERSE_DYNAMICS_H
 
#include <robot_driver/controllers/leg_controller.h>
 
 
class InverseDynamicsController : public LegController {
 public:
  InverseDynamicsController();
 
  bool computeLegCommandArray(const quad_msgs::RobotState &robot_state_msg,
                              quad_msgs::LegCommandArray &leg_command_array_msg,
                              quad_msgs::GRFArray &grf_array_msg);
 
  inline quad_msgs::RobotState getReferenceState() { return ref_state_msg_; }
 
 private:
  Eigen::VectorXd last_grf_array_;
 
  quad_msgs::RobotState ref_state_msg_;
 
  const double grf_exp_filter_const_ = 1.0;  // 1.0 = no filtering
};
 
#endif  // INVERSE_DYNAMICS_H