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, int plan_index_diff,
                      const grid_map::GridMap &terrain,
                      Eigen::MatrixXd &state_traj,
                      Eigen::MatrixXd &control_traj);
 
  Eigen::VectorXi updateAdaptiveComplexitySchedule(
      const Eigen::MatrixXd &state_traj_heuristic,
      const Eigen::MatrixXd &control_traj_heuristic,
      const Eigen::MatrixXd &state_traj_lifted,
      const 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_;
 
  bool enable_mixed_complexity_ = false;
 
  bool enable_adaptive_complexity_ = false;
 
  bool allow_new_interior_complexity_;
 
  bool is_adaptive_complexity_sparse_;
 
  int N_, N_max_, N_min_;
 
  const int n_body_ = 12, n_foot_ = 24, n_joints_ = 24, m_body_ = 12,
            m_foot_ = 24;
 
  double dt_;
 
  bool require_init_;
 
  Eigen::VectorXi adaptive_complexity_schedule_;
 
  NLPDiagnostics diagnostics_;
 
  NLPConfig config_;
};
 
#endif  // MPC_CONTROLLER_H