Merge pull request #526 from tongtybj/develop/robot_control

[Robot Control] refactor the control framework

aerial_robot_base/include/aerial_robot_base/aerial_robot_base.h

@@ -3,7 +3,7 @@
 #include <ros/ros.h>
 #include <ros/callback_queue.h>
 #include <pluginlib/class_loader.h>
-#include <aerial_robot_control/control/control_base.h>
+#include <aerial_robot_control/control/base/base.h>
 #include <aerial_robot_control/flight_navigation.h>
 #include <aerial_robot_estimation/state_estimation.h>
 #include <aerial_robot_model/model/aerial_robot_model_ros.h>

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
@@ -55,11 +56,13 @@ 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)
+  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

robots/hydrus/cfg/LQI.cfg → 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"))

aerial_robot_control/cfg/PidControl.cfg → 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"))
 
 

aerial_robot_control/include/aerial_robot_control/control/pose_linear_controller.h → aerial_robot_control/include/aerial_robot_control/control/base/pose_linear_controller.h

@@ -35,15 +35,15 @@
 
 #pragma once
 
-#include <aerial_robot_control/control/control_base.h>
+#include <aerial_robot_control/control/base/base.h>
 #include <aerial_robot_control/control/utils/pid.h>
-#include <aerial_robot_control/PidControlConfig.h>
+#include <aerial_robot_control/PIDConfig.h>
 #include <aerial_robot_msgs/DynamicReconfigureLevels.h>
 #include <aerial_robot_msgs/PoseControlPid.h>
 #include <angles/angles.h>
 #include <dynamic_reconfigure/server.h>
 
-using PidControlDynamicConfig = dynamic_reconfigure::Server<aerial_robot_control::PidControlConfig>;
+using PidControlDynamicConfig = dynamic_reconfigure::Server<aerial_robot_control::PIDConfig>;
 
 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<int> controller_indices);
+    void cfgPidCallback(aerial_robot_control::PIDConfig &config, uint32_t level, std::vector<int> controller_indices);
   };
 
 };

aerial_robot_control/include/aerial_robot_control/control/fully_actuated_controller.h

@@ -35,7 +35,7 @@
 #pragma once
 
 #include <aerial_robot_msgs/WrenchAllocationMatrix.h>
-#include <aerial_robot_control/control/pose_linear_controller.h>
+#include <aerial_robot_control/control/base/pose_linear_controller.h>
 #include <spinal/FourAxisCommand.h>
 #include <spinal/RollPitchYawTerms.h>
 #include <spinal/TorqueAllocationMatrixInv.h>

aerial_robot_control/include/aerial_robot_control/control/under_actuated_controller.h

@@ -34,7 +34,7 @@
  *********************************************************************/
 #pragma once
 
-#include <aerial_robot_control/control/pose_linear_controller.h>
+#include <aerial_robot_control/control/base/pose_linear_controller.h>
 #include <spinal/FourAxisCommand.h>
 #include <spinal/RollPitchYawTerms.h>
 #include <spinal/TorqueAllocationMatrixInv.h>

aerial_robot_control/include/aerial_robot_control/control/under_actuated_lqi_controller.h

@@ -0,0 +1,115 @@
+// -*- 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 <aerial_robot_control/control/under_actuated_controller.h>
+#include <aerial_robot_control/control/utils/care.h>
+#include <aerial_robot_control/LQIConfig.h>
+#include <aerial_robot_msgs/FourAxisGain.h>
+#include <dynamic_reconfigure/server.h>
+#include <spinal/RollPitchYawTerms.h>
+#include <spinal/PMatrixPseudoInverseWithInertia.h>
+#include <ros/ros.h>
+#include <thread>
+
+namespace aerial_robot_control
+{
+  class UnderActuatedLQIController: public PoseLinearController
+  {
+
+  public:
+    UnderActuatedLQIController();
+    virtual ~UnderActuatedLQIController();
+
+    void initialize(ros::NodeHandle nh, ros::NodeHandle nhp,
+                    boost::shared_ptr<aerial_robot_model::RobotModel> robot_model,
+                    boost::shared_ptr<aerial_robot_estimation::StateEstimator> estimator,
+                    boost::shared_ptr<aerial_robot_navigation::BaseNavigator> navigator,
+                    double ctrl_loop_rate);
+
+    void activate() override;
+
+  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<dynamic_reconfigure::Server<aerial_robot_control::LQIConfig> > lqi_server_;
+    dynamic_reconfigure::Server<aerial_robot_control::LQIConfig>::CallbackType dynamic_reconf_func_lqi_;
+
+    double target_roll_, target_pitch_;
+    double candidate_yaw_term_;
+    std::vector<float> 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<double> r_; // matrix R
+
+    std::vector<Eigen::Vector3d> 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();
+  };
+};