[Control] made external wrench estimation common feature

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/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,
@@ -175,8 +184,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()
@@ -338,6 +350,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;
@@ -371,4 +452,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_estimation/include/aerial_robot_estimation/sensor/imu.h

@@ -31,7 +31,7 @@
  *  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
 
@@ -61,12 +61,37 @@ namespace sensor_plugin
     inline tf::Vector3 getAttitude(uint8_t frame)  { return euler_; }
     inline ros::Time getStamp(){return imu_stamp_;}
 
+    inline tf::Vector3 getFilteredOmegaCog()
+    {
+      boost::lock_guard<boost::mutex> lock(omega_mutex_);
+      return filtered_omega_cog_;
+    }
+
+    inline tf::Vector3 getFilteredVelCog()
+    {
+      boost::lock_guard<boost::mutex> lock(vel_mutex_);
+      return filtered_vel_cog_;
+    }
+
+    void setFilteredOmegaCog(const tf::Vector3 filtered_omega_cog)
+    {
+      boost::lock_guard<boost::mutex> lock(omega_mutex_);
+      filtered_omega_cog_ = filtered_omega_cog;
+    }
+
+    void setFilteredVelCog(const tf::Vector3 filtered_vel_cog)
+    {
+      boost::lock_guard<boost::mutex> lock(vel_mutex_);
+      filtered_vel_cog_ = filtered_vel_cog;
+    }
+
     inline void treatImuAsGroundTruth(bool flag) { treat_imu_as_ground_truth_ = flag; }
 
   protected:
     ros::Publisher  acc_pub_;
     ros::Publisher  imu_pub_;
     ros::Subscriber imu_sub_;
+    ros::Publisher omega_filter_pub_; 
 
     /* rosparam */
     string imu_topic_name_;
@@ -93,6 +118,12 @@ namespace sensor_plugin
     tf::Vector3 acc_bias_l_; /* the acceleration bias in level frame as to baselink frame: previously is acc_i */
     tf::Vector3 acc_bias_w_; /* the acceleration bias in world frame */
     bool treat_imu_as_ground_truth_; /* whether use imu value as ground truth */
+    /* IIR filter*/
+    boost::mutex omega_mutex_;
+    boost::mutex vel_mutex_;
+    tf::Vector3 filtered_vel_cog_;
+    tf::Vector3 filtered_omega_cog_;
+    IirFilter lpf_omega_; // for gyro
 
     aerial_robot_msgs::States state_; /* for debug */
 

aerial_robot_estimation/src/sensor/imu.cpp

@@ -39,6 +39,7 @@ namespace
 {
   int bias_calib = 0;
   ros::Time prev_time;
+  bool first_flag = true;
 }
 
 namespace sensor_plugin
@@ -81,11 +82,21 @@ namespace sensor_plugin
     imu_sub_ = nh_.subscribe<spinal::Imu>(topic_name, 10, &Imu::ImuCallback, this);
     imu_pub_ = indexed_nhp_.advertise<sensor_msgs::Imu>(string("ros_converted"), 1);
     acc_pub_ = indexed_nhp_.advertise<aerial_robot_msgs::Acc>("acc_only", 2);
+
+    //low pass filter
+    double sample_freq, cutoff_freq;
+    getParam<double>("cutoff_freq", cutoff_freq, 20.0);
+    getParam<double>("sample_freq", sample_freq, 200.0);
+    lpf_omega_ = IirFilter(sample_freq, cutoff_freq, 3);
+
+    // debug
+    omega_filter_pub_ = indexed_nhp_.advertise<geometry_msgs::Vector3Stamped>(string("filter_angular_velocity"), 1);
   }
 
   void Imu::ImuCallback(const spinal::ImuConstPtr& imu_msg)
   {
     imu_stamp_ = imu_msg->stamp;
+    tf::Vector3 filtered_omega;
 
     for(int i = 0; i < 3; i++)
       {
@@ -102,6 +113,30 @@ namespace sensor_plugin
         mag_[i] = imu_msg->mag_data[i];
       }
 
+    if(first_flag)
+      {
+        lpf_omega_.setInitValues(omega_);
+        first_flag = false;
+      }
+    filtered_omega = lpf_omega_.filterFunction(omega_);
+    geometry_msgs::Vector3Stamped omega_msg;
+    omega_msg.header.stamp = imu_msg->stamp;
+    tf::vector3TFToMsg(filtered_omega, omega_msg.vector);
+    omega_filter_pub_.publish(omega_msg);
+
+    // workaround: use raw roll&pitch omega (not filtered in spinal) for both angular and linear CoG velocity estimation, yaw is still filtered
+    // note: this is different with hydrus-like control which use filtered omega for CoG estimation
+    omega_.setZ(filtered_omega.z());
+
+    // get filtered angular and linear velocity of CoG
+    tf::Transform cog2baselink_tf;
+    tf::transformKDLToTF(robot_model_->getCog2Baselink<KDL::Frame>(), cog2baselink_tf);
+    int estimate_mode = estimator_->getEstimateMode();
+    setFilteredOmegaCog(cog2baselink_tf.getBasis() * filtered_omega);
+    setFilteredVelCog(estimator_->getVel(Frame::BASELINK, estimate_mode)
+                      + estimator_->getOrientation(Frame::BASELINK, estimate_mode)
+                      * (filtered_omega.cross(cog2baselink_tf.inverse().getOrigin())));
+
     estimateProcess();
     updateHealthStamp();
   }

robots/dragon/config/quad/control/FullVectoringControlConfig.yaml

@@ -10,6 +10,7 @@ controller:
   allocation_refine_max_iteration: 5
   allocation_refine_threshold: 0.0001
 
+  wrench_estimate_flag: true
   wrench_estimate_update_rate: 100
   momentum_observer_force_weight: 3 # heavy delay, less noise, 2, 2.5, 3, light delay, more noise. The old parameter 5 has bug
   momentum_observer_torque_weight: 2.5

robots/dragon/include/dragon/control/full_vectoring_control.h

@@ -52,11 +52,7 @@ namespace aerial_robot_control
   {
   public:
     DragonFullVectoringController();
-    ~DragonFullVectoringController()
-    {
-      wrench_estimate_thread_.interrupt();
-      wrench_estimate_thread_.join();
-    }
+    ~DragonFullVectoringController() = default;
 
     void initialize(ros::NodeHandle nh, ros::NodeHandle nhp,
                     boost::shared_ptr<aerial_robot_model::RobotModel> robot_model,
@@ -69,7 +65,6 @@ namespace aerial_robot_control
     ros::Publisher flight_cmd_pub_; //for spinal
     ros::Publisher gimbal_control_pub_;
     ros::Publisher target_vectoring_force_pub_;
-    ros::Publisher estimate_external_wrench_pub_;
     ros::Publisher rotor_interfere_wrench_pub_;
     ros::Publisher interfrence_marker_pub_;
 
@@ -82,16 +77,6 @@ namespace aerial_robot_control
     bool gimbal_vectoring_check_flag_;
     double allocation_refine_threshold_;
     int allocation_refine_max_iteration_;
-    Eigen::VectorXd target_wrench_acc_cog_;
-
-    /* external wrench */
-    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 rotor_interfere_compensate_;
     double fz_bias_;
@@ -115,7 +100,6 @@ namespace aerial_robot_control
     double overlap_dist_inter_joint_thresh_;
 
 
-    void externalWrenchEstimate();
     const Eigen::VectorXd getTargetWrenchAccCog()
     {
       std::lock_guard<std::mutex> lock(wrench_mutex_);