From 6ae736b388bb5ea663d909875b429619b58456d0 Mon Sep 17 00:00:00 2001 From: Jinjie Li <45286479+Li-Jinjie@users.noreply.github.com> Date: Sat, 11 Nov 2023 00:50:54 +0900 Subject: [PATCH] [Control][MPC] use getParam to obtain constraints --- .../control/base/mpc_solver.h | 9 ++++++++- .../control/base/nmpc_controller.h | 3 ++- .../src/control/base/mpc_solver.cpp | 18 ++++++++++-------- .../src/control/base/nmpc_controller.cpp | 15 ++++++++++----- 4 files changed, 30 insertions(+), 15 deletions(-) diff --git a/aerial_robot_control/include/aerial_robot_control/control/base/mpc_solver.h b/aerial_robot_control/include/aerial_robot_control/control/base/mpc_solver.h index dde597723..bd6270a38 100644 --- a/aerial_robot_control/include/aerial_robot_control/control/base/mpc_solver.h +++ b/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); diff --git a/aerial_robot_control/include/aerial_robot_control/control/base/nmpc_controller.h b/aerial_robot_control/include/aerial_robot_control/control/base/nmpc_controller.h index 4c2799842..02cffe613 100644 --- a/aerial_robot_control/include/aerial_robot_control/control/base/nmpc_controller.h +++ b/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 diff --git a/aerial_robot_control/src/control/base/mpc_solver.cpp b/aerial_robot_control/src/control/base/mpc_solver.cpp index b169b86ab..8f8399db2 100644 --- a/aerial_robot_control/src/control/base/mpc_solver.cpp +++ b/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); diff --git a/aerial_robot_control/src/control/base/nmpc_controller.cpp b/aerial_robot_control/src/control/base/nmpc_controller.cpp index 51d5f5038..5c0143fcb 100644 --- a/aerial_robot_control/src/control/base/nmpc_controller.cpp +++ b/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(control_nh, "nmpc/v_max", physical_params.v_max, 0.5); + getParam(control_nh, "nmpc/v_min", physical_params.v_min, -0.5); + getParam(control_nh, "nmpc/w_max", physical_params.w_max, 3.0); + getParam(control_nh, "nmpc/w_min", physical_params.w_min, -3.0); + getParam(control_nh, "nmpc/T_samp", t_nmpc_samp_, 0.025); + getParam(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(target_roll_); flight_cmd_.angles[1] = static_cast(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 target_base_thrust_; target_base_thrust_.resize(motor_num_);