Added velocity limiting to the mecanum controller.

mecanum_drive_controller/CMakeLists.txt

@@ -8,6 +8,7 @@ export_windows_symbols()
 # find dependencies
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   control_msgs
+  control_toolbox
   controller_interface
   hardware_interface
   generate_parameter_library
@@ -27,8 +28,11 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()
 
+# get include dirs from control_toolbox for the custom validators
+get_target_property(TB_INCLUDE_DIRS control_toolbox::rate_limiter_parameters INTERFACE_INCLUDE_DIRECTORIES)
 generate_parameter_library(mecanum_drive_controller_parameters
   src/mecanum_drive_controller.yaml
+  ${TB_INCLUDE_DIRS}/control_toolbox/custom_validators.hpp
 )
 
 add_library(
@@ -43,6 +47,8 @@ target_include_directories(mecanum_drive_controller PUBLIC
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(mecanum_drive_controller PUBLIC
                       mecanum_drive_controller_parameters
+                      control_toolbox::rate_limiter_parameters
+                      control_toolbox::control_toolbox
                       controller_interface::controller_interface
                       hardware_interface::hardware_interface
                       pluginlib::pluginlib

mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp

@@ -26,6 +26,7 @@
 
 #include "control_msgs/msg/mecanum_drive_controller_state.hpp"
 #include "control_msgs/srv/set_odometry.hpp"
+#include "control_toolbox/rate_limiter.hpp"
 #include "controller_interface/chainable_controller_interface.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
@@ -158,6 +159,13 @@ class MecanumDriveController : public controller_interface::ChainableControllerI
   double velocity_in_center_frame_linear_x_;   // [m/s]
   double velocity_in_center_frame_linear_y_;   // [m/s]
   double velocity_in_center_frame_angular_z_;  // [rad/s]
+
+  // Speed limiters
+  std::unique_ptr<control_toolbox::RateLimiter<double>> limiter_linear_x_;
+  std::unique_ptr<control_toolbox::RateLimiter<double>> limiter_linear_y_;
+  std::unique_ptr<control_toolbox::RateLimiter<double>> limiter_angular_z_;
+  // Previous two commands for jerk limiting: queue of [linear_x, linear_y, angular_z]
+  std::queue<std::array<double, 3>> previous_two_commands_;
 };
 
 }  // namespace mecanum_drive_controller

mecanum_drive_controller/package.xml

@@ -27,6 +27,7 @@
 
   <depend>backward_ros</depend>
   <depend>control_msgs</depend>
+  <depend>control_toolbox</depend>
   <depend>controller_interface</depend>
   <depend>geometry_msgs</depend>
   <depend>hardware_interface</depend>

mecanum_drive_controller/src/mecanum_drive_controller.cpp

@@ -267,6 +267,55 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(
 
   RCLCPP_INFO(get_node()->get_logger(), "MecanumDriveController configured successfully");
 
+  // Configure speed limiters
+  try
+  {
+    limiter_linear_x_ = std::make_unique<control_toolbox::RateLimiter<double>>(
+      params_.linear.x.min_velocity, params_.linear.x.max_velocity,
+      params_.linear.x.max_acceleration_reverse, params_.linear.x.max_acceleration,
+      params_.linear.x.max_deceleration, params_.linear.x.max_deceleration_reverse,
+      params_.linear.x.min_jerk, params_.linear.x.max_jerk);
+  }
+  catch (const std::invalid_argument & e)
+  {
+    RCLCPP_ERROR(
+      get_node()->get_logger(), "Failed to configure linear x speed limiter: %s", e.what());
+    return controller_interface::CallbackReturn::ERROR;
+  }
+  try
+  {
+    limiter_linear_y_ = std::make_unique<control_toolbox::RateLimiter<double>>(
+      params_.linear.y.min_velocity, params_.linear.y.max_velocity,
+      params_.linear.y.max_acceleration_reverse, params_.linear.y.max_acceleration,
+      params_.linear.y.max_deceleration, params_.linear.y.max_deceleration_reverse,
+      params_.linear.y.min_jerk, params_.linear.y.max_jerk);
+  }
+  catch (const std::invalid_argument & e)
+  {
+    RCLCPP_ERROR(
+      get_node()->get_logger(), "Failed to configure linear y speed limiter: %s", e.what());
+    return controller_interface::CallbackReturn::ERROR;
+  }
+  try
+  {
+    limiter_angular_z_ = std::make_unique<control_toolbox::RateLimiter<double>>(
+      params_.angular.z.min_velocity, params_.angular.z.max_velocity,
+      params_.angular.z.max_acceleration_reverse, params_.angular.z.max_acceleration,
+      params_.angular.z.max_deceleration, params_.angular.z.max_deceleration_reverse,
+      params_.angular.z.min_jerk, params_.angular.z.max_jerk);
+  }
+  catch (const std::invalid_argument & e)
+  {
+    RCLCPP_ERROR(
+      get_node()->get_logger(), "Failed to configure angular z speed limiter: %s", e.what());
+    return controller_interface::CallbackReturn::ERROR;
+  }
+
+  // Initialize previous commands queue for jerk limiting
+  previous_two_commands_ = std::queue<std::array<double, 3>>();
+  previous_two_commands_.push({{0.0, 0.0, 0.0}});
+  previous_two_commands_.push({{0.0, 0.0, 0.0}});
+
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
@@ -498,6 +547,28 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma
     !std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1]) &&
     !std::isnan(reference_interfaces_[2]))
   {
+    // Apply speed limits before inverse kinematics
+    double linear_x_command = reference_interfaces_[0];
+    double linear_y_command = reference_interfaces_[1];
+    double angular_z_command = reference_interfaces_[2];
+
+    double & last_linear_x = previous_two_commands_.back()[0];
+    double & second_to_last_linear_x = previous_two_commands_.front()[0];
+    double & last_linear_y = previous_two_commands_.back()[1];
+    double & second_to_last_linear_y = previous_two_commands_.front()[1];
+    double & last_angular_z = previous_two_commands_.back()[2];
+    double & second_to_last_angular_z = previous_two_commands_.front()[2];
+
+    limiter_linear_x_->limit(
+      linear_x_command, last_linear_x, second_to_last_linear_x, period.seconds());
+    limiter_linear_y_->limit(
+      linear_y_command, last_linear_y, second_to_last_linear_y, period.seconds());
+    limiter_angular_z_->limit(
+      angular_z_command, last_angular_z, second_to_last_angular_z, period.seconds());
+
+    previous_two_commands_.pop();
+    previous_two_commands_.push({{linear_x_command, linear_y_command, angular_z_command}});
+
     tf2::Quaternion quaternion;
     quaternion.setRPY(0.0, 0.0, params_.kinematics.base_frame_offset.theta);
     /// \note The variables meaning:
@@ -508,18 +579,15 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma
 
     tf2::Matrix3x3 rotation_from_base_to_center = tf2::Matrix3x3((quaternion));
     tf2::Vector3 velocity_in_base_frame_w_r_t_center_frame_ =
-      rotation_from_base_to_center *
-      tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0);
+      rotation_from_base_to_center * tf2::Vector3(linear_x_command, linear_y_command, 0.0);
     tf2::Vector3 linear_trans_from_base_to_center = tf2::Vector3(
       params_.kinematics.base_frame_offset.x, params_.kinematics.base_frame_offset.y, 0.0);
 
