From a3d37ef2e3e652f276df721223fd08e4d4eb405e Mon Sep 17 00:00:00 2001 From: kevin-y-wang Date: Fri, 8 Nov 2019 14:19:03 -0800 Subject: [PATCH] Control: refactor Mrac controller initialization with ERROR STATUS --- modules/control/common/mrac_controller.cc | 81 +++++++++-------------- modules/control/common/mrac_controller.h | 7 +- 2 files changed, 36 insertions(+), 52 deletions(-) diff --git a/modules/control/common/mrac_controller.cc b/modules/control/common/mrac_controller.cc index c018c353f5..148f639290 100644 --- a/modules/control/common/mrac_controller.cc +++ b/modules/control/common/mrac_controller.cc @@ -17,6 +17,7 @@ #include "modules/control/common/mrac_controller.h" #include +#include #include #include "Eigen/Dense" @@ -36,22 +37,16 @@ using Matrix = Eigen::MatrixXd; double MracController::Control(const double command, const Matrix state, const double input_limit, const double input_rate_limit) { - // check if the reference/adaption model well set up during the initialization - if (!reference_model_enabled_ || !adaption_model_enabled_) { - AWARN << "MRAC: model build failed; will work as a unity compensator. The " - "reference_model building status: " - << reference_model_enabled_ - << "; The adaption_model building status: " - << adaption_model_enabled_; + // check if the current sampling time is valid and the reference/adaption + // model well set up during the initialization + if (ts_ <= 0.0 || !reference_model_enabled_ || !adaption_model_enabled_) { + AERROR << "MRAC: model build failed; will work as a unity compensator. The " + "reference_model building status: " + << reference_model_enabled_ + << "; The adaption_model building status: " + << adaption_model_enabled_ << "; Current sampling time: " << ts_; return command; // treat the mrac as a unity proportional controller } - // check if the current sampling time is valid - if (ts_ <= 0.0) { - AWARN - << "MRAC: current sampling time <= 0, will use the last control, ts_: " - << ts_; - return control_previous_; - } // update the state in the real actuation system state_action_.col(0) = state; @@ -152,19 +147,13 @@ void MracController::Init(const MracConf &mrac_conf, const double dt) { // Initialize the reference model parameters matrix_a_reference_ = Matrix::Zero(model_order_, model_order_); matrix_b_reference_ = Matrix::Zero(model_order_, 1); - if (SetReferenceModel(mrac_conf).ok()) { - BuildReferenceModel(); - } else { - reference_model_enabled_ = false; - } + reference_model_enabled_ = + (SetReferenceModel(mrac_conf).ok() && BuildReferenceModel().ok()); // Initialize the adaption model parameters matrix_p_adaption_ = Matrix::Zero(model_order_, model_order_); matrix_b_adaption_ = Matrix::Zero(model_order_, 1); - if (SetAdaptionModel(mrac_conf).ok()) { - BuildAdaptionModel(); - } else { - adaption_model_enabled_ = false; - } + adaption_model_enabled_ = + (SetAdaptionModel(mrac_conf).ok() && BuildAdaptionModel().ok()); } Status MracController::SetReferenceModel(const MracConf &mrac_conf) { @@ -221,22 +210,13 @@ Status MracController::SetAdaptionModel(const MracConf &mrac_conf) { return Status::OK(); } -void MracController::BuildReferenceModel() { - reference_model_enabled_ = true; - if (ts_ <= 0.0) { - AWARN << "ts_ <= 0, continuous-discrete transformation for reference model " - "failed, ts_: " - << ts_; - reference_model_enabled_ = false; - return; - } - +Status MracController::BuildReferenceModel() { if (model_order_ > 2) { - AWARN << "reference model order beyond the designed range, " - "model_order: " - << model_order_; - reference_model_enabled_ = false; - return; + const auto error_msg = + StrCat("mrac controller error: reference model order ", model_order_, + " is beyond the designed range"); + AERROR << error_msg; + return Status(ErrorCode::CONTROL_INIT_ERROR, error_msg); } if (model_order_ == 1) { matrix_a_reference_(0, 0) = -1.0 / tau_reference_; @@ -247,16 +227,16 @@ void MracController::BuildReferenceModel() { matrix_a_reference_(1, 1) = -2 * zeta_reference_ * wn_reference_; matrix_b_reference_(1, 0) = wn_reference_ * wn_reference_; } + return Status::OK(); } -void MracController::BuildAdaptionModel() { - adaption_model_enabled_ = true; +Status MracController::BuildAdaptionModel() { if (model_order_ > 2) { - AWARN << "Adaption model order beyond the designed range, " - "model_order: " - << model_order_; - adaption_model_enabled_ = false; - return; + const auto error_msg = + StrCat("mrac controller error: adaption model order ", model_order_, + " is beyond the designed range"); + AERROR << error_msg; + return Status(ErrorCode::CONTROL_INIT_ERROR, error_msg); } if (model_order_ == 1) { matrix_b_adaption_(0, 0) = 1.0; @@ -264,10 +244,13 @@ void MracController::BuildAdaptionModel() { matrix_b_adaption_(1, 0) = wn_reference_ * wn_reference_; } if (!CheckLyapunovPD(matrix_a_reference_, matrix_p_adaption_)) { - AWARN << "Solution of the algebraic Lyapunov equation is not symmetric " - "positive definite"; - adaption_model_enabled_ = false; + const std::string error_msg = + "Solution of the algebraic Lyapunov equation is not symmetric positive " + "definite"; + AERROR << error_msg; + return Status(ErrorCode::CONTROL_INIT_ERROR, error_msg); } + return Status::OK(); } bool MracController::CheckLyapunovPD(const Matrix matrix_a, diff --git a/modules/control/common/mrac_controller.h b/modules/control/common/mrac_controller.h index 69cec6e9e0..41f162ebe0 100644 --- a/modules/control/common/mrac_controller.h +++ b/modules/control/common/mrac_controller.h @@ -66,15 +66,16 @@ class MracController { /** * @brief build mrac (1st or 2nd) order reference model in the discrete-time form, with the bilinear transform (trapezoidal integration) method + * @return Status reference model initialization status */ - void BuildReferenceModel(); + common::Status BuildReferenceModel(); /** * @brief build mrac (1st or 2nd) order adaptive dynamic model in the * discrete-time form - * @param none + * @return Status adaption model initialization status */ - void BuildAdaptionModel(); + common::Status BuildAdaptionModel(); /** * @brief check if the solution of the algebraic Lyapunov Equation is -- GitLab