From 1f1857c390cf356dcee66b49bbadc864be071b40 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Tue, 23 Jan 2024 23:31:47 +0800 Subject: [PATCH] Encapsulate feedforward. --- .../rm_gimbal_controllers/feedforward.h | 54 ++++++++++++ .../rm_gimbal_controllers/gimbal_base.h | 15 ++-- rm_gimbal_controllers/src/feedforward.cpp | 86 +++++++++++++++++++ rm_gimbal_controllers/src/gimbal_base.cpp | 70 ++++----------- 4 files changed, 165 insertions(+), 60 deletions(-) create mode 100644 rm_gimbal_controllers/include/rm_gimbal_controllers/feedforward.h create mode 100644 rm_gimbal_controllers/src/feedforward.cpp diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/feedforward.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/feedforward.h new file mode 100644 index 00000000..f9a649ab --- /dev/null +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/feedforward.h @@ -0,0 +1,54 @@ +// +// Created by yezi on 24-1-11. +// + +#pragma once + +#include +#include +#include +#include + +class FrictionCompensation +{ +public: + void init(XmlRpc::XmlRpcValue config); + double output(double vel_act, double effort_cmd) const; + +private: + double resistance_{ 0. }; + double velocity_saturation_point_{ 0. }, effort_saturation_point_{ 0. }; +}; + +class InputFeedforward +{ +public: + void init(XmlRpc::XmlRpcValue config); + double output(double vel_desire, double accel_desire) const; + +private: + double k_v_{ 0. }, k_a_{ 0. }; +}; + +class GravityCompensation +{ +public: + void init(XmlRpc::XmlRpcValue config); + double output(rm_control::RobotStateHandle* robot_state_handle, const urdf::JointConstSharedPtr& joint_urdf, + ros::Time time) const; + +private: + geometry_msgs::Vector3 mass_origin_{}; + double gravity_{}; + bool enable_gravity_compensation_ = false; +}; + +class BaseVelCompensation +{ +public: + void init(XmlRpc::XmlRpcValue config); + double output(double base_angular_vel_z) const; + +private: + double k_chassis_vel_{ 0. }; +}; 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 b8d9f3f9..46ac66df 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -52,6 +52,8 @@ #include #include +#include "rm_gimbal_controllers/feedforward.h" + namespace rm_gimbal_controllers { class ChassisVel @@ -135,7 +137,6 @@ class Controller : public controller_interface::MultiInterfaceController chassis_vel_; enum diff --git a/rm_gimbal_controllers/src/feedforward.cpp b/rm_gimbal_controllers/src/feedforward.cpp new file mode 100644 index 00000000..6fb71645 --- /dev/null +++ b/rm_gimbal_controllers/src/feedforward.cpp @@ -0,0 +1,86 @@ +// +// Created by yezi on 24-1-11. +// + +#include +#include +#include "rm_gimbal_controllers/feedforward.h" + +void FrictionCompensation::init(XmlRpc::XmlRpcValue config) +{ + ROS_ASSERT(config.getType() == XmlRpc::XmlRpcValue::TypeStruct); + resistance_ = config.hasMember("resistance") ? static_cast(config["resistance"]) : 0.; + velocity_saturation_point_ = + config.hasMember("velocity_saturation_point") ? static_cast(config["velocity_saturation_point"]) : 0.; + effort_saturation_point_ = + config.hasMember("effort_saturation_point") ? static_cast(config["effort_saturation_point"]) : 0.; +} + +double FrictionCompensation::output(double vel_act, double effort_cmd) const +{ + double resistance_compensation = 0.; + if (std::abs(vel_act) > velocity_saturation_point_) + resistance_compensation = (vel_act > 0 ? 1 : -1) * resistance_; + else if (std::abs(effort_cmd) > effort_saturation_point_) + resistance_compensation = (effort_cmd > 0 ? 1 : -1) * resistance_; + else + resistance_compensation = effort_cmd * resistance_ / effort_saturation_point_; + return resistance_compensation; +} + +void InputFeedforward::init(XmlRpc::XmlRpcValue config) +{ + ROS_ASSERT(config.getType() == XmlRpc::XmlRpcValue::TypeStruct); + k_v_ = config.hasMember("k_v") ? static_cast(config["k_v"]) : 0.; + k_a_ = config.hasMember("k_a") ? static_cast(config["k_a"]) : 0.; +} + +double InputFeedforward::output(double vel_desire, double accel_desire) const +{ + return k_v_ * vel_desire + k_a_ * accel_desire; +} + +void GravityCompensation::init(XmlRpc::XmlRpcValue config) +{ + bool enable_feedforward; + enable_feedforward = config.hasMember("gravity_compensation"); + if (enable_feedforward) + { + ROS_ASSERT(config["gravity_compensation"].hasMember("mass_origin")); + ROS_ASSERT(config["gravity_compensation"].hasMember("gravity")); + ROS_ASSERT(config["gravity_compensation"].hasMember("enable_gravity_compensation")); + } + mass_origin_.x = enable_feedforward ? static_cast(config["gravity_compensation"]["mass_origin"][0]) : 0.; + mass_origin_.z = enable_feedforward ? static_cast(config["gravity_compensation"]["mass_origin"][2]) : 0.; + gravity_ = enable_feedforward ? static_cast(config["gravity_compensation"]["gravity"]) : 0.; + enable_gravity_compensation_ = + enable_feedforward && static_cast(config["gravity_compensation"]["enable_gravity_compensation"]); +} + +double GravityCompensation::output(rm_control::RobotStateHandle* robot_state_handle, + const urdf::JointConstSharedPtr& joint_urdf, ros::Time time) const +{ + Eigen::Vector3d gravity(0, 0, -gravity_); + tf2::doTransform(gravity, gravity, robot_state_handle->lookupTransform(joint_urdf->child_link_name, "odom", 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_urdf->child_link_name, joint_urdf->parent_link_name, + time)); + feedforward -= mass_origin.cross(gravity_compensation).y(); + } + return feedforward; +} + +void BaseVelCompensation::init(XmlRpc::XmlRpcValue config) +{ + k_chassis_vel_ = config.hasMember("k_chassis_vel") ? static_cast(config["k_chassis_vel"]) : 0.; +} + +double BaseVelCompensation::output(double base_angular_vel_z) const +{ + return -k_chassis_vel_ * base_angular_vel_z; +} diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 9549d3d3..2b68dc37 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -49,36 +49,24 @@ namespace rm_gimbal_controllers bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) { XmlRpc::XmlRpcValue xml_rpc_value; - bool enable_feedforward; - enable_feedforward = controller_nh.getParam("feedforward", xml_rpc_value); - if (enable_feedforward) - { - ROS_ASSERT(xml_rpc_value.hasMember("mass_origin")); - ROS_ASSERT(xml_rpc_value.hasMember("gravity")); - ROS_ASSERT(xml_rpc_value.hasMember("enable_gravity_compensation")); - } - mass_origin_.x = enable_feedforward ? (double)xml_rpc_value["mass_origin"][0] : 0.; - mass_origin_.z = enable_feedforward ? (double)xml_rpc_value["mass_origin"][2] : 0.; - gravity_ = enable_feedforward ? (double)xml_rpc_value["gravity"] : 0.; - enable_gravity_compensation_ = enable_feedforward && (bool)xml_rpc_value["enable_gravity_compensation"]; - - ros::NodeHandle resistance_compensation_nh(controller_nh, "yaw/resistance_compensation"); - yaw_resistance_ = getParam(resistance_compensation_nh, "resistance", 0.); - velocity_saturation_point_ = getParam(resistance_compensation_nh, "velocity_saturation_point", 0.); - effort_saturation_point_ = getParam(resistance_compensation_nh, "effort_saturation_point", 0.); + ros::NodeHandle feedforward_nh(controller_nh, "feedforward"); + feedforward_nh.getParam("yaw", xml_rpc_value); + yaw_friction_compensation_.init(xml_rpc_value); + yaw_input_feedforward_.init(xml_rpc_value); + base_vel_compensation_.init(xml_rpc_value); + feedforward_nh.getParam("pitch", xml_rpc_value); + pitch_friction_compensation_.init(xml_rpc_value); + pitch_input_feedforward_.init(xml_rpc_value); + gravity_compensation_.init(xml_rpc_value); - k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.); ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel"); chassis_vel_ = std::make_shared(chassis_vel_nh); + 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"); - yaw_k_v_ = getParam(nh_yaw, "k_v", 0.); - yaw_k_a_ = getParam(nh_yaw, "k_a", 0.); - pitch_k_v_ = getParam(nh_pitch, "k_v", 0.); - pitch_k_a_ = getParam(nh_pitch, "k_a", 0.); 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)) @@ -379,35 +367,15 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) ctrl_pitch_.setCommand(pitch_des, pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); ctrl_yaw_.update(time, period); ctrl_pitch_.update(time, period); - double resistance_compensation = 0.; - if (std::abs(ctrl_yaw_.joint_.getVelocity()) > velocity_saturation_point_) - resistance_compensation = (ctrl_yaw_.joint_.getVelocity() > 0 ? 1 : -1) * yaw_resistance_; - else if (std::abs(ctrl_yaw_.joint_.getCommand()) > effort_saturation_point_) - resistance_compensation = (ctrl_yaw_.joint_.getCommand() > 0 ? 1 : -1) * yaw_resistance_; - else - resistance_compensation = ctrl_yaw_.joint_.getCommand() * yaw_resistance_ / effort_saturation_point_; - ctrl_yaw_.joint_.setCommand(ctrl_yaw_.joint_.getCommand() - k_chassis_vel_ * chassis_vel_->angular_->z() + - yaw_k_v_ * yaw_vel_des + yaw_k_a_ * yaw_accel_des + resistance_compensation); - ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time) + pitch_k_v_ * pitch_vel_des + - pitch_k_a_ * pitch_accel_des); -} - -double Controller::feedForward(const ros::Time& time) -{ - Eigen::Vector3d gravity(0, 0, -gravity_); - tf2::doTransform(gravity, gravity, - robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->child_link_name, "odom", 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(ctrl_pitch_.joint_urdf_->child_link_name, - ctrl_pitch_.joint_urdf_->parent_link_name, time)); - feedforward -= mass_origin.cross(gravity_compensation).y(); - } - return feedforward; + ctrl_yaw_.joint_.setCommand( + ctrl_yaw_.joint_.getCommand() + base_vel_compensation_.output(chassis_vel_->angular_->z()) + + yaw_input_feedforward_.output(yaw_vel_des, yaw_accel_des) + + yaw_friction_compensation_.output(ctrl_yaw_.joint_.getVelocity(), ctrl_yaw_.joint_.getCommand())); + ctrl_pitch_.joint_.setCommand( + ctrl_pitch_.joint_.getCommand() + + gravity_compensation_.output(&robot_state_handle_, ctrl_pitch_.joint_urdf_, time) + + pitch_input_feedforward_.output(pitch_vel_des, pitch_accel_des) + + pitch_friction_compensation_.output(ctrl_pitch_.joint_.getVelocity(), ctrl_pitch_.joint_.getCommand())); } void Controller::updateChassisVel()