[estimation][control] SO3 attitude control

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)); }
@@ -557,7 +558,7 @@ namespace aerial_robot_navigation
 
     void setTargetYawFromCurrentState()
     {
-      double yaw = estimator_->getState(State::YAW_COG, estimate_mode_)[0];
+      double yaw = estimator_->getEuler(Frame::COG, estimate_mode_).z();
       setTargetYaw(yaw);
 
       // set the velocty to zero

aerial_robot_control/src/control/base/pose_linear_controller.cpp

@@ -46,6 +46,7 @@ namespace aerial_robot_control
     vel_(0,0,0), target_vel_(0,0,0),
     rpy_(0,0,0), target_rpy_(0,0,0),
     target_acc_(0,0,0),
+    target_omega_(0,0,0),
     start_rp_integration_(false)
   {
     pid_msg_.x.total.resize(1);
@@ -187,6 +188,12 @@ namespace aerial_robot_control
     start_rp_integration_ = false;
 
     for(auto& controller: pid_controllers_) controller.reset();
+
+    target_pos_.setValue(0, 0, 0);
+    target_vel_.setValue(0, 0, 0);
+    target_acc_.setValue(0, 0, 0);
+    target_rpy_.setValue(0, 0, 0);
+    target_omega_.setValue(0, 0, 0);
   }
 
   bool PoseLinearController::update()
@@ -207,10 +214,18 @@ namespace aerial_robot_control
     target_vel_ = navigator_->getTargetVel();
     target_acc_ = navigator_->getTargetAcc();
 
-    rpy_ = estimator_->getEuler(Frame::COG, estimate_mode_);
+    // rpy_ = estimator_->getEuler(Frame::COG, estimate_mode_);
+    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);
+    rpy_.setValue(r, p, y);
+
     omega_ = estimator_->getAngularVel(Frame::COG, estimate_mode_);
     target_rpy_ = navigator_->getTargetRPY();
-    target_omega_ = navigator_->getTargetOmega();
+    tf::Matrix3x3 target_rot; target_rot.setRPY(target_rpy_.x(), target_rpy_.y(), target_rpy_.z());
+    tf::Vector3 target_omega = navigator_->getTargetOmega(); // w.r.t. target cog frame
+    target_omega_ = cog_rot.inverse() * target_rot * target_omega; // w.r.t. current cog frame
     target_ang_acc_ = navigator_->getTargetAngAcc();
 
     // time diff

aerial_robot_control/src/flight_navigation.cpp

@@ -258,7 +258,7 @@ void BaseNavigator::naviCallback(const aerial_robot_msgs::FlightNavConstPtr & ms
             }
           case LOCAL_FRAME:
             {
-              double yaw_angle = estimator_->getState(State::YAW_COG, estimate_mode_)[0];
+              double yaw_angle = estimator_->getEuler(Frame::COG, estimate_mode_).z();
               tf::Vector3 target_vel = frameConversion(tf::Vector3(msg->target_vel_x, msg->target_vel_y, 0), yaw_angle);
               setTargetVelX(target_vel.x());
               setTargetVelY(target_vel.y());
@@ -300,7 +300,7 @@ void BaseNavigator::naviCallback(const aerial_robot_msgs::FlightNavConstPtr & ms
             }
           case LOCAL_FRAME:
             {
-              double yaw_angle = estimator_->getState(State::YAW_COG, estimate_mode_)[0];
+              double yaw_angle = estimator_->getEuler(Frame::COG, estimate_mode_).z();
               tf::Vector3 target_acc = frameConversion(tf::Vector3(msg->target_acc_x, msg->target_acc_y, 0), yaw_angle);
               setTargetAccX(target_acc.x());
               setTargetAccY(target_acc.y());
@@ -549,7 +549,7 @@ void BaseNavigator::joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg)
       std::string baselink = robot_model_->getBaselinkName();
       tf::transformKDLToTF(segments_tf.at(baselink).Inverse() * segments_tf.at(teleop_local_frame_), teleop_local_frame_tf);
 
-      double yaw_angle = estimator_->getState(State::YAW_COG, estimate_mode_)[0];
+      double yaw_angle = estimator_->getEuler(Frame::COG, estimate_mode_).z();
       local_frame_rot = tf::Matrix3x3(tf::createQuaternionFromYaw(yaw_angle)) * teleop_local_frame_tf.getBasis();
     }
 
