[Delta][WIP] latest version for real platform

.github/workflows/catkin_lint.yml

@@ -28,4 +28,4 @@ jobs:
           wstool init
           wstool merge aerial_robot_noetic.rosinstall
           wstool update
-          ROS_DISTRO=noetic catkin_lint --resolve-env --strict $PWD --skip-path kalman_filter --skip-path ublox_gps --skip-path aerial_robot_3rdparty  --skip-path rosserial --skip-path livox_ros_driver2 --skip-path fast_lio --skip-path mocap_optitrack
+          ROS_DISTRO=noetic catkin_lint --resolve-env --strict $PWD --skip-path kalman_filter --skip-path ublox_gps --skip-path aerial_robot_3rdparty  --skip-path rosserial --skip-path livox_ros_driver2 --skip-path fast_lio --skip-path mocap_optitrack --skip-path ros_controllers --skip-path casadi_catkin --skip-path casadi_eigen

aerial_robot/package.xml

@@ -17,6 +17,7 @@
   <exec_depend>aerial_robot_model</exec_depend>
   <exec_depend>aerial_robot_msgs</exec_depend>
   <exec_depend>aerial_robot_simulation</exec_depend>
+  <exec_depend>delta</exec_depend>
   <exec_depend>dragon</exec_depend>
   <exec_depend>hydrus</exec_depend>
   <exec_depend>hydrus_xi</exec_depend>

aerial_robot_control/CMakeLists.txt

@@ -88,20 +88,21 @@ target_link_libraries (trajectory_generation ${catkin_LIBRARIES})
 add_executable(trajectory_generation_node src/trajectory/test.cpp)
 target_link_libraries (trajectory_generation_node ${catkin_LIBRARIES} trajectory_generation)
 
-
+add_executable(motor_power_publisher src/util/motor_power_publisher.cpp)
+target_link_libraries(motor_power_publisher ${catkin_LIBRARIES})
 
 install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
 