-    velocity_in_center_frame_linear_x_ =
-      velocity_in_base_frame_w_r_t_center_frame_.x() +
-      linear_trans_from_base_to_center.y() * reference_interfaces_[2];
-    velocity_in_center_frame_linear_y_ =
-      velocity_in_base_frame_w_r_t_center_frame_.y() -
-      linear_trans_from_base_to_center.x() * reference_interfaces_[2];
-    velocity_in_center_frame_angular_z_ = reference_interfaces_[2];
+    velocity_in_center_frame_linear_x_ = velocity_in_base_frame_w_r_t_center_frame_.x() +
+                                         linear_trans_from_base_to_center.y() * angular_z_command;
+    velocity_in_center_frame_linear_y_ = velocity_in_base_frame_w_r_t_center_frame_.y() -
+                                         linear_trans_from_base_to_center.x() * angular_z_command;
+    velocity_in_center_frame_angular_z_ = angular_z_command;
 
     const double wheel_front_left_vel =
       1.0 / params_.kinematics.wheels_radius *

mecanum_drive_controller/src/mecanum_drive_controller.yaml

@@ -155,3 +155,189 @@ mecanum_drive_controller:
     description: "Diagonal values of pose covariance matrix.",
     read_only: false,
   }
+
+  linear:
+    x:
+      max_velocity: {
+        type: double,
+        default_value: .NAN,
+        validation: {
+          "control_filters::gt_eq_or_nan<>": [0.0]
+        }
+      }
+      min_velocity: {
+        type: double,
+        default_value: .NAN,
+        validation: {
+          "control_filters::lt_eq_or_nan<>": [0.0]
+        }
+      }
+      max_acceleration: {
+        type: double,
+        default_value: .NAN,
+        description: "Maximum acceleration in forward direction.",
+        validation: {
+          "control_filters::gt_eq_or_nan<>": [0.0]
+        }
+      }
+      max_deceleration: {
+        type: double,
+        default_value: .NAN,
+        description: "Maximum deceleration in forward direction.",
+        validation: {
+          "control_filters::lt_eq_or_nan<>": [0.0]
+        }
+      }
+      max_acceleration_reverse: {
+        type: double,
+        default_value: .NAN,
+        description: "Maximum acceleration in reverse direction. If not set, -max_acceleration will be used.",
+        validation: {
+          "control_filters::lt_eq_or_nan<>": [0.0]
+        }
+      }
+      max_deceleration_reverse: {
+        type: double,
+        default_value: .NAN,
+        description: "Maximum deceleration in reverse direction. If not set, -max_deceleration will be used.",
+        validation: {
+          "control_filters::gt_eq_or_nan<>": [0.0]
+        }
+      }
+      max_jerk: {
+        type: double,
+        default_value: .NAN,
+        validation: {
+          "control_filters::gt_eq_or_nan<>": [0.0]
+        }
+      }
+      min_jerk: {
+        type: double,
+        default_value: .NAN,
+        validation: {
+          "control_filters::lt_eq_or_nan<>": [0.0]
+        }
+      }
+    y:
+      max_velocity: {
+        type: double,
+        default_value: .NAN,
+        validation: {
+          "control_filters::gt_eq_or_nan<>": [0.0]
+        }
+      }
+      min_velocity: {
+        type: double,
+        default_value: .NAN,
+        validation: {
+          "control_filters::lt_eq_or_nan<>": [0.0]
+        }
+      }
+      max_acceleration: {
+        type: double,
+        default_value: .NAN,
+        description: "Maximum acceleration in forward direction.",
+        validation: {
+          "control_filters::gt_eq_or_nan<>": [0.0]
+        }
+      }
+      max_deceleration: {
+        type: double,
+        default_value: .NAN,
+        description: "Maximum deceleration in forward direction.",
+        validation: {
+          "control_filters::lt_eq_or_nan<>": [0.0]
+        }
+      }
+      max_acceleration_reverse: {
+        type: double,
+        default_value: .NAN,
+        description: "Maximum acceleration in reverse direction. If not set, -max_acceleration will be used.",
+        validation: {
+          "control_filters::lt_eq_or_nan<>": [0.0]
+        }
+      }
+      max_deceleration_reverse: {
+        type: double,
+        default_value: .NAN,
+        description: "Maximum deceleration in reverse direction. If not set, -max_deceleration will be used.",
+        validation: {
+          "control_filters::gt_eq_or_nan<>": [0.0]
+        }
+      }
+      max_jerk: {
+        type: double,
+        default_value: .NAN,
+        validation: {
+          "control_filters::gt_eq_or_nan<>": [0.0]
+        }
+      }
+      min_jerk: {
+        type: double,
+        default_value: .NAN,
+        validation: {
+          "control_filters::lt_eq_or_nan<>": [0.0]
+        }
+      }
+  angular:
+    z:
+      max_velocity: {
+        type: double,
+        default_value: .NAN,
+        validation: {
+          "control_filters::gt_eq_or_nan<>": [0.0]
+        }
+      }
+      min_velocity: {
+        type: double,
+        default_value: .NAN,
+        validation: {
+          "control_filters::lt_eq_or_nan<>": [0.0]
+        }
+      }
+      max_acceleration: {
+        type: double,
+        default_value: .NAN,
+        description: "Maximum acceleration in positive direction.",
+        validation: {
+          "control_filters::gt_eq_or_nan<>": [0.0]
+        }
+      }
+      max_deceleration: {
+        type: double,
+        default_value: .NAN,
+        description: "Maximum deceleration in positive direction.",
+        validation: {
+          "control_filters::lt_eq_or_nan<>": [0.0]
+        }
+      }
+      max_acceleration_reverse: {
+        type: double,
+        default_value: .NAN,
+        description: "Maximum acceleration in negative direction. If not set, -max_acceleration will be used.",
+        validation: {
+          "control_filters::lt_eq_or_nan<>": [0.0]
+        }
+      }
+      max_deceleration_reverse: {
+        type: double,
+        default_value: .NAN,
+        description: "Maximum deceleration in negative direction. If not set, -max_deceleration will be used.",
+        validation: {
+          "control_filters::gt_eq_or_nan<>": [0.0]
+        }
+      }
+      max_jerk: {
+        type: double,
+        default_value: .NAN,
+        validation: {
+          "control_filters::gt_eq_or_nan<>": [0.0]
+        }
+      }
+      min_jerk: {
+        type: double,
+        default_value: .NAN,
+        validation: {
+          "control_filters::lt_eq_or_nan<>": [0.0]
+        }
+      }