[Beetle][WIP] latest version for real platform

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,21 @@ 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 +103,25 @@ namespace aerial_robot_control
     tf::Vector3 rpy_, target_rpy_;
     tf::Vector3 omega_, target_omega_;
 
+    std::mutex wrench_mutex_;
+    boost::thread wrench_estimate_thread_;
+    Eigen::VectorXd init_sum_momentum_;
+    Eigen::VectorXd est_external_wrench_;
+    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

@@ -50,9 +50,10 @@ namespace aerial_robot_control
         const double p_gain = 0, const double i_gain = 0, const double d_gain = 0,
         const double limit_sum = 1e6, const double limit_p = 1e6, const double limit_i = 1e6, const double limit_d = 1e6,
         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),
+      name_(name), result_(0), err_p_(0), err_p_prev_(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),
+      i_comp_term_(0)
     {
       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,8 +65,23 @@ 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_);
       err_d_ = clamp(err_d, -limit_err_d_, limit_err_d_);
+      err_i_ = clamp(err_i_ + err_p_ * du, -limit_err_i_, limit_err_i_) + i_comp_term_;
+
+      p_term_ = clamp(err_p_ * p_gain_, -limit_p_, limit_p_);
+      i_term_ = clamp(err_i_ * i_gain_, -limit_i_, limit_i_);
+      d_term_ = clamp(err_d_ * d_gain_, -limit_d_, limit_d_);
+
+      result_ = clamp(p_term_ + i_term_ + d_term_ + feedforward_term, -limit_sum_, limit_sum_);
+    }
+
+    virtual void updateWoVel(const double err_p, const double du, const double feedforward_term = 0)
+    {
+      err_p_prev_ = err_p_;
+      err_p_ = clamp(err_p, -limit_err_p_, limit_err_p_);
+      err_i_prev_ = err_i_;
+      err_d_ = clamp((err_p_ - err_p_prev_)/ du, -limit_err_d_, limit_err_d_);
+      err_i_ = clamp(err_i_ + err_p_ * du, -limit_err_i_, limit_err_i_) + i_comp_term_;
 
       p_term_ = clamp(err_p_ * p_gain_, -limit_p_, limit_p_);
       i_term_ = clamp(err_i_ * i_gain_, -limit_i_, limit_i_);
@@ -80,7 +96,9 @@ namespace aerial_robot_control
     {
       err_i_ = 0;
       err_i_prev_ = 0;
+      err_p_prev_ = 0;
       result_ = 0;
+      i_comp_term_ = 0;
     }
 
     const double& getPGain() const { return p_gain_; }
@@ -120,6 +138,9 @@ namespace aerial_robot_control
       setLimitErrI(limit_err_i);
       setLimitErrD(limit_err_d);
     }
+    void setICompTerm(const double i_comp_term){ i_comp_term_ = i_comp_term; }
+    void setErrIRec(const double err_i_rec){ err_i_rec_ = err_i_rec; }
+    const double& getErrIRec() const { return err_i_rec_; }
 
     const std::string getName() const { return name_;}
     const double& getErrP() const { return err_p_; }
@@ -139,11 +160,14 @@ namespace aerial_robot_control
     double result_;
     double p_gain_, i_gain_, d_gain_;
     double p_term_, i_term_, d_term_;
-    double err_p_, err_i_, err_i_prev_, err_d_;
+    double err_p_, err_p_prev_, err_i_, err_i_prev_, err_d_;
+    double err_i_rec_;
     double limit_sum_, limit_p_, limit_i_, limit_d_;
     double limit_err_p_, limit_err_i_, limit_err_d_;
     double target_p_, target_d_;
     double val_p_, val_d_;
+    double i_comp_term_;
+
   };
 
 };

aerial_robot_control/include/aerial_robot_control/flight_navigation.h

@@ -151,6 +151,8 @@ namespace aerial_robot_navigation
     static constexpr float VOLTAGE_20P =  3.747;
     static constexpr float VOLTAGE_10P =  3.683;
     static constexpr float VOLTAGE_0P =  3.209;
