diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt
index 90b2851deda..608babbebce 100644
--- 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
new file mode 100644
index 00000000000..47ffd347a10
--- /dev/null
+++ b/moveit_core/filter_plugin_ruckig.xml
@@ -0,0 +1,8 @@
+
+
+
+ Jerk/acceleration/velocity-limited command smoothing.
+
+
+
diff --git a/moveit_core/online_signal_smoothing/CMakeLists.txt b/moveit_core/online_signal_smoothing/CMakeLists.txt
index bcfca1c75ed..1f1912c6d9d 100644
--- a/moveit_core/online_signal_smoothing/CMakeLists.txt
+++ b/moveit_core/online_signal_smoothing/CMakeLists.txt
@@ -13,6 +13,23 @@ 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 $)
+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(
@@ -20,10 +37,8 @@ target_include_directories(
PUBLIC $)
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 $)
-set_target_properties(moveit_acceleration_filter
+ moveit_ruckig_filter PUBLIC $)
+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
new file mode 100644
index 00000000000..047afc1e60d
--- /dev/null
+++ 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
+
+#include
+#include
+#include
+
+#include
+
+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_;
+ std::optional> ruckig_input_;
+ std::optional> ruckig_output_;
+
+ std::vector joint_velocity_bounds_;
+ std::vector joint_acceleration_bounds_;
+ std::vector 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
index e870e493f0f..e2dc886aaf3 100644
--- 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
index 4bfc69ed149..5e47aaec69a 100644
--- 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
#include
-#include
// 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_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
new file mode 100644
index 00000000000..b546fe94337
--- /dev/null
+++ 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
+#include
+#include
+
+// 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_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(num_joints_, 0.0);
+ ruckig_input_ = ruckig_input;
+
+ ruckig_output_.emplace(ruckig::OutputParameter(num_joints_));
+
+ ruckig_.emplace(ruckig::Ruckig(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_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
new file mode 100644
index 00000000000..92575e5059b
--- /dev/null
+++ 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
index 50379995131..de8376e466c 100644
--- 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.