[Attitude Treatment][WIP] use SO3 system for rotational motion

aerial_robot_control/include/aerial_robot_control/flight_navigation.h

@@ -73,14 +73,16 @@ namespace aerial_robot_navigation
     inline tf::Vector3 getTargetVel() {return target_vel_;}
     inline tf::Vector3 getTargetAcc() {return target_acc_;}
     inline tf::Vector3 getTargetRPY() {return target_rpy_;}
-    inline tf::Vector3 getTargetOmega() {return target_omega_;}
+    inline tf::Vector3 getTargetOmega() {return target_omega_;} 
 
     inline void setTargetRoll(float value) { target_rpy_.setX(value); }
-    inline void setTargetOmageX(float value) { target_omega_.setX(value); }
+    inline void setTargetOmegaX(float value) { target_omega_.setX(value); }
     inline void setTargetPitch(float value) { target_rpy_.setY(value); }
-    inline void setTargetOmageY(float value) { target_omega_.setY(value); }
+    inline void setTargetOmegaY(float value) { target_omega_.setY(value); }
     inline void setTargetYaw(float value) { target_rpy_.setZ(value); }
-    inline void setTargetOmageZ(float value) { target_omega_.setZ(value); }
+    inline void setTargetOmegaZ(float value) { target_omega_.setZ(value); }
+    inline void setTargetRPY(tf::Vector3 value) { target_rpy_ = value; }
+    inline void setTargetOmega(tf::Vector3 value) { target_omega_ = value; }
     inline void setTargetPosX( float value){  target_pos_.setX(value);}
     inline void setTargetVelX( float value){  target_vel_.setX(value);}
     inline void setTargetAccX( float value){  target_acc_.setX(value);}
@@ -497,7 +499,7 @@ namespace aerial_robot_navigation
 
     void setTargetYawFromCurrentState()
     {
-      target_rpy_.setZ(estimator_->getState(State::YAW_COG, estimate_mode_)[0]);
+      target_rpy_.setZ(estimator_->getEuler(Frame::COG, estimate_mode_).z());
 
       // set the velocty to zero
       target_omega_.setZ(0);

aerial_robot_control/src/control/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);
@@ -190,6 +191,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()
@@ -210,10 +217,19 @@ 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
+
 
     // time diff
     double du = ros::Time::now().toSec() - control_timestamp_;

aerial_robot_control/src/flight_navigation.cpp

@@ -139,12 +139,12 @@ void BaseNavigator::naviCallback(const aerial_robot_msgs::FlightNavConstPtr & ms
   if(msg->yaw_nav_mode == aerial_robot_msgs::FlightNav::POS_MODE)
     {
       setTargetYaw(angles::normalize_angle(msg->target_yaw));
-      setTargetOmageZ(0);
+      setTargetOmegaZ(0);
     }
   if(msg->yaw_nav_mode == aerial_robot_msgs::FlightNav::POS_VEL_MODE)
     {
       setTargetYaw(angles::normalize_angle(msg->target_yaw));
-      setTargetOmageZ(msg->target_omega_z);
+      setTargetOmegaZ(msg->target_omega_z);
     }
 
   /* xy control */
@@ -201,7 +201,7 @@ void BaseNavigator::naviCallback(const aerial_robot_msgs::FlightNavConstPtr & ms
             }
           case LOCAL_FRAME:
             {
-              tf::Vector3 target_vel = frameConversion(tf::Vector3(msg->target_vel_x, msg->target_vel_y, 0),  estimator_->getState(State::YAW_COG, estimate_mode_)[0]);
+              tf::Vector3 target_vel = frameConversion(tf::Vector3(msg->target_vel_x, msg->target_vel_y, 0), estimator_->getEuler(Frame::COG, estimate_mode_).z());
               setTargetVelX(target_vel.x());
               setTargetVelY(target_vel.y());
               break;
@@ -244,7 +244,7 @@ void BaseNavigator::naviCallback(const aerial_robot_msgs::FlightNavConstPtr & ms
             }
           case LOCAL_FRAME:
             {
-              tf::Vector3 target_acc = frameConversion(tf::Vector3(msg->target_acc_x, msg->target_acc_y, 0), estimator_->getState(State::YAW_COG, estimate_mode_)[0]);
+              tf::Vector3 target_acc = frameConversion(tf::Vector3(msg->target_acc_x, msg->target_acc_y, 0), estimator_->getEuler(Frame::COG, estimate_mode_).z());
               setTargetAccX(target_acc.x());
               setTargetAccY(target_acc.y());
               break;
@@ -442,10 +442,10 @@ void BaseNavigator::joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg)
   /* this is the yaw_angle control */
   if(fabs(joy_cmd.axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS]) > joy_yaw_deadzone_)
     {
-      double target_yaw = estimator_->getState(State::YAW_COG, estimate_mode_)[0]
+      double target_yaw = estimator_->getEuler(Frame::COG, estimate_mode_).z()
         + joy_cmd.axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS] * max_target_yaw_rate_;
       setTargetYaw(angles::normalize_angle(target_yaw));
-      setTargetOmageZ(joy_cmd.axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS] * max_target_yaw_rate_);
+      setTargetOmegaZ(joy_cmd.axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS] * max_target_yaw_rate_);
 
       yaw_control_flag_ = true;
     }
@@ -455,7 +455,7 @@ void BaseNavigator::joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg)
         {
           yaw_control_flag_= false;
           setTargetYawFromCurrentState();
-          setTargetOmageZ(0);
+          setTargetOmegaZ(0);
           ROS_INFO("Joy Control: fixed yaw state, target yaw angle is %f", getTargetRPY().z());
         }
     }
@@ -522,7 +522,7 @@ void BaseNavigator::joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg)
                 tf::Transform teleop_local_frame_tf;
                 tf::transformKDLToTF(segments_tf.at(robot_model_->getBaselinkName()).Inverse() * segments_tf.at(teleop_local_frame_), teleop_local_frame_tf);
 
-                target_acc_ = frameConversion(target_acc,  tf::Matrix3x3(tf::createQuaternionFromYaw(estimator_->getState(State::YAW_COG, estimate_mode_)[0])) * teleop_local_frame_tf.getBasis());
+                target_acc_ = frameConversion(target_acc,  tf::Matrix3x3(tf::createQuaternionFromYaw(estimator_->getEuler(Frame::COG, estimate_mode_).z())) * teleop_local_frame_tf.getBasis());
               }
           }
         break;
@@ -542,7 +542,7 @@ void BaseNavigator::joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg)
             if(control_frame_ == LOCAL_FRAME)
               {
                 tf::Vector3 target_vel_tmp = target_vel;
-                target_vel = frameConversion(target_vel_tmp,  estimator_->getState(State::YAW_COG, estimate_mode_)[0]);
+                target_vel = frameConversion(target_vel_tmp,  estimator_->getEuler(Frame::COG, estimate_mode_).z());
               }
 
             /* interpolation for vel target */

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_;
@@ -81,19 +78,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 wz_b_; /* unit z axis of world frame 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

@@ -67,6 +67,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_;
@@ -88,6 +91,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 */
     std::string joint_name_;