From a91456b0e362e03d0236f3ef56fb02bc2ca501ce Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sun, 18 Sep 2022 02:37:10 +0900 Subject: [PATCH 1/6] [Robot Control] remove unnecessary files --- .../control/agile_flatness_pid_controller.h | 61 ------- .../control/flatness_pid_controller.h | 158 ------------------ 2 files changed, 219 deletions(-) delete mode 100644 aerial_robot_control/include/aerial_robot_control/control/agile_flatness_pid_controller.h delete mode 100644 aerial_robot_control/include/aerial_robot_control/control/flatness_pid_controller.h diff --git a/aerial_robot_control/include/aerial_robot_control/control/agile_flatness_pid_controller.h b/aerial_robot_control/include/aerial_robot_control/control/agile_flatness_pid_controller.h deleted file mode 100644 index ddb110723..000000000 --- a/aerial_robot_control/include/aerial_robot_control/control/agile_flatness_pid_controller.h +++ /dev/null @@ -1,61 +0,0 @@ -// -*- mode: c++ -*- -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, 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 AgileFlatnessPid : virtual public aerial_robot_control::FlatnessPid - { - public: - AgileFlatnessPid(); - ~AgileFlatnessPid(){} - - 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); - - void pidUpdate() override; - }; -}; - -/* plugin registration */ - -#include -PLUGINLIB_EXPORT_CLASS(aerial_robot_control::AgileFlatnessPid, aerial_robot_control::ControlBase); diff --git a/aerial_robot_control/include/aerial_robot_control/control/flatness_pid_controller.h b/aerial_robot_control/include/aerial_robot_control/control/flatness_pid_controller.h deleted file mode 100644 index 7ae2482fe..000000000 --- a/aerial_robot_control/include/aerial_robot_control/control/flatness_pid_controller.h +++ /dev/null @@ -1,158 +0,0 @@ -// -*- mode: c++ -*- -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, 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 - -/* basic class */ -#include -#include -#include -#include -#include -#include -#include - -using boost::algorithm::clamp; -using namespace std; - -namespace aerial_robot_control -{ - class FlatnessPid : public aerial_robot_control::ControlBase - { - public: - FlatnessPid(); - ~FlatnessPid(){}; - - 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); - - - virtual void reset() - { - ControlBase::reset(); - - start_rp_integration_ = false; - z_pos_err_i_ = 0; - yaw_err_i_ = 0; - - xy_i_term_.setValue(0,0,0); - - z_control_terms_.assign(motor_num_, 0); - yaw_control_terms_.assign(motor_num_, 0); - target_roll_ = 0; - target_pitch_ = 0; - } - - virtual bool update(); - - virtual void stateError(); - virtual void pidUpdate(); - virtual void sendCmd(); - - protected: - ros::Publisher pid_pub_; - ros::Publisher flight_cmd_pub_; - - /* basic var */ - tf::Vector3 state_pos_; - tf::Vector3 state_vel_; - tf::Vector3 target_vel_; - tf::Vector3 target_pos_; - tf::Vector3 target_acc_; - tf::Vector3 pos_err_; - tf::Vector3 vel_err_; - double state_yaw_; - double state_yaw_vel_; - double target_yaw_; - double target_yaw_vel_; - double yaw_err_; - - double target_pitch_, target_roll_; - std::vector z_control_terms_; - std::vector yaw_control_terms_; - - //**** z - std::vector z_gains_; - double z_err_thresh_; - double z_limit_; - tf::Vector3 z_terms_limit_; - double z_pos_err_i_; - double landing_z_err_thresh_; - double safe_landing_height_; - double z_offset_; - - //**** xy - tf::Vector3 xy_gains_; - double xy_limit_; - tf::Vector3 xy_terms_limits_; - tf::Vector3 xy_i_term_; - double xy_hovering_i_gain_; - tf::Vector3 xy_offset_; - bool start_rp_integration_; - - //**** yaw - std::vector yaw_gains_; - double yaw_limit_; - tf::Vector3 yaw_terms_limits_; - double max_yaw_term_; - double candidate_yaw_term_; /* to reconstruct yaw PI control term in spinal */ - double yaw_err_thresh_; - double yaw_err_i_; - bool need_yaw_d_control_; - - boost::shared_ptr > xy_pid_server_; - dynamic_reconfigure::Server::CallbackType dynamic_reconf_func_xy_pid_; - - void cfgXYPidCallback(aerial_robot_control::XYPidControlConfig &config, uint32_t level); - virtual void rosParamInit(); - - - tf::Vector3 clampV(tf::Vector3 input, double min, double max) - { - return tf::Vector3(clamp(input.x(), min, max), - clamp(input.y(), min, max), - clamp(input.z(), min, max)); - } - - }; -}; - -/* plugin registration */ -#include -PLUGINLIB_EXPORT_CLASS(aerial_robot_control::FlatnessPid, aerial_robot_control::ControlBase); - From 5d2f2e745c8060b37212185457698cc32e82fcb8 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sun, 18 Sep 2022 02:59:32 +0900 Subject: [PATCH 2/6] [Robot Control] refactor the control framework for better understanding --- aerial_robot_base/include/aerial_robot_base/aerial_robot_base.h | 2 +- aerial_robot_control/CMakeLists.txt | 2 +- .../control/{control_base.h => base/base.h} | 0 .../control/{ => base}/pose_linear_controller.h | 2 +- .../aerial_robot_control/control/fully_actuated_controller.h | 2 +- .../aerial_robot_control/control/under_actuated_controller.h | 2 +- .../src/control/{ => base}/pose_linear_controller.cpp | 2 +- robots/dragon/include/dragon/control/full_vectoring_control.h | 2 +- 8 files changed, 7 insertions(+), 7 deletions(-) rename aerial_robot_control/include/aerial_robot_control/control/{control_base.h => base/base.h} (100%) rename aerial_robot_control/include/aerial_robot_control/control/{ => base}/pose_linear_controller.h (98%) rename aerial_robot_control/src/control/{ => base}/pose_linear_controller.cpp (99%) diff --git a/aerial_robot_base/include/aerial_robot_base/aerial_robot_base.h b/aerial_robot_base/include/aerial_robot_base/aerial_robot_base.h index d1576b9a6..df6ffd1a3 100644 --- a/aerial_robot_base/include/aerial_robot_base/aerial_robot_base.h +++ b/aerial_robot_base/include/aerial_robot_base/aerial_robot_base.h @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/aerial_robot_control/CMakeLists.txt b/aerial_robot_control/CMakeLists.txt index 51f0156d4..a92286261 100644 --- a/aerial_robot_control/CMakeLists.txt +++ b/aerial_robot_control/CMakeLists.txt @@ -55,7 +55,7 @@ target_link_libraries(control_utils ${EIGEN3_LIBRARIES}) ### flight control plugin add_library(flight_control_pluginlib - src/control/pose_linear_controller.cpp + src/control/base/pose_linear_controller.cpp src/control/fully_actuated_controller.cpp src/control/under_actuated_controller.cpp) diff --git a/aerial_robot_control/include/aerial_robot_control/control/control_base.h b/aerial_robot_control/include/aerial_robot_control/control/base/base.h similarity index 100% rename from aerial_robot_control/include/aerial_robot_control/control/control_base.h rename to aerial_robot_control/include/aerial_robot_control/control/base/base.h diff --git a/aerial_robot_control/include/aerial_robot_control/control/pose_linear_controller.h b/aerial_robot_control/include/aerial_robot_control/control/base/pose_linear_controller.h similarity index 98% rename from aerial_robot_control/include/aerial_robot_control/control/pose_linear_controller.h rename to aerial_robot_control/include/aerial_robot_control/control/base/pose_linear_controller.h index abc8c3e29..68d9f54a5 100644 --- a/aerial_robot_control/include/aerial_robot_control/control/pose_linear_controller.h +++ b/aerial_robot_control/include/aerial_robot_control/control/base/pose_linear_controller.h @@ -35,7 +35,7 @@ #pragma once -#include +#include #include #include #include diff --git a/aerial_robot_control/include/aerial_robot_control/control/fully_actuated_controller.h b/aerial_robot_control/include/aerial_robot_control/control/fully_actuated_controller.h index 1276243df..439710b81 100644 --- a/aerial_robot_control/include/aerial_robot_control/control/fully_actuated_controller.h +++ b/aerial_robot_control/include/aerial_robot_control/control/fully_actuated_controller.h @@ -35,7 +35,7 @@ #pragma once #include -#include +#include #include #include #include diff --git a/aerial_robot_control/include/aerial_robot_control/control/under_actuated_controller.h b/aerial_robot_control/include/aerial_robot_control/control/under_actuated_controller.h index a90860e90..2e1ea7544 100644 --- a/aerial_robot_control/include/aerial_robot_control/control/under_actuated_controller.h +++ b/aerial_robot_control/include/aerial_robot_control/control/under_actuated_controller.h @@ -34,7 +34,7 @@ *********************************************************************/ #pragma once -#include +#include #include #include #include diff --git a/aerial_robot_control/src/control/pose_linear_controller.cpp b/aerial_robot_control/src/control/base/pose_linear_controller.cpp similarity index 99% rename from aerial_robot_control/src/control/pose_linear_controller.cpp rename to aerial_robot_control/src/control/base/pose_linear_controller.cpp index b3261a9e7..dae89e910 100644 --- a/aerial_robot_control/src/control/pose_linear_controller.cpp +++ b/aerial_robot_control/src/control/base/pose_linear_controller.cpp @@ -34,7 +34,7 @@ *********************************************************************/ -#include +#include namespace aerial_robot_control { diff --git a/robots/dragon/include/dragon/control/full_vectoring_control.h b/robots/dragon/include/dragon/control/full_vectoring_control.h index 41c43ed3e..2a956da14 100644 --- a/robots/dragon/include/dragon/control/full_vectoring_control.h +++ b/robots/dragon/include/dragon/control/full_vectoring_control.h @@ -35,7 +35,7 @@ #pragma once -#include +#include #include #include #include From 4c9ca242b2a492b43a26a8a762b8f49910742ae7 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sun, 18 Sep 2022 06:35:05 +0900 Subject: [PATCH 3/6] [Control] move LQI-based controller to fundamental package (aerial_robot_control) --- aerial_robot_control/CMakeLists.txt | 9 +- .../cfg/LQI.cfg | 4 +- .../cfg/{PidControl.cfg => PID.cfg} | 2 +- .../control/base/pose_linear_controller.h | 6 +- .../control/under_actuated_lqi_controller.h | 113 ++++ .../under_actuated_tilted_lqi_controller.h | 70 +++ .../plugins/flight_control_plugins.xml | 8 + .../control/base/pose_linear_controller.cpp | 2 +- .../control/under_actuated_lqi_controller.cpp | 553 ++++++++++++++++++ .../under_actuated_tilted_lqi_controller.cpp | 179 ++++++ aerial_robot_model/CMakeLists.txt | 2 +- ...p => underactuated_tilted_robot_model.cpp} | 0 robots/dragon/CMakeLists.txt | 2 +- robots/hydrus/CMakeLists.txt | 8 +- .../include/hydrus/hydrus_lqi_controller.h | 65 +- .../hydrus/hydrus_tilted_lqi_controller.h | 25 +- robots/hydrus/package.xml | 2 - robots/hydrus/src/hydrus_lqi_controller.cpp | 449 +------------- .../src/hydrus_tilted_lqi_controller.cpp | 139 +---- 19 files changed, 972 insertions(+), 666 deletions(-) rename {robots/hydrus => aerial_robot_control}/cfg/LQI.cfg (93%) rename aerial_robot_control/cfg/{PidControl.cfg => PID.cfg} (90%) create mode 100644 aerial_robot_control/include/aerial_robot_control/control/under_actuated_lqi_controller.h create mode 100644 aerial_robot_control/include/aerial_robot_control/control/under_actuated_tilted_lqi_controller.h create mode 100644 aerial_robot_control/src/control/under_actuated_lqi_controller.cpp create mode 100644 aerial_robot_control/src/control/under_actuated_tilted_lqi_controller.cpp rename aerial_robot_model/src/model/plugin/{tilted_underactuated_robot_model.cpp => underactuated_tilted_robot_model.cpp} (100%) 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 7529cbab8..e39d73147 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); From d6a052d56148011d401962da914c76fc2b34f0ce Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sun, 18 Dec 2022 00:51:33 +0900 Subject: [PATCH 4/6] [Control][LQI] fix the update process in cfg callback --- .../src/control/under_actuated_lqi_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp b/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp index 427b14a8e..888e66274 100644 --- a/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp +++ b/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp @@ -498,7 +498,7 @@ void UnderActuatedLQIController::cfgLQICallback(aerial_robot_control::LQIConfig break; } - if (realtime_update_) { + if (!realtime_update_) { // instantly modify gain if no joint angles if(optimalGain()) { From dcbba1592528781e2d9ed7ad44e6378efd474bf5 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Mon, 19 Dec 2022 19:26:22 +0900 Subject: [PATCH 5/6] [Control] fix the bug about the timing of initial LQI gain publish --- .../control/under_actuated_lqi_controller.h | 2 ++ .../control/under_actuated_lqi_controller.cpp | 29 ++++++++++--------- 2 files changed, 17 insertions(+), 14 deletions(-) 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 index cbafcf357..dff98b775 100644 --- 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 @@ -60,6 +60,8 @@ namespace aerial_robot_control boost::shared_ptr navigator, double ctrl_loop_rate); + void activate() override; + protected: ros::Publisher flight_cmd_pub_; // for spinal diff --git a/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp b/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp index 888e66274..a27990b8c 100644 --- a/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp +++ b/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp @@ -127,6 +127,21 @@ void UnderActuatedLQIController::gainGeneratorFunc() } +void UnderActuatedLQIController::activate() +{ + ControlBase::activate(); + + // 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"); + } +} + void UnderActuatedLQIController::sendCmd() { PoseLinearController::sendCmd(); @@ -149,20 +164,6 @@ 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()); From 2beb13166715b2b4d174b0f54460f6140b6da6ef Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Thu, 29 Jun 2023 13:14:42 +0900 Subject: [PATCH 6/6] [Control] correct the flag to use to check whether the model is reconfigurable --- .../src/control/under_actuated_lqi_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp b/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp index a27990b8c..347858e5c 100644 --- a/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp +++ b/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp @@ -86,7 +86,7 @@ void UnderActuatedLQIController::initialize(ros::NodeHandle nh, 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 (!robot_model_->isModelFixed()) realtime_update_ = true; if (realtime_update_) { gain_generator_thread_ = std::thread(boost::bind(&UnderActuatedLQIController::gainGeneratorFunc, this)); }