Skip to content

Commit

Permalink
Merge pull request #151 from ye-luo-xi-tui/input_compensation
Browse files Browse the repository at this point in the history 
Compute gimbal velocity and accelaration desire under TRACK mode
  • Loading branch information
@ye-luo-xi-tui
ye-luo-xi-tui authored Jan 14, 2024
2 parents 1b2b02e + 668d852 commit 499ce1d
Show file tree
Hide file tree
Showing 5 changed files with 98 additions and 64 deletions.
2 changes: 1 addition & 1 deletion rm_gimbal_controllers/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ include_directories(
)

## Declare a cpp library
add_library(${PROJECT_NAME} src/gimbal_base.cpp src/bullet_solver.cpp)
add_library(${PROJECT_NAME} src/gimbal_base.cpp src/bullet_solver.cpp src/feedforward.cpp)

## Specify libraries to link executable targets against
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
Expand Down
20 changes: 17 additions & 3 deletions rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,16 @@ struct Config
resistance_coff_qd_30, g, delay, dt, timeout;
};

struct TargetState
{
geometry_msgs::Point current_target_center_pos;
geometry_msgs::Vector3 current_target_center_vel;
double yaw;
double v_yaw;
double r;
int armors_num;
};

class BulletSolver
{
public:
Expand All @@ -73,9 +83,8 @@ class BulletSolver
{
return -output_pitch_;
}
void getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel,
geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw,
double r1, double r2, double dz, int armors_num);
void getYawVelAndAccelDes(double& vel_des, double& accel_des);
void getPitchVelAndAccelDes(double& vel_des, double& accel_des);
void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time);
void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t);
~BulletSolver() = default;
Expand All @@ -89,11 +98,16 @@ class BulletSolver
double max_track_target_vel_;
bool dynamic_reconfig_initialized_{};
double output_yaw_{}, output_pitch_{};
double last_pitch_vel_des_{};
ros::Time last_pitch_vel_des_solve_time_{ 0 };
double bullet_speed_{}, resistance_coff_{};
int selected_armor_;
bool track_target_;

geometry_msgs::Point target_pos_{};
geometry_msgs::Vector3 target_vel_{};
geometry_msgs::Vector3 target_accel_{};
TargetState target_state_{};
double fly_time_;
visualization_msgs::Marker marker_desire_;
visualization_msgs::Marker marker_real_;
Expand Down
4 changes: 2 additions & 2 deletions rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -170,8 +170,8 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
bool enable_gravity_compensation_;

// Input feedforward
double yaw_k_v_;
double pitch_k_v_;
double yaw_k_v_, yaw_k_a_;
double pitch_k_v_, pitch_k_a_;

// Resistance compensation
double yaw_resistance_;
Expand Down
85 changes: 62 additions & 23 deletions rm_gimbal_controllers/src/bullet_solver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,6 +105,11 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
config_ = *config_rt_buffer_.readFromRT();
bullet_speed_ = bullet_speed;
resistance_coff_ = getResistanceCoefficient(bullet_speed_) != 0 ? getResistanceCoefficient(bullet_speed_) : 0.001;
target_state_ = TargetState{ .current_target_center_pos = pos,
.current_target_center_vel = vel,
.yaw = yaw,
.v_yaw = v_yaw,
.armors_num = armors_num };

double temp_z = pos.z;
double target_rho = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2));
Expand All @@ -127,6 +132,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
r = armors_num == 4 ? r2 : r1;
z = armors_num == 4 ? pos.z + dz : pos.z;
}
target_state_.r = r;
target_state_.current_target_center_pos.z = z;
int count{};
double error = 999;
if (track_target_)
Expand All @@ -143,8 +150,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
while (error >= 0.001)
{
output_yaw_ = std::atan2(target_pos_.y, target_pos_.x);
output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2)));
target_rho = std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2));
output_pitch_ = std::atan2(temp_z, target_rho);
fly_time_ =
(-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_;
double real_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) *
Expand All @@ -157,6 +164,10 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
pos.x + vel.x * fly_time_ - r * cos(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num);
target_pos_.y =
pos.y + vel.y * fly_time_ - r * sin(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num);
target_vel_.x = vel.x + v_yaw * r * sin(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num);
target_vel_.y = vel.y - v_yaw * r * cos(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num);
target_accel_.x = pow(v_yaw, 2) * r * cos(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num);
target_accel_.y = pow(v_yaw, 2) * r * sin(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num);
}
else
{
Expand All @@ -183,31 +194,59 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
return true;
}

void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel,
geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw,
double v_yaw, double r1, double r2, double dz, int armors_num)
void BulletSolver::getYawVelAndAccelDes(double& vel_des, double& accel_des)
{
double r = r1, z = pos.z;
if (armors_num == 4 && selected_armor_ != 0)
{
r = r2;
z = pos.z + dz;
}
if (track_target_)
{
armor_pos.x = pos.x - r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num);
armor_pos.y = pos.y - r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num);
armor_pos.z = z;
armor_vel.x = vel.x + v_yaw * r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num);
armor_vel.y = vel.y - v_yaw * r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num);
armor_vel.z = vel.z;
}
else
double yaw_vel_des =
(target_pos_.x * target_vel_.y - target_pos_.y * target_vel_.x) / (pow(target_pos_.x, 2) + pow(target_pos_.y, 2));
double yaw_accel_des = (pow(target_pos_.x, 3) * target_accel_.y - pow(target_pos_.y, 3) * target_accel_.x +
2 * target_pos_.x * target_pos_.y * pow(target_vel_.x, 2) -
2 * target_pos_.x * target_pos_.y * pow(target_vel_.y, 2) -
pow(target_pos_.x, 2) * target_pos_.y * target_accel_.x +
target_pos_.x * pow(target_pos_.y, 2) * target_accel_.y -
2 * pow(target_pos_.x, 2) * target_vel_.x * target_vel_.y +
2 * pow(target_pos_.y, 2) * target_vel_.x * target_vel_.y) /
pow((pow(target_pos_.x, 2) + pow(target_pos_.y, 2)), 2);
vel_des = yaw_vel_des;
accel_des = yaw_accel_des;
}

