Acceleration Limited Smoothing Plugin for Servo

moveit_core/CMakeLists.txt

@@ -20,6 +20,7 @@ find_package(kdl_parser REQUIRED)
 find_package(moveit_msgs REQUIRED)
 find_package(OCTOMAP REQUIRED)
 find_package(octomap_msgs REQUIRED)
+find_package(osqp REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(random_numbers REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -69,8 +70,10 @@ add_subdirectory(version)
 install(
   TARGETS
     collision_detector_bullet_plugin
+    moveit_acceleration_filter
+    moveit_acceleration_filter_parameters
     moveit_butterworth_filter
-    moveit_butterworth_parameters
+    moveit_butterworth_filter_parameters
     moveit_collision_detection
     moveit_collision_detection_bullet
     moveit_collision_detection_fcl
@@ -115,6 +118,7 @@ ament_export_dependencies(
   moveit_msgs
   OCTOMAP
   octomap_msgs
+  osqp
   pluginlib
   random_numbers
   rclcpp
@@ -138,6 +142,7 @@ ament_export_dependencies(
 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)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

moveit_core/filter_plugin_acceleration.xml

@@ -0,0 +1,8 @@
+<library path="moveit_acceleration_filter">
+    <class type="online_signal_smoothing::AccelerationLimitedPlugin"
+           base_class_type="online_signal_smoothing::SmoothingBaseClass">
+        <description>
+            Limits acceleration of commands to generate smooth motion.
+        </description>
+    </class>
+</library>

moveit_core/online_signal_smoothing/CMakeLists.txt

@@ -33,10 +33,10 @@ set_target_properties(moveit_butterworth_filter PROPERTIES VERSION
   "${${PROJECT_NAME}_VERSION}"
 )
 
-generate_parameter_library(moveit_butterworth_parameters src/butterworth_parameters.yaml)
+generate_parameter_library(moveit_butterworth_filter_parameters src/butterworth_parameters.yaml)
 
 target_link_libraries(moveit_butterworth_filter
-  moveit_butterworth_parameters
+  moveit_butterworth_filter_parameters
   moveit_robot_model
   moveit_smoothing_base
 )
@@ -45,18 +45,48 @@ ament_target_dependencies(moveit_butterworth_filter
   pluginlib
 )
 
+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
+)
+
+
 # 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_butterworth_filter_export.h DESTINATION include/moveit_core)
+install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_acceleration_filter_export.h DESTINATION include/moveit_core)
 
 # Testing
 
-if(BUILD_TESTING)
+if (BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   find_package(ament_cmake_gtest REQUIRED)
 
   # Lowpass filter unit test
   ament_add_gtest(test_butterworth_filter test/test_butterworth_filter.cpp)
   target_link_libraries(test_butterworth_filter moveit_butterworth_filter)
-endif()
+
+  # Acceleration filter unit test
+  ament_add_gtest(test_acceleration_filter test/test_acceleration_filter.cpp)
+  target_link_libraries(test_acceleration_filter moveit_acceleration_filter moveit_test_utils)
+endif ()

moveit_core/online_signal_smoothing/README.md

@@ -0,0 +1,12 @@
+### AccelerationLimitedPlugin
+Applies smoothing by limiting the acceleration between consecutive commands.
+The purpose of the plugin is to prevent the robot's acceleration limits from being violated by instantaneous changes
+to the servo command topics.
+
+There are three cases considered for acceleration-limiting illustrated in the following figures:
+![acceleration_limiting_diagram.png](res/acceleration_limiting_diagram.png)
+In the figures, the red arrows show the displacement that will occur given the current velocity. The blue line shows the displacement between the current position and the desired position. The black dashed line shows the maximum possible displacements that are within the acceleration limits. The green line shows the acceleration commands that will be used by this acceleration-limiting plugin.
+
+- Figure A: The desired position is within the acceleration limits. The next commanded point will be exactly the desired point.
+- Figure B: The line between the current position and the desired position intersects the acceleration limits, but the reference position is not within the bounds. The next commanded point will be the point on the displacement line that is closest to the reference.
+- Figure C: Neither the displacement line intersects the acceleration limits nor does the reference point lie within the limits. In this case, the next commanded point will be the one that minimizes the robot's velocity while maintaining its direction.

moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h

@@ -0,0 +1,162 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, PickNik Inc.
+ *  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: Paul Gesel
+Description: applies smoothing by limiting the acceleration between consecutive commands.
+The purpose of the plugin is to prevent the robot's acceleration limits from being violated by instantaneous changes
+to the servo command topics.
+
+ In the diagrams below, the c-v lines show the displacement that will occur given the current velocity. The t-c lines
+ shows the displacement between the current position and the desired position. The dashed lines shows the maximum
+ possible displacements that are within the acceleration limits. The v-t lines shows the acceleration commands that
+ will be used by this acceleration-limiting plugin. The x point shows the position that will be used for each scenario.
+
+Scenario A: The desired position is within the acceleration limits. The next commanded point will be exactly the
+desired point.
+     ________
+    |        |
+c --|-----xt |
+ \__|__ v    |
+    |________|
+
+Scenario B: The line between the current position and the desired position intersects the acceleration limits, but the
+reference position is not within the bounds. The next commanded point will be the point on the displacement line that
+is closest to the reference.
+     ________
+    |        |
+c --|--------x------t
+ \__|__ v    |
+    |________|
+
+Scenario C: Neither the displacement line intersects the acceleration limits nor does the reference point lie within
+the limits. In this case, the next commanded point will be the one that minimizes the robot's velocity while
+maintaining its direction.
+           ________
+          |        |
+c --------x--- v   |
+ \        |        |
+  \       |________|
+   t
+ */
+
+#pragma once
+
+#include <cstddef>
+
+#include <moveit/online_signal_smoothing/smoothing_base_class.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_state/robot_state.h>
+#include <moveit/utils/logger.hpp>
+#include <moveit_acceleration_filter_parameters.hpp>
+
+#include <osqp.h>
+#include <types.h>
+#include <Eigen/Sparse>
+
+namespace online_signal_smoothing
+{
+MOVEIT_STRUCT_FORWARD(OSQPDataWrapper);
+
+// Plugin
+class AccelerationLimitedPlugin : public SmoothingBaseClass
+{
+public:
+  /**
+   * Initialize the acceleration based smoothing plugin
+   * @param node ROS node, used for parameter retrieval
+   * @param robot_model typically used to retrieve vel/accel/jerk limits
+   * @param num_joints number of actuated joints in the JointGroup Servo controls
+   * @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. This function limits the change in velocity using the acceleration
+   * specified in the robot model.
+   * @param positions array of joint position commands
+   * @param velocities array of joint velocity commands
+   * @param accelerations (unused)
+   * @return True if smoothing was successful
+   */
+  bool doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) override;
+
+  /**
+   * Reset to a given joint state. This method must be called before doSmoothing.
+   * @param positions reset the filters to the joint positions
+   * @param velocities reset the filters to the joint velocities
+   * @param accelerations (unused)
+   * @return True if reset was successful
+   */
+  bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
+             const Eigen::VectorXd& accelerations) override;
+
+  /**
+   * memory allocated by osqp is freed in destructor
+   */
+  ~AccelerationLimitedPlugin()
+  {
+    if (osqp_workspace_ != nullptr)
+    {
+      osqp_cleanup(osqp_workspace_);
+    }
+  }
+
+private:
+  /** \brief Pointer to rclcpp node handle.  */
+  rclcpp::Node::SharedPtr node_;
+  /** \brief Parameters for plugin.  */
+  online_signal_smoothing::Params params_;
+  /** \brief The number of joints in the robot's planning group.  */
+  size_t num_joints_;
+  /** \brief Last velocities and positions received */
+  Eigen::VectorXd last_velocities_;
+  Eigen::VectorXd last_positions_;
+  /** \brief Intermediate variables used in calculations */
+  Eigen::VectorXd cur_acceleration_;
+  Eigen::VectorXd positions_offset_;
+  Eigen::VectorXd velocities_offset_;
+  /** \brief Extracted joint limits from robot model */
+  Eigen::VectorXd max_acceleration_limits_;
+  Eigen::VectorXd min_acceleration_limits_;
+  /** \brief Pointer to robot model */
+  moveit::core::RobotModelConstPtr robot_model_;
+  /** \brief Constraint matrix for optimization problem */
+  Eigen::SparseMatrix<double> A_sparse_;
+  /** \brief osqp types used for optimization problem */
+  OSQPDataWrapperPtr osqp_data_;
+  OSQPWorkspace* osqp_workspace_ = nullptr;
+  OSQPSettings osqp_settings_;
+};
+}  // namespace online_signal_smoothing

moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h

@@ -41,8 +41,7 @@
 
 #include <cstddef>
 
-// Auto-generated
-#include <moveit_butterworth_parameters.hpp>
+#include <moveit_butterworth_filter_parameters.hpp>
 #include <moveit/robot_model/robot_model.h>
 #include <moveit/online_signal_smoothing/smoothing_base_class.h>