From 5f56ba77e734c74c94ffa3d02d4ce28cac0e8d75 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Fri, 21 Feb 2025 15:31:05 +0800 Subject: [PATCH 01/13] Simplify gimbal controller code. --- .../rm_gimbal_controllers/gimbal_base.h | 4 +- rm_gimbal_controllers/src/gimbal_base.cpp | 87 +++++++------------ 2 files changed, 32 insertions(+), 59 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index c5a07105..36c92286 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -143,8 +143,8 @@ class Controller : public controller_interface::MultiInterfaceControllerlimits ? pitch_joint_urdf_->limits->upper : 1e16; - lower_limit = pitch_joint_urdf_->limits ? pitch_joint_urdf_->limits->lower : -1e16; - base2new_des.setRPY(0, - std::abs(angles::shortest_angular_distance(base2gimbal_current_des_pitch, upper_limit)) < - std::abs(angles::shortest_angular_distance(base2gimbal_current_des_pitch, lower_limit)) ? - upper_limit : - lower_limit, - base2gimbal_current_des_yaw); - quatToRPY(toMsg(odom2base * base2new_des), roll_temp, pitch_real_des, yaw_temp); - } - - yaw_des_in_limit_ = setDesIntoLimit(yaw_real_des, yaw_des, base2gimbal_current_des_yaw, yaw_joint_urdf_); - if (!yaw_des_in_limit_) - { - double pitch_temp; - tf2::Quaternion base2new_des; - double upper_limit, lower_limit; - upper_limit = yaw_joint_urdf_->limits ? yaw_joint_urdf_->limits->upper : 1e16; - lower_limit = yaw_joint_urdf_->limits ? yaw_joint_urdf_->limits->lower : -1e16; - base2new_des.setRPY(0, base2gimbal_current_des_pitch, - std::abs(angles::shortest_angular_distance(base2gimbal_current_des_yaw, upper_limit)) < - std::abs(angles::shortest_angular_distance(base2gimbal_current_des_yaw, lower_limit)) ? - upper_limit : - lower_limit); - quatToRPY(toMsg(odom2base * base2new_des), roll_temp, pitch_temp, yaw_real_des); - } - + pitch_des_in_limit_ = setDesIntoLimit(pitch_real_des, pitch_des, base2gimbal_current_des_pitch, + base2gimbal_current_des_yaw, pitch_joint_urdf_, base2new_des); + yaw_des_in_limit_ = setDesIntoLimit(yaw_real_des, yaw_des, base2gimbal_current_des_yaw, base2gimbal_current_des_pitch, + yaw_joint_urdf_, base2new_des); + if (!pitch_des_in_limit_ || !yaw_des_in_limit_) + quatToRPY(toMsg(odom2base * base2new_des), roll_temp, pitch_real_des, yaw_real_des); odom2gimbal_des_.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(0., pitch_real_des, yaw_real_des); odom2gimbal_des_.header.stamp = time; robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers"); @@ -378,19 +349,27 @@ void Controller::traj(const ros::Time& time) setDes(time, cmd_gimbal_.traj_yaw, cmd_gimbal_.traj_pitch); } -bool Controller::setDesIntoLimit(double& real_des, double current_des, double base2gimbal_current_des, - const urdf::JointConstSharedPtr& joint_urdf) +bool Controller::setDesIntoLimit(double& real_des, double current_des, double base2gimbal_current_des, double temp, + const urdf::JointConstSharedPtr& joint_urdf, tf2::Quaternion& base2new_des) { - double upper_limit, lower_limit; - upper_limit = joint_urdf->limits ? joint_urdf->limits->upper : 1e16; - lower_limit = joint_urdf->limits ? joint_urdf->limits->lower : -1e16; + double upper_limit = joint_urdf->limits ? joint_urdf->limits->upper : 1e16; + double lower_limit = joint_urdf->limits ? joint_urdf->limits->lower : -1e16; if ((base2gimbal_current_des <= upper_limit && base2gimbal_current_des >= lower_limit) || (angles::two_pi_complement(base2gimbal_current_des) <= upper_limit && angles::two_pi_complement(base2gimbal_current_des) >= lower_limit)) + { real_des = current_des; + return true; + } + const double new_des = std::abs(angles::shortest_angular_distance(base2gimbal_current_des, upper_limit)) < + std::abs(angles::shortest_angular_distance(base2gimbal_current_des, lower_limit)) ? + upper_limit : + lower_limit; + if (joint_urdf->name.find("pitch") != std::string::npos) + base2new_des.setRPY(0, new_des, temp); else - return false; - return true; + base2new_des.setRPY(0, temp, new_des); + return false; } void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) @@ -426,9 +405,6 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) quatToRPY(odom2pitch_.transform.rotation, roll_real, pitch_real, yaw_real); double yaw_angle_error = angles::shortest_angular_distance(yaw_real, yaw_des); double pitch_angle_error = angles::shortest_angular_distance(pitch_real, pitch_des); - pid_pitch_pos_.computeCommand(pitch_angle_error, period); - pid_yaw_pos_.computeCommand(yaw_angle_error, period); - double yaw_vel_des = 0., pitch_vel_des = 0.; if (state_ == RATE) { @@ -470,9 +446,15 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) pitch_vel_des = 0.; if (!yaw_des_in_limit_) yaw_vel_des = 0.; - pid_pitch_pos_.computeCommand(pitch_angle_error, period); pid_yaw_pos_.computeCommand(yaw_angle_error, period); + ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() - config_.k_chassis_vel_ * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); + ctrl_pitch_.setCommand(pid_pitch_pos_.getCurrentCmd() + config_.pitch_k_v_ * pitch_vel_des + + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); + ctrl_yaw_.update(time, period); + ctrl_pitch_.update(time, period); + ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); // publish state if (loop_count_ % 10 == 0) @@ -499,15 +481,6 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } } loop_count_++; - - ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() - config_.k_chassis_vel_ * chassis_vel_->angular_->z() + - config_.yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); - ctrl_pitch_.setCommand(pid_pitch_pos_.getCurrentCmd() + config_.pitch_k_v_ * pitch_vel_des + - ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); - - ctrl_yaw_.update(time, period); - ctrl_pitch_.update(time, period); - ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); } double Controller::feedForward(const ros::Time& time) From af9fc719f4fdf2edc3b380ca77115b6960864c55 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sun, 23 Feb 2025 17:24:01 +0800 Subject: [PATCH 02/13] Optimize code. --- .../rm_gimbal_controllers/gimbal_base.h | 19 +- rm_gimbal_controllers/src/gimbal_base.cpp | 319 ++++++++++-------- 2 files changed, 195 insertions(+), 143 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index 36c92286..52a33442 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -55,6 +55,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -143,43 +144,45 @@ class Controller : public controller_interface::MultiInterfaceController joint_urdfs); + std::string getBaseFrameID(std::unordered_map joint_urdfs); void commandCB(const rm_msgs::GimbalCmdConstPtr& msg); void trackCB(const rm_msgs::TrackDataConstPtr& msg); void reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t); rm_control::RobotStateHandle robot_state_handle_; hardware_interface::ImuSensorHandle imu_sensor_handle_; + std::unordered_map> ctrl_; + std::unordered_map> pid_pos_; + std::unordered_map joint_urdfs_; + std::unordered_map pos_des_in_limit_; bool has_imu_ = true; - effort_controllers::JointVelocityController ctrl_yaw_, ctrl_pitch_; - control_toolbox::Pid pid_yaw_pos_, pid_pitch_pos_; std::shared_ptr bullet_solver_; // ROS Interface ros::Time last_publish_time_{}; - std::unique_ptr> yaw_pos_state_pub_, pitch_pos_state_pub_; + std::unordered_map>> pos_state_pub_; std::shared_ptr> error_pub_; ros::Subscriber cmd_gimbal_sub_; ros::Subscriber data_track_sub_; realtime_tools::RealtimeBuffer cmd_rt_buffer_; realtime_tools::RealtimeBuffer track_rt_buffer_; - urdf::JointConstSharedPtr pitch_joint_urdf_, yaw_joint_urdf_; rm_msgs::GimbalCmd cmd_gimbal_; rm_msgs::TrackData data_track_; std::string gimbal_des_frame_id_{}, imu_name_{}; double publish_rate_{}; bool state_changed_{}; - bool pitch_des_in_limit_{}, yaw_des_in_limit_{}; int loop_count_{}; // Transform - geometry_msgs::TransformStamped odom2gimbal_des_, odom2pitch_, odom2base_, last_odom2base_; + geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_; // Gravity Compensation geometry_msgs::Vector3 mass_origin_; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 824ec87a..b4e28de4 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -85,9 +85,42 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro hardware_interface::EffortJointInterface* effort_joint_interface = robot_hw->get(); - if (!ctrl_yaw_.init(effort_joint_interface, nh_yaw) || !ctrl_pitch_.init(effort_joint_interface, nh_pitch) || - !pid_yaw_pos_.init(nh_pid_yaw_pos) || !pid_pitch_pos_.init(nh_pid_pitch_pos)) - return false; + if (controller_nh.getParam("controllers", xml_rpc_value)) + { + for (const auto& it : xml_rpc_value) + { + ros::NodeHandle nh = ros::NodeHandle(controller_nh, "controllers/" + it.first); + ros::NodeHandle nh_pid_pos = ros::NodeHandle(controller_nh, "controllers/" + it.first + "/pid_pos"); + + // Get URDF info about joint + urdf::Model urdf; + int axis; + if (!urdf.initParamWithNodeHandle("robot_description", controller_nh)) + { + ROS_ERROR("Failed to parse urdf file"); + return false; + } + auto joint_urdf = urdf.getJoint(getParam(nh, "joint_name", std::string())); + if (!joint_urdf) + { + ROS_ERROR("Could not find joint in urdf"); + return false; + } + else + { + axis = (joint_urdf->axis.x == 1) * 0 + (joint_urdf->axis.y == 1) * 1 + (joint_urdf->axis.z == 1) * 2; + joint_urdfs_.insert(std::make_pair(axis, joint_urdf)); + } + + ctrl_.insert(std::make_pair(axis, std::make_unique())); + pid_pos_.insert(std::make_pair(axis, std::make_unique())); + pos_state_pub_.insert(std::make_pair( + axis, std::make_unique>(nh, "pos_state", 1))); + + if (!ctrl_.at(axis)->init(effort_joint_interface, nh) || !pid_pos_.at(axis)->init(nh_pid_pos)) + return false; + } + } robot_state_handle_ = robot_hw->get()->getHandle("robot_state"); if (!controller_nh.hasParam("imu_name")) @@ -104,43 +137,21 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ROS_INFO("Param imu_name has not set, use motors' data instead of imu."); } - // Get URDF info about joint - urdf::Model urdf; - if (!urdf.initParamWithNodeHandle("robot_description", controller_nh)) - { - ROS_ERROR("Failed to parse urdf file"); - return false; - } - pitch_joint_urdf_ = urdf.getJoint(ctrl_pitch_.getJointName()); - yaw_joint_urdf_ = urdf.getJoint(ctrl_yaw_.getJointName()); - if (!pitch_joint_urdf_) - { - ROS_ERROR("Could not find joint pitch in urdf"); - return false; - } - if (!yaw_joint_urdf_) - { - ROS_ERROR("Could not find joint yaw in urdf"); - return false; - } - - gimbal_des_frame_id_ = pitch_joint_urdf_->child_link_name + "_des"; + gimbal_des_frame_id_ = getGimbalFrameID(joint_urdfs_) + "_des"; odom2gimbal_des_.header.frame_id = "odom"; odom2gimbal_des_.child_frame_id = gimbal_des_frame_id_; odom2gimbal_des_.transform.rotation.w = 1.; - odom2pitch_.header.frame_id = "odom"; - odom2pitch_.child_frame_id = pitch_joint_urdf_->child_link_name; - odom2pitch_.transform.rotation.w = 1.; + odom2gimbal_.header.frame_id = "odom"; + odom2gimbal_.child_frame_id = getGimbalFrameID(joint_urdfs_); + odom2gimbal_.transform.rotation.w = 1.; odom2base_.header.frame_id = "odom"; - odom2base_.child_frame_id = yaw_joint_urdf_->parent_link_name; + odom2base_.child_frame_id = getBaseFrameID(joint_urdfs_); odom2base_.transform.rotation.w = 1.; cmd_gimbal_sub_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); - yaw_pos_state_pub_.reset(new realtime_tools::RealtimePublisher(nh_yaw, "pos_state", 1)); - pitch_pos_state_pub_.reset(new realtime_tools::RealtimePublisher(nh_pitch, "pos_state", 1)); ramp_rate_pitch_ = new RampFilter(0, 0.001); ramp_rate_yaw_ = new RampFilter(0, 0.001); @@ -168,8 +179,8 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) cmd_gimbal_.rate_yaw = ramp_rate_yaw_->output(); try { - odom2pitch_ = robot_state_handle_.lookupTransform("odom", pitch_joint_urdf_->child_link_name, time); - odom2base_ = robot_state_handle_.lookupTransform("odom", yaw_joint_urdf_->parent_link_name, time); + odom2gimbal_ = robot_state_handle_.lookupTransform("odom", odom2gimbal_.child_frame_id, time); + odom2base_ = robot_state_handle_.lookupTransform("odom", odom2base_.child_frame_id, time); } catch (tf2::TransformException& ex) { @@ -206,16 +217,9 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des) tf2::fromMsg(odom2base_.transform.rotation, odom2base); odom2gimbal_des.setRPY(0, pitch_des, yaw_des); tf2::Quaternion base2gimbal_des = odom2base.inverse() * odom2gimbal_des; - double roll_temp, base2gimbal_current_des_pitch, base2gimbal_current_des_yaw; - quatToRPY(toMsg(base2gimbal_des), roll_temp, base2gimbal_current_des_pitch, base2gimbal_current_des_yaw); - double pitch_real_des, yaw_real_des; - pitch_des_in_limit_ = setDesIntoLimit(pitch_real_des, pitch_des, base2gimbal_current_des_pitch, - base2gimbal_current_des_yaw, pitch_joint_urdf_, base2new_des); - yaw_des_in_limit_ = setDesIntoLimit(yaw_real_des, yaw_des, base2gimbal_current_des_yaw, base2gimbal_current_des_pitch, - yaw_joint_urdf_, base2new_des); - if (!pitch_des_in_limit_ || !yaw_des_in_limit_) - quatToRPY(toMsg(odom2base * base2new_des), roll_temp, pitch_real_des, yaw_real_des); - odom2gimbal_des_.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(0., pitch_real_des, yaw_real_des); + for (const auto& it : joint_urdfs_) + pos_des_in_limit_.insert(std::make_pair(it.first, setDesIntoLimit(base2gimbal_des, it.second, base2new_des))); + odom2gimbal_des_.transform.rotation = tf2::toMsg(odom2base * base2new_des); odom2gimbal_des_.header.stamp = time; robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers"); } @@ -228,7 +232,7 @@ void Controller::rate(const ros::Time& time, const ros::Duration& period) ROS_INFO("[Gimbal] Enter RATE"); if (start_) { - odom2gimbal_des_.transform.rotation = odom2pitch_.transform.rotation; + odom2gimbal_des_.transform.rotation = odom2gimbal_.transform.rotation; odom2gimbal_des_.header.stamp = time; robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers"); start_ = false; @@ -250,7 +254,7 @@ void Controller::track(const ros::Time& time) ROS_INFO("[Gimbal] Enter TRACK"); } double roll_real, pitch_real, yaw_real; - quatToRPY(odom2pitch_.transform.rotation, roll_real, pitch_real, yaw_real); + quatToRPY(odom2gimbal_.transform.rotation, roll_real, pitch_real, yaw_real); double yaw_compute = yaw_real; double pitch_compute = -pitch_real; geometry_msgs::Point target_pos = data_track_.position; @@ -276,9 +280,9 @@ void Controller::track(const ros::Time& time) yaw -= 2 * M_PI; while (yaw < -M_PI) yaw += 2 * M_PI; - target_pos.x += target_vel.x * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.x; - target_pos.y += target_vel.y * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.y; - target_pos.z += target_vel.z * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.z; + target_pos.x += target_vel.x * (time - data_track_.header.stamp).toSec() - odom2gimbal_.transform.translation.x; + target_pos.y += target_vel.y * (time - data_track_.header.stamp).toSec() - odom2gimbal_.transform.translation.y; + target_pos.z += target_vel.z * (time - data_track_.header.stamp).toSec() - odom2gimbal_.transform.translation.z; target_vel.x -= chassis_vel_->linear_->x(); target_vel.y -= chassis_vel_->linear_->y(); target_vel.z -= chassis_vel_->linear_->z(); @@ -299,7 +303,7 @@ void Controller::track(const ros::Time& time) error_pub_->msg_.error = solve_success ? error : 1.0; error_pub_->unlockAndPublish(); } - bullet_solver_->bulletModelPub(odom2pitch_, time); + bullet_solver_->bulletModelPub(odom2gimbal_, time); last_publish_time_ = time; } @@ -331,11 +335,11 @@ void Controller::direct(const ros::Time& time) { ROS_WARN("%s", ex.what()); } - double yaw = std::atan2(aim_point_odom.y - odom2pitch_.transform.translation.y, - aim_point_odom.x - odom2pitch_.transform.translation.x); - double pitch = -std::atan2(aim_point_odom.z - odom2pitch_.transform.translation.z, - std::sqrt(std::pow(aim_point_odom.x - odom2pitch_.transform.translation.x, 2) + - std::pow(aim_point_odom.y - odom2pitch_.transform.translation.y, 2))); + double yaw = std::atan2(aim_point_odom.y - odom2gimbal_.transform.translation.y, + aim_point_odom.x - odom2gimbal_.transform.translation.x); + double pitch = -std::atan2(aim_point_odom.z - odom2gimbal_.transform.translation.z, + std::sqrt(std::pow(aim_point_odom.x - odom2gimbal_.transform.translation.x, 2) + + std::pow(aim_point_odom.y - odom2gimbal_.transform.translation.y, 2))); setDes(time, yaw, pitch); } @@ -349,32 +353,36 @@ void Controller::traj(const ros::Time& time) setDes(time, cmd_gimbal_.traj_yaw, cmd_gimbal_.traj_pitch); } -bool Controller::setDesIntoLimit(double& real_des, double current_des, double base2gimbal_current_des, double temp, - const urdf::JointConstSharedPtr& joint_urdf, tf2::Quaternion& base2new_des) +bool Controller::setDesIntoLimit(const tf2::Quaternion& base2gimbal_des, const urdf::JointConstSharedPtr& joint_urdf, + tf2::Quaternion& base2new_des) { + double base2gimbal_current_des[3]; + quatToRPY(toMsg(base2gimbal_des), base2gimbal_current_des[0], base2gimbal_current_des[1], base2gimbal_current_des[2]); double upper_limit = joint_urdf->limits ? joint_urdf->limits->upper : 1e16; double lower_limit = joint_urdf->limits ? joint_urdf->limits->lower : -1e16; - if ((base2gimbal_current_des <= upper_limit && base2gimbal_current_des >= lower_limit) || - (angles::two_pi_complement(base2gimbal_current_des) <= upper_limit && - angles::two_pi_complement(base2gimbal_current_des) >= lower_limit)) + int index = (joint_urdf->axis.x == 1) * 0 + (joint_urdf->axis.y == 1) * 1 + (joint_urdf->axis.z == 1) * 2; + if ((base2gimbal_current_des[index] <= upper_limit && base2gimbal_current_des[index] >= lower_limit) || + (angles::two_pi_complement(base2gimbal_current_des[index]) <= upper_limit && + angles::two_pi_complement(base2gimbal_current_des[index]) >= lower_limit)) { - real_des = current_des; + base2new_des = base2gimbal_des; return true; } - const double new_des = std::abs(angles::shortest_angular_distance(base2gimbal_current_des, upper_limit)) < - std::abs(angles::shortest_angular_distance(base2gimbal_current_des, lower_limit)) ? - upper_limit : - lower_limit; - if (joint_urdf->name.find("pitch") != std::string::npos) - base2new_des.setRPY(0, new_des, temp); else - base2new_des.setRPY(0, temp, new_des); - return false; + { + base2gimbal_current_des[index] = + std::abs(angles::shortest_angular_distance(base2gimbal_current_des[index], upper_limit)) < + std::abs(angles::shortest_angular_distance(base2gimbal_current_des[index], lower_limit)) ? + upper_limit : + lower_limit; + base2new_des.setRPY(base2gimbal_current_des[0], base2gimbal_current_des[1], base2gimbal_current_des[2]); + return false; + } } void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) { - geometry_msgs::Vector3 gyro, angular_vel_pitch, angular_vel_yaw; + geometry_msgs::Vector3 gyro, angular_vel; if (has_imu_) { gyro.x = imu_sensor_handle_.getAngularVelocity()[0]; @@ -382,12 +390,9 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) gyro.z = imu_sensor_handle_.getAngularVelocity()[2]; try { - tf2::doTransform(gyro, angular_vel_pitch, - robot_state_handle_.lookupTransform(pitch_joint_urdf_->child_link_name, - imu_sensor_handle_.getFrameId(), time)); - tf2::doTransform(gyro, angular_vel_yaw, - robot_state_handle_.lookupTransform(yaw_joint_urdf_->child_link_name, - imu_sensor_handle_.getFrameId(), time)); + tf2::doTransform(gyro, angular_vel, + robot_state_handle_.lookupTransform(odom2gimbal_.child_frame_id, imu_sensor_handle_.getFrameId(), + time)); } catch (tf2::TransformException& ex) { @@ -397,12 +402,16 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } else { - angular_vel_yaw.z = ctrl_yaw_.joint_.getVelocity(); - angular_vel_pitch.y = ctrl_pitch_.joint_.getVelocity(); + if (ctrl_.find(0) != ctrl_.end()) + angular_vel.x = ctrl_.at(0)->joint_.getVelocity(); + if (ctrl_.find(1) != ctrl_.end()) + angular_vel.y = ctrl_.at(1)->joint_.getVelocity(); + if (ctrl_.find(2) != ctrl_.end()) + angular_vel.z = ctrl_.at(2)->joint_.getVelocity(); } double roll_real, pitch_real, yaw_real, roll_des, pitch_des, yaw_des; quatToRPY(odom2gimbal_des_.transform.rotation, roll_des, pitch_des, yaw_des); - quatToRPY(odom2pitch_.transform.rotation, roll_real, pitch_real, yaw_real); + quatToRPY(odom2gimbal_.transform.rotation, roll_real, pitch_real, yaw_real); double yaw_angle_error = angles::shortest_angular_distance(yaw_real, yaw_des); double pitch_angle_error = angles::shortest_angular_distance(pitch_real, pitch_des); double yaw_vel_des = 0., pitch_vel_des = 0.; @@ -421,84 +430,102 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) tf2::Vector3 target_pos_tf, target_vel_tf; try { - geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( - yaw_joint_urdf_->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - tf2::fromMsg(target_pos, target_pos_tf); - tf2::fromMsg(target_vel, target_vel_tf); - - yaw_vel_des = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); - transform = robot_state_handle_.lookupTransform(pitch_joint_urdf_->parent_link_name, data_track_.header.frame_id, - data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - tf2::fromMsg(target_pos, target_pos_tf); - tf2::fromMsg(target_vel, target_vel_tf); - pitch_vel_des = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); + geometry_msgs::TransformStamped transform; + if (joint_urdfs_.find(2) != joint_urdfs_.end()) + { + transform = robot_state_handle_.lookupTransform(odom2base_.child_frame_id, data_track_.header.frame_id, + data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + tf2::fromMsg(target_pos, target_pos_tf); + tf2::fromMsg(target_vel, target_vel_tf); + yaw_vel_des = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); + } + if (joint_urdfs_.find(1) != joint_urdfs_.end()) + { + transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->parent_link_name, + data_track_.header.frame_id, data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + tf2::fromMsg(target_pos, target_pos_tf); + tf2::fromMsg(target_vel, target_vel_tf); + pitch_vel_des = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); + } } catch (tf2::TransformException& ex) { ROS_WARN("%s", ex.what()); } } - if (!pitch_des_in_limit_) + if (pos_des_in_limit_.find(1) != pos_des_in_limit_.end() && !pos_des_in_limit_.at(1)) pitch_vel_des = 0.; - if (!yaw_des_in_limit_) + if (pos_des_in_limit_.find(2) != pos_des_in_limit_.end() && !pos_des_in_limit_.at(2)) yaw_vel_des = 0.; - pid_pitch_pos_.computeCommand(pitch_angle_error, period); - pid_yaw_pos_.computeCommand(yaw_angle_error, period); - ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() - config_.k_chassis_vel_ * chassis_vel_->angular_->z() + - config_.yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); - ctrl_pitch_.setCommand(pid_pitch_pos_.getCurrentCmd() + config_.pitch_k_v_ * pitch_vel_des + - ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); - ctrl_yaw_.update(time, period); - ctrl_pitch_.update(time, period); - ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); + + if (pid_pos_.find(1) != pid_pos_.end()) + { + pid_pos_.at(1)->computeCommand(pitch_angle_error, period); + ctrl_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * pitch_vel_des + + ctrl_.at(1)->joint_.getVelocity() - angular_vel.y); + ctrl_.at(1)->update(time, period); + ctrl_.at(1)->joint_.setCommand(ctrl_.at(1)->joint_.getCommand() + feedForward(time)); + } + if (pid_pos_.find(2) != pid_pos_.end()) + { + pid_pos_.at(2)->computeCommand(yaw_angle_error, period); + ctrl_.at(1)->setCommand(pid_pos_.at(2)->getCurrentCmd() - config_.k_chassis_vel_ * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * yaw_vel_des + ctrl_.at(1)->joint_.getVelocity() - angular_vel.z); + + ctrl_.at(1)->update(time, period); + } // publish state if (loop_count_ % 10 == 0) { - if (yaw_pos_state_pub_ && yaw_pos_state_pub_->trylock()) - { - yaw_pos_state_pub_->msg_.header.stamp = time; - yaw_pos_state_pub_->msg_.set_point = yaw_des; - yaw_pos_state_pub_->msg_.set_point_dot = yaw_vel_des; - yaw_pos_state_pub_->msg_.process_value = yaw_real; - yaw_pos_state_pub_->msg_.error = angles::shortest_angular_distance(yaw_real, yaw_des); - yaw_pos_state_pub_->msg_.command = pid_yaw_pos_.getCurrentCmd(); - yaw_pos_state_pub_->unlockAndPublish(); - } - if (pitch_pos_state_pub_ && pitch_pos_state_pub_->trylock()) - { - pitch_pos_state_pub_->msg_.header.stamp = time; - pitch_pos_state_pub_->msg_.set_point = pitch_des; - pitch_pos_state_pub_->msg_.set_point_dot = pitch_vel_des; - pitch_pos_state_pub_->msg_.process_value = pitch_real; - pitch_pos_state_pub_->msg_.error = angles::shortest_angular_distance(pitch_real, pitch_des); - pitch_pos_state_pub_->msg_.command = pid_pitch_pos_.getCurrentCmd(); - pitch_pos_state_pub_->unlockAndPublish(); - } + // if (yaw_pos_state_pub_ && yaw_pos_state_pub_->trylock()) + // { + // yaw_pos_state_pub_->msg_.header.stamp = time; + // yaw_pos_state_pub_->msg_.set_point = yaw_des; + // yaw_pos_state_pub_->msg_.set_point_dot = yaw_vel_des; + // yaw_pos_state_pub_->msg_.process_value = yaw_real; + // yaw_pos_state_pub_->msg_.error = angles::shortest_angular_distance(yaw_real, yaw_des); + // yaw_pos_state_pub_->msg_.command = pid_yaw_pos_.getCurrentCmd(); + // yaw_pos_state_pub_->unlockAndPublish(); + // } + // if (pitch_pos_state_pub_ && pitch_pos_state_pub_->trylock()) + // { + // pitch_pos_state_pub_->msg_.header.stamp = time; + // pitch_pos_state_pub_->msg_.set_point = pitch_des; + // pitch_pos_state_pub_->msg_.set_point_dot = pitch_vel_des; + // pitch_pos_state_pub_->msg_.process_value = pitch_real; + // pitch_pos_state_pub_->msg_.error = angles::shortest_angular_distance(pitch_real, pitch_des); + // pitch_pos_state_pub_->msg_.command = pid_pitch_pos_.getCurrentCmd(); + // pitch_pos_state_pub_->unlockAndPublish(); + // } } loop_count_++; } double Controller::feedForward(const ros::Time& time) { - Eigen::Vector3d gravity(0, 0, -gravity_); - tf2::doTransform(gravity, gravity, - robot_state_handle_.lookupTransform(pitch_joint_urdf_->child_link_name, "base_link", time)); - Eigen::Vector3d mass_origin(mass_origin_.x, 0, mass_origin_.z); - double feedforward = -mass_origin.cross(gravity).y(); - if (enable_gravity_compensation_) + if (joint_urdfs_.find(1) != joint_urdfs_.end()) { - Eigen::Vector3d gravity_compensation(0, 0, gravity_); - tf2::doTransform(gravity_compensation, gravity_compensation, - robot_state_handle_.lookupTransform(pitch_joint_urdf_->child_link_name, - pitch_joint_urdf_->parent_link_name, time)); - feedforward -= mass_origin.cross(gravity_compensation).y(); + Eigen::Vector3d gravity(0, 0, -gravity_); + tf2::doTransform(gravity, gravity, + robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->child_link_name, "base_link", time)); + Eigen::Vector3d mass_origin(mass_origin_.x, 0, mass_origin_.z); + double feedforward = -mass_origin.cross(gravity).y(); + if (enable_gravity_compensation_) + { + Eigen::Vector3d gravity_compensation(0, 0, gravity_); + tf2::doTransform(gravity_compensation, gravity_compensation, + robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->child_link_name, + joint_urdfs_.at(1)->parent_link_name, time)); + feedforward -= mass_origin.cross(gravity_compensation).y(); + } + return feedforward; } - return feedforward; + return 0.; } void Controller::updateChassisVel() @@ -521,6 +548,28 @@ void Controller::updateChassisVel() last_odom2base_ = odom2base_; } +std::string getGimbalFrameID(std::unordered_map joint_urdfs) +{ + if (joint_urdfs.find(1) != joint_urdfs.end()) + return joint_urdfs.at(1)->child_link_name.c_str(); + if (joint_urdfs.find(0) != joint_urdfs.end()) + return joint_urdfs.at(0)->child_link_name.c_str(); + if (joint_urdfs.find(2) != joint_urdfs.end()) + return joint_urdfs.at(2)->child_link_name.c_str(); + return std::string(); +} + +std::string getBaseFrameID(std::unordered_map joint_urdfs) +{ + if (joint_urdfs.find(2) != joint_urdfs.end()) + return joint_urdfs.at(2)->parent_link_name.c_str(); + if (joint_urdfs.find(0) != joint_urdfs.end()) + return joint_urdfs.at(0)->parent_link_name.c_str(); + if (joint_urdfs.find(1) != joint_urdfs.end()) + return joint_urdfs.at(1)->parent_link_name.c_str(); + return std::string(); +} + void Controller::commandCB(const rm_msgs::GimbalCmdConstPtr& msg) { cmd_rt_buffer_.writeFromNonRT(*msg); From 83dc48b74a83c6bbc079edd0cc2ffd3a8c395175 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 25 Feb 2025 10:42:53 +0800 Subject: [PATCH 03/13] Use array instead of variables. --- rm_gimbal_controllers/src/gimbal_base.cpp | 71 ++++++++++------------- 1 file changed, 31 insertions(+), 40 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index b4e28de4..2e43278c 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -409,16 +409,15 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) if (ctrl_.find(2) != ctrl_.end()) angular_vel.z = ctrl_.at(2)->joint_.getVelocity(); } - double roll_real, pitch_real, yaw_real, roll_des, pitch_des, yaw_des; - quatToRPY(odom2gimbal_des_.transform.rotation, roll_des, pitch_des, yaw_des); - quatToRPY(odom2gimbal_.transform.rotation, roll_real, pitch_real, yaw_real); - double yaw_angle_error = angles::shortest_angular_distance(yaw_real, yaw_des); - double pitch_angle_error = angles::shortest_angular_distance(pitch_real, pitch_des); - double yaw_vel_des = 0., pitch_vel_des = 0.; + double pos_real[3], pos_des[3], vel_des[3], angle_error[3]; + quatToRPY(odom2gimbal_des_.transform.rotation, pos_des[0], pos_des[1], pos_des[2]); + quatToRPY(odom2gimbal_.transform.rotation, pos_real[0], pos_real[1], pos_real[2]); + for (int i = 0; i < 3; i++) + angle_error[i] = angles::shortest_angular_distance(pos_real[i], pos_des[i]); if (state_ == RATE) { - yaw_vel_des = cmd_gimbal_.rate_yaw; - pitch_vel_des = cmd_gimbal_.rate_pitch; + vel_des[2] = cmd_gimbal_.rate_yaw; + vel_des[1] = cmd_gimbal_.rate_pitch; } else if (state_ == TRACK) { @@ -439,7 +438,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) tf2::doTransform(target_vel, target_vel, transform); tf2::fromMsg(target_pos, target_pos_tf); tf2::fromMsg(target_vel, target_vel_tf); - yaw_vel_des = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); + vel_des[2] = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); } if (joint_urdfs_.find(1) != joint_urdfs_.end()) { @@ -449,7 +448,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) tf2::doTransform(target_vel, target_vel, transform); tf2::fromMsg(target_pos, target_pos_tf); tf2::fromMsg(target_vel, target_vel_tf); - pitch_vel_des = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); + vel_des[1] = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); } } catch (tf2::TransformException& ex) @@ -457,24 +456,23 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) ROS_WARN("%s", ex.what()); } } - if (pos_des_in_limit_.find(1) != pos_des_in_limit_.end() && !pos_des_in_limit_.at(1)) - pitch_vel_des = 0.; - if (pos_des_in_limit_.find(2) != pos_des_in_limit_.end() && !pos_des_in_limit_.at(2)) - yaw_vel_des = 0.; + for (const auto& in_limit : pos_des_in_limit_) + if (!in_limit.second) + vel_des[in_limit.first] = 0.; if (pid_pos_.find(1) != pid_pos_.end()) { - pid_pos_.at(1)->computeCommand(pitch_angle_error, period); - ctrl_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * pitch_vel_des + + pid_pos_.at(1)->computeCommand(angle_error[1], period); + ctrl_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + ctrl_.at(1)->joint_.getVelocity() - angular_vel.y); ctrl_.at(1)->update(time, period); ctrl_.at(1)->joint_.setCommand(ctrl_.at(1)->joint_.getCommand() + feedForward(time)); } if (pid_pos_.find(2) != pid_pos_.end()) { - pid_pos_.at(2)->computeCommand(yaw_angle_error, period); + pid_pos_.at(2)->computeCommand(angle_error[2], period); ctrl_.at(1)->setCommand(pid_pos_.at(2)->getCurrentCmd() - config_.k_chassis_vel_ * chassis_vel_->angular_->z() + - config_.yaw_k_v_ * yaw_vel_des + ctrl_.at(1)->joint_.getVelocity() - angular_vel.z); + config_.yaw_k_v_ * vel_des[2] + ctrl_.at(1)->joint_.getVelocity() - angular_vel.z); ctrl_.at(1)->update(time, period); } @@ -482,26 +480,19 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) // publish state if (loop_count_ % 10 == 0) { - // if (yaw_pos_state_pub_ && yaw_pos_state_pub_->trylock()) - // { - // yaw_pos_state_pub_->msg_.header.stamp = time; - // yaw_pos_state_pub_->msg_.set_point = yaw_des; - // yaw_pos_state_pub_->msg_.set_point_dot = yaw_vel_des; - // yaw_pos_state_pub_->msg_.process_value = yaw_real; - // yaw_pos_state_pub_->msg_.error = angles::shortest_angular_distance(yaw_real, yaw_des); - // yaw_pos_state_pub_->msg_.command = pid_yaw_pos_.getCurrentCmd(); - // yaw_pos_state_pub_->unlockAndPublish(); - // } - // if (pitch_pos_state_pub_ && pitch_pos_state_pub_->trylock()) - // { - // pitch_pos_state_pub_->msg_.header.stamp = time; - // pitch_pos_state_pub_->msg_.set_point = pitch_des; - // pitch_pos_state_pub_->msg_.set_point_dot = pitch_vel_des; - // pitch_pos_state_pub_->msg_.process_value = pitch_real; - // pitch_pos_state_pub_->msg_.error = angles::shortest_angular_distance(pitch_real, pitch_des); - // pitch_pos_state_pub_->msg_.command = pid_pitch_pos_.getCurrentCmd(); - // pitch_pos_state_pub_->unlockAndPublish(); - // } + for (const auto& pub : pos_state_pub_) + { + if (pub.second && pub.second->trylock()) + { + pub.second->msg_.header.stamp = time; + pub.second->msg_.set_point = pos_des[pub.first]; + pub.second->msg_.set_point_dot = vel_des[pub.first]; + pub.second->msg_.process_value = pos_real[pub.first]; + pub.second->msg_.error = angle_error[pub.first]; + pub.second->msg_.command = pid_pos_.at(pub.first)->getCurrentCmd(); + pub.second->unlockAndPublish(); + } + } } loop_count_++; } @@ -548,7 +539,7 @@ void Controller::updateChassisVel() last_odom2base_ = odom2base_; } -std::string getGimbalFrameID(std::unordered_map joint_urdfs) +std::string Controller::getGimbalFrameID(std::unordered_map joint_urdfs) { if (joint_urdfs.find(1) != joint_urdfs.end()) return joint_urdfs.at(1)->child_link_name.c_str(); @@ -559,7 +550,7 @@ std::string getGimbalFrameID(std::unordered_map return std::string(); } -std::string getBaseFrameID(std::unordered_map joint_urdfs) +std::string Controller::getBaseFrameID(std::unordered_map joint_urdfs) { if (joint_urdfs.find(2) != joint_urdfs.end()) return joint_urdfs.at(2)->parent_link_name.c_str(); From f2670e6101c7f0f16a035b5f39eae7fec5234b06 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 25 Feb 2025 11:22:16 +0800 Subject: [PATCH 04/13] Fix bug. --- rm_gimbal_controllers/src/gimbal_base.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 2e43278c..8f4afa31 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -100,7 +100,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ROS_ERROR("Failed to parse urdf file"); return false; } - auto joint_urdf = urdf.getJoint(getParam(nh, "joint_name", std::string())); + auto joint_urdf = urdf.getJoint(getParam(nh, "joint", std::string())); if (!joint_urdf) { ROS_ERROR("Could not find joint in urdf"); @@ -471,10 +471,10 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) if (pid_pos_.find(2) != pid_pos_.end()) { pid_pos_.at(2)->computeCommand(angle_error[2], period); - ctrl_.at(1)->setCommand(pid_pos_.at(2)->getCurrentCmd() - config_.k_chassis_vel_ * chassis_vel_->angular_->z() + + ctrl_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - config_.k_chassis_vel_ * chassis_vel_->angular_->z() + config_.yaw_k_v_ * vel_des[2] + ctrl_.at(1)->joint_.getVelocity() - angular_vel.z); - ctrl_.at(1)->update(time, period); + ctrl_.at(2)->update(time, period); } // publish state From 67de01c48612edec9f43754a84fdc36cb694b9d1 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 25 Feb 2025 14:41:13 +0800 Subject: [PATCH 05/13] Fix bug. --- rm_gimbal_controllers/src/gimbal_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 8f4afa31..38354238 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -472,7 +472,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) { pid_pos_.at(2)->computeCommand(angle_error[2], period); ctrl_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - config_.k_chassis_vel_ * chassis_vel_->angular_->z() + - config_.yaw_k_v_ * vel_des[2] + ctrl_.at(1)->joint_.getVelocity() - angular_vel.z); + config_.yaw_k_v_ * vel_des[2] + ctrl_.at(2)->joint_.getVelocity() - angular_vel.z); ctrl_.at(2)->update(time, period); } From 03026f94af9f7047ac6e6665c8f132ed3c9855f7 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 25 Feb 2025 15:47:01 +0800 Subject: [PATCH 06/13] Fix set des into limit bug, add frame id for traj mode. --- rm_gimbal_controllers/src/gimbal_base.cpp | 31 ++++++++++++++++++++--- 1 file changed, 27 insertions(+), 4 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 38354238..1b0a1259 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -213,13 +213,13 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des) { - tf2::Quaternion odom2base, odom2gimbal_des, base2new_des; + tf2::Quaternion odom2base, odom2gimbal_des; tf2::fromMsg(odom2base_.transform.rotation, odom2base); 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, base2new_des))); - odom2gimbal_des_.transform.rotation = tf2::toMsg(odom2base * base2new_des); + pos_des_in_limit_.insert(std::make_pair(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"); } @@ -350,7 +350,30 @@ void Controller::traj(const ros::Time& time) state_changed_ = false; ROS_INFO("[Gimbal] Enter TRAJ"); } - setDes(time, cmd_gimbal_.traj_yaw, cmd_gimbal_.traj_pitch); + double traj[3] = { 0. }; + try + { + if (!cmd_gimbal_.traj_frame_id.empty()) + { + tf2::Quaternion odom2traj_quat, traj2target, odom2target; + geometry_msgs::TransformStamped odom2traj = + robot_state_handle_.lookupTransform("odom", cmd_gimbal_.traj_frame_id, time); + tf2::fromMsg(odom2traj.transform.rotation, odom2traj_quat); + traj2target.setRPY(0., cmd_gimbal_.traj_pitch, cmd_gimbal_.traj_yaw); + odom2target = odom2traj_quat * traj2target; + quatToRPY(toMsg(odom2target), traj[0], traj[1], traj[2]); + } + else + { + traj[1] = cmd_gimbal_.traj_pitch; + traj[2] = cmd_gimbal_.traj_yaw; + } + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + } + setDes(time, traj[2], traj[1]); } bool Controller::setDesIntoLimit(const tf2::Quaternion& base2gimbal_des, const urdf::JointConstSharedPtr& joint_urdf, From 3e2cbc9626a4289783cc26c572819cc865943140 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 25 Feb 2025 15:59:38 +0800 Subject: [PATCH 07/13] Modify name. --- .../rm_gimbal_controllers/gimbal_base.h | 2 +- rm_gimbal_controllers/src/gimbal_base.cpp | 30 +++++++++---------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index 52a33442..630ac2e9 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -157,7 +157,7 @@ class Controller : public controller_interface::MultiInterfaceController> ctrl_; + std::unordered_map> ctrls_; std::unordered_map> pid_pos_; std::unordered_map joint_urdfs_; std::unordered_map pos_des_in_limit_; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 1b0a1259..b3086ee6 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -112,12 +112,12 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro joint_urdfs_.insert(std::make_pair(axis, joint_urdf)); } - ctrl_.insert(std::make_pair(axis, std::make_unique())); + ctrls_.insert(std::make_pair(axis, std::make_unique())); pid_pos_.insert(std::make_pair(axis, std::make_unique())); pos_state_pub_.insert(std::make_pair( axis, std::make_unique>(nh, "pos_state", 1))); - if (!ctrl_.at(axis)->init(effort_joint_interface, nh) || !pid_pos_.at(axis)->init(nh_pid_pos)) + if (!ctrls_.at(axis)->init(effort_joint_interface, nh) || !pid_pos_.at(axis)->init(nh_pid_pos)) return false; } } @@ -425,12 +425,12 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } else { - if (ctrl_.find(0) != ctrl_.end()) - angular_vel.x = ctrl_.at(0)->joint_.getVelocity(); - if (ctrl_.find(1) != ctrl_.end()) - angular_vel.y = ctrl_.at(1)->joint_.getVelocity(); - if (ctrl_.find(2) != ctrl_.end()) - angular_vel.z = ctrl_.at(2)->joint_.getVelocity(); + if (ctrls_.find(0) != ctrls_.end()) + angular_vel.x = ctrls_.at(0)->joint_.getVelocity(); + if (ctrls_.find(1) != ctrls_.end()) + angular_vel.y = ctrls_.at(1)->joint_.getVelocity(); + if (ctrls_.find(2) != ctrls_.end()) + angular_vel.z = ctrls_.at(2)->joint_.getVelocity(); } double pos_real[3], pos_des[3], vel_des[3], angle_error[3]; quatToRPY(odom2gimbal_des_.transform.rotation, pos_des[0], pos_des[1], pos_des[2]); @@ -486,18 +486,18 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) if (pid_pos_.find(1) != pid_pos_.end()) { pid_pos_.at(1)->computeCommand(angle_error[1], period); - ctrl_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + - ctrl_.at(1)->joint_.getVelocity() - angular_vel.y); - ctrl_.at(1)->update(time, period); - ctrl_.at(1)->joint_.setCommand(ctrl_.at(1)->joint_.getCommand() + feedForward(time)); + ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + + ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); + ctrls_.at(1)->update(time, period); + ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + feedForward(time)); } if (pid_pos_.find(2) != pid_pos_.end()) { pid_pos_.at(2)->computeCommand(angle_error[2], period); - ctrl_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - config_.k_chassis_vel_ * chassis_vel_->angular_->z() + - config_.yaw_k_v_ * vel_des[2] + ctrl_.at(2)->joint_.getVelocity() - angular_vel.z); + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - config_.k_chassis_vel_ * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); - ctrl_.at(2)->update(time, period); + ctrls_.at(2)->update(time, period); } // publish state From 2b0e687f66142b5135a860f1353e08b9d387bc87 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Wed, 26 Feb 2025 10:07:58 +0800 Subject: [PATCH 08/13] Remove blank line. --- .../include/rm_gimbal_controllers/gimbal_base.h | 4 ++-- rm_gimbal_controllers/src/gimbal_base.cpp | 1 - 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index 630ac2e9..b34747ea 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -149,11 +149,11 @@ class Controller : public controller_interface::MultiInterfaceController joint_urdfs); - std::string getBaseFrameID(std::unordered_map joint_urdfs); void commandCB(const rm_msgs::GimbalCmdConstPtr& msg); void trackCB(const rm_msgs::TrackDataConstPtr& msg); void reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t); + std::string getGimbalFrameID(std::unordered_map joint_urdfs); + std::string getBaseFrameID(std::unordered_map joint_urdfs); rm_control::RobotStateHandle robot_state_handle_; hardware_interface::ImuSensorHandle imu_sensor_handle_; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index b3086ee6..0ae3e333 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -496,7 +496,6 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) pid_pos_.at(2)->computeCommand(angle_error[2], period); ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - config_.k_chassis_vel_ * chassis_vel_->angular_->z() + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); - ctrls_.at(2)->update(time, period); } From f3a65a8a34744345e5002a73593f0e16b3743ec2 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sun, 9 Mar 2025 13:39:41 +0800 Subject: [PATCH 09/13] Modify traj logic. --- rm_gimbal_controllers/src/gimbal_base.cpp | 27 +++++++++-------------- 1 file changed, 10 insertions(+), 17 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 0ae3e333..696ce4e9 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -67,16 +67,11 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); bullet_solver_ = std::make_shared(nh_bullet_solver); - ros::NodeHandle nh_yaw = ros::NodeHandle(controller_nh, "yaw"); - ros::NodeHandle nh_pitch = ros::NodeHandle(controller_nh, "pitch"); - ros::NodeHandle nh_pid_yaw_pos = ros::NodeHandle(controller_nh, "yaw/pid_pos"); - ros::NodeHandle nh_pid_pitch_pos = ros::NodeHandle(controller_nh, "pitch/pid_pos"); - - config_ = { .yaw_k_v_ = getParam(nh_yaw, "k_v", 0.), - .pitch_k_v_ = getParam(nh_pitch, "k_v", 0.), - .k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.), - .accel_pitch_ = getParam(controller_nh, "pitch/accel", 99.), - .accel_yaw_ = getParam(controller_nh, "yaw/accel", 99.) }; + config_ = { .yaw_k_v_ = getParam(controller_nh, "controllers/yaw/k_v", 0.), + .pitch_k_v_ = getParam(controller_nh, "controllers/pitch/k_v", 0.), + .k_chassis_vel_ = getParam(controller_nh, "controllers/yaw/k_chassis_vel", 0.), + .accel_pitch_ = getParam(controller_nh, "controllers/pitch/accel", 99.), + .accel_yaw_ = getParam(controller_nh, "controllers/yaw/accel", 99.) }; config_rt_buffer_.initRT(config_); d_srv_ = new dynamic_reconfigure::Server(controller_nh); dynamic_reconfigure::Server::CallbackType cb = @@ -350,18 +345,16 @@ void Controller::traj(const ros::Time& time) state_changed_ = false; ROS_INFO("[Gimbal] Enter TRAJ"); } - double traj[3] = { 0. }; + double traj[3]{ 0. }; try { if (!cmd_gimbal_.traj_frame_id.empty()) { - tf2::Quaternion odom2traj_quat, traj2target, odom2target; geometry_msgs::TransformStamped odom2traj = robot_state_handle_.lookupTransform("odom", cmd_gimbal_.traj_frame_id, time); - tf2::fromMsg(odom2traj.transform.rotation, odom2traj_quat); - traj2target.setRPY(0., cmd_gimbal_.traj_pitch, cmd_gimbal_.traj_yaw); - odom2target = odom2traj_quat * traj2target; - quatToRPY(toMsg(odom2target), traj[0], traj[1], traj[2]); + quatToRPY(odom2traj.transform.rotation, traj[0], traj[1], traj[2]); + traj[1] += cmd_gimbal_.traj_pitch; + traj[2] += cmd_gimbal_.traj_yaw; } else { @@ -432,7 +425,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) if (ctrls_.find(2) != ctrls_.end()) angular_vel.z = ctrls_.at(2)->joint_.getVelocity(); } - double pos_real[3], pos_des[3], vel_des[3], angle_error[3]; + double pos_real[3]{ 0. }, pos_des[3]{ 0. }, vel_des[3]{ 0. }, angle_error[3]{ 0. }; quatToRPY(odom2gimbal_des_.transform.rotation, pos_des[0], pos_des[1], pos_des[2]); quatToRPY(odom2gimbal_.transform.rotation, pos_real[0], pos_real[1], pos_real[2]); for (int i = 0; i < 3; i++) From 4fe01f3805a2a602a0ef5593f83828b93680894a Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 11 Mar 2025 20:48:50 +0800 Subject: [PATCH 10/13] Add some judgement. --- rm_gimbal_controllers/src/gimbal_base.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 696ce4e9..f9cc2d6e 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -476,7 +476,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) if (!in_limit.second) vel_des[in_limit.first] = 0.; - if (pid_pos_.find(1) != pid_pos_.end()) + if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) { pid_pos_.at(1)->computeCommand(angle_error[1], period); ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + @@ -484,7 +484,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) ctrls_.at(1)->update(time, period); ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + feedForward(time)); } - if (pid_pos_.find(2) != pid_pos_.end()) + if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) { pid_pos_.at(2)->computeCommand(angle_error[2], period); ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - config_.k_chassis_vel_ * chassis_vel_->angular_->z() + From 15c9cdbfe73d076a20895522ff5138d21b871605 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sun, 30 Mar 2025 20:18:06 +0800 Subject: [PATCH 11/13] Resolve merge conflicts. --- rm_gimbal_controllers/src/gimbal_base.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index dde0c400..71566f4b 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -69,7 +69,10 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro config_ = { .yaw_k_v_ = getParam(controller_nh, "controllers/yaw/k_v", 0.), .pitch_k_v_ = getParam(controller_nh, "controllers/pitch/k_v", 0.), - .k_chassis_vel_ = getParam(controller_nh, "controllers/yaw/k_chassis_vel", 0.), + .chassis_comp_a_ = getParam(controller_nh, "controllers/yaw/chassis_comp_a", 0.), + .chassis_comp_b_ = getParam(controller_nh, "controllers/yaw/chassis_comp_b", 0.), + .chassis_comp_c_ = getParam(controller_nh, "controllers/yaw/chassis_comp_c", 0.), + .chassis_comp_d_ = getParam(controller_nh, "controllers/yaw/chassis_comp_d", 0.), .accel_pitch_ = getParam(controller_nh, "controllers/pitch/accel", 99.), .accel_yaw_ = getParam(controller_nh, "controllers/yaw/accel", 99.) }; config_rt_buffer_.initRT(config_); @@ -487,7 +490,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) { pid_pos_.at(2)->computeCommand(angle_error[2], period); - ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - config_.k_chassis_vel_ * chassis_vel_->angular_->z() + + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); ctrls_.at(2)->update(time, period); } From 5e250e9f5cecf8394f9e90507ab38356a12c68a8 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sun, 30 Mar 2025 20:53:43 +0800 Subject: [PATCH 12/13] Use first order lag instead of ramp filter. --- .../rm_gimbal_controllers/gimbal_base.h | 9 ++++++--- rm_gimbal_controllers/src/gimbal_base.cpp | 19 +++++++------------ 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index 075bb7c5..e1b716f9 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -147,9 +147,13 @@ class Controller : public controller_interface::MultiInterfaceController config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; - RampFilter*ramp_rate_pitch_{}, *ramp_rate_yaw_{}; - enum { RATE, @@ -210,6 +212,7 @@ class Controller : public controller_interface::MultiInterfaceController(controller_nh, "error", 100)); - ramp_rate_pitch_ = new RampFilter(0, 0.001); - ramp_rate_yaw_ = new RampFilter(0, 0.001); - return true; } @@ -169,12 +166,6 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) cmd_gimbal_ = *cmd_rt_buffer_.readFromRT(); data_track_ = *track_rt_buffer_.readFromNonRT(); config_ = *config_rt_buffer_.readFromRT(); - ramp_rate_pitch_->setAcc(config_.accel_pitch_); - ramp_rate_yaw_->setAcc(config_.accel_yaw_); - ramp_rate_pitch_->input(cmd_gimbal_.rate_pitch); - ramp_rate_yaw_->input(cmd_gimbal_.rate_yaw); - cmd_gimbal_.rate_pitch = ramp_rate_pitch_->output(); - cmd_gimbal_.rate_yaw = ramp_rate_yaw_->output(); try { odom2gimbal_ = robot_state_handle_.lookupTransform("odom", odom2gimbal_.child_frame_id, time); @@ -234,13 +225,17 @@ void Controller::rate(const ros::Time& time, const ros::Duration& period) odom2gimbal_des_.header.stamp = time; robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers"); start_ = false; + rate_yaw_ = 0.; + rate_pitch_ = 0.; } } else { double roll{}, pitch{}, yaw{}; quatToRPY(odom2gimbal_des_.transform.rotation, roll, pitch, yaw); - setDes(time, yaw + period.toSec() * cmd_gimbal_.rate_yaw, pitch + period.toSec() * cmd_gimbal_.rate_pitch); + rate_yaw_ = firstOrderLag(cmd_gimbal_.rate_yaw, rate_yaw_, 0.02); + rate_pitch_ = firstOrderLag(cmd_gimbal_.rate_pitch, rate_pitch_, 0.02); + setDes(time, yaw + period.toSec() * rate_yaw_, pitch + period.toSec() * rate_pitch_); } } @@ -435,8 +430,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) angle_error[i] = angles::shortest_angular_distance(pos_real[i], pos_des[i]); if (state_ == RATE) { - vel_des[2] = cmd_gimbal_.rate_yaw; - vel_des[1] = cmd_gimbal_.rate_pitch; + vel_des[2] = rate_yaw_; + vel_des[1] = rate_pitch_; } else if (state_ == TRACK) { From cf6517e159b28a574ffbec72435b8eedf21eecff Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 1 Apr 2025 12:28:20 +0800 Subject: [PATCH 13/13] Remove first order lag. --- .../include/rm_gimbal_controllers/gimbal_base.h | 5 ----- rm_gimbal_controllers/src/gimbal_base.cpp | 10 +++------- 2 files changed, 3 insertions(+), 12 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index e1b716f9..71b5b7c5 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -150,10 +150,6 @@ class Controller : public controller_interface::MultiInterfaceController