void BulletSolver::getPitchVelAndAccelDes(double& vel_des, double& accel_des)
{
double dt = 0.01;
double r = target_state_.r;
double pos_x = target_state_.current_target_center_pos.x +
target_state_.current_target_center_vel.x * (fly_time_ + dt) -
r * cos(target_state_.yaw + target_state_.v_yaw * (fly_time_ + dt) +
selected_armor_ * 2 * M_PI / target_state_.armors_num);
double pos_y = target_state_.current_target_center_pos.y +
target_state_.current_target_center_vel.y * (fly_time_ + dt) -
r * sin(target_state_.yaw + target_state_.v_yaw * (fly_time_ + dt) +
selected_armor_ * 2 * M_PI / target_state_.armors_num);
double pos_z =
target_state_.current_target_center_pos.z + (fly_time_ + dt) * target_state_.current_target_center_vel.z;
double target_rho = std::sqrt(std::pow(pos_x, 2) + std::pow(pos_y, 2));
double temp_z = target_rho * tan(output_pitch_);
double output_pitch_next = output_pitch_;
double error_z = 999;
while (std::abs(error_z) >= 1e-9)
{
armor_pos = pos;
armor_pos.z = z;
armor_vel = vel;
output_pitch_next = std::atan2(temp_z, target_rho);
double fly_time = (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_next)))) /
resistance_coff_;
double real_z = (bullet_speed_ * std::sin(output_pitch_next) + (config_.g / resistance_coff_)) *
(1 - std::exp(-resistance_coff_ * fly_time)) / resistance_coff_ -
config_.g * fly_time / resistance_coff_;
error_z = pos_z - real_z;
temp_z += error_z;
}
double pitch_vel_des, pitch_accel_des;
pitch_vel_des = (output_pitch_next - output_pitch_) / dt;
ros::Time now = ros::Time::now();
pitch_accel_des = (pitch_vel_des - last_pitch_vel_des_) / (now - last_pitch_vel_des_solve_time_).toSec();
last_pitch_vel_des_ = pitch_vel_des;
last_pitch_vel_des_solve_time_ = now;
vel_des = pitch_vel_des;
accel_des = pitch_accel_des;
}

void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time)
Expand Down
51 changes: 16 additions & 35 deletions rm_gimbal_controllers/src/gimbal_base.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,9 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro
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<hardware_interface::EffortJointInterface>();
if (!ctrl_yaw_.init(effort_joint_interface, nh_yaw) || !ctrl_pitch_.init(effort_joint_interface, nh_pitch))
Expand Down Expand Up @@ -248,14 +250,16 @@ void Controller::track(const ros::Time& time)
{
ROS_WARN("%s", ex.what());
}
target_pos.x -= odom2pitch_.transform.translation.x;
target_pos.y -= odom2pitch_.transform.translation.y;
target_pos.z -= odom2pitch_.transform.translation.z;
ros::Time now = ros::Time::now();
target_pos.x += target_vel.x * (now - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.x;
target_pos.y += target_vel.y * (now - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.y;
target_pos.z += target_vel.z * (now - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.z;
target_vel.x -= chassis_vel_->linear_->x();
target_vel.y -= chassis_vel_->linear_->y();
target_vel.z -= chassis_vel_->linear_->z();
double yaw = data_track_.yaw + data_track_.v_yaw * (now - data_track_.header.stamp).toSec();
bool solve_success =
bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, data_track_.yaw, data_track_.v_yaw,
bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, yaw, data_track_.v_yaw,
data_track_.radius_1, data_track_.radius_2, data_track_.dz, data_track_.armors_num);

if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time)
Expand Down Expand Up @@ -360,42 +364,18 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period)
quatToRPY(base_frame2des.transform.rotation, roll_des, pitch_des, yaw_des);

double yaw_vel_des = 0., pitch_vel_des = 0.;
double yaw_accel_des = 0., pitch_accel_des = 0.;
if (state_ == RATE)
{
yaw_vel_des = cmd_gimbal_.rate_yaw;
yaw_accel_des = cmd_gimbal_.accel_yaw;
pitch_vel_des = cmd_gimbal_.rate_pitch;
pitch_accel_des = cmd_gimbal_.accel_pitch;
}
else if (state_ == TRACK)
{
geometry_msgs::Point target_pos;
geometry_msgs::Vector3 target_vel;
bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, data_track_.position, data_track_.velocity,
data_track_.yaw, data_track_.v_yaw, data_track_.radius_1,
data_track_.radius_2, data_track_.dz, data_track_.armors_num);
tf2::Vector3 target_pos_tf, target_vel_tf;

try
{
geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform(
ctrl_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(ctrl_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);
}
catch (tf2::TransformException& ex)
{
ROS_WARN("%s", ex.what());
}
bullet_solver_->getYawVelAndAccelDes(yaw_vel_des, yaw_accel_des);
bullet_solver_->getPitchVelAndAccelDes(pitch_vel_des, pitch_accel_des);
}

ctrl_yaw_.setCommand(yaw_des, yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z);
Expand All @@ -410,8 +390,9 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period)
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 + resistance_compensation);
ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time) + pitch_k_v_ * pitch_vel_des);
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)
Expand Down

0 comments on commit 499ce1d

Please sign in to comment.