@@ -735,7 +735,7 @@ void BaseNavigator::update()
               setTargetAngAccZ(target_ang_acc_z);
 
               tf::Vector3 curr_pos = estimator_->getPos(Frame::COG, estimate_mode_);
-              double yaw_angle = estimator_->getState(State::YAW_COG, estimate_mode_)[0];
+              double yaw_angle = estimator_->getEuler(Frame::COG, estimate_mode_).z();
               ROS_INFO_THROTTLE(0.5, "[Nav] trajectory mode, target pos&yaw: [%f, %f, %f, %f], curr pos&yaw: [%f, %f, %f, %f]", target_state.p(0), target_state.p(1), target_state.p(2), target_yaw, curr_pos.x(), curr_pos.y(), curr_pos.z(), yaw_angle);
             }
         }

aerial_robot_estimation/include/aerial_robot_estimation/sensor/imu.h

@@ -58,11 +58,8 @@ namespace sensor_plugin
     ~Imu() {}
     Imu();
 
-    inline tf::Vector3 getAttitude(uint8_t frame)  { return euler_; }
     inline ros::Time getStamp(){return imu_stamp_;}
 
-    inline void treatImuAsGroundTruth(bool flag) { treat_imu_as_ground_truth_ = flag; }
-
   protected:
     ros::Publisher  acc_pub_;
     ros::Publisher  imu_pub_;
@@ -80,19 +77,19 @@ namespace sensor_plugin
     double sensor_dt_;
 
     /* imu */
-    tf::Vector3 euler_; /* the euler angle of both body and cog frame */
-    tf::Vector3 omega_; /* the omega both body and cog frame */
-    tf::Vector3 mag_; /* the magnetometer both body and cog frame */
+    tf::Vector3 g_b_; /* the *opposite* gravity vector in baselink frame */
+    tf::Vector3 omega_; /* the omega both of body frame */
+    tf::Vector3 mag_; /* the magnetometer of body frame */
     /* acc */
     tf::Vector3 acc_b_; /* the acceleration in baselink frame */
-    tf::Vector3 acc_l_; /* the acceleration in level frame as to baselink frame: previously is acc_i */
-    tf::Vector3 acc_w_; /* the acceleration in world frame */
-    tf::Vector3 acc_non_bias_w_; /* the acceleration without bias in world frame */
+    std::array<tf::Vector3, 2> acc_w_; /* the acceleration in world frame, for estimate_mode and expriment_mode */
+    std::array<tf::Vector3, 2> acc_non_bias_w_; /* the acceleration without bias in world frame for estimate_mode and expriment_mode */
     /* acc bias */
     tf::Vector3 acc_bias_b_; /* the acceleration bias in baselink frame, only use z axis  */
-    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 */
+    std::array<tf::Vector3, 2> acc_bias_w_; /* the acceleration bias in world frame for estimate_mode and expriment_mode*/
+
+    /* orientation */
+    std::array<tf::Matrix3x3, 2> cog_rot_, base_rot_;
 
     aerial_robot_msgs::States state_; /* for debug */
 

aerial_robot_estimation/include/aerial_robot_estimation/sensor/vo.h

@@ -70,6 +70,9 @@ namespace sensor_plugin
 
     bool reset();
 
+    const bool rotValid() const { return rot_valid_; }
+    const tf::Transform& getRawBaselinkTF() const { return baselink_tf_; }
+
   private:
     /* ros */
     ros::Subscriber vo_sub_;
