leg_controller.h

#ifndef LEG_CONTORLLER_H
#define LEG_CONTORLLER_H
 
#include <eigen_conversions/eigen_msg.h>
#include <quad_msgs/GRFArray.h>
#include <quad_msgs/LegCommand.h>
#include <quad_msgs/LegCommandArray.h>
#include <quad_msgs/MotorCommand.h>
#include <quad_msgs/MultiFootPlanContinuous.h>
#include <quad_msgs/RobotPlan.h>
#include <quad_msgs/RobotState.h>
#include <quad_utils/math_utils.h>
#include <quad_utils/ros_utils.h>
#include <robot_driver/hardware_interfaces/spirit_interface.h>
#include <ros/ros.h>
#include <std_msgs/UInt8.h>
 
#include <cmath>
#include <eigen3/Eigen/Eigen>
#define MATH_PI 3.141592
 
 
class LegController {
 public:
  LegController();
 
  virtual void init(double kp, double kd);
 
  virtual void init(const std::vector<double> &kp,
                    const std::vector<double> &kd);
 
  virtual void init(const std::vector<double> &stance_kp,
                    const std::vector<double> &stance_kd,
                    const std::vector<double> &swing_kp,
                    const std::vector<double> &swing_kd,
                    const std::vector<double> &swing_kp_cart,
                    const std::vector<double> &swing_kd_cart);
 
  void updateLocalPlanMsg(quad_msgs::RobotPlan::ConstPtr msg,
                          const ros::Time &t_msg);
 
  virtual bool computeLegCommandArray(
      const quad_msgs::RobotState &robot_state_msg,
      quad_msgs::LegCommandArray &leg_command_array_msg,
      quad_msgs::GRFArray &grf_array_msg) = 0;
 
  inline bool overrideStateMachine() { return override_state_machine_; }
 
 protected:
  const int num_feet_ = 4;
 
  std::shared_ptr<quad_utils::QuadKD> quadKD_;
 
  std::vector<double> stance_kp_;
  std::vector<double> stance_kd_;
 
  std::vector<double> swing_kp_;
  std::vector<double> swing_kd_;
 
  std::vector<double> swing_kp_cart_;
  std::vector<double> swing_kd_cart_;
 
  quad_msgs::RobotPlan::ConstPtr last_local_plan_msg_;
 
  ros::Time last_local_plan_time_;
 
  bool override_state_machine_;
};
 
#endif  // LEG_CONTORLLER_H