Revert "[Spinal] Workaround to use Spinal 4.1 firmware in TwinHammer."

.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
+          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

aerial_robot_base/bin/rosbag_control_data.sh

@@ -1,3 +1,3 @@
 #!/bin/bash
 
-rosrun aerial_robot_base rosbag_raw_sensors.sh $1 rosout rosout_agg $1/four_axes/command $1/rpy/pid  $1/rpy/gain $1/debug/pose/pid $1/desire_coordinate $1/flight_config_cmd $1/kf/imu1/data  $1/kf/alt1/data $1/kf/gps1/data $1/kf/mocap1/data $1/kf/vo1/data $1/ground_truth $1/debug/four_axes/gain $1/motor_info $1/p_matrix_pseudo_inverse_inertia $1/servo/torque_enable $1/servo/target_states /tf_static /tf $1/uav/baselink/odom $1/uav/cog/odom $1/uav/full_state $1/uav_info $1/uav_power $1/uav/nav $1/joints_ctrl $1/joint_states  ${@:2}
+rosrun aerial_robot_base rosbag_raw_sensors.sh $1 rosout rosout_agg $1/four_axes/command $1/rpy/pid  $1/rpy/gain $1/debug/pose/pid $1/desire_coordinate $1/flight_config_cmd $1/kf/imu1/data  $1/kf/alt1/data $1/kf/gps1/data $1/kf/mocap1/data $1/kf/vo1/data $1/ground_truth $1/debug/four_axes/gain $1/motor_info $1/p_matrix_pseudo_inverse_inertia $1/servo/torque_enable $1/servo/target_states /tf_static /tf $1/uav/baselink/odom $1/uav/cog/odom $1/uav/full_state $1/uav_info $1/uav_power $1/uav/nav $1/joints_ctrl $1/joint_states $1/joy ${@:2}

aerial_robot_base/config/sensors/lio/livox_mid360.yaml

@@ -0,0 +1,16 @@
+sensor_plugin:
+  vo:
+    vo_sub_topic_name: Odometry_precede
+    level_pos_noise_sigma: 0.01  # 0.01 is better that 0.001
+    z_pos_noise_sigma: 0.01 # 0.01 is better than 0.001
+    z_vel_mode: false
+    vel_noise_sigma: 0.01 # TODO: modification
+    sensor_frame: lidar_imu
+    reference_frame: fc
+    vio_mode: true
+    local_vel_mode: false # the twist/twist/velocity is described in global frame
+    time_sync: false
+    reset_duration: 0.5
+
+    fusion_mode: 2 # ONLY_POS_MODE = 0, ONLY_VEL_MODE = 1, POS_VEL_MODE = 2
+    #param_verbose: true

aerial_robot_base/launch/external_module/mocap.launch

@@ -6,8 +6,7 @@
     type="mocap_node"
     name="mocap_node"
     respawn="false"
-    launch-prefix=""
-    required="true">
+    launch-prefix="">
     <rosparam subst_value="true">
       rigid_bodies:
          '$(arg robot_id)':

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() {}
@@ -463,9 +467,6 @@ namespace aerial_robot_navigation
       if(!teleop_flag_) return;
 
       setNaviState(LAND_STATE);
-
-      setTargetXyFromCurrentState();
-      setTargetYawFromCurrentState();
       ROS_INFO("Land state");
     }
 

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();
+                                                }
+                                            });
+  }
 };