@@ -93,6 +96,7 @@ namespace sensor_plugin
     /* heuristic sepecial flag for fusion */
     bool outdoor_;
     bool z_no_delay_;
+    bool rot_valid_; // the estimated orientatin by VO is whether valid or not.
 
 
     /* servo */

aerial_robot_estimation/include/aerial_robot_estimation/state_estimation.h

@@ -83,14 +83,25 @@ namespace State
       X_BASE, //x axis of Baselink
       Y_BASE, //y axis of Baselink
       Z_BASE, //y axis of Baselink
-      ROLL_COG, //roll of CoG in world coord
-      PITCH_COG, //pitch of CoG in world coord
-      YAW_COG, //yaw of CoG in world coord
-      ROLL_BASE, //roll of baselink in world coord
-      PITCH_BASE, //pitch of baselink in world coord
-      YAW_BASE, //yaw of baselink in world coord
       TOTAL_NUM,
     };
+
+  namespace CoG
+  {
+    enum
+      {
+       Rot = 10,
+      };
+  };
+
+  namespace Base
+  {
+    enum
+      {
+       Rot = 11,
+      };
+  };
+
 };
 
 namespace Sensor
@@ -135,29 +146,42 @@ namespace aerial_robot_estimation
     int getStateStatus(uint8_t axis, uint8_t estimate_mode)
     {
       boost::lock_guard<boost::mutex> lock(state_mutex_);
-      assert(axis < State::TOTAL_NUM);
-      return state_[axis][estimate_mode].first;
+      if(axis < State::TOTAL_NUM)
+        return state_[axis][estimate_mode].first;
+      else if(axis == State::Base::Rot)
+        return base_rot_status_.at(estimate_mode);
+      else if(axis == State::CoG::Rot)
+        return cog_rot_status_.at(estimate_mode);
+      else
+        return 0;
     }
 
-    void setStateStatus( uint8_t axis, uint8_t estimate_mode, bool status)
+    void setStateStatus(uint8_t axis, uint8_t estimate_mode, bool status)
     {
       boost::lock_guard<boost::mutex> lock(state_mutex_);
-      assert(axis < State::TOTAL_NUM);
-      if(status) state_[axis][estimate_mode].first ++;
+
+      int* incre_status;
+      if(axis < State::TOTAL_NUM)
+        incre_status = &(state_[axis][estimate_mode].first);
+      else if(axis == State::Base::Rot)
+        incre_status = &(base_rot_status_.at(estimate_mode));
+      else if(axis == State::CoG::Rot)
+        incre_status = &(cog_rot_status_.at(estimate_mode));
+      else
+        return;
+
+      if(status) (*incre_status) ++;
       else
         {
-          if(state_[axis][estimate_mode].first > 0)
-            state_[axis][estimate_mode].first --;
+          if(*incre_status > 0) (*incre_status) --;
           else
-            ROS_ERROR("wrong status update for axis: %d, estimate mode: %d", axis, estimate_mode);
+            ROS_WARN("wrong status update for axis: %d, estimate mode: %d", axis, estimate_mode);
         }
     }
 
-    /* axis: state axis (11) */
     AxisState getState( uint8_t axis)
     {
       boost::lock_guard<boost::mutex> lock(state_mutex_);
-      assert(axis < State::TOTAL_NUM);
       return state_[axis];
     }
 
@@ -219,71 +243,72 @@ namespace aerial_robot_estimation
     tf::Matrix3x3 getOrientation(int frame, int estimate_mode)
     {
       boost::lock_guard<boost::mutex> lock(state_mutex_);
-      tf::Matrix3x3 r;
-      r.setRPY((state_[State::ROLL_COG + frame * 3][estimate_mode].second)[0],
-               (state_[State::PITCH_COG + frame * 3][estimate_mode].second)[0],
-               (state_[State::YAW_COG + frame * 3][estimate_mode].second)[0]);
-      return r;
+
+      if(frame == Frame::COG)
+        return cog_rots_.at(estimate_mode);
+      else if(frame == Frame::BASELINK)
+        return base_rots_.at(estimate_mode);
+
+      return tf::Matrix3x3::getIdentity();
     }
 
