[Ninja][Navigation][WIP] workaound to make morphing motion smooth.

sugihara-16 · Sep 20, 2024 · fb61f59 · fb61f59
1 parent 90c0628
commit fb61f59
Show file tree

Hide file tree

Showing 5 changed files with 146 additions and 62 deletions.
diff --git a/robots/gimbalrotor/include/gimbalrotor/gimbalrotor_navigation.h b/robots/gimbalrotor/include/gimbalrotor/gimbalrotor_navigation.h
@@ -24,6 +24,7 @@ namespace aerial_robot_navigation
     tf::Vector3 getFinalTargetBaselinkRot(){return final_target_baselink_rot_;}
 
     void setFinalTargetBaselinkRot(tf::Vector3 final_target_baselink_rot){final_target_baselink_rot_ = final_target_baselink_rot;}
+    void forceSetTargetBaselinkRot(tf::Vector3 target_baselink_rot);
   protected:
     void rosParamInit() override;
     virtual void setFinalTargetBaselinkRotCallback(const spinal::DesireCoordConstPtr & msg);

diff --git a/robots/gimbalrotor/src/gimbalrotor_navigation.cpp b/robots/gimbalrotor/src/gimbalrotor_navigation.cpp
@@ -64,6 +64,15 @@ void GimbalrotorNavigator::baselinkRotationProcess()
     }
 }
 
+void GimbalrotorNavigator::forceSetTargetBaselinkRot(tf::Vector3 target_baselink_rot){
+  final_target_baselink_rot_ = target_baselink_rot;
+  curr_target_baselink_rot_ = target_baselink_rot;
+  spinal::DesireCoord target_baselink_rot_msg;
+  target_baselink_rot_msg.roll = curr_target_baselink_rot_.x();
+  target_baselink_rot_msg.pitch = curr_target_baselink_rot_.y();
+  curr_target_baselink_rot_pub_.publish(target_baselink_rot_msg);  
+}
+
 void GimbalrotorNavigator::rosParamInit()
 {
   BaseNavigator::rosParamInit();

diff --git a/robots/ninja/config/NavigationConfig.yaml b/robots/ninja/config/NavigationConfig.yaml
@@ -37,7 +37,9 @@ navigation:
   # desire tilt
   baselink_rot_pub_interval: 0.5
   baselink_rot_change_thresh: 0.01 # 0.2 hz -> 0.04; 0.01 hz-> 0.02
+  com_rot_change_thrush: 0.01
 
   #morphing
-  morphing_vel: 0.05
+  morphing_vel: 0.04
   joint_process_func: 2 # 0 -> CONSTANT, 1 -> FRAC, 2 -> EXP
+  free_joint_flag: false
diff --git a/robots/ninja/include/ninja/ninja_navigation.h b/robots/ninja/include/ninja/ninja_navigation.h
@@ -26,18 +26,18 @@ namespace aerial_robot_navigation
   class ModuleData
   {
   public:
-    ModuleData(int id): id_(id), joint_pos_(), goal_joint_pos_(), start_joint_pos_(), module_tree_(), first_joint_processed_time_(), joint_process_coef_(){}
-    ModuleData(): id_(1), joint_pos_(), goal_joint_pos_(), start_joint_pos_(), module_tree_(), first_joint_processed_time_(), joint_process_coef_(){}
+    ModuleData(int id): id_(id), des_joint_pos_(), est_joint_pos_(), goal_joint_pos_(), start_joint_pos_(), module_tree_(), first_joint_processed_time_(), joint_process_coef_(){}
+    ModuleData(): id_(1), des_joint_pos_(), est_joint_pos_(), goal_joint_pos_(), start_joint_pos_(), module_tree_(), first_joint_processed_time_(), joint_process_coef_(){}
     int id_;
-    KDL::JntArray joint_pos_;
+    KDL::JntArray des_joint_pos_;
+    KDL::JntArray est_joint_pos_;
     KDL::Tree module_tree_;
 
     std::vector<double> first_joint_processed_time_;
     std::vector<double> joint_process_coef_;
     KDL::JntArray goal_joint_pos_;
     KDL::JntArray start_joint_pos_;
 
-
   };
   class NinjaNavigator : public BeetleNavigator
   {
@@ -51,6 +51,7 @@ namespace aerial_robot_navigation
                     double loop_du) override;
     void update() override;
     void setTargetComRot(KDL::Rotation target_com_rot){ target_com_rot_ = target_com_rot;}
+    void setGoalComRot(KDL::Rotation goal_com_rot){ goal_com_rot_ = goal_com_rot;}
     void setTargetCoMPoseFromCurrState();
     void setCoM2Base(const KDL::Frame com2base){com2base_ = com2base;}
     void morphingProcess();
@@ -68,8 +69,9 @@ namespace aerial_robot_navigation
 
   private:
     void convertTargetPosFromCoG2CoM() override;
-    void setTargetCoMRotCallback(const spinal::DesireCoordConstPtr & msg);
+    void setGoalCoMRotCallback(const spinal::DesireCoordConstPtr & msg);
     void assemblyJointPosCallback(const sensor_msgs::JointStateConstPtr& msg);
+    void comRotationProcess();
 
     ros::Publisher target_com_pose_pub_;
     boost::shared_ptr<NinjaRobotModel> ninja_robot_model_;
@@ -80,6 +82,7 @@ namespace aerial_robot_navigation
     std::map<int, ModuleData> assembled_modules_data_;
     std::vector<double> joint_pos_errs_;
 
+    KDL::Rotation goal_com_rot_;
     KDL::Rotation target_com_rot_;
     KDL::Frame com2base_;
     KDL::Frame test_frame_;
@@ -90,6 +93,12 @@ namespace aerial_robot_navigation
     int joint_process_func_;
 
     bool free_joint_flag_;
+
+    double morphing_process_interval_;
+    double prev_morphing_stamp_;
+    bool morphing_flag_;
+
+    double com_rot_change_thresh_;
 
   };
   template<> inline KDL::Frame NinjaNavigator::getCom2Base()