diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 90b2851ded..608babbebc 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 0000000000..47ffd347a1 --- /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 bcfca1c75e..1f1912c6d9 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 0000000000..13cf36a742 --- /dev/null +++ b/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 + +#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: + /** + * 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& joint_velocity_bounds, std::vector& joint_acceleration_bounds, + std::vector& 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_; + std::optional> ruckig_input_; + std::optional> ruckig_output_; +}; +} // 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 e870e493f0..e2dc886aaf 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 4bfc69ed14..5e47aaec69 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 0000000000..b689620761 --- /dev/null +++ b/moveit_core/online_signal_smoothing/src/ruckig_filter.cpp @@ -0,0 +1,219 @@ +/********************************************************************* + * 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"); +} +} // namespace + +bool RuckigFilterPlugin::initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, + size_t 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 bounds + // TODO: Ruckig says the jerk bounds can be optional. We require them, for now. + ruckig::InputParameter ruckig_input(num_joints); + if (!getVelAccelJerkBounds(ruckig_input.max_velocity, ruckig_input.max_acceleration, ruckig_input.max_jerk)) + { + return false; + } + ruckig_input.current_position = std::vector(num_joints, 0.0); + ruckig_input.current_velocity = std::vector(num_joints, 0.0); + ruckig_input.current_acceleration = std::vector(num_joints, 0.0); + ruckig_input.synchronization = ruckig::Synchronization::Phase; + + 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) +{ + if (!ruckig_input_ || !ruckig_output_ || !ruckig_) + { + RCLCPP_ERROR_STREAM(getLogger(), "The Ruckig smoother was not initialized"); + return false; + } + + if (have_initial_ruckig_output_) + { + ruckig_output_->pass_to_input(*ruckig_input_); + } + + // Update Ruckig target state + ruckig_input_->target_position = std::vector(positions.data(), positions.data() + positions.size()); + // We don't know what the next command will be. Assume velocity continues forward based on current state, + // target_acceleration is zero. + const size_t num_joints = ruckig_input_->current_acceleration.size(); + for (size_t i = 0; i < num_joints; ++i) + { + ruckig_input_->target_velocity.at(i) = + ruckig_input_->current_velocity.at(i) + ruckig_input_->current_acceleration.at(i) * params_.update_period; + } + // target_acceleration remains a vector of zeroes + + // Call the Ruckig algorithm + ruckig::Result ruckig_result = ruckig_->update(*ruckig_input_, *ruckig_output_); + + // Finished means the target state can be reached in this timestep. + // Working means the target state can be reached but not in this timestep. + // ErrorSynchronizationCalculation means smoothing was successful but the robot will deviate a bit from the desired + // path. + // See https://github.com/pantor/ruckig/blob/master/include/ruckig/input_parameter.hpp + if (ruckig_result != ruckig::Result::Finished && ruckig_result != ruckig::Result::Working && + ruckig_result != ruckig::Result::ErrorSynchronizationCalculation) + + { + RCLCPP_ERROR_STREAM(getLogger(), "Ruckig jerk-limited smoothing failed with code: " << ruckig_result); + printRuckigState(); + // Return without modifying the position/vel/accel + have_initial_ruckig_output_ = false; + return true; + } + + // Update the target state with Ruckig output + positions = Eigen::Map(ruckig_output_->new_position.data(), + ruckig_output_->new_position.size()); + velocities = Eigen::Map(ruckig_output_->new_velocity.data(), + ruckig_output_->new_velocity.size()); + accelerations = Eigen::Map(ruckig_output_->new_acceleration.data(), + ruckig_output_->new_acceleration.size()); + have_initial_ruckig_output_ = true; + + return true; +} + +bool RuckigFilterPlugin::reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const Eigen::VectorXd& accelerations) +{ + // Initialize Ruckig + ruckig_input_->current_position = std::vector(positions.data(), positions.data() + positions.size()); + ruckig_input_->current_velocity = std::vector(velocities.data(), velocities.data() + velocities.size()); + ruckig_input_->current_acceleration = + std::vector(accelerations.data(), accelerations.data() + accelerations.size()); + + have_initial_ruckig_output_ = false; + return true; +} + +bool RuckigFilterPlugin::getVelAccelJerkBounds(std::vector& joint_velocity_bounds, + std::vector& joint_acceleration_bounds, + std::vector& joint_jerk_bounds) +{ + if (!robot_model_) + { + RCLCPP_ERROR(getLogger(), "The robot model was not initialized."); + return false; + } + + joint_velocity_bounds.clear(); + joint_acceleration_bounds.clear(); + joint_jerk_bounds.clear(); + + 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_STREAM(getLogger(), "No joint velocity limit defined for " << joint->getName() << "."); + return false; + } + + if (bound.acceleration_bounded_) + { + // Assume symmetric limits + joint_acceleration_bounds.push_back(bound.max_acceleration_); + } + else + { + RCLCPP_WARN_STREAM(getLogger(), "No joint acceleration limit defined for " << joint->getName() << "."); + return false; + } + + if (bound.jerk_bounded_) + { + // Assume symmetric limits + joint_jerk_bounds.push_back(bound.max_jerk_); + } + // else, return false + else + { + RCLCPP_WARN_STREAM(getLogger(), "No joint jerk limit was defined for " + << joint->getName() << ". The output from Ruckig will not be jerk-limited."); + return false; + } + } + + return true; +} + +void RuckigFilterPlugin::printRuckigState() +{ + RCLCPP_INFO_STREAM(getLogger(), ruckig_->delta_time << "\nRuckig input:\n" + << ruckig_input_->to_string() << "\nRuckig output:\n" + << ruckig_output_->to_string()); +} +} // 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 0000000000..92575e5059 --- /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 ea6e6147e3..980370ca18 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. diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp index 0d1315f6dc..43a0e94fa2 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp @@ -91,7 +91,7 @@ int main(int argc, char* argv[]) std::chrono::seconds timeout_duration(3); std::chrono::seconds time_elapsed(0); - auto start_time = std::chrono::steady_clock::now(); + const auto start_time = std::chrono::steady_clock::now(); // create command queue to build trajectory message and add current robot state std::deque joint_cmd_rolling_window; @@ -104,7 +104,7 @@ int main(int argc, char* argv[]) KinematicState joint_state = servo.getNextJointState(robot_state, joint_jog); const StatusCode status = servo.getStatus(); - auto current_time = std::chrono::steady_clock::now(); + const auto current_time = std::chrono::steady_clock::now(); time_elapsed = std::chrono::duration_cast(current_time - start_time); if (time_elapsed > timeout_duration) { diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp index eb816754eb..8c92fa4cfc 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp @@ -94,7 +94,7 @@ int main(int argc, char* argv[]) std::chrono::seconds timeout_duration(4); std::chrono::seconds time_elapsed(0); - auto start_time = std::chrono::steady_clock::now(); + const auto start_time = std::chrono::steady_clock::now(); // create command queue to build trajectory message and add current robot state std::deque joint_cmd_rolling_window; @@ -107,7 +107,7 @@ int main(int argc, char* argv[]) KinematicState joint_state = servo.getNextJointState(robot_state, target_twist); const StatusCode status = servo.getStatus(); - auto current_time = std::chrono::steady_clock::now(); + const auto current_time = std::chrono::steady_clock::now(); time_elapsed = std::chrono::duration_cast(current_time - start_time); if (time_elapsed > timeout_duration) { diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp index 663e4f7d2f..d946bdc833 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -118,6 +118,7 @@ class Servo /** * \brief Get the current state of the robot as given by planning scene monitor. + * This may block if a current robot state is not available immediately. * @param block_for_current_state If true, we explicitly wait for a new robot state * @return The current state of the robot. */ diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index c8e27ee17c..329e4a84a8 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -79,16 +79,6 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptrgetStateMonitor()->getCurrentState(); - // Load the smoothing plugin - if (servo_params_.use_smoothing) - { - setSmoothingPlugin(); - } - else - { - RCLCPP_WARN(logger_, "No smoothing plugin loaded"); - } - // Create the collision checker and start collision checking. collision_monitor_ = std::make_unique(planning_scene_monitor_, servo_params_, std::ref(collision_velocity_scale_)); @@ -125,6 +115,17 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr(std::string(sub_group_name), std::move(new_map))); } + + // Load the smoothing plugin + if (servo_params_.use_smoothing) + { + setSmoothingPlugin(); + } + else + { + RCLCPP_WARN(logger_, "No smoothing plugin loaded"); + } + RCLCPP_INFO_STREAM(logger_, "Servo initialized successfully"); } diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 9eae434869..bfc989adf3 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -383,7 +383,6 @@ void ServoNode::servoLoop() { // if no new command was created, use current robot state updateSlidingWindow(current_state, joint_cmd_rolling_window_, servo_params_.max_expected_latency, cur_time); - servo_->resetSmoothing(current_state); } status_msg.code = static_cast(servo_->getStatus()); diff --git a/moveit_ros/moveit_servo/src/utils/common.cpp b/moveit_ros/moveit_servo/src/utils/common.cpp index e2f52ee0eb..7ce13c1805 100644 --- a/moveit_ros/moveit_servo/src/utils/common.cpp +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -521,7 +521,16 @@ KinematicState extractRobotState(const moveit::core::RobotStatePtr& robot_state, current_state.joint_names = joint_names; robot_state->copyJointGroupPositions(joint_model_group, current_state.positions); robot_state->copyJointGroupVelocities(joint_model_group, current_state.velocities); + robot_state->copyJointGroupAccelerations(joint_model_group, current_state.accelerations); + // If any acceleration is nan, set all accelerations to zero + // TODO: fix the root cause so this isn't necessary (#2958) + if (std::any_of(current_state.accelerations.begin(), current_state.accelerations.end(), + [](double v) { return isnan(v); })) + { + robot_state->zeroAccelerations(); + robot_state->copyJointGroupAccelerations(joint_model_group, current_state.accelerations); + } return current_state; }