Implement realtime Ruckig jerk-limited smoothing

moveit_core/CMakeLists.txt

@@ -92,6 +92,8 @@ install(
           moveit_robot_model
           moveit_robot_state
           moveit_robot_trajectory
+          moveit_ruckig_filter
+          moveit_ruckig_filter_parameters
           moveit_smoothing_base
           moveit_test_utils
           moveit_trajectory_processing
@@ -142,9 +144,10 @@ pluginlib_export_plugin_description_file(moveit_core
                                          collision_detector_fcl_description.xml)
 pluginlib_export_plugin_description_file(
   moveit_core collision_detector_bullet_description.xml)
-pluginlib_export_plugin_description_file(moveit_core
-                                         filter_plugin_butterworth.xml)
 pluginlib_export_plugin_description_file(moveit_core
                                          filter_plugin_acceleration.xml)
+pluginlib_export_plugin_description_file(moveit_core
+                                         filter_plugin_butterworth.xml)
+pluginlib_export_plugin_description_file(moveit_core filter_plugin_ruckig.xml)
 
 ament_package(CONFIG_EXTRAS ConfigExtras.cmake)

moveit_core/filter_plugin_ruckig.xml

@@ -0,0 +1,8 @@
+<library path="moveit_ruckig_filter">
+    <class type="online_signal_smoothing::RuckigFilterPlugin"
+           base_class_type="online_signal_smoothing::SmoothingBaseClass">
+        <description>
+            Jerk/acceleration/velocity-limited command smoothing.
+        </description>
+    </class>
+</library>

moveit_core/online_signal_smoothing/CMakeLists.txt

@@ -13,17 +13,32 @@ set_target_properties(moveit_smoothing_base
 ament_target_dependencies(moveit_smoothing_base rclcpp tf2_eigen)
 
 # Plugin implementations
+add_library(moveit_acceleration_filter SHARED src/acceleration_filter.cpp)
+generate_export_header(moveit_acceleration_filter)
+target_include_directories(
+  moveit_acceleration_filter
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>)
+set_target_properties(moveit_acceleration_filter
+                      PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+generate_parameter_library(moveit_acceleration_filter_parameters
+                           src/acceleration_filter_parameters.yaml)
+target_link_libraries(
+  moveit_acceleration_filter moveit_acceleration_filter_parameters
+  moveit_robot_state moveit_smoothing_base osqp::osqp)
+ament_target_dependencies(
+  moveit_acceleration_filter srdfdom # include dependency from
+                                     # moveit_robot_model
+  pluginlib)
+
 add_library(moveit_butterworth_filter SHARED src/butterworth_filter.cpp)
 generate_export_header(moveit_butterworth_filter)
 target_include_directories(
   moveit_butterworth_filter
   PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>)
 set_target_properties(moveit_butterworth_filter
                       PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-
 generate_parameter_library(moveit_butterworth_filter_parameters
                            src/butterworth_parameters.yaml)
-
 target_link_libraries(
   moveit_butterworth_filter moveit_butterworth_filter_parameters
   moveit_robot_model moveit_smoothing_base)
@@ -32,32 +47,30 @@ ament_target_dependencies(
   srdfdom # include dependency from moveit_robot_model
   pluginlib)
 
-add_library(moveit_acceleration_filter SHARED src/acceleration_filter.cpp)
-generate_export_header(moveit_acceleration_filter)
+add_library(moveit_ruckig_filter SHARED src/ruckig_filter.cpp)
+generate_export_header(moveit_ruckig_filter)
 target_include_directories(
-  moveit_acceleration_filter
-  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>)
-set_target_properties(moveit_acceleration_filter
+  moveit_ruckig_filter PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>)
+set_target_properties(moveit_ruckig_filter
                       PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-
-generate_parameter_library(moveit_acceleration_filter_parameters
-                           src/acceleration_filter_parameters.yaml)
-
+generate_parameter_library(moveit_ruckig_filter_parameters
+                           src/ruckig_filter_parameters.yaml)
 target_link_libraries(
-  moveit_acceleration_filter moveit_acceleration_filter_parameters
-  moveit_robot_state moveit_smoothing_base osqp::osqp)
+  moveit_ruckig_filter moveit_robot_state moveit_ruckig_filter_parameters
+  moveit_smoothing_base ruckig::ruckig)
 ament_target_dependencies(
-  moveit_acceleration_filter srdfdom # include dependency from
-                                     # moveit_robot_model
+  moveit_ruckig_filter srdfdom # include dependency from moveit_robot_model
   pluginlib)
 
 # Installation
 install(DIRECTORY include/ DESTINATION include/moveit_core)
 install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_smoothing_base_export.h
         DESTINATION include/moveit_core)
+install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_acceleration_filter_export.h
+        DESTINATION include/moveit_core)
 install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_butterworth_filter_export.h
         DESTINATION include/moveit_core)
-install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_acceleration_filter_export.h
+install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_ruckig_filter_export.h
         DESTINATION include/moveit_core)
 
 # Testing

moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.h

@@ -0,0 +1,106 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, Andrew Zelenak
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Andy Zelenak
+Description: Applies jerk/acceleration/velocity limits to online motion commands
+ */
+
+#pragma once
+
+#include <cstddef>
+
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/online_signal_smoothing/smoothing_base_class.h>
+#include <moveit_ruckig_filter_parameters.hpp>
+
+#include <ruckig/ruckig.hpp>
+
+namespace online_signal_smoothing
+{
+
+class RuckigFilterPlugin : public SmoothingBaseClass
+{
+public:
+  /**
+   * Initialize the smoothing algorithm
+   * @param node ROS node, used for parameter retrieval
+   * @param robot_model used to retrieve vel/accel/jerk limits
+   * @param num_joints number of actuated joints in the JointGroup
+   * @return True if initialization was successful
+   */
+  bool initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model,
+                  size_t num_joints) override;
+
+  /**
+   * Smooth the command signals for all DOF
+   * @param positions array of joint position commands
+   * @param velocities array of joint velocity commands
+   * @param accelerations array of joint acceleration commands
+   * @return True if initialization was successful
+   */
+  bool doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) override;
+
+  /**
+   * Reset to a given joint state
+   * @param positions reset the filters to these joint positions
+   * @param velocities (unused)
+   * @param accelerations (unused)
+   * @return True if reset was successful
+   */
+  bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
+             const Eigen::VectorXd& accelerations) override;
+
+private:
+  /**
+   * A utility to print Ruckig's internal state
+   */
+  void printRuckigState();
+
+  /**
+   * A utility to get velocity/acceleration/jerk bounds from the robot model
+   * @return true if all bounds are defined
+   */
+  bool getVelAccelJerkBounds(std::vector<double>& joint_velocity_bounds, std::vector<double>& joint_acceleration_bounds,
+                             std::vector<double>& joint_jerk_bounds);
+
+  /** \brief Parameters loaded from yaml file at runtime */
+  online_signal_smoothing::Params params_;
+  /** \brief The robot model contains the vel/accel/jerk limits that Ruckig requires */
+  moveit::core::RobotModelConstPtr robot_model_;
+  bool have_initial_ruckig_output_ = false;
+  std::optional<ruckig::Ruckig<ruckig::DynamicDOFs>> ruckig_;
+  std::optional<ruckig::InputParameter<ruckig::DynamicDOFs>> ruckig_input_;
+  std::optional<ruckig::OutputParameter<ruckig::DynamicDOFs>> ruckig_output_;
+};
+}  // namespace online_signal_smoothing

moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h

@@ -89,6 +89,5 @@ class SmoothingBaseClass
    */
   virtual bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
                      const Eigen::VectorXd& accelerations) = 0;
-  ;
 };
 }  // namespace online_signal_smoothing

moveit_core/online_signal_smoothing/src/acceleration_filter.cpp

@@ -34,7 +34,6 @@
 
 #include <moveit/online_signal_smoothing/acceleration_filter.h>
 #include <rclcpp/logging.hpp>
-#include <Eigen/Sparse>
 
 // Disable -Wold-style-cast because all _THROTTLE macros trigger this
 #pragma GCC diagnostic ignored "-Wold-style-cast"
@@ -345,5 +344,4 @@ bool AccelerationLimitedPlugin::reset(const Eigen::VectorXd& positions, const Ei
 }  // namespace online_signal_smoothing
 
 #include <pluginlib/class_list_macros.hpp>
-
 PLUGINLIB_EXPORT_CLASS(online_signal_smoothing::AccelerationLimitedPlugin, online_signal_smoothing::SmoothingBaseClass)