diff --git a/aerial_robot_control/CMakeLists.txt b/aerial_robot_control/CMakeLists.txt index a92286261..001c62619 100644 --- a/aerial_robot_control/CMakeLists.txt +++ b/aerial_robot_control/CMakeLists.txt @@ -17,7 +17,8 @@ find_package(catkin REQUIRED COMPONENTS find_package(Eigen3 REQUIRED) generate_dynamic_reconfigure_options( - cfg/PidControl.cfg + cfg/PID.cfg + cfg/LQI.cfg ) ### Temporary soulition because of https://github.com/ros/ros_comm/issues/1404 @@ -57,9 +58,11 @@ target_link_libraries(control_utils ${EIGEN3_LIBRARIES}) add_library(flight_control_pluginlib src/control/base/pose_linear_controller.cpp src/control/fully_actuated_controller.cpp - src/control/under_actuated_controller.cpp) + src/control/under_actuated_controller.cpp + src/control/under_actuated_lqi_controller.cpp + src/control/under_actuated_tilted_lqi_controller.cpp) -target_link_libraries(flight_control_pluginlib ${catkin_LIBRARIES}) +target_link_libraries(flight_control_pluginlib ${catkin_LIBRARIES} control_utils) add_dependencies(flight_control_pluginlib ${PROJECT_NAME}_gencfg) ### flight navigation diff --git a/robots/hydrus/cfg/LQI.cfg b/aerial_robot_control/cfg/LQI.cfg similarity index 93% rename from robots/hydrus/cfg/LQI.cfg rename to aerial_robot_control/cfg/LQI.cfg index 8c6032d86..bf6795dcb 100755 --- a/robots/hydrus/cfg/LQI.cfg +++ b/aerial_robot_control/cfg/LQI.cfg @@ -1,5 +1,5 @@ #!/usr/bin/env python -PACKAGE = "hydrus" +PACKAGE = "aerial_robot_control" from dynamic_reconfigure.parameter_generator_catkin import * from aerial_robot_msgs.msg import DynamicReconfigureLevels @@ -20,4 +20,4 @@ gen.add("z_p", double_t, DynamicReconfigureLevels.RECONFIGURE_LQI_Z_P, "LQI Z P gen.add("z_i", double_t, DynamicReconfigureLevels.RECONFIGURE_LQI_Z_I, "LQI Z I Control Weight", 1, 0, 20) gen.add("z_d", double_t, DynamicReconfigureLevels.RECONFIGURE_LQI_Z_D, "LQI Z D Control Weight", 10, -200, 200) -exit(gen.generate(PACKAGE, "hydrus", "LQI")) +exit(gen.generate(PACKAGE, "aerial_robot_control", "LQI")) diff --git a/aerial_robot_control/cfg/PidControl.cfg b/aerial_robot_control/cfg/PID.cfg similarity index 90% rename from aerial_robot_control/cfg/PidControl.cfg rename to aerial_robot_control/cfg/PID.cfg index 60c5997b9..90786f04a 100755 --- a/aerial_robot_control/cfg/PidControl.cfg +++ b/aerial_robot_control/cfg/PID.cfg @@ -12,6 +12,6 @@ gen.add("p_gain", double_t, DynamicReconfigureLevels.RECONFIGURE_P_GAIN, "p gain gen.add("i_gain", double_t, DynamicReconfigureLevels.RECONFIGURE_I_GAIN, "i gain", 0.01, 0, 20) gen.add("d_gain", double_t, DynamicReconfigureLevels.RECONFIGURE_D_GAIN, "d gain", 0.15, 0, 50) -exit(gen.generate(PACKAGE, "aerial_robot_control", "PidControl")) +exit(gen.generate(PACKAGE, "aerial_robot_control", "PID")) diff --git a/aerial_robot_control/include/aerial_robot_control/control/base/pose_linear_controller.h b/aerial_robot_control/include/aerial_robot_control/control/base/pose_linear_controller.h index 68d9f54a5..fdcb82a00 100644 --- a/aerial_robot_control/include/aerial_robot_control/control/base/pose_linear_controller.h +++ b/aerial_robot_control/include/aerial_robot_control/control/base/pose_linear_controller.h @@ -37,13 +37,13 @@ #include #include -#include +#include #include #include #include #include -using PidControlDynamicConfig = dynamic_reconfigure::Server; +using PidControlDynamicConfig = dynamic_reconfigure::Server; namespace aerial_robot_control { @@ -92,7 +92,7 @@ namespace aerial_robot_control virtual void sendCmd(); - void cfgPidCallback(aerial_robot_control::PidControlConfig &config, uint32_t level, std::vector controller_indices); + void cfgPidCallback(aerial_robot_control::PIDConfig &config, uint32_t level, std::vector controller_indices); }; }; diff --git a/aerial_robot_control/include/aerial_robot_control/control/under_actuated_lqi_controller.h b/aerial_robot_control/include/aerial_robot_control/control/under_actuated_lqi_controller.h new file mode 100644 index 000000000..cbafcf357 --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/control/under_actuated_lqi_controller.h @@ -0,0 +1,113 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace aerial_robot_control +{ + class UnderActuatedLQIController: public PoseLinearController + { + + public: + UnderActuatedLQIController(); + virtual ~UnderActuatedLQIController(); + + void initialize(ros::NodeHandle nh, ros::NodeHandle nhp, + boost::shared_ptr robot_model, + boost::shared_ptr estimator, + boost::shared_ptr navigator, + double ctrl_loop_rate); + + protected: + + ros::Publisher flight_cmd_pub_; // for spinal + ros::Publisher rpy_gain_pub_; // for spinal + ros::Publisher four_axis_gain_pub_; + ros::Publisher p_matrix_pseudo_inverse_inertia_pub_; + + bool verbose_; + boost::shared_ptr > lqi_server_; + dynamic_reconfigure::Server::CallbackType dynamic_reconf_func_lqi_; + + double target_roll_, target_pitch_; + double candidate_yaw_term_; + std::vector target_base_thrust_; + + int lqi_mode_; + bool clamp_gain_; + Eigen::MatrixXd K_; + + Eigen::Vector3d lqi_roll_pitch_weight_, lqi_yaw_weight_, lqi_z_weight_; + std::vector r_; // matrix R + + std::vector pitch_gains_, roll_gains_, yaw_gains_, z_gains_; + + bool gyro_moment_compensation_; + + bool realtime_update_; + std::thread gain_generator_thread_; + + //private functions + virtual bool checkRobotModel(); + void resetGain() { K_ = Eigen::MatrixXd(); } + + virtual void rosParamInit(); + virtual void controlCore() override; + + virtual bool optimalGain(); + virtual void clampGain(); + virtual void publishGain(); + + virtual void sendCmd() override; + virtual void sendFourAxisCommand(); + + virtual void allocateYawTerm(); + void cfgLQICallback(aerial_robot_control::LQIConfig &config, uint32_t level); //dynamic reconfigure + + void sendRotationalInertiaComp(); + + void gainGeneratorFunc(); + }; +}; diff --git a/aerial_robot_control/include/aerial_robot_control/control/under_actuated_tilted_lqi_controller.h b/aerial_robot_control/include/aerial_robot_control/control/under_actuated_tilted_lqi_controller.h new file mode 100644 index 000000000..2225b4f36 --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/control/under_actuated_tilted_lqi_controller.h @@ -0,0 +1,70 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include +#include + +namespace aerial_robot_control +{ + class UnderActuatedTiltedLQIController: public UnderActuatedLQIController + { + public: + UnderActuatedTiltedLQIController() {} + virtual ~UnderActuatedTiltedLQIController() = default; + + void initialize(ros::NodeHandle nh, ros::NodeHandle nhp, + boost::shared_ptr robot_model, + boost::shared_ptr estimator, + boost::shared_ptr navigator, + double ctrl_loop_rate); + + protected: + + ros::Publisher desired_baselink_rot_pub_; + + double trans_constraint_weight_; + double att_control_weight_; + + double z_limit_; + + void controlCore() override; + bool optimalGain() override; + void publishGain() override; + void rosParamInit() override; + + }; +}; diff --git a/aerial_robot_control/plugins/flight_control_plugins.xml b/aerial_robot_control/plugins/flight_control_plugins.xml index 2decd12e1..ec15a9b83 100644 --- a/aerial_robot_control/plugins/flight_control_plugins.xml +++ b/aerial_robot_control/plugins/flight_control_plugins.xml @@ -8,4 +8,12 @@ controller for under actuated model (e.g., quad) + + controller for under actuated model (e.g., quad) with LQI controller + + + + controller for under actuated model (e.g., quad) with LQI Tilted controller + + diff --git a/aerial_robot_control/src/control/base/pose_linear_controller.cpp b/aerial_robot_control/src/control/base/pose_linear_controller.cpp index dae89e910..4f5ef73fc 100644 --- a/aerial_robot_control/src/control/base/pose_linear_controller.cpp +++ b/aerial_robot_control/src/control/base/pose_linear_controller.cpp @@ -357,7 +357,7 @@ namespace aerial_robot_control pid_pub_.publish(pid_msg_); } - void PoseLinearController::cfgPidCallback(aerial_robot_control::PidControlConfig &config, uint32_t level, std::vector controller_indices) + void PoseLinearController::cfgPidCallback(aerial_robot_control::PIDConfig &config, uint32_t level, std::vector controller_indices) { using Levels = aerial_robot_msgs::DynamicReconfigureLevels; if(config.pid_control_flag) diff --git a/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp b/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp new file mode 100644 index 000000000..427b14a8e --- /dev/null +++ b/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp @@ -0,0 +1,553 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +using namespace aerial_robot_control; + +UnderActuatedLQIController::UnderActuatedLQIController(): + target_roll_(0), target_pitch_(0), candidate_yaw_term_(0) +{ + lqi_roll_pitch_weight_.setZero(); + lqi_yaw_weight_.setZero(); + lqi_z_weight_.setZero(); +} + + +void UnderActuatedLQIController::initialize(ros::NodeHandle nh, + ros::NodeHandle nhp, + boost::shared_ptr robot_model, + boost::shared_ptr estimator, + boost::shared_ptr navigator, + double ctrl_loop_rate) +{ + PoseLinearController::initialize(nh, nhp, robot_model, estimator, navigator, ctrl_loop_rate); + + rosParamInit(); + + //publisher + rpy_gain_pub_ = nh_.advertise("rpy/gain", 1); + flight_cmd_pub_ = nh_.advertise("four_axes/command", 1); + four_axis_gain_pub_ = nh_.advertise("debug/four_axes/gain", 1); + p_matrix_pseudo_inverse_inertia_pub_ = nh_.advertise("p_matrix_pseudo_inverse_inertia", 1); + + //dynamic reconfigure server + ros::NodeHandle control_nh(nh_, "controller"); + lqi_server_ = boost::make_shared >(ros::NodeHandle(control_nh, "lqi")); + dynamic_reconf_func_lqi_ = boost::bind(&UnderActuatedLQIController::cfgLQICallback, this, _1, _2); + lqi_server_->setCallback(dynamic_reconf_func_lqi_); + + //gains + pitch_gains_.resize(motor_num_, Eigen::Vector3d(0,0,0)); + roll_gains_.resize(motor_num_, Eigen::Vector3d(0,0,0)); + z_gains_.resize(motor_num_, Eigen::Vector3d(0,0,0)); + yaw_gains_.resize(motor_num_, Eigen::Vector3d(0,0,0)); + + //message + target_base_thrust_.resize(motor_num_); + pid_msg_.z.total.resize(motor_num_); + pid_msg_.z.p_term.resize(motor_num_); + pid_msg_.z.i_term.resize(motor_num_); + pid_msg_.z.d_term.resize(motor_num_); + pid_msg_.yaw.total.resize(motor_num_); + pid_msg_.yaw.p_term.resize(motor_num_); + pid_msg_.yaw.i_term.resize(motor_num_); + pid_msg_.yaw.d_term.resize(motor_num_); + + if (robot_model_->getJointNum() > 0) realtime_update_ = true; + if (realtime_update_) { + gain_generator_thread_ = std::thread(boost::bind(&UnderActuatedLQIController::gainGeneratorFunc, this)); + } +} + +UnderActuatedLQIController::~UnderActuatedLQIController() +{ + if(realtime_update_) gain_generator_thread_.join(); +} + +void UnderActuatedLQIController::gainGeneratorFunc() +{ + double rate; + ros::NodeHandle control_nh(nh_, "controller"); + ros::NodeHandle lqi_nh(control_nh, "lqi"); + lqi_nh.param("gain_generate_rate", rate, 15.0); + ros::Rate loop_rate(rate); + + while(ros::ok()) + { + if(checkRobotModel()) + { + if(optimalGain()) + { + clampGain(); + publishGain(); + } + else + ROS_ERROR_NAMED("LQI gain generator", "LQI gain generator: can not solve hamilton matrix"); + } + else + { + resetGain(); + } + + loop_rate.sleep(); + } +} + + +void UnderActuatedLQIController::sendCmd() +{ + PoseLinearController::sendCmd(); + + sendFourAxisCommand(); + sendRotationalInertiaComp(); +} + +void UnderActuatedLQIController::sendFourAxisCommand() +{ + spinal::FourAxisCommand flight_command_data; + flight_command_data.angles[0] = target_roll_; + flight_command_data.angles[1] = target_pitch_; + flight_command_data.angles[2] = candidate_yaw_term_; + flight_command_data.base_thrust = target_base_thrust_; + flight_cmd_pub_.publish(flight_command_data); +} + +void UnderActuatedLQIController::controlCore() +{ + PoseLinearController::controlCore(); + + if (navigator_->getNaviState() == aerial_robot_navigation::START_STATE && robot_model_->getJointNum() == 0) { + + // publish gains in start phase for general multirotor + + if(optimalGain()) { + clampGain(); + publishGain(); + ROS_INFO_NAMED("LQI gain generator", "LQI gain generator: send LQI gains"); + } + else { + ROS_ERROR_NAMED("LQI gain generator", "LQI gain generator: can not solve hamilton matrix"); + } + } + + tf::Vector3 target_acc_w(pid_controllers_.at(X).result(), + pid_controllers_.at(Y).result(), + pid_controllers_.at(Z).result()); + tf::Vector3 target_acc_dash = (tf::Matrix3x3(tf::createQuaternionFromYaw(rpy_.z()))).inverse() * target_acc_w; + + target_pitch_ = target_acc_dash.x() / aerial_robot_estimation::G; + target_roll_ = -target_acc_dash.y() / aerial_robot_estimation::G; + + Eigen::VectorXd target_thrust_z_term = Eigen::VectorXd::Zero(motor_num_); + for(int i = 0; i < motor_num_; i++) + { + double p_term = z_gains_.at(i)[0] * pid_controllers_.at(Z).getErrP(); + double i_term = z_gains_.at(i)[1] * pid_controllers_.at(Z).getErrI(); + double d_term = z_gains_.at(i)[2] * pid_controllers_.at(Z).getErrD(); + target_thrust_z_term(i) = p_term + i_term + d_term; + pid_msg_.z.p_term.at(i) = p_term; + pid_msg_.z.i_term.at(i) = i_term; + pid_msg_.z.d_term.at(i) = d_term; + } + + // constraint z (also I term) + int index; + double max_term = target_thrust_z_term.cwiseAbs().maxCoeff(&index); + double residual = max_term - pid_controllers_.at(Z).getLimitSum(); + if(residual > 0) + { + pid_controllers_.at(Z).setErrI(pid_controllers_.at(Z).getPrevErrI()); + target_thrust_z_term *= (1 - residual / max_term); + } + + for(int i = 0; i < motor_num_; i++) + { + target_base_thrust_.at(i) = target_thrust_z_term(i); + pid_msg_.z.total.at(i) = target_thrust_z_term(i); + } + + allocateYawTerm(); +} + +void UnderActuatedLQIController::allocateYawTerm() +{ + Eigen::VectorXd target_thrust_yaw_term = Eigen::VectorXd::Zero(motor_num_); + for(int i = 0; i < motor_num_; i++) + { + double p_term = yaw_gains_.at(i)[0] * pid_controllers_.at(YAW).getErrP(); + double i_term = yaw_gains_.at(i)[1] * pid_controllers_.at(YAW).getErrI(); + double d_term = yaw_gains_.at(i)[2] * pid_controllers_.at(YAW).getErrD(); + target_thrust_yaw_term(i) = p_term + i_term + d_term; + pid_msg_.yaw.p_term.at(i) = p_term; + pid_msg_.yaw.i_term.at(i) = i_term; + pid_msg_.yaw.d_term.at(i) = d_term; + } + // constraint yaw (also I term) + int index; + double max_term = target_thrust_yaw_term.cwiseAbs().maxCoeff(&index); + double residual = max_term - pid_controllers_.at(YAW).getLimitSum(); + if(residual > 0) + { + pid_controllers_.at(YAW).setErrI(pid_controllers_.at(YAW).getPrevErrI()); + target_thrust_yaw_term *= (1 - residual / max_term); + } + + // special process for yaw since the bandwidth between PC and spinal + double max_yaw_scale = 0; // for reconstruct yaw control term in spinal + for(int i = 0; i < motor_num_; i++) + { + pid_msg_.yaw.total.at(i) = target_thrust_yaw_term(i); + + if(yaw_gains_[i][2] > max_yaw_scale) + { + max_yaw_scale = yaw_gains_[i][2]; + candidate_yaw_term_ = target_thrust_yaw_term(i); + } + } +} + +bool UnderActuatedLQIController::optimalGain() +{ + // referece: + // M, Zhao, et.al, "Transformable multirotor with two-dimensional multilinks: modeling, control, and whole-body aerial manipulation" + // Sec. 3.2 + + Eigen::MatrixXd P = robot_model_->calcWrenchMatrixOnCoG(); + Eigen::MatrixXd P_dash = Eigen::MatrixXd::Zero(lqi_mode_, motor_num_); + Eigen::MatrixXd inertia = robot_model_->getInertia(); + P_dash.row(0) = P.row(2) / robot_model_->getMass(); // z + P_dash.bottomRows(lqi_mode_ - 1) = (inertia.inverse() * P.bottomRows(3)).topRows(lqi_mode_ - 1); // roll, pitch, yaw + + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(lqi_mode_ * 3, lqi_mode_ * 3); + Eigen::MatrixXd B = Eigen::MatrixXd::Zero(lqi_mode_ * 3, motor_num_); + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(lqi_mode_, lqi_mode_ * 3); + for(int i = 0; i < lqi_mode_; i++) + { + A(2 * i, 2 * i + 1) = 1; + B.row(2 * i + 1) = P_dash.row(i); + C(i, 2 * i) = 1; + } + A.block(lqi_mode_ * 2, 0, lqi_mode_, lqi_mode_ * 3) = -C; + + ROS_DEBUG_STREAM_NAMED("LQI gain generator", "LQI gain generator: B: \n" << B ); + + Eigen::VectorXd q_diagonals(lqi_mode_ * 3); + if(lqi_mode_ == 3) + { + q_diagonals << lqi_z_weight_(0), lqi_z_weight_(2), lqi_roll_pitch_weight_(0), lqi_roll_pitch_weight_(2), lqi_roll_pitch_weight_(0), lqi_roll_pitch_weight_(2), lqi_z_weight_(1), lqi_roll_pitch_weight_(1), lqi_roll_pitch_weight_(1); + } + else + { + q_diagonals << lqi_z_weight_(0), lqi_z_weight_(2), lqi_roll_pitch_weight_(0), lqi_roll_pitch_weight_(2), lqi_roll_pitch_weight_(0), lqi_roll_pitch_weight_(2), lqi_yaw_weight_(0), lqi_yaw_weight_(2), lqi_z_weight_(1), lqi_roll_pitch_weight_(1), lqi_roll_pitch_weight_(1), lqi_yaw_weight_(1); + } + Eigen::MatrixXd Q = q_diagonals.asDiagonal(); + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(motor_num_, motor_num_); + for(int i = 0; i < motor_num_; ++i) R(i,i) = r_.at(i); + + /* solve continuous-time algebraic Ricatti equation */ + double t = ros::Time::now().toSec(); + + if(K_.cols() != lqi_mode_ * 3) + { + resetGain(); // four axis -> three axis and vice versa + } + + bool use_kleinman_method = true; + if(K_.cols() == 0 || K_.rows() == 0) + { + ROS_DEBUG_STREAM_NAMED("LQI gain generator", "LQI gain generator: do not use kleinman method"); + use_kleinman_method = false; + } + if(!control_utils::care(A, B, R, Q, K_, use_kleinman_method)) + { + ROS_ERROR_STREAM_NAMED("LQI gain generator", "LQI gain generator: error in solver of continuous-time algebraic riccati equation"); + return false; + } + + ROS_DEBUG_STREAM_NAMED("LQI gain generator", "LQI gain generator: CARE: %f sec" << ros::Time::now().toSec() - t); + ROS_DEBUG_STREAM_NAMED("LQI gain generator", "LQI gain generator: K \n" << K_); + + + for(int i = 0; i < motor_num_; ++i) + { + roll_gains_.at(i) = Eigen::Vector3d(-K_(i,2), K_(i, lqi_mode_ * 2 + 1), -K_(i,3)); + pitch_gains_.at(i) = Eigen::Vector3d(-K_(i,4), K_(i, lqi_mode_ * 2 + 2), -K_(i,5)); + z_gains_.at(i) = Eigen::Vector3d(-K_(i,0), K_(i, lqi_mode_ * 2), -K_(i,1)); + if(lqi_mode_ == 4) yaw_gains_.at(i) = Eigen::Vector3d(-K_(i,6), K_(i, lqi_mode_ * 2 + 3), -K_(i,7)); + else yaw_gains_.at(i).setZero(); + } + + return true; +} + +void UnderActuatedLQIController::clampGain() +{ + /* avoid the violation of 16int_t range because of spinal::RollPitchYawTerms */ + double max_gain_thresh = 32.767; + double max_roll_p_gain = 0, max_roll_d_gain = 0, max_pitch_p_gain = 0, max_pitch_d_gain = 0, max_yaw_d_gain = 0; + for(int i = 0; i < motor_num_; ++i) + { + if(max_roll_p_gain < fabs(roll_gains_.at(i)[0])) max_roll_p_gain = fabs(roll_gains_.at(i)[0]); + if(max_roll_d_gain < fabs(roll_gains_.at(i)[2])) max_roll_d_gain = fabs(roll_gains_.at(i)[2]); + if(max_pitch_p_gain < fabs(pitch_gains_.at(i)[0])) max_pitch_p_gain = fabs(pitch_gains_.at(i)[0]); + if(max_pitch_d_gain < fabs(pitch_gains_.at(i)[2])) max_pitch_d_gain = fabs(pitch_gains_.at(i)[2]); + if(max_yaw_d_gain < fabs(yaw_gains_.at(i)[2])) max_yaw_d_gain = fabs(yaw_gains_.at(i)[2]); + } + + double roll_p_gain_scale = 1, roll_d_gain_scale = 1, pitch_p_gain_scale = 1, pitch_d_gain_scale = 1, yaw_d_gain_scale = 1; + if(max_roll_p_gain > max_gain_thresh) + { + ROS_WARN_STREAM_NAMED("LQI gain generator", "LQI gain generator: the max roll p gain violate the range of int16_t: " << max_roll_p_gain); + roll_p_gain_scale = max_gain_thresh / max_roll_p_gain; + } + if(max_roll_d_gain > max_gain_thresh) + { + ROS_WARN_STREAM_NAMED("LQI gain generator", "LQI gain generator: the max roll d gain violate the range of int16_t: " << max_roll_d_gain); + roll_d_gain_scale = max_gain_thresh / max_roll_d_gain; + } + if(max_pitch_p_gain > max_gain_thresh) + { + ROS_WARN_STREAM_NAMED("LQI gain generator", "LQI gain generator: the max pitch p gain violate the range of int16_t: " << max_pitch_p_gain); + pitch_p_gain_scale = max_gain_thresh / max_pitch_p_gain; + } + if(max_pitch_d_gain > max_gain_thresh) + { + ROS_WARN_STREAM_NAMED("LQI gain generator", "LQI gain generator: the max pitch d gain violate the range of int16_t: " << max_pitch_d_gain); + pitch_d_gain_scale = max_gain_thresh / max_pitch_d_gain; + } + if(max_yaw_d_gain > max_gain_thresh) + { + ROS_WARN_STREAM_NAMED("LQI gain generator", "LQI gain generator: the max yaw d gain violate the range of int16_t: " << max_yaw_d_gain); + yaw_d_gain_scale = max_gain_thresh / max_yaw_d_gain; + } + + for(int i = 0; i < motor_num_; ++i) + { + roll_gains_.at(i)[0] *= roll_p_gain_scale; + roll_gains_.at(i)[2] *= roll_d_gain_scale; + + pitch_gains_.at(i)[0] *= pitch_p_gain_scale; + pitch_gains_.at(i)[2] *= pitch_d_gain_scale; + + yaw_gains_.at(i)[2] *= yaw_d_gain_scale; + } +} + +bool UnderActuatedLQIController::checkRobotModel() +{ + if(!robot_model_->initialized()) + { + ROS_DEBUG_NAMED("LQI gain generator", "LQI gain generator: robot model is not initiliazed"); + return false; + } + + return true; +} + + +void UnderActuatedLQIController::rosParamInit() +{ + ros::NodeHandle control_nh(nh_, "controller"); + ros::NodeHandle lqi_nh(control_nh, "lqi"); + getParam(lqi_nh, "clamp_gain", clamp_gain_, true); + getParam(lqi_nh, "realtime_update", realtime_update_, false); + getParam(lqi_nh, "gyro_moment_compensation", gyro_moment_compensation_, false); + + /* propeller direction and lqi R */ + r_.resize(motor_num_); // motor_num is not set + for(int i = 0; i < robot_model_->getRotorNum(); ++i) { + std::stringstream ss; + ss << i + 1; + /* R */ + getParam(lqi_nh, std::string("r") + ss.str(), r_.at(i), 1.0); + } + + getParam(lqi_nh, "roll_pitch_p", lqi_roll_pitch_weight_[0], 1.0); + getParam(lqi_nh, "roll_pitch_i", lqi_roll_pitch_weight_[1], 1.0); + getParam(lqi_nh, "roll_pitch_d", lqi_roll_pitch_weight_[2], 1.0); + getParam(lqi_nh, "yaw_p", lqi_yaw_weight_[0], 1.0); + getParam(lqi_nh, "yaw_i", lqi_yaw_weight_[1], 1.0); + getParam(lqi_nh, "yaw_d", lqi_yaw_weight_[2], 1.0); + getParam(lqi_nh, "z_p", lqi_z_weight_[0], 1.0); + getParam(lqi_nh, "z_i", lqi_z_weight_[1], 1.0); + getParam(lqi_nh, "z_d", lqi_z_weight_[2], 1.0); + + getParam(lqi_nh, "lqi_mode", lqi_mode_, 4); + if (lqi_mode_ != 3 && lqi_mode_ != 4) { + ROS_ERROR_STREAM_NAMED("LQI gain generator", "LQI gain generator: lqi model should be 3 or 4, " << lqi_mode_ << " is not allowed."); + } +} + +void UnderActuatedLQIController::publishGain() +{ + aerial_robot_msgs::FourAxisGain four_axis_gain_msg; + spinal::RollPitchYawTerms rpy_gain_msg; // to spinal + rpy_gain_msg.motors.resize(motor_num_); + + for(int i = 0; i < motor_num_; ++i) + { + four_axis_gain_msg.roll_p_gain.push_back(roll_gains_.at(i)[0]); + four_axis_gain_msg.roll_i_gain.push_back(roll_gains_.at(i)[1]); + four_axis_gain_msg.roll_d_gain.push_back(roll_gains_.at(i)[2]); + + four_axis_gain_msg.pitch_p_gain.push_back(pitch_gains_.at(i)[0]); + four_axis_gain_msg.pitch_i_gain.push_back(pitch_gains_.at(i)[1]); + four_axis_gain_msg.pitch_d_gain.push_back(pitch_gains_.at(i)[2]); + + four_axis_gain_msg.yaw_p_gain.push_back(yaw_gains_.at(i)[0]); + four_axis_gain_msg.yaw_i_gain.push_back(yaw_gains_.at(i)[1]); + four_axis_gain_msg.yaw_d_gain.push_back(yaw_gains_.at(i)[2]); + + four_axis_gain_msg.z_p_gain.push_back(z_gains_.at(i)[0]); + four_axis_gain_msg.z_i_gain.push_back(z_gains_.at(i)[1]); + four_axis_gain_msg.z_d_gain.push_back(z_gains_.at(i)[2]); + + /* to flight controller via rosserial scaling by 1000 */ + rpy_gain_msg.motors[i].roll_p = roll_gains_.at(i)[0] * 1000; + rpy_gain_msg.motors[i].roll_i = roll_gains_.at(i)[1] * 1000; + rpy_gain_msg.motors[i].roll_d = roll_gains_.at(i)[2] * 1000; + + rpy_gain_msg.motors[i].pitch_p = pitch_gains_.at(i)[0] * 1000; + rpy_gain_msg.motors[i].pitch_i = pitch_gains_.at(i)[1] * 1000; + rpy_gain_msg.motors[i].pitch_d = pitch_gains_.at(i)[2] * 1000; + + rpy_gain_msg.motors[i].yaw_d = yaw_gains_.at(i)[2] * 1000; + } + rpy_gain_pub_.publish(rpy_gain_msg); + four_axis_gain_pub_.publish(four_axis_gain_msg); +} + +void UnderActuatedLQIController::cfgLQICallback(aerial_robot_control::LQIConfig &config, uint32_t level) +{ + using Levels = aerial_robot_msgs::DynamicReconfigureLevels; + if(config.lqi_flag) + { + switch(level) + { + case Levels::RECONFIGURE_LQI_ROLL_PITCH_P: + ROS_INFO_STREAM_NAMED("LQI gain generator", "LQI gain generator: change the p gain weight of roll and pitch from " << lqi_roll_pitch_weight_.x() << " to " << config.roll_pitch_p); + lqi_roll_pitch_weight_.x() = config.roll_pitch_p; + break; + case Levels::RECONFIGURE_LQI_ROLL_PITCH_I: + ROS_INFO_STREAM_NAMED("LQI gain generator", "LQI gain generator: change the i gain weight of roll and pitch from " << lqi_roll_pitch_weight_.y() << " to " << config.roll_pitch_i); + lqi_roll_pitch_weight_.y() = config.roll_pitch_i; + break; + case Levels::RECONFIGURE_LQI_ROLL_PITCH_D: + ROS_INFO_STREAM_NAMED("LQI gain generator", "LQI gain generator: change the d gain weight of roll and pitch from " << lqi_roll_pitch_weight_.z() << " to " << config.roll_pitch_d); + lqi_roll_pitch_weight_.z() = config.roll_pitch_d; + break; + case Levels::RECONFIGURE_LQI_YAW_P: + ROS_INFO_STREAM_NAMED("LQI gain generator", "LQI gain generator: change the p gain weight of yaw from " << lqi_yaw_weight_.x() << " to " << config.yaw_p); + lqi_yaw_weight_.x() = config.yaw_p; + break; + case Levels::RECONFIGURE_LQI_YAW_I: + ROS_INFO_STREAM_NAMED("LQI gain generator", "LQI gain generator: change the i gain weight of yaw from " << lqi_yaw_weight_.y() << " to " << config.yaw_i); + lqi_yaw_weight_.y() = config.yaw_i; + break; + case Levels::RECONFIGURE_LQI_YAW_D: + ROS_INFO_STREAM_NAMED("LQI gain generator", "LQI gain generator: change the d gain weight of yaw from " << lqi_yaw_weight_.z() << " to " << config.yaw_d); + lqi_yaw_weight_.z() = config.yaw_d; + break; + case Levels::RECONFIGURE_LQI_Z_P: + ROS_INFO_STREAM_NAMED("LQI gain generator", "LQI gain generator: change the p gain weight of z from " << lqi_z_weight_.x() << " to " << config.z_p); + lqi_z_weight_.x() = config.z_p; + break; + case Levels::RECONFIGURE_LQI_Z_I: + ROS_INFO_STREAM_NAMED("LQI gain generator", "LQI gain generator: change the i gain weight of z from " << lqi_z_weight_.y() << " to " << config.z_i); + lqi_z_weight_.y() = config.z_i; + break; + case Levels::RECONFIGURE_LQI_Z_D: + ROS_INFO_STREAM_NAMED("LQI gain generator", "LQI gain generator: change the d gain weight of z from " << lqi_z_weight_.z() << " to " << config.z_d); + lqi_z_weight_.z() = config.z_d; + break; + default : + break; + } + + if (realtime_update_) { + // instantly modify gain if no joint angles + + if(optimalGain()) { + clampGain(); + publishGain(); + } + else { + ROS_ERROR_NAMED("LQI gain generator", "LQI gain generator: can not solve hamilton matrix"); + } + } + + } +} + +void UnderActuatedLQIController::sendRotationalInertiaComp() +{ + if(!gyro_moment_compensation_) return; + + Eigen::MatrixXd P = robot_model_->calcWrenchMatrixOnCoG(); + Eigen::MatrixXd p_mat_pseudo_inv_ = aerial_robot_model::pseudoinverse(P.middleRows(2, lqi_mode_)); + + spinal::PMatrixPseudoInverseWithInertia p_pseudo_inverse_with_inertia_msg; // to spinal + p_pseudo_inverse_with_inertia_msg.pseudo_inverse.resize(motor_num_); + + for(int i = 0; i < motor_num_; ++i) + { + /* the p matrix pseudo inverse and inertia */ + p_pseudo_inverse_with_inertia_msg.pseudo_inverse[i].r = p_mat_pseudo_inv_(i, 1) * 1000; + p_pseudo_inverse_with_inertia_msg.pseudo_inverse[i].p = p_mat_pseudo_inv_(i, 2) * 1000; + if(lqi_mode_ == 4) + p_pseudo_inverse_with_inertia_msg.pseudo_inverse[i].y = p_mat_pseudo_inv_(i, 3) * 1000; + else + p_pseudo_inverse_with_inertia_msg.pseudo_inverse[i].y = 0; + } + + /* the articulated inertia */ + Eigen::Matrix3d inertia = robot_model_->getInertia(); + p_pseudo_inverse_with_inertia_msg.inertia[0] = inertia(0, 0) * 1000; + p_pseudo_inverse_with_inertia_msg.inertia[1] = inertia(1, 1) * 1000; + p_pseudo_inverse_with_inertia_msg.inertia[2] = inertia(2, 2) * 1000; + p_pseudo_inverse_with_inertia_msg.inertia[3] = inertia(0, 1) * 1000; + p_pseudo_inverse_with_inertia_msg.inertia[4] = inertia(1, 2) * 1000; + p_pseudo_inverse_with_inertia_msg.inertia[5] = inertia(0, 2) * 1000; + + p_matrix_pseudo_inverse_inertia_pub_.publish(p_pseudo_inverse_with_inertia_msg); +} + + + +/* plugin registration */ +#include +PLUGINLIB_EXPORT_CLASS(aerial_robot_control::UnderActuatedLQIController, aerial_robot_control::ControlBase); diff --git a/aerial_robot_control/src/control/under_actuated_tilted_lqi_controller.cpp b/aerial_robot_control/src/control/under_actuated_tilted_lqi_controller.cpp new file mode 100644 index 000000000..8a52965ae --- /dev/null +++ b/aerial_robot_control/src/control/under_actuated_tilted_lqi_controller.cpp @@ -0,0 +1,179 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +using namespace aerial_robot_control; + +void UnderActuatedTiltedLQIController::initialize(ros::NodeHandle nh, + ros::NodeHandle nhp, + boost::shared_ptr robot_model, + boost::shared_ptr estimator, + boost::shared_ptr navigator, + double ctrl_loop_rate) +{ + UnderActuatedLQIController::initialize(nh, nhp, robot_model, estimator, navigator, ctrl_loop_rate); + + desired_baselink_rot_pub_ = nh_.advertise("desire_coordinate", 1); + + pid_msg_.z.p_term.resize(1); + pid_msg_.z.i_term.resize(1); + pid_msg_.z.d_term.resize(1); + z_limit_ = pid_controllers_.at(Z).getLimitSum(); + pid_controllers_.at(Z).setLimitSum(1e6); // do not clamp the sum of PID terms for z axis +} + +void UnderActuatedTiltedLQIController::controlCore() +{ + PoseLinearController::controlCore(); + + tf::Vector3 target_acc_w(pid_controllers_.at(X).result(), + pid_controllers_.at(Y).result(), + pid_controllers_.at(Z).result()); + + tf::Vector3 target_acc_dash = (tf::Matrix3x3(tf::createQuaternionFromYaw(rpy_.z()))).inverse() * target_acc_w; + + target_pitch_ = atan2(target_acc_dash.x(), target_acc_dash.z()); + target_roll_ = atan2(-target_acc_dash.y(), sqrt(target_acc_dash.x() * target_acc_dash.x() + target_acc_dash.z() * target_acc_dash.z())); + + if(navigator_->getForceLandingFlag()) + { + target_pitch_ = 0; + target_roll_ = 0; + } + + Eigen::VectorXd f = robot_model_->getStaticThrust(); + Eigen::VectorXd allocate_scales = f / f.sum() * robot_model_->getMass(); + Eigen::VectorXd target_thrust_z_term = allocate_scales * target_acc_w.length(); + + // constraint z (also I term) + int index; + double max_term = target_thrust_z_term.cwiseAbs().maxCoeff(&index); + double residual = max_term - z_limit_; + + if(residual > 0) + { + pid_controllers_.at(Z).setErrI(pid_controllers_.at(Z).getPrevErrI()); + target_thrust_z_term *= (1 - residual / max_term); + } + + for(int i = 0; i < motor_num_; i++) + { + target_base_thrust_.at(i) = target_thrust_z_term(i); + pid_msg_.z.total.at(i) = target_thrust_z_term(i); + } + + allocateYawTerm(); +} + +bool UnderActuatedTiltedLQIController::optimalGain() +{ + /* calculate the P_orig pseudo inverse */ + Eigen::MatrixXd P = robot_model_->calcWrenchMatrixOnCoG(); + Eigen::MatrixXd inertia = robot_model_->getInertia(); + Eigen::MatrixXd P_dash = inertia.inverse() * P.bottomRows(3); // roll, pitch, yaw + + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(9, 9); + Eigen::MatrixXd B = Eigen::MatrixXd::Zero(9, motor_num_); + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(3, 9); + for(int i = 0; i < 3; i++) + { + A(2 * i, 2 * i + 1) = 1; + B.row(2 * i + 1) = P_dash.row(i); + C(i, 2 * i) = 1; + } + A.block(6, 0, 3, 9) = -C; + + ROS_DEBUG_STREAM_NAMED("LQI gain generator", "LQI gain generator: B: \n" << B ); + + Eigen::VectorXd q_diagonals(9); + q_diagonals << lqi_roll_pitch_weight_(0), lqi_roll_pitch_weight_(2), lqi_roll_pitch_weight_(0), lqi_roll_pitch_weight_(2), lqi_yaw_weight_(0), lqi_yaw_weight_(2), lqi_roll_pitch_weight_(1), lqi_roll_pitch_weight_(1), lqi_yaw_weight_(1); + Eigen::MatrixXd Q = q_diagonals.asDiagonal(); + + Eigen::MatrixXd P_trans = P.topRows(3) / robot_model_->getMass() ; + Eigen::MatrixXd R_trans = P_trans.transpose() * P_trans; + Eigen::MatrixXd R_input = Eigen::MatrixXd::Identity(motor_num_, motor_num_); + Eigen::MatrixXd R = R_trans * trans_constraint_weight_ + R_input * att_control_weight_; + + double t = ros::Time::now().toSec(); + bool use_kleinman_method = true; + if(K_.cols() == 0 || K_.rows() == 0) use_kleinman_method = false; + if(!control_utils::care(A, B, R, Q, K_, use_kleinman_method)) + { + ROS_ERROR_STREAM("error in solver of continuous-time algebraic riccati equation"); + return false; + } + + ROS_DEBUG_STREAM_NAMED("LQI gain generator", "LQI gain generator: CARE: %f sec" << ros::Time::now().toSec() - t); + ROS_DEBUG_STREAM_NAMED("LQI gain generator", "LQI gain generator: K \n" << K_); + + for(int i = 0; i < motor_num_; ++i) + { + roll_gains_.at(i) = Eigen::Vector3d(-K_(i,0), K_(i,6), -K_(i,1)); + pitch_gains_.at(i) = Eigen::Vector3d(-K_(i,2), K_(i,7), -K_(i,3)); + yaw_gains_.at(i) = Eigen::Vector3d(-K_(i,4), K_(i,8), -K_(i,5)); + } + + return true; +} + +void UnderActuatedTiltedLQIController::publishGain() +{ + UnderActuatedLQIController::publishGain(); + + double roll,pitch, yaw; + robot_model_->getCogDesireOrientation().GetRPY(roll, pitch, yaw); + + spinal::DesireCoord coord_msg; + coord_msg.roll = roll; + coord_msg.pitch = pitch; + desired_baselink_rot_pub_.publish(coord_msg); +} + +void UnderActuatedTiltedLQIController::rosParamInit() +{ + UnderActuatedLQIController::rosParamInit(); + + ros::NodeHandle control_nh(nh_, "controller"); + ros::NodeHandle lqi_nh(control_nh, "lqi"); + + getParam(lqi_nh, "trans_constraint_weight", trans_constraint_weight_, 1.0); + getParam(lqi_nh, "att_control_weight", att_control_weight_, 1.0); +} + + +/* plugin registration */ +#include +PLUGINLIB_EXPORT_CLASS(aerial_robot_control::UnderActuatedTiltedLQIController, aerial_robot_control::ControlBase); diff --git a/aerial_robot_model/CMakeLists.txt b/aerial_robot_model/CMakeLists.txt index 61c2ece0f..84fba5355 100644 --- a/aerial_robot_model/CMakeLists.txt +++ b/aerial_robot_model/CMakeLists.txt @@ -89,7 +89,7 @@ add_dependencies(aerial_robot_model_ros ${PROJECT_NAME}_generate_messages_cpp sp add_library(robot_model_pluginlib src/model/plugin/multirotor_robot_model.cpp - src/model/plugin/tilted_underactuated_robot_model.cpp) + src/model/plugin/underactuated_tilted_robot_model.cpp) target_link_libraries(robot_model_pluginlib ${catkin_LIBRARIES} aerial_robot_model) add_library(servo_bridge src/servo_bridge/servo_bridge.cpp) diff --git a/aerial_robot_model/src/model/plugin/tilted_underactuated_robot_model.cpp b/aerial_robot_model/src/model/plugin/underactuated_tilted_robot_model.cpp similarity index 100% rename from aerial_robot_model/src/model/plugin/tilted_underactuated_robot_model.cpp rename to aerial_robot_model/src/model/plugin/underactuated_tilted_robot_model.cpp diff --git a/robots/dragon/CMakeLists.txt b/robots/dragon/CMakeLists.txt index db06670a4..4fadd1f0f 100644 --- a/robots/dragon/CMakeLists.txt +++ b/robots/dragon/CMakeLists.txt @@ -48,7 +48,7 @@ target_link_libraries(dragon_robot_model ${catkin_LIBRARIES} ${NLOPT_LIBRARIES}) add_library(dragon_aerial_robot_controllib src/control/lqi_gimbal_control.cpp src/control/full_vectoring_control.cpp) target_link_libraries (dragon_aerial_robot_controllib dragon_robot_model dragon_navigation dragon_sensor_pluginlib ${catkin_LIBRARIES} ${Eigen3_LIBRARIES}) -add_dependencies(dragon_aerial_robot_controllib aerial_robot_msgs_generate_messages_cpp hydrus_gencfg) +add_dependencies(dragon_aerial_robot_controllib aerial_robot_msgs_generate_messages_cpp) add_library(dragon_navigation src/dragon_navigation.cpp) target_link_libraries(dragon_navigation ${catkin_LIBRARIES}) diff --git a/robots/hydrus/CMakeLists.txt b/robots/hydrus/CMakeLists.txt index 4c42d02f3..6a60cdce6 100644 --- a/robots/hydrus/CMakeLists.txt +++ b/robots/hydrus/CMakeLists.txt @@ -12,18 +12,14 @@ find_package(catkin REQUIRED COMPONENTS eigen_conversions tf_conversions std_srvs - dynamic_reconfigure ) find_package(Eigen3 REQUIRED) -generate_dynamic_reconfigure_options( - cfg/LQI.cfg) - catkin_package( INCLUDE_DIRS include test LIBRARIES hydrus_robot_model hydrus_controller_pluginlib hydrus_numerical_jacobians - CATKIN_DEPENDS roscpp aerial_robot_msgs aerial_robot_control aerial_robot_model spinal eigen_conversions tf_conversions std_srvs dynamic_reconfigure + CATKIN_DEPENDS roscpp aerial_robot_msgs aerial_robot_control aerial_robot_model spinal eigen_conversions tf_conversions std_srvs ) # Eigen requires optimization to get good performance @@ -59,7 +55,7 @@ add_library(hydrus_controller_pluginlib src/hydrus_lqi_controller.cpp src/hydrus_tilted_lqi_controller.cpp) target_link_libraries(hydrus_controller_pluginlib hydrus_robot_model ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES}) -add_dependencies(hydrus_controller_pluginlib aerial_robot_msgs_generate_messages_cpp spinal_generate_messages_cpp ${PROJECT_NAME}_gencfg) +add_dependencies(hydrus_controller_pluginlib aerial_robot_msgs_generate_messages_cpp spinal_generate_messages_cpp) # test # pre-build test code diff --git a/robots/hydrus/include/hydrus/hydrus_lqi_controller.h b/robots/hydrus/include/hydrus/hydrus_lqi_controller.h index b643242b2..ee257432e 100644 --- a/robots/hydrus/include/hydrus/hydrus_lqi_controller.h +++ b/robots/hydrus/include/hydrus/hydrus_lqi_controller.h @@ -35,26 +35,17 @@ #pragma once -#include -#include -#include -#include +#include #include -#include -#include -#include -#include -#include -#include namespace aerial_robot_control { - class HydrusLQIController: public PoseLinearController + class HydrusLQIController: public UnderActuatedLQIController { public: HydrusLQIController(); - virtual ~HydrusLQIController(); + virtual ~HydrusLQIController() = default; void initialize(ros::NodeHandle nh, ros::NodeHandle nhp, boost::shared_ptr robot_model, @@ -62,56 +53,8 @@ namespace aerial_robot_control boost::shared_ptr navigator, double ctrl_loop_rate); - const Eigen::MatrixXd& getPMatrix() const { return p_mat_; } - Eigen::MatrixXd getPMatrixPseudoInv() const { return p_mat_pseudo_inv_; } - protected: - boost::shared_ptr hydrus_robot_model_; - - ros::Publisher flight_cmd_pub_; //for spinal - ros::Publisher rpy_gain_pub_; //for spinal - ros::Publisher four_axis_gain_pub_; - ros::Publisher p_matrix_pseudo_inverse_inertia_pub_; - - int lqi_mode_; - std::thread gain_generator_thread_; - bool verbose_; - boost::shared_ptr > lqi_server_; - dynamic_reconfigure::Server::CallbackType dynamic_reconf_func_lqi_; - - double target_roll_, target_pitch_; // under-actuated - double candidate_yaw_term_; - std::vector target_base_thrust_; - - bool gyro_moment_compensation_; - bool clamp_gain_; - Eigen::MatrixXd K_; - - Eigen::Vector3d lqi_roll_pitch_weight_, lqi_yaw_weight_, lqi_z_weight_; - std::vector r_; // matrix R - - std::vector pitch_gains_, roll_gains_, yaw_gains_, z_gains_; - - //private functions - void resetGain() { K_ = Eigen::MatrixXd(); } - bool checkRobotModel(); - - virtual void rosParamInit(); - virtual void controlCore() override; - - virtual bool optimalGain(); - virtual void clampGain(); - virtual void publishGain(); - - virtual void sendCmd() override; - virtual void sendFourAxisCommand(); - - virtual void allocateYawTerm(); - void gainGeneratorFunc(); - void cfgLQICallback(hydrus::LQIConfig &config, uint32_t level); //dynamic reconfigure - - Eigen::MatrixXd p_mat_pseudo_inv_; // for compensation of cross term in the rotional dynamics - Eigen::MatrixXd p_mat_; + bool checkRobotModel() override; }; }; diff --git a/robots/hydrus/include/hydrus/hydrus_tilted_lqi_controller.h b/robots/hydrus/include/hydrus/hydrus_tilted_lqi_controller.h index 9ca42d1f4..68540d51d 100644 --- a/robots/hydrus/include/hydrus/hydrus_tilted_lqi_controller.h +++ b/robots/hydrus/include/hydrus/hydrus_tilted_lqi_controller.h @@ -35,16 +35,17 @@ #pragma once -#include -#include +#include +#include +#include namespace aerial_robot_control { - class HydrusTiltedLQIController: public HydrusLQIController + class HydrusTiltedLQIController: public UnderActuatedTiltedLQIController { public: - HydrusTiltedLQIController() {} - virtual ~HydrusTiltedLQIController() = default; + HydrusTiltedLQIController(); + ~HydrusTiltedLQIController() = default; void initialize(ros::NodeHandle nh, ros::NodeHandle nhp, boost::shared_ptr robot_model, @@ -53,18 +54,6 @@ namespace aerial_robot_control double ctrl_loop_rate); protected: - - ros::Publisher desired_baselink_rot_pub_; - - double trans_constraint_weight_; - double att_control_weight_; - - double z_limit_; - - void controlCore() override; - bool optimalGain() override; - void publishGain() override; - void rosParamInit() override; - + bool checkRobotModel() override; }; }; diff --git a/robots/hydrus/package.xml b/robots/hydrus/package.xml index 5207c5e82..41ce546bf 100644 --- a/robots/hydrus/package.xml +++ b/robots/hydrus/package.xml @@ -14,7 +14,6 @@ aerial_robot_control aerial_robot_model aerial_robot_msgs - dynamic_reconfigure eigen_conversions roscpp spinal @@ -26,7 +25,6 @@ aerial_robot_model aerial_robot_msgs aerial_robot_simulation - dynamic_reconfigure eigen_conversions roscpp spinal diff --git a/robots/hydrus/src/hydrus_lqi_controller.cpp b/robots/hydrus/src/hydrus_lqi_controller.cpp index 2fc0cb84f..815a33be1 100644 --- a/robots/hydrus/src/hydrus_lqi_controller.cpp +++ b/robots/hydrus/src/hydrus_lqi_controller.cpp @@ -3,11 +3,8 @@ using namespace aerial_robot_control; HydrusLQIController::HydrusLQIController(): - target_roll_(0), target_pitch_(0), candidate_yaw_term_(0) + UnderActuatedLQIController() { - lqi_roll_pitch_weight_.setZero(); - lqi_yaw_weight_.setZero(); - lqi_z_weight_.setZero(); } @@ -18,178 +15,13 @@ void HydrusLQIController::initialize(ros::NodeHandle nh, boost::shared_ptr navigator, double ctrl_loop_rate) { - PoseLinearController::initialize(nh, nhp, robot_model, estimator, navigator, ctrl_loop_rate); - - rosParamInit(); - - hydrus_robot_model_ = boost::dynamic_pointer_cast(robot_model); - - //publisher - rpy_gain_pub_ = nh_.advertise("rpy/gain", 1); - flight_cmd_pub_ = nh_.advertise("four_axes/command", 1); - four_axis_gain_pub_ = nh_.advertise("debug/four_axes/gain", 1); - p_matrix_pseudo_inverse_inertia_pub_ = nh_.advertise("p_matrix_pseudo_inverse_inertia", 1); - - //dynamic reconfigure server - ros::NodeHandle control_nh(nh_, "controller"); - lqi_server_ = boost::make_shared >(ros::NodeHandle(control_nh, "lqi")); - dynamic_reconf_func_lqi_ = boost::bind(&HydrusLQIController::cfgLQICallback, this, _1, _2); - lqi_server_->setCallback(dynamic_reconf_func_lqi_); - gain_generator_thread_ = std::thread(boost::bind(&HydrusLQIController::gainGeneratorFunc, this)); - - //gains - pitch_gains_.resize(motor_num_, Eigen::Vector3d(0,0,0)); - roll_gains_.resize(motor_num_, Eigen::Vector3d(0,0,0)); - z_gains_.resize(motor_num_, Eigen::Vector3d(0,0,0)); - yaw_gains_.resize(motor_num_, Eigen::Vector3d(0,0,0)); - - //message - target_base_thrust_.resize(motor_num_); - pid_msg_.z.total.resize(motor_num_); - pid_msg_.z.p_term.resize(motor_num_); - pid_msg_.z.i_term.resize(motor_num_); - pid_msg_.z.d_term.resize(motor_num_); - pid_msg_.yaw.total.resize(motor_num_); - pid_msg_.yaw.p_term.resize(motor_num_); - pid_msg_.yaw.i_term.resize(motor_num_); - pid_msg_.yaw.d_term.resize(motor_num_); -} - -HydrusLQIController::~HydrusLQIController() -{ - gain_generator_thread_.join(); -} - - -void HydrusLQIController::sendCmd() -{ - PoseLinearController::sendCmd(); - - sendFourAxisCommand(); -} - -void HydrusLQIController::sendFourAxisCommand() -{ - spinal::FourAxisCommand flight_command_data; - flight_command_data.angles[0] = target_roll_; - flight_command_data.angles[1] = target_pitch_; - flight_command_data.angles[2] = candidate_yaw_term_; - flight_command_data.base_thrust = target_base_thrust_; - flight_cmd_pub_.publish(flight_command_data); -} - -void HydrusLQIController::controlCore() -{ - PoseLinearController::controlCore(); - - tf::Vector3 target_acc_w(pid_controllers_.at(X).result(), - pid_controllers_.at(Y).result(), - pid_controllers_.at(Z).result()); - tf::Vector3 target_acc_dash = (tf::Matrix3x3(tf::createQuaternionFromYaw(rpy_.z()))).inverse() * target_acc_w; - - target_pitch_ = target_acc_dash.x() / aerial_robot_estimation::G; - target_roll_ = -target_acc_dash.y() / aerial_robot_estimation::G; - - Eigen::VectorXd target_thrust_z_term = Eigen::VectorXd::Zero(motor_num_); - for(int i = 0; i < motor_num_; i++) - { - double p_term = z_gains_.at(i)[0] * pid_controllers_.at(Z).getErrP(); - double i_term = z_gains_.at(i)[1] * pid_controllers_.at(Z).getErrI(); - double d_term = z_gains_.at(i)[2] * pid_controllers_.at(Z).getErrD(); - target_thrust_z_term(i) = p_term + i_term + d_term; - pid_msg_.z.p_term.at(i) = p_term; - pid_msg_.z.i_term.at(i) = i_term; - pid_msg_.z.d_term.at(i) = d_term; - } - - // constraint z (also I term) - int index; - double max_term = target_thrust_z_term.cwiseAbs().maxCoeff(&index); - double residual = max_term - pid_controllers_.at(Z).getLimitSum(); - if(residual > 0) - { - pid_controllers_.at(Z).setErrI(pid_controllers_.at(Z).getPrevErrI()); - target_thrust_z_term *= (1 - residual / max_term); - } - - for(int i = 0; i < motor_num_; i++) - { - target_base_thrust_.at(i) = target_thrust_z_term(i); - pid_msg_.z.total.at(i) = target_thrust_z_term(i); - } - - allocateYawTerm(); -} - -void HydrusLQIController::allocateYawTerm() -{ - Eigen::VectorXd target_thrust_yaw_term = Eigen::VectorXd::Zero(motor_num_); - for(int i = 0; i < motor_num_; i++) - { - double p_term = yaw_gains_.at(i)[0] * pid_controllers_.at(YAW).getErrP(); - double i_term = yaw_gains_.at(i)[1] * pid_controllers_.at(YAW).getErrI(); - double d_term = yaw_gains_.at(i)[2] * pid_controllers_.at(YAW).getErrD(); - target_thrust_yaw_term(i) = p_term + i_term + d_term; - pid_msg_.yaw.p_term.at(i) = p_term; - pid_msg_.yaw.i_term.at(i) = i_term; - pid_msg_.yaw.d_term.at(i) = d_term; - } - // constraint yaw (also I term) - int index; - double max_term = target_thrust_yaw_term.cwiseAbs().maxCoeff(&index); - double residual = max_term - pid_controllers_.at(YAW).getLimitSum(); - if(residual > 0) - { - pid_controllers_.at(YAW).setErrI(pid_controllers_.at(YAW).getPrevErrI()); - target_thrust_yaw_term *= (1 - residual / max_term); - } - - // special process for yaw since the bandwidth between PC and spinal - double max_yaw_scale = 0; // for reconstruct yaw control term in spinal - for(int i = 0; i < motor_num_; i++) - { - pid_msg_.yaw.total.at(i) = target_thrust_yaw_term(i); - - if(yaw_gains_[i][2] > max_yaw_scale) - { - max_yaw_scale = yaw_gains_[i][2]; - candidate_yaw_term_ = target_thrust_yaw_term(i); - } - } -} - -void HydrusLQIController::gainGeneratorFunc() -{ - double rate; - ros::NodeHandle control_nh(nh_, "controller"); - ros::NodeHandle lqi_nh(control_nh, "lqi"); - lqi_nh.param("gain_generate_rate", rate, 15.0); - ros::Rate loop_rate(rate); - - while(ros::ok()) - { - if(checkRobotModel()) - { - if(optimalGain()) - { - clampGain(); - publishGain(); - } - else - ROS_ERROR_NAMED("LQI gain generator", "LQI gain generator: can not solve hamilton matrix"); - } - else - { - resetGain(); - } - - loop_rate.sleep(); - } + UnderActuatedLQIController::initialize(nh, nhp, robot_model, estimator, navigator, ctrl_loop_rate); } bool HydrusLQIController::checkRobotModel() { - lqi_mode_ = hydrus_robot_model_->getWrenchDof(); + boost::shared_ptr hydrus_robot_model = boost::dynamic_pointer_cast(robot_model_); + lqi_mode_ = hydrus_robot_model->getWrenchDof(); if(!robot_model_->initialized()) { @@ -197,11 +29,10 @@ bool HydrusLQIController::checkRobotModel() return false; } - if(!robot_model_->stabilityCheck(verbose_)) { ROS_ERROR_NAMED("LQI gain generator", "LQI gain generator: invalid pose, stability is invalid"); - if(hydrus_robot_model_->getWrenchDof() == 4 && hydrus_robot_model_->getFeasibleControlRollPitchMin() > hydrus_robot_model_->getFeasibleControlRollPitchMinThre()) + if(hydrus_robot_model->getWrenchDof() == 4 && hydrus_robot_model->getFeasibleControlRollPitchMin() > hydrus_robot_model->getFeasibleControlRollPitchMinThre()) { ROS_WARN_NAMED("LQI gain generator", "LQI gain generator: change to three axis stable mode"); lqi_mode_ = 3; @@ -213,276 +44,6 @@ bool HydrusLQIController::checkRobotModel() return true; } -bool HydrusLQIController::optimalGain() -{ - // referece: - // M, Zhao, et.al, "Transformable multirotor with two-dimensional multilinks: modeling, control, and whole-body aerial manipulation" - // Sec. 3.2 - - Eigen::MatrixXd P = robot_model_->calcWrenchMatrixOnCoG(); - Eigen::MatrixXd P_dash = Eigen::MatrixXd::Zero(lqi_mode_, motor_num_); - Eigen::MatrixXd inertia = robot_model_->getInertia(); - P_dash.row(0) = P.row(2) / robot_model_->getMass(); // z - P_dash.bottomRows(lqi_mode_ - 1) = (inertia.inverse() * P.bottomRows(3)).topRows(lqi_mode_ - 1); // roll, pitch, yaw - - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(lqi_mode_ * 3, lqi_mode_ * 3); - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(lqi_mode_ * 3, motor_num_); - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(lqi_mode_, lqi_mode_ * 3); - for(int i = 0; i < lqi_mode_; i++) - { - A(2 * i, 2 * i + 1) = 1; - B.row(2 * i + 1) = P_dash.row(i); - C(i, 2 * i) = 1; - } - A.block(lqi_mode_ * 2, 0, lqi_mode_, lqi_mode_ * 3) = -C; - - ROS_DEBUG_STREAM_NAMED("LQI gain generator", "LQI gain generator: B: \n" << B ); - - Eigen::VectorXd q_diagonals(lqi_mode_ * 3); - if(lqi_mode_ == 3) - { - q_diagonals << lqi_z_weight_(0), lqi_z_weight_(2), lqi_roll_pitch_weight_(0), lqi_roll_pitch_weight_(2), lqi_roll_pitch_weight_(0), lqi_roll_pitch_weight_(2), lqi_z_weight_(1), lqi_roll_pitch_weight_(1), lqi_roll_pitch_weight_(1); - } - else - { - q_diagonals << lqi_z_weight_(0), lqi_z_weight_(2), lqi_roll_pitch_weight_(0), lqi_roll_pitch_weight_(2), lqi_roll_pitch_weight_(0), lqi_roll_pitch_weight_(2), lqi_yaw_weight_(0), lqi_yaw_weight_(2), lqi_z_weight_(1), lqi_roll_pitch_weight_(1), lqi_roll_pitch_weight_(1), lqi_yaw_weight_(1); - } - Eigen::MatrixXd Q = q_diagonals.asDiagonal(); - - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(motor_num_, motor_num_); - for(int i = 0; i < motor_num_; ++i) R(i,i) = r_.at(i); - - /* solve continuous-time algebraic Ricatti equation */ - double t = ros::Time::now().toSec(); - - if(K_.cols() != lqi_mode_ * 3) - { - resetGain(); // four axis -> three axis and vice versa - } - - bool use_kleinman_method = true; - if(K_.cols() == 0 || K_.rows() == 0) - { - ROS_DEBUG_STREAM_NAMED("LQI gain generator", "LQI gain generator: do not use kleinman method"); - use_kleinman_method = false; - } - if(!control_utils::care(A, B, R, Q, K_, use_kleinman_method)) - { - ROS_ERROR_STREAM_NAMED("LQI gain generator", "LQI gain generator: error in solver of continuous-time algebraic riccati equation"); - return false; - } - - ROS_DEBUG_STREAM_NAMED("LQI gain generator", "LQI gain generator: CARE: %f sec" << ros::Time::now().toSec() - t); - ROS_DEBUG_STREAM_NAMED("LQI gain generator", "LQI gain generator: K \n" << K_); - - - for(int i = 0; i < motor_num_; ++i) - { - roll_gains_.at(i) = Eigen::Vector3d(-K_(i,2), K_(i, lqi_mode_ * 2 + 1), -K_(i,3)); - pitch_gains_.at(i) = Eigen::Vector3d(-K_(i,4), K_(i, lqi_mode_ * 2 + 2), -K_(i,5)); - z_gains_.at(i) = Eigen::Vector3d(-K_(i,0), K_(i, lqi_mode_ * 2), -K_(i,1)); - if(lqi_mode_ == 4) yaw_gains_.at(i) = Eigen::Vector3d(-K_(i,6), K_(i, lqi_mode_ * 2 + 3), -K_(i,7)); - else yaw_gains_.at(i).setZero(); - } - - // compensation for gyro moment - p_mat_pseudo_inv_ = aerial_robot_model::pseudoinverse(P.middleRows(2, lqi_mode_)); - return true; -} - -void HydrusLQIController::clampGain() -{ - /* avoid the violation of 16int_t range because of spinal::RollPitchYawTerms */ - double max_gain_thresh = 32.767; - double max_roll_p_gain = 0, max_roll_d_gain = 0, max_pitch_p_gain = 0, max_pitch_d_gain = 0, max_yaw_d_gain = 0; - for(int i = 0; i < motor_num_; ++i) - { - if(max_roll_p_gain < fabs(roll_gains_.at(i)[0])) max_roll_p_gain = fabs(roll_gains_.at(i)[0]); - if(max_roll_d_gain < fabs(roll_gains_.at(i)[2])) max_roll_d_gain = fabs(roll_gains_.at(i)[2]); - if(max_pitch_p_gain < fabs(pitch_gains_.at(i)[0])) max_pitch_p_gain = fabs(pitch_gains_.at(i)[0]); - if(max_pitch_d_gain < fabs(pitch_gains_.at(i)[2])) max_pitch_d_gain = fabs(pitch_gains_.at(i)[2]); - if(max_yaw_d_gain < fabs(yaw_gains_.at(i)[2])) max_yaw_d_gain = fabs(yaw_gains_.at(i)[2]); - } - - double roll_p_gain_scale = 1, roll_d_gain_scale = 1, pitch_p_gain_scale = 1, pitch_d_gain_scale = 1, yaw_d_gain_scale = 1; - if(max_roll_p_gain > max_gain_thresh) - { - ROS_WARN_STREAM_NAMED("LQI gain generator", "LQI gain generator: the max roll p gain violate the range of int16_t: " << max_roll_p_gain); - roll_p_gain_scale = max_gain_thresh / max_roll_p_gain; - } - if(max_roll_d_gain > max_gain_thresh) - { - ROS_WARN_STREAM_NAMED("LQI gain generator", "LQI gain generator: the max roll d gain violate the range of int16_t: " << max_roll_d_gain); - roll_d_gain_scale = max_gain_thresh / max_roll_d_gain; - } - if(max_pitch_p_gain > max_gain_thresh) - { - ROS_WARN_STREAM_NAMED("LQI gain generator", "LQI gain generator: the max pitch p gain violate the range of int16_t: " << max_pitch_p_gain); - pitch_p_gain_scale = max_gain_thresh / max_pitch_p_gain; - } - if(max_pitch_d_gain > max_gain_thresh) - { - ROS_WARN_STREAM_NAMED("LQI gain generator", "LQI gain generator: the max pitch d gain violate the range of int16_t: " << max_pitch_d_gain); - pitch_d_gain_scale = max_gain_thresh / max_pitch_d_gain; - } - if(max_yaw_d_gain > max_gain_thresh) - { - ROS_WARN_STREAM_NAMED("LQI gain generator", "LQI gain generator: the max yaw d gain violate the range of int16_t: " << max_yaw_d_gain); - yaw_d_gain_scale = max_gain_thresh / max_yaw_d_gain; - } - - for(int i = 0; i < motor_num_; ++i) - { - roll_gains_.at(i)[0] *= roll_p_gain_scale; - roll_gains_.at(i)[2] *= roll_d_gain_scale; - - pitch_gains_.at(i)[0] *= pitch_p_gain_scale; - pitch_gains_.at(i)[2] *= pitch_d_gain_scale; - - yaw_gains_.at(i)[2] *= yaw_d_gain_scale; - } -} - -void HydrusLQIController::rosParamInit() -{ - ros::NodeHandle control_nh(nh_, "controller"); - ros::NodeHandle lqi_nh(control_nh, "lqi"); - getParam(lqi_nh, "gyro_moment_compensation", gyro_moment_compensation_, false); - getParam(lqi_nh, "clamp_gain", clamp_gain_, true); - - /* propeller direction and lqi R */ - r_.resize(motor_num_); // motor_num is not set - for(int i = 0; i < robot_model_->getRotorNum(); ++i) { - std::stringstream ss; - ss << i + 1; - /* R */ - getParam(lqi_nh, std::string("r") + ss.str(), r_.at(i), 1.0); - } - - getParam(lqi_nh, "roll_pitch_p", lqi_roll_pitch_weight_[0], 1.0); - getParam(lqi_nh, "roll_pitch_i", lqi_roll_pitch_weight_[1], 1.0); - getParam(lqi_nh, "roll_pitch_d", lqi_roll_pitch_weight_[2], 1.0); - getParam(lqi_nh, "yaw_p", lqi_yaw_weight_[0], 1.0); - getParam(lqi_nh, "yaw_i", lqi_yaw_weight_[1], 1.0); - getParam(lqi_nh, "yaw_d", lqi_yaw_weight_[2], 1.0); - getParam(lqi_nh, "z_p", lqi_z_weight_[0], 1.0); - getParam(lqi_nh, "z_i", lqi_z_weight_[1], 1.0); - getParam(lqi_nh, "z_d", lqi_z_weight_[2], 1.0); -} - -void HydrusLQIController::publishGain() -{ - aerial_robot_msgs::FourAxisGain four_axis_gain_msg; - spinal::RollPitchYawTerms rpy_gain_msg; // to spinal - spinal::PMatrixPseudoInverseWithInertia p_pseudo_inverse_with_inertia_msg; // to spinal - - rpy_gain_msg.motors.resize(motor_num_); - p_pseudo_inverse_with_inertia_msg.pseudo_inverse.resize(motor_num_); - - for(int i = 0; i < motor_num_; ++i) - { - four_axis_gain_msg.roll_p_gain.push_back(roll_gains_.at(i)[0]); - four_axis_gain_msg.roll_i_gain.push_back(roll_gains_.at(i)[1]); - four_axis_gain_msg.roll_d_gain.push_back(roll_gains_.at(i)[2]); - - four_axis_gain_msg.pitch_p_gain.push_back(pitch_gains_.at(i)[0]); - four_axis_gain_msg.pitch_i_gain.push_back(pitch_gains_.at(i)[1]); - four_axis_gain_msg.pitch_d_gain.push_back(pitch_gains_.at(i)[2]); - - four_axis_gain_msg.yaw_p_gain.push_back(yaw_gains_.at(i)[0]); - four_axis_gain_msg.yaw_i_gain.push_back(yaw_gains_.at(i)[1]); - four_axis_gain_msg.yaw_d_gain.push_back(yaw_gains_.at(i)[2]); - - four_axis_gain_msg.z_p_gain.push_back(z_gains_.at(i)[0]); - four_axis_gain_msg.z_i_gain.push_back(z_gains_.at(i)[1]); - four_axis_gain_msg.z_d_gain.push_back(z_gains_.at(i)[2]); - - /* to flight controller via rosserial scaling by 1000 */ - rpy_gain_msg.motors[i].roll_p = roll_gains_.at(i)[0] * 1000; - rpy_gain_msg.motors[i].roll_i = roll_gains_.at(i)[1] * 1000; - rpy_gain_msg.motors[i].roll_d = roll_gains_.at(i)[2] * 1000; - - rpy_gain_msg.motors[i].pitch_p = pitch_gains_.at(i)[0] * 1000; - rpy_gain_msg.motors[i].pitch_i = pitch_gains_.at(i)[1] * 1000; - rpy_gain_msg.motors[i].pitch_d = pitch_gains_.at(i)[2] * 1000; - - rpy_gain_msg.motors[i].yaw_d = yaw_gains_.at(i)[2] * 1000; - - /* the p matrix pseudo inverse and inertia */ - p_pseudo_inverse_with_inertia_msg.pseudo_inverse[i].r = p_mat_pseudo_inv_(i, 1) * 1000; - p_pseudo_inverse_with_inertia_msg.pseudo_inverse[i].p = p_mat_pseudo_inv_(i, 2) * 1000; - if(lqi_mode_ == 4) - p_pseudo_inverse_with_inertia_msg.pseudo_inverse[i].y = p_mat_pseudo_inv_(i, 3) * 1000; - else - p_pseudo_inverse_with_inertia_msg.pseudo_inverse[i].y = 0; - } - rpy_gain_pub_.publish(rpy_gain_msg); - four_axis_gain_pub_.publish(four_axis_gain_msg); - - - /* the multilink inertia */ - Eigen::Matrix3d inertia = robot_model_->getInertia(); - p_pseudo_inverse_with_inertia_msg.inertia[0] = inertia(0, 0) * 1000; - p_pseudo_inverse_with_inertia_msg.inertia[1] = inertia(1, 1) * 1000; - p_pseudo_inverse_with_inertia_msg.inertia[2] = inertia(2, 2) * 1000; - p_pseudo_inverse_with_inertia_msg.inertia[3] = inertia(0, 1) * 1000; - p_pseudo_inverse_with_inertia_msg.inertia[4] = inertia(1, 2) * 1000; - p_pseudo_inverse_with_inertia_msg.inertia[5] = inertia(0, 2) * 1000; - - if(gyro_moment_compensation_) - p_matrix_pseudo_inverse_inertia_pub_.publish(p_pseudo_inverse_with_inertia_msg); -} - -void HydrusLQIController::cfgLQICallback(hydrus::LQIConfig &config, uint32_t level) -{ - using Levels = aerial_robot_msgs::DynamicReconfigureLevels; - if(config.lqi_flag) - { - switch(level) - { - case Levels::RECONFIGURE_LQI_ROLL_PITCH_P: - ROS_INFO_STREAM_NAMED("LQI gain generator", "LQI gain generator: change the p gain weight of roll and pitch from " << lqi_roll_pitch_weight_.x() << " to " << config.roll_pitch_p); - lqi_roll_pitch_weight_.x() = config.roll_pitch_p; - break; - case Levels::RECONFIGURE_LQI_ROLL_PITCH_I: - ROS_INFO_STREAM_NAMED("LQI gain generator", "LQI gain generator: change the i gain weight of roll and pitch from " << lqi_roll_pitch_weight_.y() << " to " << config.roll_pitch_i); - lqi_roll_pitch_weight_.y() = config.roll_pitch_i; - break; - case Levels::RECONFIGURE_LQI_ROLL_PITCH_D: - ROS_INFO_STREAM_NAMED("LQI gain generator", "LQI gain generator: change the d gain weight of roll and pitch from " << lqi_roll_pitch_weight_.z() << " to " << config.roll_pitch_d); - lqi_roll_pitch_weight_.z() = config.roll_pitch_d; - break; - case Levels::RECONFIGURE_LQI_YAW_P: - ROS_INFO_STREAM_NAMED("LQI gain generator", "LQI gain generator: change the p gain weight of yaw from " << lqi_yaw_weight_.x() << " to " << config.yaw_p); - lqi_yaw_weight_.x() = config.yaw_p; - break; - case Levels::RECONFIGURE_LQI_YAW_I: - ROS_INFO_STREAM_NAMED("LQI gain generator", "LQI gain generator: change the i gain weight of yaw from " << lqi_yaw_weight_.y() << " to " << config.yaw_i); - lqi_yaw_weight_.y() = config.yaw_i; - break; - case Levels::RECONFIGURE_LQI_YAW_D: - ROS_INFO_STREAM_NAMED("LQI gain generator", "LQI gain generator: change the d gain weight of yaw from " << lqi_yaw_weight_.z() << " to " << config.yaw_d); - lqi_yaw_weight_.z() = config.yaw_d; - break; - case Levels::RECONFIGURE_LQI_Z_P: - ROS_INFO_STREAM_NAMED("LQI gain generator", "LQI gain generator: change the p gain weight of z from " << lqi_z_weight_.x() << " to " << config.z_p); - lqi_z_weight_.x() = config.z_p; - break; - case Levels::RECONFIGURE_LQI_Z_I: - ROS_INFO_STREAM_NAMED("LQI gain generator", "LQI gain generator: change the i gain weight of z from " << lqi_z_weight_.y() << " to " << config.z_i); - lqi_z_weight_.y() = config.z_i; - break; - case Levels::RECONFIGURE_LQI_Z_D: - ROS_INFO_STREAM_NAMED("LQI gain generator", "LQI gain generator: change the d gain weight of z from " << lqi_z_weight_.z() << " to " << config.z_d); - lqi_z_weight_.z() = config.z_d; - break; - default : - break; - } - } -} - - /* plugin registration */ #include PLUGINLIB_EXPORT_CLASS(aerial_robot_control::HydrusLQIController, aerial_robot_control::ControlBase); diff --git a/robots/hydrus/src/hydrus_tilted_lqi_controller.cpp b/robots/hydrus/src/hydrus_tilted_lqi_controller.cpp index 0907e764d..d9587a1ca 100644 --- a/robots/hydrus/src/hydrus_tilted_lqi_controller.cpp +++ b/robots/hydrus/src/hydrus_tilted_lqi_controller.cpp @@ -2,145 +2,38 @@ using namespace aerial_robot_control; -void HydrusTiltedLQIController::initialize(ros::NodeHandle nh, - ros::NodeHandle nhp, - boost::shared_ptr robot_model, - boost::shared_ptr estimator, - boost::shared_ptr navigator, - double ctrl_loop_rate) +HydrusTiltedLQIController::HydrusTiltedLQIController(): + UnderActuatedTiltedLQIController() { - HydrusLQIController::initialize(nh, nhp, robot_model, estimator, navigator, ctrl_loop_rate); - - desired_baselink_rot_pub_ = nh_.advertise("desire_coordinate", 1); - - pid_msg_.z.p_term.resize(1); - pid_msg_.z.i_term.resize(1); - pid_msg_.z.d_term.resize(1); - z_limit_ = pid_controllers_.at(Z).getLimitSum(); - pid_controllers_.at(Z).setLimitSum(1e6); // do not clamp the sum of PID terms for z axis } -void HydrusTiltedLQIController::controlCore() +void HydrusTiltedLQIController::initialize(ros::NodeHandle nh, + ros::NodeHandle nhp, + boost::shared_ptr robot_model, + boost::shared_ptr estimator, + boost::shared_ptr navigator, + double ctrl_loop_rate) { - PoseLinearController::controlCore(); - - tf::Vector3 target_acc_w(pid_controllers_.at(X).result(), - pid_controllers_.at(Y).result(), - pid_controllers_.at(Z).result()); - - tf::Vector3 target_acc_dash = (tf::Matrix3x3(tf::createQuaternionFromYaw(rpy_.z()))).inverse() * target_acc_w; - - target_pitch_ = atan2(target_acc_dash.x(), target_acc_dash.z()); - target_roll_ = atan2(-target_acc_dash.y(), sqrt(target_acc_dash.x() * target_acc_dash.x() + target_acc_dash.z() * target_acc_dash.z())); - - if(navigator_->getForceLandingFlag()) - { - target_pitch_ = 0; - target_roll_ = 0; - } - - Eigen::VectorXd f = robot_model_->getStaticThrust(); - Eigen::VectorXd allocate_scales = f / f.sum() * robot_model_->getMass(); - Eigen::VectorXd target_thrust_z_term = allocate_scales * target_acc_w.length(); - - // constraint z (also I term) - int index; - double max_term = target_thrust_z_term.cwiseAbs().maxCoeff(&index); - double residual = max_term - z_limit_; - - if(residual > 0) - { - pid_controllers_.at(Z).setErrI(pid_controllers_.at(Z).getPrevErrI()); - target_thrust_z_term *= (1 - residual / max_term); - } - - for(int i = 0; i < motor_num_; i++) - { - target_base_thrust_.at(i) = target_thrust_z_term(i); - pid_msg_.z.total.at(i) = target_thrust_z_term(i); - } - - allocateYawTerm(); + UnderActuatedTiltedLQIController::initialize(nh, nhp, robot_model, estimator, navigator, ctrl_loop_rate); } -bool HydrusTiltedLQIController::optimalGain() +bool HydrusTiltedLQIController::checkRobotModel() { - /* calculate the P_orig pseudo inverse */ - Eigen::MatrixXd P = robot_model_->calcWrenchMatrixOnCoG(); - Eigen::MatrixXd inertia = robot_model_->getInertia(); - Eigen::MatrixXd P_dash = inertia.inverse() * P.bottomRows(3); // roll, pitch, yaw - - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(9, 9); - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(9, motor_num_); - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(3, 9); - for(int i = 0; i < 3; i++) + if(!robot_model_->initialized()) { - A(2 * i, 2 * i + 1) = 1; - B.row(2 * i + 1) = P_dash.row(i); - C(i, 2 * i) = 1; - } - A.block(6, 0, 3, 9) = -C; - - ROS_DEBUG_STREAM_NAMED("LQI gain generator", "LQI gain generator: B: \n" << B ); - - Eigen::VectorXd q_diagonals(9); - q_diagonals << lqi_roll_pitch_weight_(0), lqi_roll_pitch_weight_(2), lqi_roll_pitch_weight_(0), lqi_roll_pitch_weight_(2), lqi_yaw_weight_(0), lqi_yaw_weight_(2), lqi_roll_pitch_weight_(1), lqi_roll_pitch_weight_(1), lqi_yaw_weight_(1); - Eigen::MatrixXd Q = q_diagonals.asDiagonal(); - - Eigen::MatrixXd P_trans = P.topRows(3) / robot_model_->getMass() ; - Eigen::MatrixXd R_trans = P_trans.transpose() * P_trans; - Eigen::MatrixXd R_input = Eigen::MatrixXd::Identity(motor_num_, motor_num_); - Eigen::MatrixXd R = R_trans * trans_constraint_weight_ + R_input * att_control_weight_; - - double t = ros::Time::now().toSec(); - bool use_kleinman_method = true; - if(K_.cols() == 0 || K_.rows() == 0) use_kleinman_method = false; - if(!control_utils::care(A, B, R, Q, K_, use_kleinman_method)) - { - ROS_ERROR_STREAM("error in solver of continuous-time algebraic riccati equation"); + ROS_DEBUG_NAMED("LQI gain generator", "LQI gain generator: robot model is not initiliazed"); return false; } - ROS_DEBUG_STREAM_NAMED("LQI gain generator", "LQI gain generator: CARE: %f sec" << ros::Time::now().toSec() - t); - ROS_DEBUG_STREAM_NAMED("LQI gain generator", "LQI gain generator: K \n" << K_); - - for(int i = 0; i < motor_num_; ++i) + if(!robot_model_->stabilityCheck(verbose_)) { - roll_gains_.at(i) = Eigen::Vector3d(-K_(i,0), K_(i,6), -K_(i,1)); - pitch_gains_.at(i) = Eigen::Vector3d(-K_(i,2), K_(i,7), -K_(i,3)); - yaw_gains_.at(i) = Eigen::Vector3d(-K_(i,4), K_(i,8), -K_(i,5)); - } + ROS_ERROR_NAMED("LQI gain generator", "LQI gain generator: invalid pose, stability is invalid"); - // compensation for gyro moment - p_mat_pseudo_inv_ = aerial_robot_model::pseudoinverse(P.middleRows(2, 4)); + return false; + } return true; } -void HydrusTiltedLQIController::publishGain() -{ - HydrusLQIController::publishGain(); - - double roll,pitch, yaw; - robot_model_->getCogDesireOrientation().GetRPY(roll, pitch, yaw); - - spinal::DesireCoord coord_msg; - coord_msg.roll = roll; - coord_msg.pitch = pitch; - desired_baselink_rot_pub_.publish(coord_msg); -} - -void HydrusTiltedLQIController::rosParamInit() -{ - HydrusLQIController::rosParamInit(); - - ros::NodeHandle control_nh(nh_, "controller"); - ros::NodeHandle lqi_nh(control_nh, "lqi"); - - getParam(lqi_nh, "trans_constraint_weight", trans_constraint_weight_, 1.0); - getParam(lqi_nh, "att_control_weight", att_control_weight_, 1.0); -} - - /* plugin registration */ #include PLUGINLIB_EXPORT_CLASS(aerial_robot_control::HydrusTiltedLQIController, aerial_robot_control::ControlBase);