nmpc_controller.h

#ifndef NMPC_CONTROLLER_H
#define NMPC_CONTROLLER_H
 
#include <math.h>
#include <quad_msgs/GRFArray.h>
#include <quad_msgs/LegCommand.h>
#include <quad_msgs/MultiFootPlanDiscrete.h>
#include <quad_msgs/RobotPlan.h>
#include <quad_msgs/RobotState.h>
#include <quad_utils/ros_utils.h>
#include <ros/ros.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
#include <Eigen/Dense>
 
#include "IpIpoptApplication.hpp"
#include "nmpc_controller/quad_nlp.h"
 
 
class NMPCController {
 public:
  NMPCController(ros::NodeHandle &nh, int type);
 
  bool computePlan(const Eigen::VectorXd &initial_state,
                   const Eigen::MatrixXd &ref_traj,
                   const std::vector<std::vector<bool>> &contact_schedule,
                   Eigen::MatrixXd &foot_positions,
                   Eigen::MatrixXd &foot_velocities,
                   Eigen::MatrixXd &state_traj, Eigen::MatrixXd &control_traj);
 
  bool computeLegPlan(
      const Eigen::VectorXd &initial_state, const Eigen::MatrixXd &ref_traj,
      const Eigen::MatrixXd &foot_positions_body,
      Eigen::MatrixXd &foot_positions_world, Eigen::MatrixXd &foot_velocities,
      const std::vector<std::vector<bool>> &contact_schedule,
      const Eigen::VectorXd &ref_ground_height,
      const double &first_element_duration, const bool &same_plan_index,
      const grid_map::GridMap &terrain, Eigen::MatrixXd &state_traj,
      Eigen::MatrixXd &control_traj);
 
  Eigen::VectorXi updateAdaptiveComplexitySchedule(
      Eigen::MatrixXd &state_traj_lifted, Eigen::MatrixXd &control_traj_lifted);
 
  void updateHorizonLength();
 
  inline NLPDiagnostics getNLPDiagnostics() const { return diagnostics_; }
 
 private:
  ros::NodeHandle nh_;
 
  int robot_id_;
 
  std::string robot_ns_;
 
  double update_rate_;
 
  SmartPtr<quadNLP> mynlp_;
 
  SmartPtr<IpoptApplication> app_;
 
  std::shared_ptr<quad_utils::QuadKD> quadKD_;
 
  bool enable_variable_horizon_;
 
  int N_, N_max_, N_min_;
 
  // Number of states in different components
  const int n_body_ = 12, n_foot_ = 24, n_joints_ = 24, n_tail_ = 4,
            m_body_ = 12, m_foot_ = 24, m_tail_ = 2;
 
  double dt_;
 
  bool require_init_;
 
  Eigen::VectorXi adaptive_complexity_schedule_;
 
  NLPDiagnostics diagnostics_;
 
  NLPConfig config_;
};
 
#endif  // MPC_CONTROLLER_H