From c4c5e5dd622d6ecb7041fb34372f02a46e757b3e Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Sat, 21 Sep 2024 16:45:08 +0900 Subject: [PATCH] [Ninja][Navigation] make com rotation motion smoothly. --- robots/ninja/config/NavigationConfig.yaml | 4 +- robots/ninja/include/ninja/ninja_navigation.h | 4 +- robots/ninja/src/ninja_navigation.cpp | 96 +++++++++++-------- 3 files changed, 62 insertions(+), 42 deletions(-) diff --git a/robots/ninja/config/NavigationConfig.yaml b/robots/ninja/config/NavigationConfig.yaml index e82b3672e..f0c17832b 100644 --- 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 + com_roll_change_thresh: 0.005 + com_pitch_change_thresh: 0.005 + com_yaw_change_thresh: 0.02 #morphing morphing_vel: 0.04 diff --git a/robots/ninja/include/ninja/ninja_navigation.h b/robots/ninja/include/ninja/ninja_navigation.h index 01230b126..f64878dac 100644 --- a/robots/ninja/include/ninja/ninja_navigation.h +++ b/robots/ninja/include/ninja/ninja_navigation.h @@ -98,7 +98,9 @@ namespace aerial_robot_navigation double prev_morphing_stamp_; bool morphing_flag_; - double com_rot_change_thresh_; + double com_roll_change_thresh_; + double com_pitch_change_thresh_; + double com_yaw_change_thresh_; }; template<> inline KDL::Frame NinjaNavigator::getCom2Base() diff --git a/robots/ninja/src/ninja_navigation.cpp b/robots/ninja/src/ninja_navigation.cpp index b20c87f28..acaf09b54 100644 --- a/robots/ninja/src/ninja_navigation.cpp +++ b/robots/ninja/src/ninja_navigation.cpp @@ -50,10 +50,13 @@ void NinjaNavigator::update() KDL::Frame current_com; KDL::Frame target_cog_pose; KDL::Frame target_com_pose; + KDL::Frame raw_base2cog; // base2cog conversion without desire coord process geometry_msgs::TransformStamped transformStamped; target_com_pose.M = target_com_rot_; tf::vectorTFToKDL(getTargetPosCand(),target_com_pose.p); - target_cog_pose = target_com_pose * getCom2Base() * ninja_robot_model_->getCog2Baselink().Inverse(); + raw_base2cog.p = ninja_robot_model_->getCog2Baselink().Inverse().p; + // target_cog_pose = target_com_pose * getCom2Base() * ninja_robot_model_->getCog2Baselink().Inverse(); + target_cog_pose = target_com_pose * getCom2Base() * raw_base2cog; geometry_msgs::TransformStamped tf; @@ -388,30 +391,7 @@ void NinjaNavigator::convertTargetPosFromCoG2CoM() return; }else if(!current_assembled){ return; - }else if(my_id_ == leader_id_){ - KDL::Frame target_my_pose; - if(getNaviState() == TAKEOFF_STATE || getNaviState() == LAND_STATE) - { - setTargetPosX(target_pos_candidate_.x()); - setTargetPosY(target_pos_candidate_.y()); - setTargetPosZ(getTargetPos().z()); - } - else - { - setTargetPos(target_pos_candidate_); - } - tf::Vector3 target_rot; - double target_roll, target_pitch, target_yaw; - target_com_rot_.GetEulerZYX(target_yaw, target_pitch, target_roll); - ROS_ERROR_STREAM("id : " << my_id_ <<" yaw: "<< target_yaw << " pitch: "<() * ninja_robot_model_->getCog2Baselink().Inverse(); // com -> cog + raw_base2cog.p = ninja_robot_model_->getCog2Baselink().Inverse().p; + // target_my_pose = target_com_pose * getCom2Base() * ninja_robot_model_->getCog2Baselink().Inverse(); // com -> cog + target_my_pose = target_com_pose * getCom2Base() * raw_base2cog; // com -> cog tf::Vector3 target_pos, target_rot; double target_roll, target_pitch, target_yaw; @@ -434,6 +417,7 @@ void NinjaNavigator::convertTargetPosFromCoG2CoM() target_rot.setX(target_roll); target_rot.setY(target_pitch); target_rot.setZ(target_yaw); + ROS_ERROR_STREAM("id : " << my_id_ <<" yaw: "<< target_yaw << " pitch: "< com_rot_change_thresh_) - target_com_rot += ((goal_com_rot - target_com_rot).normalize() * com_rot_change_thresh_); - else - target_com_rot = goal_com_rot; - - setTargetComRot(KDL::Rotation::RPY(target_com_rot.x(), target_com_rot.y(), target_com_rot.z())); + //roll + if(target_roll != goal_roll){ + if(abs(goal_roll- target_roll) > com_roll_change_thresh_) + target_roll += ((goal_roll - target_roll) * com_roll_change_thresh_); + else + target_roll = goal_roll; + } + //pitch + if(target_pitch != goal_pitch) + { + if(abs(goal_pitch- target_pitch) > com_pitch_change_thresh_) + target_pitch += ((goal_pitch - target_pitch) * com_pitch_change_thresh_); + else + target_pitch = goal_pitch; + } + //yaw + if(target_yaw != goal_yaw) + { + if(abs(goal_yaw- target_yaw) > com_yaw_change_thresh_) + target_yaw += ((goal_yaw - target_yaw) * com_yaw_change_thresh_); + else + target_yaw = goal_yaw; + } + setTargetComRot(KDL::Rotation::RPY(target_roll, target_pitch, target_yaw)); } void NinjaNavigator::setTargetCoMPoseFromCurrState() @@ -481,9 +480,23 @@ void NinjaNavigator::setTargetCoMPoseFromCurrState() tf::transformMsgToKDL(transformStamped.transform, current_com); tf::Vector3 target_com_pos; tf::vectorKDLToTF(current_com.p, target_com_pos); - if(getNaviState() == TAKEOFF_STATE || getNaviState() == LAND_STATE) target_com_pos.setZ(getTargetPos().z()); - setTargetPosCand(target_com_pos); - setTargetComRot(current_com.M); + + double current_roll, current_pitch, current_yaw; + current_com.M.GetEulerZYX(current_yaw, current_pitch, current_roll); + + if(getNaviState() == TAKEOFF_STATE || getNaviState() == LAND_STATE) + { + target_com_pos.setZ(getTargetPos().z()); + setTargetPosCand(target_com_pos); + setTargetComRot(KDL::Rotation::RPY(0,0,current_yaw)); + setGoalComRot(KDL::Rotation::RPY(0,0,current_yaw)); + } + else + { + setTargetPosCand(target_com_pos); + setTargetComRot(current_com.M); + setGoalComRot(current_com.M); + } } catch (tf2::TransformException& ex) { @@ -593,7 +606,7 @@ void NinjaNavigator::assemblyJointPosCallback(const sensor_msgs::JointStateConst void NinjaNavigator::morphingProcess() { - if(!getCurrentAssembled()) return; + if(!getCurrentAssembled() || !control_flag_) return; sensor_msgs::JointState joints_ctrl_msg; bool joint_send_flag = false; std::map joint_map; @@ -718,7 +731,10 @@ void NinjaNavigator::rosParamInit() getParam(nh, "joint_pos_change_thresh", joint_pos_chnage_thresh_, 0.01); getParam(nh, "joint_process_func", joint_process_func_, CONSTANT); getParam(nh, "free_joint_flag", free_joint_flag_, false); - getParam(nh, "com_rot_change_thresh", com_rot_change_thresh_, 0.02); + getParam(nh, "com_roll_change_thresh", com_roll_change_thresh_, 0.02); + getParam(nh, "com_yaw_pitch_thresh", com_pitch_change_thresh_, 0.02); + getParam(nh, "com_yaw_change_thresh", com_yaw_change_thresh_, 0.02); + getParam(nh, "morphing_process_interval", morphing_process_interval_, 0.1); BeetleNavigator::rosParamInit(); }