[Beetle][Navigation] workaround to support roll pitch rotation when a…

…ssembly state
sugihara-16 · Sep 15, 2023 · fec08f7 · fec08f7
1 parent c4a1353
commit fec08f7
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 5 deletions.
diff --git a/robots/beetle/src/beetle_navigation.cpp b/robots/beetle/src/beetle_navigation.cpp
@@ -257,12 +257,12 @@ void BeetleNavigator::convertTargetPosFromCoG2CoM()
   }
 
   if( int(pre_target_pos_.z() * 1000) != int(getTargetPos().z() * 1000)){
-    float target_z_com = getTargetPos().z() + cog2com_tf.getOrigin().z();
+    float target_z_com = getTargetPos().z() + (tf::Matrix3x3(tf::createQuaternionFromYaw(getTargetRPY().z())) * cog2com_tf.getOrigin()).z();
     setTargetPosCandZ(target_z_com);
   }
 
   tf::Vector3 target_cog_pos = getTargetPosCand();
-  target_cog_pos -= tf::Matrix3x3(tf::createQuaternionFromYaw(getTargetRPY().z())) * cog2com_tf.getOrigin();
+  target_cog_pos -=  tf::Matrix3x3(tf::createQuaternionFromYaw(getTargetRPY().z())) * cog2com_tf.getOrigin();
 
   if( getNaviState() == HOVER_STATE){
     setTargetPosX(target_cog_pos.x());

diff --git a/robots/beetle/src/model/beetle_robot_model.cpp b/robots/beetle/src/model/beetle_robot_model.cpp
@@ -44,7 +44,7 @@ void BeetleRobotModel::calcCenterOfMoving()
     if(!value) continue;
     try
       {
-        transformStamped = tfBuffer_.lookupTransform(cog_name, "beetle" + std::to_string(id) + std::string("/root") , ros::Time(0));
+        transformStamped = tfBuffer_.lookupTransform(cog_name, "beetle" + std::to_string(id) + std::string("/cog") , ros::Time(0));
         auto& trans = transformStamped.transform.translation;
         Eigen::Vector3d module_root(trans.x,trans.y,trans.z); 
         center_of_moving += module_root;
@@ -72,8 +72,7 @@ void BeetleRobotModel::calcCenterOfMoving()
   tf.transform.rotation.w = 1;
   br_.sendTransform(tf);
   KDL::Frame com_frame = tf2::transformToKDL(tf);
-  setCog2CoM(getCog<KDL::Frame>().Inverse() * com_frame);
-
+  setCog2CoM(com_frame);
 }
 
 /* plugin registration */