+    float pre_battery_percentage_;
+
 
 
     /* playstation dualschock 3 joystick */
@@ -256,6 +258,8 @@ namespace aerial_robot_navigation
 
     bool param_verbose_;
 
+    bool joy_rotation_flag_;
+
     uint8_t navi_state_;
 
     int  xy_control_mode_;
@@ -269,7 +273,7 @@ namespace aerial_robot_navigation
     bool trajectory_mode_;
     bool lock_teleop_;
     ros::Time force_landing_start_time_;
-
+    
     double hover_convergent_start_time_;
     double hover_convergent_duration_;
     double land_check_start_time_;
@@ -346,8 +350,8 @@ namespace aerial_robot_navigation
     virtual void rosParamInit();
     void poseCallback(const geometry_msgs::PoseStampedConstPtr & msg);
     void simpleMoveBaseGoalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
-    void naviCallback(const aerial_robot_msgs::FlightNavConstPtr & msg);
-    void joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg);
+    virtual void naviCallback(const aerial_robot_msgs::FlightNavConstPtr & msg);
+    virtual void joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg);
     void batteryCheckCallback(const std_msgs::Float32ConstPtr &msg);
 
     virtual void halt() {}

aerial_robot_control/src/control/base/pose_linear_controller.cpp

@@ -74,6 +74,15 @@ namespace aerial_robot_control
     pid_msg_.yaw.d_term.resize(1);
   }
 
+  PoseLinearController::~PoseLinearController()
+  {
+    if(wrench_estimate_flag_)
+      {
+        wrench_estimate_thread_.interrupt();
+        wrench_estimate_thread_.join();
+      } 
+  }
+
   void PoseLinearController::initialize(ros::NodeHandle nh,
                                ros::NodeHandle nhp,
                                boost::shared_ptr<aerial_robot_model::RobotModel> robot_model,
@@ -177,8 +186,11 @@ namespace aerial_robot_control
     pid_reconf_servers_.push_back(boost::make_shared<PidControlDynamicConfig>(yaw_nh));
     pid_reconf_servers_.back()->setCallback(boost::bind(&PoseLinearController::cfgPidCallback, this, _1, _2, std::vector<int>(1, YAW)));
 
-
     pid_pub_ = nh_.advertise<aerial_robot_msgs::PoseControlPid>("debug/pose/pid", 10);
+
+    /* external wrench estimation*/
+    getParam<bool>(control_nh, "wrench_estimate_flag", wrench_estimate_flag_, false);
+    if(wrench_estimate_flag_) startWrenchEstimation();
   }
 
   void PoseLinearController::reset()
@@ -341,6 +353,75 @@ namespace aerial_robot_control
     pid_pub_.publish(pid_msg_);
   }
 
