[Control][MPC] use getParam to obtain constraints

aerial_robot_control/include/aerial_robot_control/control/base/mpc_solver.h

@@ -56,8 +56,15 @@ namespace MPC
 
 struct PhysicalParams
 {
+  // physical params
   double mass;
   double gravity;
+
+  // physical constraints
+  double v_max;
+  double v_min;
+  double w_max;
+  double w_min;
   double c_thrust_max;
   double c_thrust_min;
 };
@@ -69,7 +76,7 @@ class MPCSolver
 
   MPCSolver();
   ~MPCSolver();
-  void initialize(PhysicalParams& physical_params);
+  void initialize(PhysicalParams& phys_params);
   void reset(const aerial_robot_msgs::PredXU& x_u);
   int solve(const nav_msgs::Odometry& odom_now, const aerial_robot_msgs::PredXU& x_u_ref);
 

aerial_robot_control/include/aerial_robot_control/control/base/nmpc_controller.h

@@ -57,6 +57,8 @@ class NMPCController : public ControlBase
 private:
   double mass_;
   double gravity_const_;
+  double t_nmpc_samp_;
+  double t_nmpc_integ_;
 
   double target_roll_, target_pitch_;
   double candidate_yaw_term_;
@@ -68,7 +70,6 @@ class NMPCController : public ControlBase
   MPC::MPCSolver mpc_solver_;
 
   nav_msgs::Odometry getOdom();
-  void sendFlightCmd();
 };
 
 };  // namespace aerial_robot_control

aerial_robot_control/src/control/base/mpc_solver.cpp

@@ -9,7 +9,7 @@ MPC::MPCSolver::MPCSolver()
 {
 }
 