-install(TARGETS control_utils flight_control_pluginlib flight_navigation trajectory_generation
+install(TARGETS control_utils flight_control_pluginlib flight_navigation motor_power_publisher trajectory_generation
   DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 )
 
 install(TARGETS trajectory_generation_node
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
-install(DIRECTORY scripts plugins
+install(DIRECTORY launch scripts plugins
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
   USE_SOURCE_PERMISSIONS
 )

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

@@ -38,6 +38,7 @@
 #include <aerial_robot_control/control/base/base.h>
 #include <aerial_robot_control/control/utils/pid.h>
 #include <aerial_robot_control/PIDConfig.h>
+#include <aerial_robot_estimation/sensor/imu.h>
 #include <aerial_robot_msgs/DynamicReconfigureLevels.h>
 #include <aerial_robot_msgs/PoseControlPid.h>
 #include <angles/angles.h>
@@ -56,7 +57,8 @@ namespace aerial_robot_control
   {
   public:
     PoseLinearController();
-    virtual ~PoseLinearController() = default;
+    virtual ~PoseLinearController();
+
     void virtual initialize(ros::NodeHandle nh,
                             ros::NodeHandle nhp,
                             boost::shared_ptr<aerial_robot_model::RobotModel> robot_model,
@@ -67,8 +69,20 @@ namespace aerial_robot_control
     virtual bool update() override;
     virtual void reset() override;
 
+    const Eigen::VectorXd getTargetWrenchAccCog()
+    {
+      std::lock_guard<std::mutex> lock(wrench_mutex_);
+      return target_wrench_acc_cog_;
+    }
+    void setTargetWrenchAccCog(const Eigen::VectorXd target_wrench_acc_cog)
+    {
+      std::lock_guard<std::mutex> lock(wrench_mutex_);
+      target_wrench_acc_cog_ = target_wrench_acc_cog;
+    }
+
   protected:
     ros::Publisher pid_pub_;
+    ros::Publisher estimate_external_wrench_pub_;
 
     std::vector<PID> pid_controllers_;
     std::vector<boost::shared_ptr<PidControlDynamicConfig> > pid_reconf_servers_;
@@ -88,11 +102,26 @@ namespace aerial_robot_control
     tf::Vector3 rpy_, target_rpy_;
     tf::Vector3 omega_, target_omega_;
 
+    std::string tf_prefix_;
+    std::mutex wrench_mutex_;
+    boost::thread wrench_estimate_thread_;
+    Eigen::VectorXd init_sum_momentum_;
+    Eigen::VectorXd est_external_wrench_;
+    Eigen::VectorXd est_external_wrench_cog_;
+    Eigen::MatrixXd momentum_observer_matrix_;
+    Eigen::VectorXd integrate_term_;
+    double prev_est_wrench_timestamp_;
+    bool wrench_estimate_flag_;
+    Eigen::VectorXd target_wrench_acc_cog_;
+
     virtual void controlCore();
     virtual void sendCmd();
 
 
     void cfgPidCallback(aerial_robot_control::PIDConfig &config, uint32_t level, std::vector<int> controller_indices);
+    void startWrenchEstimation();
+    virtual void externalWrenchEstimate();
+
   };
 
 };

aerial_robot_control/include/aerial_robot_control/control/utils/pid.h

@@ -52,7 +52,8 @@ namespace aerial_robot_control
         const double limit_err_p = 1e6, const double limit_err_i = 1e6, const double limit_err_d = 1e6):
       name_(name), result_(0), err_p_(0), err_i_(0), err_i_prev_(0), err_d_(0),
       target_p_(0), target_d_(0), val_p_(0), val_d_(0),
-      p_term_(0), i_term_(0), d_term_(0)
+      p_term_(0), i_term_(0), d_term_(0),
+      err_i_update_flag_(true)
     {
       setGains(p_gain, i_gain, d_gain);
       setLimits(limit_sum, limit_p, limit_i, limit_d, limit_err_p, limit_err_i, limit_err_d);
@@ -64,7 +65,7 @@ namespace aerial_robot_control
     {
       err_p_ = clamp(err_p, -limit_err_p_, limit_err_p_);
       err_i_prev_ = err_i_;
-      err_i_ = clamp(err_i_ + err_p_ * du, -limit_err_i_, limit_err_i_);
+      if(err_i_update_flag_) err_i_ = clamp(err_i_ + err_p_ * du, -limit_err_i_, limit_err_i_);
       err_d_ = clamp(err_d, -limit_err_d_, limit_err_d_);
 
       p_term_ = clamp(err_p_ * p_gain_, -limit_p_, limit_p_);
@@ -81,6 +82,7 @@ namespace aerial_robot_control
       err_i_ = 0;
       err_i_prev_ = 0;
       result_ = 0;
+      err_i_update_flag_ = true;
     }
 
     const double& getPGain() const { return p_gain_; }
@@ -89,6 +91,15 @@ namespace aerial_robot_control
 
     void setPGain(const double p_gain) { p_gain_ = p_gain; }
     void setIGain(const double i_gain) { i_gain_ = i_gain; }
+    void setIGainHoldingTerm(const double i_gain)
+    {
+      if(i_gain == 0) return;
+      else
+        {
+          err_i_ = err_i_ * i_gain_ / i_gain;
+          i_gain_ = i_gain;
+        }
+    }
     void setDGain(const double d_gain) { d_gain_ = d_gain; }
 
     void setGains(const double p_gain, const double i_gain, const double d_gain)
@@ -103,6 +114,8 @@ namespace aerial_robot_control
     const double& getLimitP() const { return limit_p_; }
     const double& getLimitI() const { return limit_i_; }
     const double& getLimitD() const { return limit_d_; }
+    void setTargetP(const double target_p) {target_p_ = target_p; }
+    void setTargetD(const double target_d) {target_d_ = target_d; }
     void setLimitSum(const double limit_sum) {limit_sum_ = limit_sum; }
     void setLimitP(const double limit_p) {limit_p_ = limit_p; }
     void setLimitI(const double limit_i) {limit_i_ = limit_i; }
@@ -128,11 +141,19 @@ namespace aerial_robot_control
     const double& getErrD() const { return err_d_; }
     void setErrP(const double err_p) { err_p_ = err_p; }
     void setErrI(const double err_i) { err_i_ = err_i; }
+    void setITerm(const double i_term)
+    {
+      i_term_ = i_term;
+      if(err_i_update_flag_ && i_gain_ != 0) err_i_ = i_term / i_gain_;
+    }
 
     const double& getPTerm() const { return p_term_; }
     const double& getITerm() const { return i_term_; }
     const double& getDTerm() const { return d_term_; }
 
+    void setErrIUpdateFlag(bool flag) {err_i_update_flag_ = flag;}
+    bool getErrIUpdateFlag() {return err_i_update_flag_;}
+
   protected:
 
     std::string name_;
@@ -144,6 +165,7 @@ namespace aerial_robot_control
     double limit_err_p_, limit_err_i_, limit_err_d_;
     double target_p_, target_d_;
     double val_p_, val_d_;
+    bool err_i_update_flag_;
   };
 
 };

aerial_robot_control/include/aerial_robot_control/flight_navigation.h

@@ -101,6 +101,7 @@ namespace aerial_robot_navigation
     inline void setTargetYaw(float value) { target_rpy_.setZ(value); }
     inline void addTargetYaw(float value) { setTargetYaw(angles::normalize_angle(target_rpy_.z() + value)); }
     inline void setTargetOmegaZ(float value) { target_omega_.setZ(value); }
+    inline void setTargetRPY(tf::Vector3 value) { target_rpy_ = value; }
     inline void setTargetAngAcc(tf::Vector3 acc) { target_ang_acc_ = acc; }
     inline void setTargetAngAcc(double x, double y, double z) { setTargetAngAcc(tf::Vector3(x, y, z)); }
     inline void setTargetZeroAngAcc() { setTargetAngAcc(tf::Vector3(0,0,0)); }
@@ -361,7 +362,7 @@ namespace aerial_robot_navigation
       land_height_ = 0;
     }
 