-    tf::Vector3 getEuler(int frame, int estimate_mode)
+    void setOrientation(int frame, int estimate_mode, tf::Matrix3x3 rot)
     {
       boost::lock_guard<boost::mutex> lock(state_mutex_);
 
-      return tf::Vector3((state_[State::ROLL_COG + frame * 3][estimate_mode].second)[0],
-                         (state_[State::PITCH_COG + frame * 3][estimate_mode].second)[0],
-                         (state_[State::YAW_COG + frame * 3][estimate_mode].second)[0]);
+      if(frame == Frame::COG)
+        cog_rots_.at(estimate_mode) = rot;
+      else if(frame == Frame::BASELINK)
+        base_rots_.at(estimate_mode) = rot;
     }
 
-    void setEuler(int frame, int estimate_mode, tf::Vector3 euler)
+    tf::Vector3 getEuler(int frame, int estimate_mode)
     {
-      boost::lock_guard<boost::mutex> lock(state_mutex_);
-      (state_[State::ROLL_COG + frame * 3][estimate_mode].second)[0] = euler[0];
-      (state_[State::PITCH_COG + frame * 3][estimate_mode].second)[0] = euler[1];
-      (state_[State::YAW_COG + frame * 3][estimate_mode].second)[0] = euler[2];
+      tf::Matrix3x3 rot = getOrientation(frame, estimate_mode);
+      tfScalar r = 0, p = 0, y = 0;
+      rot.getRPY(r, p, y);
+
+      return tf::Vector3(r, p, y);
     }
 
     tf::Vector3 getAngularVel(int frame, int estimate_mode)
     {
       boost::lock_guard<boost::mutex> lock(state_mutex_);
-      return tf::Vector3((state_[State::ROLL_COG + frame * 3][estimate_mode].second)[1],
-                         (state_[State::PITCH_COG + frame * 3][estimate_mode].second)[1],
-                         (state_[State::YAW_COG + frame * 3][estimate_mode].second)[1]);
+
+      if(frame == Frame::COG)
+        return cog_omegas_.at(estimate_mode);
+      else if(frame == Frame::BASELINK)
+        return base_omegas_.at(estimate_mode);
+
+      return tf::Vector3(0,0,0);
     }
 
     void setAngularVel(int frame, int estimate_mode, tf::Vector3 omega)
     {
       boost::lock_guard<boost::mutex> lock(state_mutex_);
-      (state_[State::ROLL_COG + frame * 3][estimate_mode].second)[1] = omega[0];
-      (state_[State::PITCH_COG + frame * 3][estimate_mode].second)[1] = omega[1];
-      (state_[State::YAW_COG + frame * 3][estimate_mode].second)[1] = omega[2];
+
+      if(frame == Frame::COG)
+        cog_omegas_.at(estimate_mode) = omega;
+      else if(frame == Frame::BASELINK)
+        base_omegas_.at(estimate_mode) = omega;
     }
 
     inline void setQueueSize(const int& qu_size) {qu_size_ = qu_size;}
