Question about smoother controller change in Rotation Shim

[turtlewizard73]

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 =

[SteveMacenski]

First, what branch are you using? We added a feature in main that might be useful to you to change the rules for disengagement of the rotation fc7e086

Second, I don't totally understand the idea, but we could have it set to slow down the rotation as its approaching the target heading as if it were going to stop at the target heading (see: spin behavior). We have the disengagement threshold in main (or the threshold in general in other branches) that would pass it off to the controller before we actually stop-stop, so the deceleration would occur based on the distance remaining and the maximum acceleration allowed until that pass off is handled.

The 2 edge cases we'd need to capture are:

• If the threshold is 0.0, so that if we stop the robot at the end of the rotation, we pass off to the child controller. This actually sounds like a really, really useful behavior to have. Imagine having a rigid straight line and turn path like those QR code robots.
• The user would need to make sure that the maximum angular velocity at the disengagement threshold while decelerating is less than or equal to the controller's maximum angular velocity, as you mention. That is an easy kinematic calculation however and from my back of the envelope calculations, the majority of realistic accelerations and ratios spinning to controlling angular velocities work out fine!

[turtlewizard73]

Hi @SteveMacenski !
I am using humble but checked rolling also, I have read the issues and pr about the mentioned disengagement threshold, but thats not what i am looking for. My understanding is that it is an extra constraint in [rad] dimension. I am looking for a constraint in velocity [rad/s] dimension when the angle of robot approaches the controller switch threshold.

I checked out the spin behavior, my thoughts/understanding about that:

• the velocity is calculated from the the remaining angle difference, which is good in a way that is slowes down as the robot gets closer
• but does not ensure a compatible velocity for the child controller in all cases (depending on rotational_acc_lim_)
• implementing the same logic in shim controller could be a short path of doing it but users had to calculate their own rotational_acc_lim_ so it gives the maximum velocity controllable by child at threshold (i think thats what you mean in your second bullet point)
• which in therefore limits the maximum angular acceleration in rotation when remaining_yaw> disengage threshold

In terms of points in a coordinate system (x,y) -> [angle difference from target angle, angular velocity], my proposition is to push the spin velocity's equation to point [child controller max angular velocity, yaw threshold] from origo.
Instead of:
double vel = sqrt(2 * rotational_acc_lim_ * remaining_yaw);
use:
double vel = child_controller_max_ang_vel + sqrt(2 * rotational_acc_lim_ * (remaining_yaw-angular_thresh));

• child_controller_max_ang_vel == 0 then it passes the control when velocity is zero
• while rotating with shim the rotational_acc_lim_ can be as big as one wants it to be because it will always go to the child_controller_max_ang_vel

I really hope it makes sense.

[SteveMacenski]

but does not ensure a compatible velocity for the child controller in all cases (depending on rotational_acc_lim_)

You are correct - I'm providing a suggestion for a different way of dealing with the problem that I think is actually more natural in terms of the behavior it provides as the output for both the situation you bring up as well as the general dynamic feasibility of the rotation shim controller passing off to the child controller.

If you do the back of the envelope math for your ranges of angular velocity maximums / accelerations, I think you'll see that this works well for good values of disengagement threshold sizes and dynamic constraints. Yes, you may need to calculate to make sure for a particular situation you don't go past that limit, but you'd have to do something adjacent to tune the value of regulate_ang_vel_threshold (which TBH is pretty unintuitive in naming / meaning to require quite a bit of documentation explanations).

Also, even if we add in the change for the differences in rotational velocities of the parent and child controllers, I still think it would be wise to have these dynamic limits in place to slow down on approach to the target orientation. I think this largely knocks out both in one contribution (with a small amount of math).

---

Additionally but separately, I think this also really solves what you might want anyway:

• Use rotation shim controller to rotate from a start heading to some path heading
• Accelerate by angular dynamic limits to full speed, but decelerate on approach to the path heading
• This makes sure you start control with MPPI without going full-bore rotating in some direction, which would be a little wonky to get out from
• Nicely too, if we do this, then you could set the angular threshold for disengagement to be very small, say 15 degrees. That would virtually have your robot rotate in place and come to a near-stop at the path heading before passing off to the MPPI controller. That seems like a pretty slick and ideal move both from a starting position of the child controller but also just from a human intuition level of someone watching the the robot. It would rotate in place, slow to a virtual stop and then start moving along its path.

Thus, this one change would deal with the issue you bring up, decoupling the rotational velocity from the controller's dynamic settings, while also (a) improving the starting conditions given to the controller and (b) being more intuitive.

I'd be really interested in that actually being the default, out of the box behavior of the rotation shim controller, by tightening up the disengagement threshold after the deceleration is added. I think that's mostly what people would want anyway.