-    void startTakeoff()
+    virtual void startTakeoff()
     {
       if(getNaviState() == TAKEOFF_STATE) return;
 
@@ -557,7 +558,11 @@ namespace aerial_robot_navigation
 
     void setTargetYawFromCurrentState()
     {
-      double yaw = estimator_->getState(State::YAW_COG, estimate_mode_)[0];
+      tf::Quaternion cog2baselink_rot;
+      tf::quaternionKDLToTF(robot_model_->getCogDesireOrientation<KDL::Rotation>(), cog2baselink_rot);
+      tf::Matrix3x3 cog_rot = estimator_->getOrientation(Frame::BASELINK, estimate_mode_) * tf::Matrix3x3(cog2baselink_rot).inverse();
+      double r, p, y; cog_rot.getRPY(r, p, y);
+      double yaw = y;
       setTargetYaw(yaw);
 
       // set the velocty to zero

aerial_robot_control/include/aerial_robot_control/util/motor_power_publisher.h

@@ -0,0 +1,40 @@
+#pragma once
+
+#include <ros/ros.h>
+#include <spinal/PwmInfo.h>
+#include <spinal/MotorInfo.h>
+#include <spinal/Pwms.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Float32MultiArray.h>
+
+class motorPowerPublisher
+{
+public:
+  ros::NodeHandle nh_;
+  ros::NodeHandle nhp_;
+  ros::Publisher motor_currencies_pub_;
+  ros::Publisher motor_power_pub_;
+  ros::Subscriber motor_pwms_sub_;
+  ros::Subscriber battery_voltage_sub_;
+  ros::Timer timer_;
+
+  std::vector<float> motor_currencies_;
+  std::vector<double> voltages_;
+  std::vector<std::vector<double>> motor_infos_;
+  std::vector<int> motor_pwms_;
+  double battery_voltage_;
+
+  motorPowerPublisher(ros::NodeHandle nh, ros::NodeHandle nhp);
+  virtual ~motorPowerPublisher() = default;
+
+  void motorPwmsCallback(const spinal::PwmsPtr& msg);
+  void batteryVoltageStatusCallback(const std_msgs::Float32Ptr& msg);
+  void timerCallback(const ros::TimerEvent& e);
+  void rosParamInit();
+
+  template<class T> void getParam(ros::NodeHandle nh, std::string param_name, T& param, T default_value)
+  {
+    nh.param<T>(param_name, param, default_value);
+  }
+
+};

aerial_robot_control/launch/motor_power.launch

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="robot_ns" default=""/>
+  <arg name="config_path" default=""/>
+
+  <node pkg="aerial_robot_control" type="motor_power_publisher" name="motor_power_publisher" ns="$(arg robot_ns)" output="screen"/>
+  <group ns="$(arg robot_ns)">
+    <rosparam file="$(arg config_path)" command="load"/>
+  </group>
+
+</launch>