Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Update chassis compensation #184

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

Update chassis compensation #184

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
7be74d4
Update chassis compensation.
321Aurora Nov 17, 2024
fb462ec
Merge branch 'rm-controls:master' into chassis-compensation
321Aurora Nov 23, 2024
8899129
Fix code specification.
321Aurora Nov 23, 2024
e435c20
Merge branch 'rm-controls:master' into chassis-compensation
321Aurora Nov 23, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion rm_gimbal_controllers/cfg/GimbalBase.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,8 @@ gen.add("yaw_k_v_", double_t, 0, "Yaw input feedforward scale", 0.0, 0, 1.0)
gen.add("pitch_k_v_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 1.0)
gen.add("accel_yaw_", double_t, 0, "Acceleration of rate yaw", 0.0, 0, 999.0)
gen.add("accel_pitch_", double_t, 0, "Acceleration of rate pitch", 0.0, 0, 999.0)
gen.add("k_chassis_vel_", double_t, 0, "Yaw chassis velocity feedforward scale", 0.0, 0, 1.0)
gen.add("chassis_comp_a_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0)
gen.add("chassis_comp_b_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0)
gen.add("chassis_comp_c_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0)

exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase"))
4 changes: 3 additions & 1 deletion rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ namespace rm_gimbal_controllers
{
struct GimbalConfig
{
double yaw_k_v_, pitch_k_v_, k_chassis_vel_;
double yaw_k_v_, pitch_k_v_, chassis_comp_a_, chassis_comp_b_, chassis_comp_c_;
double accel_pitch_{}, accel_yaw_{};
};

Expand Down Expand Up @@ -148,6 +148,7 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
void moveJoint(const ros::Time& time, const ros::Duration& period);
double feedForward(const ros::Time& time);
void updateChassisVel();
double updateCompensation(double chassis_vel_angular_z);
void commandCB(const rm_msgs::GimbalCmdConstPtr& msg);
void trackCB(const rm_msgs::TrackDataConstPtr& msg);
void reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t);
Expand Down Expand Up @@ -188,6 +189,7 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont

// Chassis
std::shared_ptr<ChassisVel> chassis_vel_;
double chassis_compensation_;

bool dynamic_reconfig_initialized_{};
GimbalConfig config_{};
Expand Down
22 changes: 18 additions & 4 deletions rm_gimbal_controllers/src/gimbal_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,9 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro

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.),
.chassis_comp_a_ = getParam(controller_nh, "yaw/chassis_comp_a", 0.),
.chassis_comp_b_ = getParam(controller_nh, "yaw/chassis_comp_b", 0.),
.chassis_comp_c_ = getParam(controller_nh, "yaw/chassis_comp_c", 0.),
.accel_pitch_ = getParam(controller_nh, "pitch/accel", 99.),
.accel_yaw_ = getParam(controller_nh, "yaw/accel", 99.) };
config_rt_buffer_.initRT(config_);
Expand Down Expand Up @@ -500,7 +502,8 @@ 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() +
ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() -
updateCompensation(chassis_vel_->angular_->z()) * 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);
Expand Down Expand Up @@ -548,6 +551,13 @@ void Controller::updateChassisVel()
last_odom2base_ = odom2base_;
}

double Controller::updateCompensation(double chassis_vel_angular_z)
{
chassis_compensation_ = config_.chassis_comp_a_ * pow(chassis_vel_angular_z, 2) +
config_.chassis_comp_b_ * chassis_vel_angular_z + config_.chassis_comp_c_;
return chassis_compensation_;
}

void Controller::commandCB(const rm_msgs::GimbalCmdConstPtr& msg)
{
cmd_rt_buffer_.writeFromNonRT(*msg);
Expand All @@ -568,14 +578,18 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin
GimbalConfig init_config = *config_rt_buffer_.readFromNonRT(); // config init use yaml
config.yaw_k_v_ = init_config.yaw_k_v_;
config.pitch_k_v_ = init_config.pitch_k_v_;
config.k_chassis_vel_ = init_config.k_chassis_vel_;
config.chassis_comp_a_ = init_config.chassis_comp_a_;
config.chassis_comp_b_ = init_config.chassis_comp_b_;
config.chassis_comp_c_ = init_config.chassis_comp_c_;
config.accel_pitch_ = init_config.accel_pitch_;
config.accel_yaw_ = init_config.accel_yaw_;
dynamic_reconfig_initialized_ = true;
}
GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_,
.pitch_k_v_ = config.pitch_k_v_,
.k_chassis_vel_ = config.k_chassis_vel_,
.chassis_comp_a_ = config.chassis_comp_a_,
.chassis_comp_b_ = config.chassis_comp_b_,
.chassis_comp_c_ = config.chassis_comp_c_,
.accel_pitch_ = config.accel_pitch_,
.accel_yaw_ = config.accel_yaw_ };
config_rt_buffer_.writeFromNonRT(config_non_rt);
Expand Down
Loading