diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 107ce7fa6f..499732ea7c 100644 --- a/moveit_core/CMakeLists.txt +++ b/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) diff --git a/moveit_core/filter_plugin_acceleration.xml b/moveit_core/filter_plugin_acceleration.xml new file mode 100644 index 0000000000..943ecf3f55 --- /dev/null +++ b/moveit_core/filter_plugin_acceleration.xml @@ -0,0 +1,8 @@ + + + + Limits acceleration of commands to generate smooth motion. + + + diff --git a/moveit_core/online_signal_smoothing/CMakeLists.txt b/moveit_core/online_signal_smoothing/CMakeLists.txt index 6fb01e2aad..f9b9cbaabe 100644 --- a/moveit_core/online_signal_smoothing/CMakeLists.txt +++ b/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 + $ +) +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 () diff --git a/moveit_core/online_signal_smoothing/README.md b/moveit_core/online_signal_smoothing/README.md new file mode 100644 index 0000000000..f6197856c1 --- /dev/null +++ b/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. diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h new file mode 100644 index 0000000000..361b14852e --- /dev/null +++ b/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 + +#include +#include +#include +#include +#include + +#include +#include +#include + +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() override + { + 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 constraints_sparse_; + /** \brief osqp types used for optimization problem */ + OSQPDataWrapperPtr osqp_data_; + OSQPWorkspace* osqp_workspace_ = nullptr; + OSQPSettings osqp_settings_; +}; +} // namespace online_signal_smoothing diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h index bdcf6a7e5b..80212d2fd2 100644 --- a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h @@ -41,8 +41,7 @@ #include -// Auto-generated -#include +#include #include #include diff --git a/moveit_core/online_signal_smoothing/res/acceleration_limiting_diagram.png b/moveit_core/online_signal_smoothing/res/acceleration_limiting_diagram.png new file mode 100644 index 0000000000..fe32aa78d9 Binary files /dev/null and b/moveit_core/online_signal_smoothing/res/acceleration_limiting_diagram.png differ diff --git a/moveit_core/online_signal_smoothing/src/acceleration_filter.cpp b/moveit_core/online_signal_smoothing/src/acceleration_filter.cpp new file mode 100644 index 0000000000..3af91484f2 --- /dev/null +++ b/moveit_core/online_signal_smoothing/src/acceleration_filter.cpp @@ -0,0 +1,349 @@ +/********************************************************************* + * 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. + *********************************************************************/ + +#include +#include +#include + +// Disable -Wold-style-cast because all _THROTTLE macros trigger this +#pragma GCC diagnostic ignored "-Wold-style-cast" + +namespace online_signal_smoothing +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("AccelerationLimitedPlugin"); +} + +// The threshold below which any velocity or position difference is considered zero (rad and rad/s). +constexpr double COMMAND_DIFFERENCE_THRESHOLD = 1E-4; +// The scaling parameter alpha between the current point and commanded point must be less than 1.0 +constexpr double ALPHA_UPPER_BOUND = 1.0; +// The scaling parameter alpha must also be greater than 0.0 +constexpr double ALPHA_LOWER_BOUND = 0.0; + +/** \brief Wrapper struct to make memory management easier for using osqp's C sparse_matrix types */ +struct CSCWrapper +{ + /// row indices, size nzmax starting from 0 + std::vector row_indices; + /// column pointers (size n+1); col indices (size nzmax) + std::vector column_pointers; + /// holds the non-zero values in Compressed Sparse Column (CSC) form + std::vector elements; + /// osqp C sparse_matrix type + csc csc_sparse_matrix; + + CSCWrapper(Eigen::SparseMatrix& M) + { + M.makeCompressed(); + + csc_sparse_matrix.n = M.cols(); + csc_sparse_matrix.m = M.rows(); + row_indices.assign(M.innerSize(), 0); + csc_sparse_matrix.i = row_indices.data(); + column_pointers.assign(M.outerSize() + 1, 0); + csc_sparse_matrix.p = column_pointers.data(); + csc_sparse_matrix.nzmax = M.nonZeros(); + csc_sparse_matrix.nz = -1; + elements.assign(M.nonZeros(), 0.0); + csc_sparse_matrix.x = elements.data(); + + update(M); + } + + /// Update the the data point to by sparse_matrix without reallocating memory + void update(Eigen::SparseMatrix& M) + { + for (size_t ind = 0; ind < row_indices.size(); ++ind) + { + row_indices[ind] = M.innerIndexPtr()[ind]; + } + + for (size_t ind = 0; ind < column_pointers.size(); ++ind) + { + column_pointers[ind] = M.outerIndexPtr()[ind]; + } + for (size_t ind = 0; ind < elements.size(); ++ind) + { + elements[ind] = M.data().at(ind); + } + } +}; + +MOVEIT_STRUCT_FORWARD(OSQPDataWrapper); + +/** \brief Wrapper struct to make memory management easier for using osqp's C API */ +struct OSQPDataWrapper +{ + OSQPDataWrapper(Eigen::SparseMatrix& objective_sparse, Eigen::SparseMatrix& constraints_sparse) + : P{ objective_sparse }, A{ constraints_sparse } + { + data.n = objective_sparse.rows(); + data.m = constraints_sparse.rows(); + data.P = &P.csc_sparse_matrix; + q = Eigen::VectorXd::Zero(objective_sparse.rows()); + data.q = q.data(); + data.A = &A.csc_sparse_matrix; + l = Eigen::VectorXd::Zero(constraints_sparse.rows()); + data.l = l.data(); + u = Eigen::VectorXd::Zero(constraints_sparse.rows()); + data.u = u.data(); + } + + /// Update the constraint matrix A without reallocating memory + void updateA(OSQPWorkspace* work, Eigen::SparseMatrix& constraints_sparse) + { + constraints_sparse.makeCompressed(); + A.update(constraints_sparse); + osqp_update_A(work, A.elements.data(), OSQP_NULL, A.elements.size()); + } + + CSCWrapper P; + CSCWrapper A; + Eigen::VectorXd q; + Eigen::VectorXd l; + Eigen::VectorXd u; + OSQPData data{}; +}; + +bool AccelerationLimitedPlugin::initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, + size_t num_joints) +{ + // copy inputs into member variables + node_ = node; + num_joints_ = num_joints; + robot_model_ = robot_model; + cur_acceleration_ = Eigen::VectorXd::Zero(num_joints); + + // get node parameters and store in member variables + auto param_listener = online_signal_smoothing::ParamListener(node_); + params_ = param_listener.get_params(); + + // get robot acceleration limits and store in member variables + auto joint_model_group = robot_model_->getJointModelGroup(params_.planning_group_name); + auto joint_bounds = joint_model_group->getActiveJointModelsBounds(); + min_acceleration_limits_ = Eigen::VectorXd::Zero(num_joints); + max_acceleration_limits_ = Eigen::VectorXd::Zero(num_joints); + size_t ind = 0; + for (const auto& joint_bound : joint_bounds) + { + for (const auto& variable_bound : *joint_bound) + { + if (variable_bound.acceleration_bounded_) + { + min_acceleration_limits_[ind] = variable_bound.min_acceleration_; + max_acceleration_limits_[ind] = variable_bound.max_acceleration_; + } + else + { + RCLCPP_ERROR(getLogger(), "The robot must have acceleration joint limits specified for all joints to " + "use AccelerationLimitedPlugin."); + return false; + } + } + ind++; + } + + // setup osqp optimization problem + Eigen::SparseMatrix objective_sparse(1, 1); + objective_sparse.insert(0, 0) = 1.0; + size_t num_constraints = num_joints + 1; + constraints_sparse_ = Eigen::SparseMatrix(num_constraints, 1); + for (size_t i = 0; i < num_constraints - 1; ++i) + { + constraints_sparse_.insert(i, 0) = 0; + } + constraints_sparse_.insert(num_constraints - 1, 0) = 0; + osqp_set_default_settings(&osqp_settings_); + osqp_settings_.warm_start = 0; + osqp_settings_.verbose = 0; + osqp_data_ = std::make_shared(objective_sparse, constraints_sparse_); + osqp_data_->q[0] = 0; + + if (osqp_setup(&osqp_workspace_, &osqp_data_->data, &osqp_settings_) != 0) + { + osqp_settings_.verbose = 1; + // call setup again with verbose enables to trigger error message printing + osqp_setup(&osqp_workspace_, &osqp_data_->data, &osqp_settings_); + RCLCPP_ERROR(getLogger(), "Failed to initialize osqp problem."); + return false; + } + + return true; +} + +double jointLimitAccelerationScalingFactor(const Eigen::VectorXd& accelerations, + const moveit::core::JointBoundsVector& joint_bounds) +{ + double min_scaling_factor = 1.0; + + // Now get the scaling factor from joint limits. + size_t idx = 0; + for (const auto& joint_bound : joint_bounds) + { + for (const auto& variable_bound : *joint_bound) + { + const auto& target_accel = accelerations(idx); + if (variable_bound.acceleration_bounded_ && target_accel != 0.0) + { + // Find the ratio of clamped acceleration to original acceleration + const auto bounded_vel = + std::clamp(target_accel, variable_bound.min_acceleration_, variable_bound.max_acceleration_); + double joint_scaling_factor = bounded_vel / target_accel; + min_scaling_factor = std::min(min_scaling_factor, joint_scaling_factor); + } + ++idx; + } + } + + return min_scaling_factor; +} + +inline bool updateData(const OSQPDataWrapperPtr& data, OSQPWorkspace* work, + Eigen::SparseMatrix& constraints_sparse, const Eigen::VectorXd& lower_bound, + const Eigen::VectorXd& upper_bound) +{ + data->updateA(work, constraints_sparse); + size_t num_constraints = constraints_sparse.rows(); + data->u.block(0, 0, num_constraints - 1, 1) = upper_bound; + data->l.block(0, 0, num_constraints - 1, 1) = lower_bound; + data->u[num_constraints - 1] = ALPHA_UPPER_BOUND; + data->l[num_constraints - 1] = ALPHA_LOWER_BOUND; + return 0 == osqp_update_bounds(work, data->l.data(), data->u.data()); +} + +bool AccelerationLimitedPlugin::doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, + Eigen::VectorXd& /* unused */) +{ + const size_t num_positions = velocities.size(); + if (num_positions != num_joints_) + { + RCLCPP_ERROR_THROTTLE( + getLogger(), *node_->get_clock(), 1000, + "The length of the joint positions parameter is not equal to the number of joints, expected %zu got %zu.", + num_joints_, num_positions); + return false; + } + else if (last_positions_.size() != positions.size()) + { + RCLCPP_ERROR_THROTTLE(getLogger(), *node_->get_clock(), 1000, + "The length of the last joint positions not equal to the current, expected %zu got %zu. Make " + "sure the reset was called.", + last_positions_.size(), positions.size()); + return false; + } + + // formulate a quadratic program to find the best new reference point subject to the robot's acceleration limits + // p_c: robot's current position + // v_c: robot's current velocity + // p_t: robot's target position + // acc: acceleration to be applied + // p_n: next position + // dt: time step + // p_n_hat: parameterize solution to be along the line from p_c to p_t + // p_n_hat = p_t*alpha + p_c*(1-alpha) + // define constraints + // p_c + v_c*dt + acc_min*dt^2 < p_n_hat < p_c + v_c*dt + acc_max*dt^2 + // p_c + v_c*dt -p_t + acc_min*dt^2 < (p_c-p_t)alpha < p_c + v_c*dt -p_t + acc_max*dt^2 + // 0 < alpha < 1 + // define optimization + // opt ||alpha|| + // s.t. constraints + // p_n = p_t*alpha + p_c*(1-alpha) + + double& update_period = params_.update_period; + size_t num_constraints = num_joints_ + 1; + positions_offset_ = last_positions_ - positions; + velocities_offset_ = last_velocities_ - velocities; + for (size_t i = 0; i < num_constraints - 1; ++i) + { + constraints_sparse_.coeffRef(i, 0) = positions_offset_[i]; + } + constraints_sparse_.coeffRef(num_constraints - 1, 0) = 1; + Eigen::VectorXd vel_point = last_positions_ + last_velocities_ * update_period; + Eigen::VectorXd upper_bound = vel_point - positions + max_acceleration_limits_ * (update_period * update_period); + Eigen::VectorXd lower_bound = vel_point - positions + min_acceleration_limits_ * (update_period * update_period); + if (!updateData(osqp_data_, osqp_workspace_, constraints_sparse_, lower_bound, upper_bound)) + { + RCLCPP_ERROR_THROTTLE(getLogger(), *node_->get_clock(), 1000, + "failed to set osqp_update_bounds. Make sure the robot's acceleration limits are valid"); + return false; + } + + if (positions_offset_.norm() < COMMAND_DIFFERENCE_THRESHOLD && + velocities_offset_.norm() < COMMAND_DIFFERENCE_THRESHOLD) + { + positions = last_positions_; + velocities = last_velocities_; + } + else if (osqp_solve(osqp_workspace_) == 0 && + osqp_workspace_->solution->x[0] >= ALPHA_LOWER_BOUND - osqp_settings_.eps_abs && + osqp_workspace_->solution->x[0] <= ALPHA_UPPER_BOUND + osqp_settings_.eps_abs) + { + double alpha = osqp_workspace_->solution->x[0]; + positions = alpha * last_positions_ + (1.0 - alpha) * positions.eval(); + velocities = (positions - last_positions_) / update_period; + } + else + { + auto joint_model_group = robot_model_->getJointModelGroup(params_.planning_group_name); + auto joint_bounds = joint_model_group->getActiveJointModelsBounds(); + cur_acceleration_ = -(last_velocities_) / update_period; + cur_acceleration_ *= jointLimitAccelerationScalingFactor(cur_acceleration_, joint_bounds); + velocities = last_velocities_ + cur_acceleration_ * update_period; + positions = last_positions_ + velocities * update_period; + } + + last_velocities_ = velocities; + last_positions_ = positions; + + return true; +} + +bool AccelerationLimitedPlugin::reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const Eigen::VectorXd& /* unused */) +{ + last_velocities_ = velocities; + last_positions_ = positions; + cur_acceleration_ = Eigen::VectorXd::Zero(num_joints_); + + return true; +} + +} // 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/acceleration_filter_parameters.yaml b/moveit_core/online_signal_smoothing/src/acceleration_filter_parameters.yaml new file mode 100644 index 0000000000..92575e5059 --- /dev/null +++ b/moveit_core/online_signal_smoothing/src/acceleration_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_core/online_signal_smoothing/test/test_acceleration_filter.cpp b/moveit_core/online_signal_smoothing/test/test_acceleration_filter.cpp new file mode 100644 index 0000000000..aeff731f2a --- /dev/null +++ b/moveit_core/online_signal_smoothing/test/test_acceleration_filter.cpp @@ -0,0 +1,207 @@ +/********************************************************************* + * 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. + *********************************************************************/ + +#include +#include +#include + +constexpr std::string_view PLANNING_GROUP_NAME = "panda_arm"; +constexpr size_t PANDA_NUM_JOINTS = 7u; +constexpr std::string_view ROBOT_MODEL = "panda"; + +class AccelerationFilterTest : public testing::Test +{ +protected: + void SetUp() override + { + robot_model_ = moveit::core::loadTestingRobotModel(ROBOT_MODEL.data()); + }; + + void setLimits(std::optional acceleration_limits) + { + const std::vector joint_models = robot_model_->getJointModels(); + auto joint_model_group = robot_model_->getJointModelGroup(PLANNING_GROUP_NAME.data()); + size_t ind = 0; + for (auto& joint_model : joint_models) + { + if (!joint_model_group->hasJointModel(joint_model->getName())) + { + continue; + } + std::vector joint_bounds_msg(joint_model->getVariableBoundsMsg()); + for (auto& joint_bound : joint_bounds_msg) + { + joint_bound.has_acceleration_limits = acceleration_limits.has_value(); + if (joint_bound.has_acceleration_limits) + { + joint_bound.max_acceleration = acceleration_limits.value()[ind]; + } + } + joint_model->setVariableBounds(joint_bounds_msg); + ind++; + } + } + +protected: + moveit::core::RobotModelPtr robot_model_; +}; + +TEST_F(AccelerationFilterTest, FilterInitialize) +{ + online_signal_smoothing::AccelerationLimitedPlugin filter; + rclcpp::Node::SharedPtr node = std::make_shared("AccelerationFilterTest"); + + // fail because the update_period parameter is not set + EXPECT_THROW(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS), + rclcpp::exceptions::ParameterUninitializedException); + + node = std::make_shared("AccelerationFilterTest"); + node->declare_parameter("planning_group_name", PLANNING_GROUP_NAME.data()); + node->declare_parameter("update_period", 0.01); + + // fail because the number of joints is wrong + EXPECT_FALSE(filter.initialize(node, robot_model_, 3u)); + + // fail because the robot does not have acceleration limits + setLimits({}); + EXPECT_FALSE(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS)); + + // succeed with acceleration limits + Eigen::VectorXd acceleration_limits = 1.2 * Eigen::VectorXd::Ones(PANDA_NUM_JOINTS); + setLimits(acceleration_limits); + EXPECT_TRUE(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS)); +} + +TEST_F(AccelerationFilterTest, FilterDoSmooth) +{ + online_signal_smoothing::AccelerationLimitedPlugin filter; + + rclcpp::Node::SharedPtr node = std::make_shared("AccelerationFilterTest"); + node->declare_parameter("planning_group_name", PLANNING_GROUP_NAME.data()); + const double update_period = 1.0; + node->declare_parameter("update_period", update_period); + Eigen::VectorXd acceleration_limits = 1.2 * Eigen::VectorXd::Ones(PANDA_NUM_JOINTS); + setLimits(acceleration_limits); + + EXPECT_TRUE(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS)); + + // fail when called with the wrong number of joints + Eigen::VectorXd position = Eigen::VectorXd::Zero(5); + Eigen::VectorXd velocity = Eigen::VectorXd::Zero(5); + Eigen::VectorXd acceleration = Eigen::VectorXd::Zero(5); + EXPECT_FALSE(filter.doSmoothing(position, velocity, acceleration)); + + // fail because reset was not called + position = Eigen::VectorXd::Zero(PANDA_NUM_JOINTS); + velocity = Eigen::VectorXd::Zero(PANDA_NUM_JOINTS); + acceleration = Eigen::VectorXd::Zero(PANDA_NUM_JOINTS); + EXPECT_FALSE(filter.doSmoothing(position, velocity, acceleration)); + + // succeed + filter.reset(Eigen::VectorXd::Zero(PANDA_NUM_JOINTS), Eigen::VectorXd::Zero(PANDA_NUM_JOINTS), + Eigen::VectorXd::Zero(PANDA_NUM_JOINTS)); + EXPECT_TRUE(filter.doSmoothing(position, velocity, acceleration)); + + // succeed: apply acceleration limits + filter.reset(Eigen::VectorXd::Zero(PANDA_NUM_JOINTS), Eigen::VectorXd::Zero(PANDA_NUM_JOINTS), + Eigen::VectorXd::Zero(PANDA_NUM_JOINTS)); + position.array() = 3.0; + EXPECT_TRUE(filter.doSmoothing(position, velocity, acceleration)); + EXPECT_TRUE((position.array() - update_period * update_period * acceleration_limits.array()).matrix().norm() < 1E-3); + + // succeed: apply acceleration limits + position.array() = 0.9; + filter.reset(position * 0, velocity * 0, acceleration * 0); + EXPECT_TRUE(filter.doSmoothing(position, velocity, acceleration)); + EXPECT_TRUE((position.array() - 0.9).matrix().norm() < 1E-3); + + // succeed: slow down + velocity = 10 * Eigen::VectorXd::Ones(PANDA_NUM_JOINTS); + filter.reset(Eigen::VectorXd::Zero(PANDA_NUM_JOINTS), velocity, Eigen::VectorXd::Zero(PANDA_NUM_JOINTS)); + position.array() = 0.01; + Eigen::VectorXd expected_offset = + velocity.array() * update_period - update_period * update_period * acceleration_limits.array(); + EXPECT_TRUE(filter.doSmoothing(position, velocity, acceleration)); + EXPECT_TRUE((velocity * update_period - expected_offset).norm() < 1E-3); +} + +TEST_F(AccelerationFilterTest, FilterBadAccelerationConfig) +{ + online_signal_smoothing::AccelerationLimitedPlugin filter; + + rclcpp::Node::SharedPtr node = std::make_shared("AccelerationFilterTest"); + node->declare_parameter("planning_group_name", PLANNING_GROUP_NAME.data()); + const double update_period = 0.1; + node->declare_parameter("update_period", update_period); + Eigen::VectorXd acceleration_limits = -1.0 * Eigen::VectorXd::Ones(PANDA_NUM_JOINTS); + setLimits(acceleration_limits); + EXPECT_TRUE(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS)); + Eigen::VectorXd position = Eigen::VectorXd::Zero(PANDA_NUM_JOINTS); + Eigen::VectorXd velocity = Eigen::VectorXd::Zero(PANDA_NUM_JOINTS); + Eigen::VectorXd acceleration = Eigen::VectorXd::Zero(PANDA_NUM_JOINTS); + EXPECT_TRUE(filter.reset(position, velocity, acceleration)); + EXPECT_FALSE(filter.doSmoothing(position, velocity, acceleration)); +} + +TEST_F(AccelerationFilterTest, FilterDoSmoothRandomized) +{ + online_signal_smoothing::AccelerationLimitedPlugin filter; + rclcpp::Node::SharedPtr node = std::make_shared("AccelerationFilterTest"); + const double update_period = 0.1; + node->declare_parameter("planning_group_name", PLANNING_GROUP_NAME.data()); + node->declare_parameter("update_period", update_period); + Eigen::VectorXd acceleration_limits = 1.2 * (1.0 + Eigen::VectorXd::Random(PANDA_NUM_JOINTS).array()); + setLimits(acceleration_limits); + EXPECT_TRUE(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS)); + + for (size_t iter = 0; iter < 64; ++iter) + { + acceleration_limits = 1.2 * (1.0 + Eigen::VectorXd::Random(PANDA_NUM_JOINTS).array()); + setLimits(acceleration_limits); + EXPECT_TRUE(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS)); + filter.reset(Eigen::VectorXd::Zero(PANDA_NUM_JOINTS), Eigen::VectorXd::Zero(PANDA_NUM_JOINTS), + Eigen::VectorXd::Zero(PANDA_NUM_JOINTS)); + Eigen::VectorXd position = Eigen::VectorXd::Random(PANDA_NUM_JOINTS); + Eigen::VectorXd velocity = Eigen::VectorXd::Random(PANDA_NUM_JOINTS); + Eigen::VectorXd acceleration = Eigen::VectorXd::Random(PANDA_NUM_JOINTS); + EXPECT_TRUE(filter.doSmoothing(position, velocity, acceleration)); + } +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_core/package.xml b/moveit_core/package.xml index 2b22cc7cab..bf07bc6849 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -31,8 +31,8 @@ boost bullet common_interfaces - eigen_stl_containers eigen + eigen_stl_containers generate_parameter_library geometric_shapes geometry_msgs @@ -41,8 +41,9 @@ libfcl-dev moveit_common moveit_msgs - octomap_msgs octomap + octomap_msgs + osqp_vendor pluginlib random_numbers rclcpp @@ -52,14 +53,14 @@ shape_msgs srdfdom std_msgs + tf2 tf2_eigen tf2_geometry_msgs tf2_kdl - tf2 trajectory_msgs urdf - urdfdom_headers urdfdom + urdfdom_headers visualization_msgs python3-sphinx-rtd-theme diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml index 878f27b141..ac3d0eb068 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::ButterworthFilterPlugin" +smoothing_filter_plugin_name: "online_signal_smoothing::AccelerationLimitedPlugin" # 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/launch/demo_joint_jog.launch.py b/moveit_ros/moveit_servo/launch/demo_joint_jog.launch.py index 77cecddbd6..62404efc2a 100644 --- a/moveit_ros/moveit_servo/launch/demo_joint_jog.launch.py +++ b/moveit_ros/moveit_servo/launch/demo_joint_jog.launch.py @@ -10,7 +10,7 @@ def generate_launch_description(): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") - .joint_limits() + .joint_limits(file_path="config/hard_joint_limits.yaml") .robot_description_kinematics() .to_moveit_configs() ) diff --git a/moveit_ros/moveit_servo/launch/demo_pose.launch.py b/moveit_ros/moveit_servo/launch/demo_pose.launch.py index aef3d2bda0..f2a77201e9 100644 --- a/moveit_ros/moveit_servo/launch/demo_pose.launch.py +++ b/moveit_ros/moveit_servo/launch/demo_pose.launch.py @@ -10,7 +10,7 @@ def generate_launch_description(): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") - .joint_limits() + .joint_limits(file_path="config/hard_joint_limits.yaml") .robot_description_kinematics() .to_moveit_configs() ) diff --git a/moveit_ros/moveit_servo/launch/demo_ros_api.launch.py b/moveit_ros/moveit_servo/launch/demo_ros_api.launch.py index 2768ea9268..ac008bfb66 100644 --- a/moveit_ros/moveit_servo/launch/demo_ros_api.launch.py +++ b/moveit_ros/moveit_servo/launch/demo_ros_api.launch.py @@ -12,8 +12,7 @@ def generate_launch_description(): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") - .joint_limits() - .robot_description_kinematics() + .joint_limits(file_path="config/hard_joint_limits.yaml") .to_moveit_configs() ) @@ -29,9 +28,9 @@ def generate_launch_description(): .to_dict() } - # This filter parameter should be >1. Increase it for greater smoothing but slower motion. - low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} - + # This sets the update rate and planning group name for the acceleration limiting filter. + acceleration_filter_update_period = {"update_period": 0.01} + planning_group_name = {"planning_group_name": "panda_arm"} # RViz rviz_config_file = ( get_package_share_directory("moveit_servo") @@ -98,7 +97,8 @@ def generate_launch_description(): name="servo_node", parameters=[ servo_params, - low_pass_filter_coeff, + acceleration_filter_update_period, + planning_group_name, moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, @@ -129,7 +129,8 @@ def generate_launch_description(): name="servo_node", parameters=[ servo_params, - low_pass_filter_coeff, + acceleration_filter_update_period, + planning_group_name, moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, diff --git a/moveit_ros/moveit_servo/launch/demo_twist.launch.py b/moveit_ros/moveit_servo/launch/demo_twist.launch.py index 1230aa168c..4c4867fb82 100644 --- a/moveit_ros/moveit_servo/launch/demo_twist.launch.py +++ b/moveit_ros/moveit_servo/launch/demo_twist.launch.py @@ -10,7 +10,7 @@ def generate_launch_description(): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") - .joint_limits() + .joint_limits(file_path="config/hard_joint_limits.yaml") .robot_description_kinematics() .to_moveit_configs() ) diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 09ef4a477f..9f44cb5462 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -514,16 +514,6 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot // Compute the joint velocities required to reach positions target_state.velocities = joint_position_delta / servo_params_.publish_period; - // Apply smoothing to the positions if a smoother was provided. - doSmoothing(target_state); - - // Apply collision scaling to the joint position delta - target_state.positions = - current_state.positions + collision_velocity_scale_ * (target_state.positions - current_state.positions); - - // Compute velocities based on smoothed joint positions - target_state.velocities = (target_state.positions - current_state.positions) / servo_params_.publish_period; - // Scale down the velocity based on joint velocity limit or user defined scaling if applicable. const double joint_velocity_limit_scale = jointLimitVelocityScalingFactor( target_state.velocities, joint_bounds, servo_params_.override_velocity_scaling_factor); @@ -536,6 +526,16 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot // Adjust joint position based on scaled down velocity target_state.positions = current_state.positions + (target_state.velocities * servo_params_.publish_period); + // Apply smoothing to the positions if a smoother was provided. + doSmoothing(target_state); + + // Apply collision scaling to the joint position delta + target_state.positions = + current_state.positions + collision_velocity_scale_ * (target_state.positions - current_state.positions); + + // Compute velocities based on smoothed joint positions + target_state.velocities = (target_state.positions - current_state.positions) / servo_params_.publish_period; + // Check if any joints are going past joint position limits const std::vector joints_to_halt = jointsToHalt(target_state.positions, target_state.velocities, joint_bounds, servo_params_.joint_limit_margins); @@ -548,6 +548,9 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot } } + // Update internal state of filter with final calculated command. + resetSmoothing(target_state); + return target_state; } @@ -678,6 +681,7 @@ std::pair Servo::smoothHalt(const KinematicState& halt_sta } } + resetSmoothing(target_state); return std::make_pair(stopped, target_state); }