From b2840e32b376f00fd5513ba228d79c578d478f17 Mon Sep 17 00:00:00 2001 From: 321aurora <2508260166@qq.com> Date: Sun, 27 Apr 2025 18:18:19 +0800 Subject: [PATCH] Fix the problem of pitch feedforward. --- rm_gimbal_controllers/src/gimbal_base.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 2c10a4df..e98d628b 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -112,6 +112,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ctrls_.insert(std::make_pair(axis, std::make_unique())); pid_pos_.insert(std::make_pair(axis, std::make_unique())); + pos_des_in_limit_.insert(std::make_pair(axis, true)); pos_state_pub_.insert(std::make_pair( axis, std::make_unique>(nh, "pos_state", 1))); @@ -207,7 +208,7 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des) odom2gimbal_des.setRPY(0, pitch_des, yaw_des); tf2::Quaternion base2gimbal_des = odom2base.inverse() * odom2gimbal_des; for (const auto& it : joint_urdfs_) - pos_des_in_limit_.insert(std::make_pair(it.first, setDesIntoLimit(base2gimbal_des, it.second, base2gimbal_des))); + pos_des_in_limit_[it.first] = setDesIntoLimit(base2gimbal_des, it.second, base2gimbal_des); odom2gimbal_des_.transform.rotation = tf2::toMsg(odom2base * base2gimbal_des); odom2gimbal_des_.header.stamp = time; robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers");