Question about smoother controller change in Rotation Shim #4828
Description
Feature request
Feature description
When changing controller the last velocity published by Shim could be different, from what the child controller can handle.
For example: Shim is rotating with 1 [rad/s], but under it a MPPI controller's wz_max is smaller. In a use case someone might want to rotate faster while having information about that the robot is stationary (at the start of the path for example) and a hand the control to the inner controller with velocity that it has been tuned to.
Implementation considerations
So an idea is to make a linear interpolation between the maximal possible angular velocity (calculated from acc limit) and the target velocity (upper bound provided by the child controller) when the angular distance from goal getting near to the angular threshold.
I thought about an implementation (so its just a doodle at this point), wanted to ask for feedback on whether is this a good way of doing it?
The base idea is before clamping the velocity calculate a linearly extrapolated relative velocity.
My handwriting might be totally worthless but I am happy to explain.
Short code change (not tested) just for an idea.
diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp
index 28fdc37a..431fec37 100644
--- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp
+++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp
@@ -54,6 +54,11 @@ void RotationShimController::configure(
node, plugin_name_ + ".angular_dist_threshold", rclcpp::ParameterValue(0.785)); // 45 deg
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".angular_disengage_threshold", rclcpp::ParameterValue(0.785));
+ // stop before giving control to primary controller
+ nav2_util::declare_parameter_if_not_declared(
+ node, plugin_name_ + ".child_controller_max_ang_vel", rclcpp::ParameterValue(0.0));
+ nav2_util::declare_parameter_if_not_declared(
+ node, plugin_name_ + ".regulate_ang_vel_threshold", rclcpp::ParameterValue(0.175)); // 10 deg
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".forward_sampling_distance", rclcpp::ParameterValue(0.5));
nav2_util::declare_parameter_if_not_declared(
@@ -69,6 +74,9 @@ void RotationShimController::configure(
node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_);
node->get_parameter(plugin_name_ + ".angular_disengage_threshold", angular_disengage_threshold_);
+ node->get_parameter(
+ plugin_name_ + ".child_controller_max_ang_vel", child_controller_max_ang_vel_);
+ node->get_parameter(plugin_name_ + ".regulate_ang_vel_threshold", regulate_ang_vel_threshold_);
node->get_parameter(plugin_name_ + ".forward_sampling_distance", forward_sampling_distance_);
node->get_parameter(
plugin_name_ + ".rotate_to_heading_angular_vel",
@@ -279,11 +287,20 @@ RotationShimController::computeRotateToHeadingCommand(
const geometry_msgs::msg::PoseStamped & pose,
const geometry_msgs::msg::Twist & velocity)
{
+ const double & dt = control_duration_;
+
geometry_msgs::msg::TwistStamped cmd_vel;
cmd_vel.header = pose.header;
const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0;
- const double angular_vel = sign * rotate_to_heading_angular_vel_;
- const double & dt = control_duration_;
+ const double max_rel_angular_vel = max_angular_accel_ * dt;
+ const double tmp_t = angular_disengage_threshold_;
+ // child controller max ang vel should be smaller
+ // tmp_t should be this or angular_dist_threshold?
+ const double rel_angular_vel = (max_rel_angular_vel - child_controller_max_ang_vel_) /
+ regulate_ang_vel_threshold_ * (angular_distance_to_heading - tmp_t) +
+ child_controller_max_ang_vel_;
+
+ const double angular_vel = sign * rel_angular_vel;
const double min_feasible_angular_speed = velocity.angular.z - max_angular_accel_ * dt;
const double max_feasible_angular_speed = velocity.angular.z + max_angular_accel_ * dt;
cmd_vel.twist.angular.z =