+  void PoseLinearController::externalWrenchEstimate()
+  {
+    const Eigen::VectorXd target_wrench_acc_cog = getTargetWrenchAccCog();
+
+    if(navigator_->getNaviState() != aerial_robot_navigation::HOVER_STATE &&
+       navigator_->getNaviState() != aerial_robot_navigation::LAND_STATE)
+      {
+        prev_est_wrench_timestamp_ = 0;
+        integrate_term_ = Eigen::VectorXd::Zero(6);
+        return;
+      }else if(target_wrench_acc_cog.size() == 0){
+        ROS_WARN("Target wrench value for wrench estimation is not setted.");
+        prev_est_wrench_timestamp_ = 0;
+        integrate_term_ = Eigen::VectorXd::Zero(6);
+        return;
+      }
+
+    Eigen::Vector3d vel_w, omega_cog; // workaround: use the filtered value
+    auto imu_handler = boost::dynamic_pointer_cast<sensor_plugin::Imu>(estimator_->getImuHandler(0));
+    tf::vectorTFToEigen(imu_handler->getFilteredVelCog(), vel_w);
+    tf::vectorTFToEigen(imu_handler->getFilteredOmegaCog(), omega_cog);
+    Eigen::Matrix3d cog_rot;
+    tf::matrixTFToEigen(estimator_->getOrientation(Frame::COG, estimate_mode_), cog_rot);
+
+    Eigen::Matrix3d inertia = robot_model_->getInertia<Eigen::Matrix3d>();
+    double mass = robot_model_->getMass();
+
+    Eigen::VectorXd sum_momentum = Eigen::VectorXd::Zero(6);
+    sum_momentum.head(3) = mass * vel_w;
+    sum_momentum.tail(3) = inertia * omega_cog;
+
+    Eigen::VectorXd target_wrench_cog = Eigen::VectorXd::Zero(6);
+    target_wrench_cog.head(3) = mass * target_wrench_acc_cog.head(3);
+    target_wrench_cog.tail(3) = inertia * target_wrench_acc_cog.tail(3);
+
+    Eigen::MatrixXd J_t = Eigen::MatrixXd::Identity(6,6);
+    J_t.topLeftCorner(3,3) = cog_rot;
+
+    Eigen::VectorXd N = mass * robot_model_->getGravity();
+    N.tail(3) = aerial_robot_model::skew(omega_cog) * (inertia * omega_cog);
+
+    if(prev_est_wrench_timestamp_ == 0)
+      {
+        prev_est_wrench_timestamp_ = ros::Time::now().toSec();
+        init_sum_momentum_ = sum_momentum; // not good
+      }
+
+    double dt = ros::Time::now().toSec() - prev_est_wrench_timestamp_;
+
+    integrate_term_ += (J_t * target_wrench_cog - N + est_external_wrench_) * dt;
+
+    est_external_wrench_ = momentum_observer_matrix_ * (sum_momentum - init_sum_momentum_ - integrate_term_);
+
+    Eigen::VectorXd est_external_wrench_cog = est_external_wrench_;
+    est_external_wrench_cog.head(3) = cog_rot.inverse() * est_external_wrench_.head(3);
+
+    geometry_msgs::WrenchStamped wrench_msg;
+    wrench_msg.header.stamp.fromSec(estimator_->getImuLatestTimeStamp());
+    wrench_msg.wrench.force.x = est_external_wrench_(0);
+    wrench_msg.wrench.force.y = est_external_wrench_(1);
+    wrench_msg.wrench.force.z = est_external_wrench_(2);
+    wrench_msg.wrench.torque.x = est_external_wrench_(3);
+    wrench_msg.wrench.torque.y = est_external_wrench_(4);
+    wrench_msg.wrench.torque.z = est_external_wrench_(5);
+    estimate_external_wrench_pub_.publish(wrench_msg);
+
+    prev_est_wrench_timestamp_ = ros::Time::now().toSec();
+  }
+
   void PoseLinearController::cfgPidCallback(aerial_robot_control::PIDConfig &config, uint32_t level, std::vector<int> controller_indices)
   {
     using Levels = aerial_robot_msgs::DynamicReconfigureLevels;
@@ -374,4 +455,36 @@ namespace aerial_robot_control
           }
       }
   }
+
+  void PoseLinearController::startWrenchEstimation()
+  {
+    estimate_external_wrench_pub_ = nh_.advertise<geometry_msgs::WrenchStamped>("estimated_external_wrench", 1);
+
+    est_external_wrench_ = Eigen::VectorXd::Zero(6);
+    init_sum_momentum_ = Eigen::VectorXd::Zero(6);
+    integrate_term_ = Eigen::VectorXd::Zero(6);
+    momentum_observer_matrix_ = Eigen::MatrixXd::Identity(6,6);
+    prev_est_wrench_timestamp_ = 0;
+
+    double force_weight, torque_weight;
+    ros::NodeHandle control_nh(nh_, "controller");
+    getParam<double>(control_nh, "momentum_observer_force_weight", force_weight, 10.0);
+    getParam<double>(control_nh, "momentum_observer_torque_weight", torque_weight, 10.0);
+    momentum_observer_matrix_.topRows(3) *= force_weight;
+    momentum_observer_matrix_.bottomRows(3) *= torque_weight;
+
+    wrench_estimate_thread_ = boost::thread([this]()
+                                            {
+                                              double update_rate;
+                                              ros::NodeHandle control_nh(nh_, "controller");
+                                              control_nh.param ("wrench_estimate_update_rate", update_rate, 100.0);
+
+                                              ros::Rate loop_rate(update_rate);
+                                              while(ros::ok())
+                                                {
+                                                  externalWrenchEstimate();
+                                                  loop_rate.sleep();
+                                                }
+                                            });
+  }
 };

