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.