-
Notifications
You must be signed in to change notification settings - Fork 61
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #156 from ye-luo-xi-tui/dev
Encapsulate feedforward
- Loading branch information
Showing
4 changed files
with
165 additions
and
60 deletions.
There are no files selected for viewing
54 changes: 54 additions & 0 deletions
54
rm_gimbal_controllers/include/rm_gimbal_controllers/feedforward.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,54 @@ | ||
| // | ||
| // Created by yezi on 24-1-11. | ||
| // | ||
|
|
||
| #pragma once | ||
|
|
||
| #include <XmlRpc.h> | ||
| #include <geometry_msgs/Vector3.h> | ||
| #include <urdf/model.h> | ||
| #include <rm_common/hardware_interface/robot_state_interface.h> | ||
|
|
||
| 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. }; | ||
| }; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,86 @@ | ||
| // | ||
| // Created by yezi on 24-1-11. | ||
| // | ||
|
|
||
| #include <ros/ros.h> | ||
| #include <tf2_eigen/tf2_eigen.h> | ||
| #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<double>(config["resistance"]) : 0.; | ||
| velocity_saturation_point_ = | ||
| config.hasMember("velocity_saturation_point") ? static_cast<double>(config["velocity_saturation_point"]) : 0.; | ||
| effort_saturation_point_ = | ||
| config.hasMember("effort_saturation_point") ? static_cast<double>(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<double>(config["k_v"]) : 0.; | ||
| k_a_ = config.hasMember("k_a") ? static_cast<double>(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<double>(config["gravity_compensation"]["mass_origin"][0]) : 0.; | ||
| mass_origin_.z = enable_feedforward ? static_cast<double>(config["gravity_compensation"]["mass_origin"][2]) : 0.; | ||
| gravity_ = enable_feedforward ? static_cast<double>(config["gravity_compensation"]["gravity"]) : 0.; | ||
| enable_gravity_compensation_ = | ||
| enable_feedforward && static_cast<bool>(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<double>(config["k_chassis_vel"]) : 0.; | ||
| } | ||
|
|
||
| double BaseVelCompensation::output(double base_angular_vel_z) const | ||
| { | ||
| return -k_chassis_vel_ * base_angular_vel_z; | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters