Tune Servo params so it does not get stuck so easily

AndyZe · AndyZe · commit c2935dca5fda · 2024-08-08T10:16:03.000-05:00
diff --git a/moveit_core/CMakeLists.txt b/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)
diff --git a/moveit_core/filter_plugin_ruckig.xml b/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>
diff --git a/moveit_core/online_signal_smoothing/CMakeLists.txt b/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
diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.h
@@ -0,0 +1,102 @@
+/*********************************************************************
+ * 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:
+  rclcpp::Node::SharedPtr node_;
+  void getDefaultJointVelAccelBounds();
+
+  online_signal_smoothing::Params params_;
+
+  size_t num_joints_;
+  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_;
+
+  std::vector<double> joint_velocity_bounds_;
+  std::vector<double> joint_acceleration_bounds_;
+  std::vector<double> joint_jerk_bounds_;
+};
+}  // namespace online_signal_smoothing
diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h b/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
diff --git a/moveit_core/online_signal_smoothing/src/acceleration_filter.cpp b/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)
diff --git a/moveit_core/online_signal_smoothing/src/ruckig_filter.cpp b/moveit_core/online_signal_smoothing/src/ruckig_filter.cpp
@@ -0,0 +1,133 @@
+/*********************************************************************
+ * 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.
+ *********************************************************************/
+
+#include <moveit/online_signal_smoothing/ruckig_filter.h>
+#include <rclcpp/clock.hpp>
+#include <rclcpp/logging.hpp>
+
+// Disable -Wold-style-cast because all _THROTTLE macros trigger this
+#pragma GCC diagnostic ignored "-Wold-style-cast"
+
+namespace online_signal_smoothing
+{
+namespace
+{
+rclcpp::Logger getLogger()
+{
+  return moveit::getLogger("moveit.core.ruckig_filter_plugin");
+}
+inline constexpr double DEFAULT_JERK_BOUND = 300;  // rad/s^3
+}  // namespace
+
+bool RuckigFilterPlugin::initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model,
+                                    size_t num_joints)
+{
+  node_ = node;
+  num_joints_ = num_joints;
+  robot_model_ = robot_model;
+
+  // get node parameters and store in member variables
+  auto param_listener = online_signal_smoothing::ParamListener(node_);
+  params_ = param_listener.get_params();
+
+  // Ruckig needs the joint vel/accel/jerk bounds.
+  getDefaultJointVelAccelBounds();
+  ruckig::InputParameter<ruckig::DynamicDOFs> ruckig_input(num_joints_);
+  ruckig_input.max_velocity = joint_velocity_bounds_;
+  ruckig_input.max_acceleration = joint_acceleration_bounds_;
+  ruckig_input.max_jerk = joint_jerk_bounds_;
+  // initialize positions. All other quantities are initialized to zero in the constructor.
+  // This will be overwritten in the first control loop
+  ruckig_input.current_position = std::vector<double>(num_joints_, 0.0);
+  ruckig_input_ = ruckig_input;
+
+  ruckig_output_.emplace(ruckig::OutputParameter<ruckig::DynamicDOFs>(num_joints_));
+
+  ruckig_.emplace(ruckig::Ruckig<ruckig::DynamicDOFs>(num_joints_, params_.update_period));
+
+  return true;
+}
+
+bool RuckigFilterPlugin::doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities,
+                                     Eigen::VectorXd& accelerations)
+{
+  return true;
+}
+
+bool RuckigFilterPlugin::reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
+                               const Eigen::VectorXd& accelerations)
+{
+  return true;
+}
+
+void RuckigFilterPlugin::getDefaultJointVelAccelBounds()
+{
+  auto joint_model_group = robot_model_->getJointModelGroup(params_.planning_group_name);
+  for (const auto& joint : joint_model_group->getActiveJointModels())
+  {
+    const auto& bound = joint->getVariableBounds(joint->getName());
+
+    if (bound.velocity_bounded_)
+    {
+      // Assume symmetric limits
+      joint_velocity_bounds_.push_back(bound.max_velocity_);
+    }
+    else
+    {
+      RCLCPP_ERROR(getLogger(), "No joint vel limit defined. Exiting for safety.");
+      std::exit(EXIT_FAILURE);
+    }
+
+    if (bound.acceleration_bounded_)
+    {
+      // Assume symmetric limits
+      joint_acceleration_bounds_.push_back(bound.max_acceleration_);
+    }
+    else
+    {
+      RCLCPP_ERROR(getLogger(), "WARNING: No joint accel limit defined. Very large accelerations will be "
+                                "possible.");
+      joint_acceleration_bounds_.push_back(DBL_MAX);
+    }
+
+    // MoveIt and the URDF don't support jerk limits, so use a safe default always
+    joint_jerk_bounds_.push_back(DEFAULT_JERK_BOUND);
+  }
+
+  assert(joint_jerk_bounds_.size() == num_joints_);
+}
+}  // namespace online_signal_smoothing
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(online_signal_smoothing::RuckigFilterPlugin, online_signal_smoothing::SmoothingBaseClass)
diff --git a/moveit_core/online_signal_smoothing/src/ruckig_filter_parameters.yaml b/moveit_core/online_signal_smoothing/src/ruckig_filter_parameters.yaml
@@ -0,0 +1,16 @@
+online_signal_smoothing:
+  update_period: {
+    type: double,
+    description: "The expected time in seconds between calls to `doSmoothing` method.",
+    read_only: true,
+    validation: {
+      gt<>: 0.0
+    }
+  }
+  planning_group_name: {
+    type: string,
+    read_only: true,
+    description: "The name of the MoveIt planning group of the robot \
+                This parameter does not have a default value and \
+                must be passed to the node during launch time."
+  }
diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml
@@ -28,7 +28,7 @@ publish_joint_accelerations: false
 
 ## Plugins for smoothing outgoing commands
 use_smoothing: true
-smoothing_filter_plugin_name: "online_signal_smoothing::AccelerationLimitedPlugin"
+smoothing_filter_plugin_name: "online_signal_smoothing::RuckigFilterPlugin"
 
 # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
 # which other nodes can use as a source for information about the planning environment.