-void MPC::MPCSolver::initialize(PhysicalParams& physical_params)
+void MPC::MPCSolver::initialize(PhysicalParams& phys_params)
 {
   /* Allocate the array and fill it accordingly */
   acados_ocp_capsule_ = qd_body_rate_model_acados_create_capsule();
@@ -38,22 +38,24 @@ void MPC::MPCSolver::initialize(PhysicalParams& physical_params)
   // Please note that the constraints have been set up inside the python interface. Only minimum adjustments are needed.
   // bx_0: initial state. Note that the value of lbx0 and ubx0 will be set in solve() function, feedback constraints
   // bx
-  double lbx[NBX] = { -1.0, -1.0, -1.0 };  // TODO: use rosparam for all params
-  double ubx[NBX] = { 1.0, 1.0, 1.0 };
+  double lbx[NBX] = { phys_params.v_min, phys_params.v_min, phys_params.v_min };
+  double ubx[NBX] = { phys_params.v_max, phys_params.v_max, phys_params.v_max };
   for (int i = 1; i < NN; i++)
   {
     ocp_nlp_constraints_model_set(nlp_config_, nlp_dims_, nlp_in_, i, "lbx", lbx);
     ocp_nlp_constraints_model_set(nlp_config_, nlp_dims_, nlp_in_, i, "ubx", ubx);
   }
   // bx_e: terminal state
-  double lbx_e[NBXN] = { -1.0, -1.0, -1.0 };
-  double ubx_e[NBXN] = { 1.0, 1.0, 1.0 };
+  double lbx_e[NBXN] = { phys_params.v_min, phys_params.v_min, phys_params.v_min };
+  double ubx_e[NBXN] = { phys_params.v_max, phys_params.v_max, phys_params.v_max };
   ocp_nlp_constraints_model_set(nlp_config_, nlp_dims_, nlp_in_, NN, "lbx", lbx_e);
   ocp_nlp_constraints_model_set(nlp_config_, nlp_dims_, nlp_in_, NN, "ubx", ubx_e);
 
   // bu
-  double lbu[NBU] = { -6.0, -6.0, -6.0, physical_params.c_thrust_min / physical_params.mass };
-  double ubu[NBU] = { 6.0, 6.0, 6.0, physical_params.c_thrust_max / physical_params.mass };
+  double lbu[NBU] = { phys_params.w_min, phys_params.w_min, phys_params.w_min,
+                      phys_params.c_thrust_min / phys_params.mass };
+  double ubu[NBU] = { phys_params.w_max, phys_params.w_max, phys_params.w_max,
+                      phys_params.c_thrust_max / phys_params.mass };
   for (int i = 0; i < NN; i++)
   {
     ocp_nlp_constraints_model_set(nlp_config_, nlp_dims_, nlp_in_, i, "lbu", lbu);
@@ -63,7 +65,7 @@ void MPC::MPCSolver::initialize(PhysicalParams& physical_params)
   /* Set parameters */
   // Note that the code here initializes all parameters, including variables and constants.
   // Constants are initialized only once, while variables are initialized in every iteration
-  double p[NP] = { 1.0, 0.0, 0.0, 0.0, physical_params.gravity };
+  double p[NP] = { 1.0, 0.0, 0.0, 0.0, phys_params.gravity };
   for (int i = 0; i < NN; i++)
   {
     qd_body_rate_model_acados_update_params(acados_ocp_capsule_, i, p, NP);

aerial_robot_control/src/control/base/nmpc_controller.cpp

@@ -22,12 +22,18 @@ void aerial_robot_control::NMPCController::initialize(
   // TODO: wired. I think the original value should be -9.8 in ENU frame
   gravity_const_ = robot_model_->getGravity()[2];
 
+  // get params and initialize nmpc solver
   MPC::PhysicalParams physical_params{};
   physical_params.mass = robot_model_->getMass();
   physical_params.gravity = robot_model_->getGravity()[2];
   physical_params.c_thrust_max = robot_model_->getThrustUpperLimit() * robot_model_->getRotorNum();
   physical_params.c_thrust_min = robot_model_->getThrustLowerLimit() * robot_model_->getRotorNum();
-
+  getParam<double>(control_nh, "nmpc/v_max", physical_params.v_max, 0.5);
+  getParam<double>(control_nh, "nmpc/v_min", physical_params.v_min, -0.5);
+  getParam<double>(control_nh, "nmpc/w_max", physical_params.w_max, 3.0);
+  getParam<double>(control_nh, "nmpc/w_min", physical_params.w_min, -3.0);
+  getParam<double>(control_nh, "nmpc/T_samp", t_nmpc_samp_, 0.025);
+  getParam<double>(control_nh, "nmpc/T_integ", t_nmpc_integ_, 0.1);
   mpc_solver_.initialize(physical_params);
 
   /* timers */
@@ -147,10 +153,9 @@ void aerial_robot_control::NMPCController::controlCore()
   tf::Matrix3x3(q0).getRPY(rpy0[0], rpy0[1], rpy0[2]);
   tf::Matrix3x3(q1).getRPY(rpy1[0], rpy1[1], rpy1[2]);
 
-  // TODO: should be set by rosparam   0.025 - 40hz for controller; 0.1 - 10hz for predict horizon
   // - roll, pitch
-  target_roll_ = (0.025 * rpy0[0] + (0.1 - 0.025) * rpy1[0]) / 0.1;
-  target_pitch_ = (0.025 * rpy0[1] + (0.1 - 0.025) * rpy1[1]) / 0.1;
+  target_roll_ = (t_nmpc_samp_ * rpy0[0] + (t_nmpc_integ_ - t_nmpc_samp_) * rpy1[0]) / t_nmpc_integ_;
+  target_pitch_ = (t_nmpc_samp_ * rpy0[1] + (t_nmpc_integ_ - t_nmpc_samp_) * rpy1[1]) / t_nmpc_integ_;
 
   flight_cmd_.angles[0] = static_cast<float>(target_roll_);
   flight_cmd_.angles[1] = static_cast<float>(target_pitch_);
@@ -171,7 +176,7 @@ void aerial_robot_control::NMPCController::controlCore()
 
   Eigen::VectorXd static_thrust = robot_model_->getStaticThrust();
   Eigen::VectorXd g = robot_model_->getGravity();
-  Eigen::VectorXd target_thrusts = acc_body_z * static_thrust / gravity_const_;
+  Eigen::VectorXd target_thrusts = acc_body_z * static_thrust / g.norm();
 
   std::vector<float> target_base_thrust_;
   target_base_thrust_.resize(motor_num_);