Skip to content

Added velocity dependent omega accel limits #1

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

Open
ipa-dmo wants to merge 1 commit into
base: kilted_vel_smoother_true
Choose a base branch
from
Open

Added velocity dependent omega accel limits #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
10 changes: 10 additions & 0 deletions nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,6 +125,13 @@ class VelocitySmoother : public nav2_util::LifecycleNode
*/
void smootherTimer();

/**
* @brief Get omega acceleration limit based on current linear velocity
* @param lin_vel current linear velocity
* @return omega acceleration limit
*/
double getCurrentOmegaAccelLimit(const double lin_vel);

/**
* @brief Dynamic reconfigure callback
* @param parameters Parameter list to change
Expand Down Expand Up @@ -157,6 +164,9 @@ class VelocitySmoother : public nav2_util::LifecycleNode
rclcpp::Duration velocity_timeout_{0, 0};
rclcpp::Time last_command_time_;

double d_theta_max_;
double wheel_base_;

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
};

Expand Down
27 changes: 25 additions & 2 deletions nav2_velocity_smoother/src/velocity_smoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -120,6 +120,12 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state)
node->get_parameter("velocity_timeout", velocity_timeout_dbl);
velocity_timeout_ = rclcpp::Duration::from_seconds(velocity_timeout_dbl);

// Extra parameter for variable omega acceleration limits
declare_parameter_if_not_declared(node, "d_theta_max", rclcpp::ParameterValue(0.2));
declare_parameter_if_not_declared(node, "wheel_base", rclcpp::ParameterValue(1.87));
node->get_parameter("d_theta_max", d_theta_max_);
node->get_parameter("wheel_base", wheel_base_);

if (max_velocities_.size() != 3 || min_velocities_.size() != 3 ||
max_accels_.size() != 3 || max_decels_.size() != 3 || deadband_velocities_.size() != 3)
{
Expand Down Expand Up @@ -248,6 +254,23 @@ void VelocitySmoother::inputCommandCallback(
inputCommandStampedCallback(twist_stamped);
}

double VelocitySmoother::getCurrentOmegaAccelLimit(const double lin_vel)
{
// Impact of the current theta is not that big in our range [-23°, 23°] (cos(theta) in range [1, 0.92])
// Since we do not have that info currently we use a medium value of 16° (cos(theta)=0.96)
double current_theta = 0.28;
// The dependency on the velocity is more significant

// Einspurmodell
return d_theta_max_ * lin_vel / ( std::pow(std::cos(current_theta), 2) * wheel_base_ );

// Zweispurmodell?
// d_omega_max_outside = theta_max_ * lin_vel * wheel_base_ / ( std::pow(std::cos(current_theta),2) * std::pow((wheel_base_ + track_width_/2 * std::tan(current_theta)), 2) );
// d_omega_max_inside = theta_max_ * lin_vel * wheel_base_ / ( std::pow(std::cos(current_theta),2) * (wheel_base_ - track_width_/2 * std::tan(current_theta)), 2) );
// return std::min(d_omega_max_outside, d_omega_max_inside);
}


double VelocitySmoother::findEtaConstraint(
const double v_curr, const double v_cmd, const double accel, const double decel)
{
Expand Down Expand Up @@ -383,7 +406,7 @@ void VelocitySmoother::smootherTimer()
}

curr_eta = findEtaConstraint(
current_.twist.angular.z, command_->twist.angular.z, max_accels_[2], max_decels_[2]);
current_.twist.angular.z, command_->twist.angular.z, getCurrentOmegaAccelLimit(current_.twist.linear.x), -getCurrentOmegaAccelLimit(current_.twist.linear.x));
if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) {
eta = curr_eta;
}
Expand All @@ -394,7 +417,7 @@ void VelocitySmoother::smootherTimer()
cmd_vel->twist.linear.y = applyConstraints(
current_.twist.linear.y, command_->twist.linear.y, max_accels_[1], max_decels_[1], eta);
cmd_vel->twist.angular.z = applyConstraints(
current_.twist.angular.z, command_->twist.angular.z, max_accels_[2], max_decels_[2], eta);
current_.twist.angular.z, command_->twist.angular.z, getCurrentOmegaAccelLimit(current_.twist.linear.x), -getCurrentOmegaAccelLimit(current_.twist.linear.x), eta);
last_cmd_ = *cmd_vel;

// Apply deadband restrictions & publish
Expand Down
Loading