[Ninja][Nav] Debug of desire shape update during disassembly process.

sugihara-16 · Nov 21, 2024 · 226a0ec · 226a0ec
1 parent df1cdc1
commit 226a0ec
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 1 deletion.
diff --git a/robots/ninja/include/ninja/ninja_navigation.h b/robots/ninja/include/ninja/ninja_navigation.h
@@ -108,6 +108,8 @@ namespace aerial_robot_navigation
     double com_roll_change_thresh_;
     double com_pitch_change_thresh_;
     double com_yaw_change_thresh_;
+
+    bool disassembly_flag_;
 
   };
   template<> inline KDL::Frame NinjaNavigator::getCom2Base()

diff --git a/robots/ninja/src/ninja_navigation.cpp b/robots/ninja/src/ninja_navigation.cpp
@@ -141,6 +141,7 @@ void NinjaNavigator::updateEntSysState()
   }
   if(control_flag_){
     reconfig_flag_ =  (pre_assembled_modules_ != module_num_) ? true : false;
+    disassembly_flag_ = (module_num_ < pre_assembled_modules_) ? true : false;
     pre_assembled_modules_ = module_num_;
   }
 }
@@ -402,7 +403,7 @@ void NinjaNavigator::convertTargetPosFromCoG2CoM()
     return;
   } else if((!pre_assembled_  && current_assembled) || (current_assembled && reconfig_flag)){ //assembly or reconfig process
     setTargetCoMPoseFromCurrState();
-    setTargetJointPosFromCurrState();
+    if(!disassembly_flag_) setTargetJointPosFromCurrState(); //bad implementation
     ROS_INFO("switched");
     pre_assembled_ = current_assembled;
     return;