-    void updateQueue(const double timestamp, const double roll, const double pitch, const tf::Vector3 omega)
+    void updateQueue(const double timestamp, const tf::Matrix3x3 r_ee, const tf::Matrix3x3 r_ex, const tf::Vector3 omega)
     {
-      tf::Matrix3x3 r_ee, r_ex, r_gt;
-      r_ee.setRPY(roll, pitch, (getState(State::YAW_BASE, EGOMOTION_ESTIMATE))[0]);
-      r_ex.setRPY(roll, pitch, (getState(State::YAW_BASE, EXPERIMENT_ESTIMATE))[0]);
-      r_gt.setRPY(roll, pitch, (getState(State::YAW_BASE, GROUND_TRUTH))[0]);
+      boost::lock_guard<boost::mutex> lock(queue_mutex_);
+      timestamp_qu_.push_back(timestamp);
+      rot_ee_qu_.push_back(r_ee);
+      rot_ex_qu_.push_back(r_ex);
+      omega_qu_.push_back(omega);
 
-      {
-        boost::lock_guard<boost::mutex> lock(queue_mutex_);
-        timestamp_qu_.push_back(timestamp);
-        rot_ee_qu_.push_back(r_ee);
-        rot_ex_qu_.push_back(r_ex);
-        rot_gt_qu_.push_back(r_gt);
-        omega_qu_.push_back(omega);
-
-        if(timestamp_qu_.size() > qu_size_)
-          {
-            timestamp_qu_.pop_front();
-            rot_ee_qu_.pop_front();
-            rot_ex_qu_.pop_front();
-            rot_gt_qu_.pop_front();
-            omega_qu_.pop_front();
-          }
-      }
+      if(timestamp_qu_.size() > qu_size_)
+        {
+          timestamp_qu_.pop_front();
+          rot_ee_qu_.pop_front();
+          rot_ex_qu_.pop_front();
+          omega_qu_.pop_front();
+        }
     }
 
     bool findRotOmega(const double timestamp, const int mode, tf::Matrix3x3& r, tf::Vector3& omega, bool verbose = true)
@@ -358,9 +383,6 @@ namespace aerial_robot_estimation
         case EXPERIMENT_ESTIMATE:
           r = rot_ex_qu_.at(candidate_index);
           break;
-        case GROUND_TRUTH:
-          r = rot_gt_qu_.at(candidate_index);
-          break;
         default:
           ROS_ERROR("estimation search state with timestamp: wrong mode %d", mode);
           return false;
@@ -391,6 +413,8 @@ namespace aerial_robot_estimation
     /* latitude & longitude value for GPS based navigation */
     inline void setCurrGpsPoint(const geographic_msgs::GeoPoint point) {curr_wgs84_poiont_ = point;}
     inline const geographic_msgs::GeoPoint& getCurrGpsPoint() const {return curr_wgs84_poiont_;}
+    inline const bool hasGroundTruthOdom() const {return has_groundtruth_odom_; }
+    inline void receiveGroundTruthOdom(bool flag) {has_groundtruth_odom_ = flag; }
 
 
     const SensorFuser& getFuser(int mode)
@@ -452,13 +476,19 @@ namespace aerial_robot_estimation
     boost::shared_ptr<aerial_robot_model::RobotModel> robot_model_;
     std::string tf_prefix_;
 
-    /* 9: x_w, y_w, z_w, roll_w, pitch_w, yaw_cog_w, x_b, y_b, yaw_board_w */
-    array<AxisState, State::TOTAL_NUM> state_;
+    /* 6: x_w, y_w, z_w, x_b, y_b */
+    /* TODO: check to vector3 */
+    array<AxisState, 6> state_;
+    array<tf::Matrix3x3, 3> base_rots_, cog_rots_; // TODO: should abolish the different between orientation of baselink and that of CoG
+    array<tf::Vector3, 3> base_omegas_, cog_omegas_; // TODO: should abolish the different between orientation of baselink and that of CoG
+    array<int, 3> base_rot_status_, cog_rot_status_;
+
+    bool has_groundtruth_odom_; // whether receive entire groundthtruth odometry (e.g., for simulation mode)
 
     /* for calculate the sensor to baselink with the consideration of time delay */
     int qu_size_;
     deque<double> timestamp_qu_;
-    deque<tf::Matrix3x3> rot_ee_qu_, rot_ex_qu_, rot_gt_qu_;
+    deque<tf::Matrix3x3> rot_ee_qu_, rot_ex_qu_;
     deque<tf::Vector3> omega_qu_;
 
     /* sensor fusion */