From fb61f5920ee1603b74a155d75c7f1cc80cd8ec91 Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Fri, 20 Sep 2024 17:26:30 +0900 Subject: [PATCH] [Ninja][Navigation][WIP] workaound to make morphing motion smooth. --- .../gimbalrotor/gimbalrotor_navigation.h | 1 + .../src/gimbalrotor_navigation.cpp | 9 + robots/ninja/config/NavigationConfig.yaml | 4 +- robots/ninja/include/ninja/ninja_navigation.h | 19 +- robots/ninja/src/ninja_navigation.cpp | 175 ++++++++++++------ 5 files changed, 146 insertions(+), 62 deletions(-) diff --git a/robots/gimbalrotor/include/gimbalrotor/gimbalrotor_navigation.h b/robots/gimbalrotor/include/gimbalrotor/gimbalrotor_navigation.h index 8a7a3b9d3..105ace8d3 100644 --- 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 index 9647561aa..7a217ec15 100644 --- 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 index 4733045d4..e82b3672e 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 #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 index f3bb8d9b2..01230b126 100644 --- a/robots/ninja/include/ninja/ninja_navigation.h +++ b/robots/ninja/include/ninja/ninja_navigation.h @@ -26,10 +26,11 @@ 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 first_joint_processed_time_; @@ -37,7 +38,6 @@ namespace aerial_robot_navigation 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 ninja_robot_model_; @@ -80,6 +82,7 @@ namespace aerial_robot_navigation std::map assembled_modules_data_; std::vector 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() diff --git a/robots/ninja/src/ninja_navigation.cpp b/robots/ninja/src/ninja_navigation.cpp index 4ff2a3147..b20c87f28 100644 --- a/robots/ninja/src/ninja_navigation.cpp +++ b/robots/ninja/src/ninja_navigation.cpp @@ -7,7 +7,8 @@ using namespace aerial_robot_navigation; NinjaNavigator::NinjaNavigator(): BeetleNavigator(), - module_joint_num_(2) + module_joint_num_(2), + morphing_flag_(false) { } @@ -20,15 +21,27 @@ void NinjaNavigator::initialize(ros::NodeHandle nh, ros::NodeHandle nhp, ninja_robot_model_ = boost::dynamic_pointer_cast(robot_model); target_com_pose_pub_ = nh_.advertise("target_com_pose", 1); //for debug joint_control_pub_ = nh_.advertise("joints_ctrl", 1); - target_com_rot_sub_ = nh_.subscribe("/target_com_rot", 1, &NinjaNavigator::setTargetCoMRotCallback, this); + target_com_rot_sub_ = nh_.subscribe("/target_com_rot", 1, &NinjaNavigator::setGoalCoMRotCallback, this); target_joints_pos_sub_ = nh_.subscribe("/assembly/target_joint_pos", 1, &NinjaNavigator::assemblyJointPosCallback, this); joint_pos_errs_.resize(module_joint_num_); + prev_morphing_stamp_ = ros::Time::now().toSec(); } void NinjaNavigator::update() { updateEntSysState(); - morphingProcess(); + if(ros::Time::now().toSec() - prev_morphing_stamp_ > morphing_process_interval_) + { + comRotationProcess(); + morphingProcess(); + morphing_flag_ = true; + prev_morphing_stamp_ = ros::Time::now().toSec(); + } + else + { + morphing_flag_ = false; + } + BeetleNavigator::update(); bool current_assembled = getCurrentAssembled(); if(current_assembled){ @@ -88,7 +101,8 @@ void NinjaNavigator::updateEntSysState() assembled_modules_ids_.push_back(id); ModuleData module_data(id); ninja_robot_model_->copyTreeStructure(ninja_robot_model_->getInitModuleTree(), module_data.module_tree_); - module_data.joint_pos_ = KDL::JntArray(module_joint_num_); //docking yaw and pitch + module_data.des_joint_pos_ = KDL::JntArray(module_joint_num_); //docking yaw and pitch + module_data.est_joint_pos_ = KDL::JntArray(module_joint_num_); module_data.goal_joint_pos_ = KDL::JntArray(module_joint_num_); module_data.start_joint_pos_ = KDL::JntArray(module_joint_num_); module_data.first_joint_processed_time_ = std::vector(2,-1); @@ -212,7 +226,7 @@ void NinjaNavigator::calcCenterOfMoving() KDL::ChainFkSolverPos_recursive fk_solver(chain); KDL::Frame frame; KDL::JntArray joint_positions(1); - joint_positions(0) = data.joint_pos_(YAW); + joint_positions(0) = data.des_joint_pos_(YAW); if (fk_solver.JntToCart(joint_positions, frame) < 0) { ROS_ERROR_STREAM("Failed to compute FK for module" << it.first); @@ -230,8 +244,8 @@ void NinjaNavigator::calcCenterOfMoving() KDL::ChainFkSolverPos_recursive fk_solver(chain); KDL::Frame frame; KDL::JntArray joint_positions(2); - joint_positions(0) = data.joint_pos_(PITCH); - joint_positions(1) = data.joint_pos_(YAW); + joint_positions(0) = data.des_joint_pos_(PITCH); + joint_positions(1) = data.des_joint_pos_(YAW); if (fk_solver.JntToCart(joint_positions, frame) < 0) { ROS_ERROR_STREAM("Failed to compute FK for module" << it.first); return; @@ -248,7 +262,7 @@ void NinjaNavigator::calcCenterOfMoving() KDL::ChainFkSolverPos_recursive fk_solver(chain); KDL::Frame frame; KDL::JntArray joint_positions(1); - joint_positions(0) = data.joint_pos_(PITCH); + joint_positions(0) = data.des_joint_pos_(PITCH); if (fk_solver.JntToCart(joint_positions, frame) < 0) { ROS_ERROR_STREAM("Failed to compute FK for module" << it.first); return; @@ -282,7 +296,7 @@ void NinjaNavigator::calcCenterOfMoving() KDL::ChainFkSolverPos_recursive fk_solver(chain); KDL::Frame frame; KDL::JntArray joint_positions(1); - joint_positions(0) = data.joint_pos_(YAW); + joint_positions(0) = data.des_joint_pos_(YAW); if (fk_solver.JntToCart(joint_positions, frame) < 0) { ROS_ERROR_STREAM("Failed to compute FK for module" << it.first); return; @@ -299,8 +313,8 @@ void NinjaNavigator::calcCenterOfMoving() KDL::ChainFkSolverPos_recursive fk_solver(chain); KDL::Frame frame; KDL::JntArray joint_positions(2); - joint_positions(0) = data.joint_pos_(PITCH); - joint_positions(1) = data.joint_pos_(YAW); + joint_positions(0) = data.des_joint_pos_(PITCH); + joint_positions(1) = data.des_joint_pos_(YAW); if (fk_solver.JntToCart(joint_positions, frame) < 0) { ROS_ERROR_STREAM("Failed to compute FK for module" << it.first); return; @@ -317,7 +331,7 @@ void NinjaNavigator::calcCenterOfMoving() KDL::ChainFkSolverPos_recursive fk_solver(chain); KDL::Frame frame; KDL::JntArray joint_positions(1); - joint_positions(0) = data.joint_pos_(PITCH); + joint_positions(0) = data.des_joint_pos_(PITCH); if (fk_solver.JntToCart(joint_positions, frame) < 0) { ROS_ERROR_STREAM("Failed to compute FK for module" << it.first); return; @@ -356,7 +370,7 @@ void NinjaNavigator::calcCenterOfMoving() void NinjaNavigator::convertTargetPosFromCoG2CoM() { - if(!control_flag_) return; + if(!control_flag_ || !morphing_flag_) return; bool current_assembled = getCurrentAssembled(); bool reconfig_flag = getReconfigFlag(); @@ -389,10 +403,11 @@ void NinjaNavigator::convertTargetPosFromCoG2CoM() 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: "< 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())); +} + void NinjaNavigator::setTargetCoMPoseFromCurrState() { bool current_assembled = getCurrentAssembled(); @@ -461,9 +494,9 @@ void NinjaNavigator::setTargetCoMPoseFromCurrState() } -void NinjaNavigator::setTargetCoMRotCallback(const spinal::DesireCoordConstPtr & msg) +void NinjaNavigator::setGoalCoMRotCallback(const spinal::DesireCoordConstPtr & msg) { - setTargetComRot(KDL::Rotation::RPY(msg->roll, msg->pitch, msg->yaw)); + setGoalComRot(KDL::Rotation::RPY(msg->roll, msg->pitch, msg->yaw)); } void NinjaNavigator::assemblyJointPosCallback(const sensor_msgs::JointStateConstPtr& msg) @@ -527,9 +560,9 @@ void NinjaNavigator::assemblyJointPosCallback(const sensor_msgs::JointStateConst if(target_joint_name == "yaw_dock_joint") axis = YAW; if(target_joint_name == "pitch_dock_joint") axis = PITCH; data.goal_joint_pos_(axis) = target_joint_angle; - data.start_joint_pos_(axis) = data.joint_pos_(axis); + data.start_joint_pos_(axis) = data.des_joint_pos_(axis); data.first_joint_processed_time_[axis] = ros::Time::now().toSec(); - double conv_time = abs(data.goal_joint_pos_(axis) - data.joint_pos_(axis)) / morphing_vel_; + double conv_time = abs(data.goal_joint_pos_(axis) - data.des_joint_pos_(axis)) / morphing_vel_; switch(joint_process_func_) { case CONSTANT: @@ -540,7 +573,7 @@ void NinjaNavigator::assemblyJointPosCallback(const sensor_msgs::JointStateConst joints_ctrl_msg.position.push_back(target_joint_angle); } data.first_joint_processed_time_[axis] = -1; - data.joint_pos_(axis) = target_joint_angle; + data.des_joint_pos_(axis) = target_joint_angle; break; case FRAC: data.joint_process_coef_[axis] = 99.0/conv_time; @@ -574,26 +607,26 @@ void NinjaNavigator::morphingProcess() { double t = ros::Time::now().toSec() - data.first_joint_processed_time_[i]; if(data.first_joint_processed_time_[i] < 0) continue; - if(abs(data.goal_joint_pos_(i) - data.joint_pos_(i)) < joint_pos_chnage_thresh_) + if(abs(data.goal_joint_pos_(i) - data.des_joint_pos_(i)) < joint_pos_chnage_thresh_) { data.first_joint_processed_time_[i] = -1.0; - data.joint_pos_(i) = data.goal_joint_pos_(i); + data.des_joint_pos_(i) = data.goal_joint_pos_(i); if(id == my_id_ && !free_joint_flag_) { joint_send_flag = true; joints_ctrl_msg.name.push_back(joint_map[i]); - joints_ctrl_msg.position.push_back(data.joint_pos_(i)); + joints_ctrl_msg.position.push_back(data.des_joint_pos_(i)); ROS_INFO_STREAM("Morphing of module"< leader_id_) - neighbor_id_ = assembled_modules_ids_[my_index_-1]; - else - return; - try + for(auto it = assembled_modules_data_.begin(); it != assembled_modules_data_.end(); ++it) { - KDL::Frame myCog2NeighborCog; - geometry_msgs::TransformStamped transformStamped; - transformStamped = tfBuffer_.lookupTransform(my_name_ + std::to_string(my_id_) + std::string("/fc") , - my_name_ + std::to_string(neighbor_id_) + std::string("/fc"), - ros::Time(0)); - tf::transformMsgToKDL(transformStamped.transform, myCog2NeighborCog); - double joint_pos_roll, joint_pos_pitch, joint_pos_yaw; - // myCog2NeighborCog.M.GetRPY(joint_pos_roll, joint_pos_pitch, joint_pos_yaw); - if(my_id_ < leader_id_) + int id = it->first; + ModuleData& data = it->second; + int left_id, right_id; + if(it != assembled_modules_data_.begin()) { - joint_pos_yaw = std::atan2(-myCog2NeighborCog.M(0,1),myCog2NeighborCog.M(0,0)); - joint_pos_pitch = std::asin(myCog2NeighborCog.M(0,2)); - joint_pos_errs_[0] = assembled_modules_data_[neighbor_id_].goal_joint_pos_(0) - joint_pos_pitch; - joint_pos_errs_[1] = assembled_modules_data_[my_id_].goal_joint_pos_(1) - joint_pos_yaw; + auto left = std::prev(it); + left_id = left->first; + try + { + KDL::Frame myCog2LeftCog; + geometry_msgs::TransformStamped transformStamped; + transformStamped = tfBuffer_.lookupTransform(my_name_ + std::to_string(id) + std::string("/fc") , + my_name_ + std::to_string(left_id) + std::string("/fc"), + ros::Time(0)); + tf::transformMsgToKDL(transformStamped.transform, myCog2LeftCog); + data.est_joint_pos_(PITCH) = std::asin(myCog2LeftCog.M(0,2)); + } + catch (tf2::TransformException& ex) + { + ROS_ERROR("cannot estimate joint position"); + return; + } } - else if(my_id_ > leader_id_) + if(it != assembled_modules_data_.end()) { - joint_pos_yaw = std::atan2(-myCog2NeighborCog.M(0,1),myCog2NeighborCog.M(0,0)); - joint_pos_pitch = std::asin(myCog2NeighborCog.M(0,2)); - joint_pos_errs_[0] = assembled_modules_data_[my_id_].goal_joint_pos_(0) - (-joint_pos_pitch); - joint_pos_errs_[1] = assembled_modules_data_[neighbor_id_].goal_joint_pos_(1) - (-joint_pos_yaw); + auto right = std::next(it); + right_id = right->first; + try + { + KDL::Frame myCog2RightCog; + geometry_msgs::TransformStamped transformStamped; + transformStamped = tfBuffer_.lookupTransform(my_name_ + std::to_string(id) + std::string("/fc") , + my_name_ + std::to_string(right_id) + std::string("/fc"), + ros::Time(0)); + tf::transformMsgToKDL(transformStamped.transform, myCog2RightCog); + data.est_joint_pos_(YAW) = std::atan2(-myCog2RightCog.M(0,1),myCog2RightCog.M(0,0)); + } + catch (tf2::TransformException& ex) + { + ROS_ERROR("cannot estimate joint position"); + return; + } } } - catch (tf2::TransformException& ex) + + int neighbor_id_; + if(my_id_ < leader_id_) + { + neighbor_id_ = assembled_modules_ids_[my_index_+1]; + joint_pos_errs_[PITCH] = assembled_modules_data_[neighbor_id_].goal_joint_pos_(PITCH) - assembled_modules_data_[neighbor_id_].est_joint_pos_(PITCH); + joint_pos_errs_[YAW] = assembled_modules_data_[my_id_].goal_joint_pos_(YAW) - assembled_modules_data_[my_id_].est_joint_pos_(YAW); + } + else if(my_id_ > leader_id_) + { + neighbor_id_ = assembled_modules_ids_[my_index_-1]; + joint_pos_errs_[PITCH] = assembled_modules_data_[my_id_].goal_joint_pos_(PITCH) - assembled_modules_data_[my_id_].est_joint_pos_(PITCH); + joint_pos_errs_[YAW] = assembled_modules_data_[neighbor_id_].goal_joint_pos_(YAW) - assembled_modules_data_[neighbor_id_].est_joint_pos_(YAW); + // ROS_ERROR_STREAM("pitch: " << joint_pos_errs_[PITCH]); + // ROS_ERROR_STREAM("yaw: " << joint_pos_errs_[YAW]); + } + else { - ROS_ERROR_STREAM("not exist neighbor mentioned. ID is "<(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, "morphing_process_interval", morphing_process_interval_, 0.1); BeetleNavigator::rosParamInit(); }