aerial_robot_control/src/flight_navigation.cpp

@@ -25,7 +25,8 @@ BaseNavigator::BaseNavigator():
   joy_stick_heart_beat_(false),
   joy_stick_prev_time_(0),
   teleop_flag_(true),
-  land_check_start_time_(0)
+  land_check_start_time_(0),
+  pre_battery_percentage_(100)
 {
   setNaviState(ARM_OFF_STATE);
 }
@@ -99,7 +100,7 @@ void BaseNavigator::batteryCheckCallback(const std_msgs::Float32ConstPtr &msg)
   float average_voltage = voltage / bat_cell_;
   float input_cell = voltage / VOLTAGE_100P;
   float percentage = 0;
-  if(average_voltage  > VOLTAGE_90P) percentage = (average_voltage - VOLTAGE_90P) / (VOLTAGE_100P - VOLTAGE_90P) * 10 + 90;
+  if(average_voltage  > VOLTAGE_90P)percentage = (average_voltage - VOLTAGE_90P) / (VOLTAGE_100P - VOLTAGE_90P) * 10 + 90;
   else if (average_voltage  > VOLTAGE_80P) percentage = (average_voltage - VOLTAGE_80P) / (VOLTAGE_90P - VOLTAGE_80P) * 10 + 80;
   else if (average_voltage  > VOLTAGE_70P) percentage = (average_voltage - VOLTAGE_70P) / (VOLTAGE_80P - VOLTAGE_70P) * 10 + 70;
   else if (average_voltage  > VOLTAGE_60P) percentage = (average_voltage - VOLTAGE_60P) / (VOLTAGE_70P - VOLTAGE_60P) * 10 + 60;
@@ -111,6 +112,11 @@ void BaseNavigator::batteryCheckCallback(const std_msgs::Float32ConstPtr &msg)
   else percentage = (average_voltage - VOLTAGE_0P) / (VOLTAGE_10P - VOLTAGE_0P) * 10;
 
   if (percentage > 100) percentage = 100;
+
+  if(percentage < pre_battery_percentage_ - 10){
+    pre_battery_percentage_ = percentage - (int)percentage % 10 + 10; // e.g. percentage = 85% -> 90%
+    ROS_WARN_STREAM("Battery : " << int(pre_battery_percentage_) <<"%");
+  }
   if(percentage < 0)
     {
       /* can remove this information */
@@ -454,10 +460,14 @@ void BaseNavigator::joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg)
 
   teleop_reset_time_ = teleop_reset_duration_ + ros::Time::now().toSec();
 
-  double raw_x_cmd = joy_cmd.axes[PS3_AXIS_STICK_LEFT_UPWARDS];
-  double raw_y_cmd = joy_cmd.axes[PS3_AXIS_STICK_LEFT_LEFTWARDS];
+  double raw_x_cmd = 0;
+  double raw_y_cmd = 0;
   double raw_z_cmd = joy_cmd.axes[PS3_AXIS_STICK_RIGHT_UPWARDS];
   double raw_yaw_cmd = joy_cmd.axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS];
+  if(!joy_rotation_flag_){
+    raw_x_cmd = joy_cmd.axes[PS3_AXIS_STICK_LEFT_UPWARDS];
+    raw_y_cmd = joy_cmd.axes[PS3_AXIS_STICK_LEFT_LEFTWARDS];
+  }
 
   /* Motion: Z (Height) */
   if(getNaviState() == HOVER_STATE)