[Beetle] add new feature that change CoM conversion when the assemble…

…d num is changed.
sugihara-16 · Nov 14, 2023 · a3b98c2 · a3b98c2
1 parent 0d59872
commit a3b98c2
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 16 deletions.
diff --git a/robots/beetle/include/beetle/model/beetle_robot_model.h b/robots/beetle/include/beetle/model/beetle_robot_model.h
@@ -29,6 +29,8 @@ class BeetleRobotModel : public GimbalrotorRobotModel{
   template<class T> T getCog2CoM();
   bool getCurrentAssembled(){return current_assembled_;}
   int getModuleState(){return module_state_;}
+  int getReconfigFlag(){return reconfig_flag_;}
+  int getMyID(){return my_id_;}
 
   void setContactFrame(const KDL::Frame contact_frame){contact_frame_ = contact_frame;}
   void setCog2Cp(const KDL::Frame Cog2Cp){Cog2Cp_ = Cog2Cp;}
@@ -59,6 +61,7 @@ class BeetleRobotModel : public GimbalrotorRobotModel{
   int pre_assembled_modules_;
   int my_id_;
   std::map<int, bool> assembly_flags_;
+  bool reconfig_flag_;
   bool current_assembled_;
   bool hovering_flag_;
   int module_state_;

diff --git a/robots/beetle/src/beetle_navigation.cpp b/robots/beetle/src/beetle_navigation.cpp
@@ -504,21 +504,12 @@ void BeetleNavigator::assemblyFlagCallback(const diagnostic_msgs::KeyValue & msg
   int module_id = std::stoi(msg.key);
   int assembly_flag = std::stoi(msg.value);
   beetle_robot_model_->setAssemblyFlag(module_id,assembly_flag);
-  beetle_robot_model_->calcCenterOfMoving();
-  map<int, bool> flags = beetle_robot_model_->getAssemblyFlags();
-  for(const auto & item : flags){
-    if(item.second)
-      {
-        std::cout << "id: " << item.first << " -> assembled"<< std::endl;
-      } else {
-      std::cout << "id: " << item.first << " -> separated"<< std::endl;
-    }
-  }
 }
 
 void BeetleNavigator::update()
 {
   rotateContactPointFrame();
+  beetle_robot_model_->calcCenterOfMoving();
   convertTargetPosFromCoG2CoM();
   GimbalrotorNavigator::update();
   beetle_robot_model_->setHoveringFlag((getNaviState() == HOVER_STATE) ? true : false);
@@ -542,6 +533,7 @@ void BeetleNavigator::convertTargetPosFromCoG2CoM()
   tf::matrixEigenToTF(beetle_robot_model_->getCogDesireOrientation<Eigen::Matrix3d>(),cog_orientation_tf);
   tf::Vector3 com_conversion = cog_orientation_tf *  tf::Matrix3x3(tf::createQuaternionFromYaw(getTargetRPY().z())) * cog2com_tf.getOrigin();
   bool current_assembled = beetle_robot_model_->getCurrentAssembled();
+  bool reconfig_flag = beetle_robot_model_->getReconfigFlag();
 
   if(pre_assembled_  && !current_assembled){ //disassembly process
     setTargetPosCandX(getTargetPos().x());
@@ -551,10 +543,11 @@ void BeetleNavigator::convertTargetPosFromCoG2CoM()
     pre_assembled_ = current_assembled;
   }
 
-  if(!pre_assembled_  && current_assembled){ //assembly process
+  if((!pre_assembled_  && current_assembled) || (current_assembled && reconfig_flag)){ //assembly or reconfig process
+    int my_id = beetle_robot_model_->getMyID();
     tf::Vector3 pos_cog = estimator_->getPos(Frame::COG, estimate_mode_);
     tf::Vector3 orientation_err = getTargetRPY() - estimator_ ->getEuler(Frame::COG, estimate_mode_);
-    ROS_INFO_STREAM("orientation_err is "<< "(" << orientation_err.x() << ", " << orientation_err.y() << ", " << orientation_err.z() << ")");
+    ROS_INFO_STREAM("ID: " << my_id << "'s orientation_err is "<< "(" << orientation_err.x() << ", " << orientation_err.y() << ", " << orientation_err.z() << ")");
     tf::Matrix3x3 att_err_mat = tf::Matrix3x3(tf::createQuaternionFromRPY(orientation_err.x(), orientation_err.y(),orientation_err.z()));
     tf::Vector3 corrected_target_pos =  tf::Matrix3x3(tf::createQuaternionFromRPY(orientation_err.x(), orientation_err.y(),orientation_err.z())) * pos_cog;
     setTargetPosCandX(pos_cog.x() + (att_err_mat.inverse() * com_conversion).x());
@@ -563,7 +556,7 @@ void BeetleNavigator::convertTargetPosFromCoG2CoM()
     ROS_INFO("switched");
     pre_assembled_ = current_assembled;
   }
-  pre_assembled_ = current_assembled;
+
 
   /* Check whether the target value was changed by someway other than uav nav */
   /* Target pos candidate represents a target pos in a assembly frame */

diff --git a/robots/beetle/src/model/beetle_robot_model.cpp b/robots/beetle/src/model/beetle_robot_model.cpp
@@ -27,12 +27,11 @@ void BeetleRobotModel::updateRobotModelImpl(const KDL::JntArray& joint_positions
   KDL::Frame Cog2Cp;
   setCog2Cp(getCog<KDL::Frame>().Inverse() * variable_cp_frame);
 
-  calcCenterOfMoving();
+  // calcCenterOfMoving();
 }
 
 void BeetleRobotModel::calcCenterOfMoving()
 {
-
   std::string cog_name = "beetle" + std::to_string(my_id_) + "/cog";
   Eigen::Vector3f center_of_moving = Eigen::Vector3f::Zero();
   int assembled_module = 0;
@@ -98,7 +97,8 @@ void BeetleRobotModel::calcCenterOfMoving()
   br_.sendTransform(tf);
 
   //update com-cog distance only during hovering
-  if(pre_assembled_modules_ != assembled_module && hovering_flag_){
+  reconfig_flag_ =  (pre_assembled_modules_ != assembled_module) ? true : false;
+  if(reconfig_flag_ && hovering_flag_){
     Eigen::Vector3f cog_com_dist(center_of_moving.norm() * center_of_moving.x()/fabs(center_of_moving.x()),0,0);
     ROS_INFO_STREAM("cog_com_dist is " << cog_com_dist.transpose());
     tf.transform.translation.x = cog_com_dist.x();
@@ -107,6 +107,14 @@ void BeetleRobotModel::calcCenterOfMoving()
     setCog2CoM(tf2::transformToKDL(tf));
     pre_assembled_modules_ = assembled_module;
     Eigen::VectorXi id_vector = Eigen::Map<Eigen::VectorXi>(assembled_modules_ids.data(), assembled_modules_ids.size());
+    for(const auto & item : assembly_flags_){
+      if(item.second)
+        {
+          std::cout << "id: " << item.first << " -> assembled"<< std::endl;
+        } else {
+        std::cout << "id: " << item.first << " -> separated"<< std::endl;
+      }
+    }
     ROS_INFO_STREAM("Leader's ID is " <<leader_id);
   }
     cog_com_dist_msg.x = Cog2CoM_.p.x();