From 5d81a987ef206f4ef429e1b294241156f2022202 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 17 Sep 2021 11:56:20 +0200 Subject: [PATCH 01/37] Added controller parameters structures and movement interfaces. --- .../controller_parameters.hpp | 212 ++++++++++++++++++ .../src/controller_parameters.cpp | 119 ++++++++++ .../types/hardware_interface_type_values.hpp | 7 + 3 files changed, 338 insertions(+) create mode 100644 controller_interface/include/controller_interface/controller_parameters.hpp create mode 100644 controller_interface/src/controller_parameters.cpp diff --git a/controller_interface/include/controller_interface/controller_parameters.hpp b/controller_interface/include/controller_interface/controller_parameters.hpp new file mode 100644 index 0000000000..93368688c5 --- /dev/null +++ b/controller_interface/include/controller_interface/controller_parameters.hpp @@ -0,0 +1,212 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \author: Denis Stogl + +#ifndef CONTROLLER_INTERFACE__CONTROLLER_PARAMETERS_HPP_ +#define CONTROLLER_INTERFACE__CONTROLLER_PARAMETERS_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/node.hpp" +#include "rcutils/logging_macros.h" + +namespace controller_interface +{ +struct Parameter +{ + Parameter() = default; + + explicit Parameter(const std::string & name, bool configurable = false) + : name(name), configurable(configurable) + { + } + + std::string name; + bool configurable; +}; + +class ControllerParameters +{ +public: + ControllerParameters( + int nr_bool_params = 0, int nr_double_params = 0, int nr_string_params = 0, + int nr_string_list_params = 0); + + virtual ~ControllerParameters() = default; + + virtual void declare_parameters(rclcpp::Node::SharedPtr node); + + /** + * Gets all defined parameters from. + * + * \param[node] shared pointer to the node where parameters should be read. + * \return true if all parameters are read Successfully, false if a parameter is not provided or + * parameter configuration is wrong. + */ + virtual bool get_parameters(rclcpp::Node::SharedPtr node, bool check_validity = true); + + virtual rcl_interfaces::msg::SetParametersResult set_parameter_callback( + const std::vector & parameters); + + /** + * Default implementation of parameter check. No check is done. Always returns true. + * + * \return true + */ + virtual bool check_if_parameters_are_valid() { return true; } + + virtual void update() = 0; + + // TODO(destogl): Test this: "const rclcpp::Node::SharedPtr & node" + +protected: + void add_bool_parameter( + const std::string & name, bool configurable = false, bool default_value = false) + { + bool_parameters_.push_back({Parameter(name, configurable), default_value}); + } + + void add_double_parameter( + const std::string & name, bool configurable = false, + double default_value = std::numeric_limits::quiet_NaN()) + { + double_parameters_.push_back({Parameter(name, configurable), default_value}); + } + + void add_string_parameter( + const std::string & name, bool configurable = false, const std::string & default_value = "") + { + string_parameters_.push_back({Parameter(name, configurable), default_value}); + } + + void add_string_list_parameter( + const std::string & name, bool configurable = false, + const std::vector & default_value = {}) + { + string_list_parameters_.push_back({Parameter(name, configurable), default_value}); + } + + template + bool get_parameters_from_list( + const rclcpp::Node::SharedPtr node, std::vector> & parameters) + { + bool ret = true; + for (auto & parameter : parameters) + { + try + { + rclcpp::Parameter input_parameter; // TODO(destogl): will this be allocated each time? + ret = node->get_parameter(parameter.first.name, input_parameter); + parameter.second = input_parameter.get_value(); + } + catch (rclcpp::ParameterTypeException & e) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str()); + ret = false; + break; + } + } + return ret; + } + + template + bool empty_parameter_in_list(const std::vector> & parameters) + { + bool ret = false; + for (const auto & parameter : parameters) + { + if (parameter.second.empty()) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter is empty", parameter.first.name.c_str()); + ret = true; + } + } + return ret; + } + + bool empty_parameter_in_list(const std::vector> & parameters) + { + bool ret = false; + for (const auto & parameter : parameters) + { + if (std::isnan(parameter.second)) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter is not set", parameter.first.name.c_str()); + ret = true; + } + } + return ret; + } + + template + bool find_and_assign_parameter_value( + std::vector> & parameter_list, + const rclcpp::Parameter & input_parameter) + { + auto is_in_list = [&](const auto & parameter) + { return parameter.first.name == input_parameter.get_name(); }; + + auto it = std::find_if(parameter_list.begin(), parameter_list.end(), is_in_list); + + if (it != parameter_list.end()) + { + if (!it->first.configurable) + { + throw std::runtime_error( + "Parameter " + input_parameter.get_name() + " is not configurable."); + } + else + { + it->second = input_parameter.get_value(); + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter is updated to value: %s", it->first.name.c_str(), + input_parameter.value_to_string().c_str()); + return true; + } + } + else + { + return false; + } + } + + std::vector> bool_parameters_; + std::vector> double_parameters_; + std::vector> string_parameters_; + std::vector>> string_list_parameters_; + + std::string logger_name_; + +private: + template + void declare_parameters_from_list( + rclcpp::Node::SharedPtr node, const std::vector> & parameters) + { + for (const auto & parameter : parameters) + { + node->declare_parameter(parameter.first.name, parameter.second); + } + } +}; + +} // namespace controller_interface + +#endif // CONTROLLER_INTERFACE__CONTROLLER_PARAMETERS_HPP_ diff --git a/controller_interface/src/controller_parameters.cpp b/controller_interface/src/controller_parameters.cpp new file mode 100644 index 0000000000..efb87c2dba --- /dev/null +++ b/controller_interface/src/controller_parameters.cpp @@ -0,0 +1,119 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \author: Denis Stogl + +#include "controller_interface/controller_parameters.hpp" + +#include +#include + +#include "rcutils/logging_macros.h" + +namespace controller_interface +{ +ControllerParameters::ControllerParameters( + int nr_bool_params, int nr_double_params, int nr_string_params, int nr_string_list_params) +{ + bool_parameters_.reserve(nr_bool_params); + double_parameters_.reserve(nr_double_params); + string_parameters_.reserve(nr_string_params); + string_list_parameters_.reserve(nr_string_list_params); +} + +void ControllerParameters::declare_parameters(rclcpp::Node::SharedPtr node) +{ + logger_name_ = std::string(node->get_name()) + "::parameters"; + + declare_parameters_from_list(node, bool_parameters_); + declare_parameters_from_list(node, double_parameters_); + declare_parameters_from_list(node, string_parameters_); + declare_parameters_from_list(node, string_list_parameters_); +} + +/** + * Gets all defined parameters from. + * + * \param[node] shared pointer to the node where parameters should be read. + * \return true if all parameters are read Successfully, false if a parameter is not provided or + * parameter configuration is wrong. + */ +bool ControllerParameters::get_parameters(rclcpp::Node::SharedPtr node, bool check_validity) +{ + bool ret = false; + + ret = get_parameters_from_list(node, bool_parameters_) && + get_parameters_from_list(node, double_parameters_) && + get_parameters_from_list(node, string_parameters_) && + get_parameters_from_list(node, string_list_parameters_); + + if (ret && check_validity) + { + ret = check_if_parameters_are_valid(); + } + + return ret; +} + +rcl_interfaces::msg::SetParametersResult ControllerParameters::set_parameter_callback( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto & input_parameter : parameters) + { + bool found = false; + + try + { + found = find_and_assign_parameter_value(bool_parameters_, input_parameter); + if (!found) + { + found = find_and_assign_parameter_value(double_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(string_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(string_list_parameters_, input_parameter); + } + + RCUTILS_LOG_INFO_EXPRESSION_NAMED( + found, logger_name_.c_str(), + "Dynamic parameters got changed! To update the parameters internally please " + "restart the controller."); + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { + result.successful = false; + result.reason = e.what(); + RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str()); + break; + } + catch (const std::runtime_error & e) + { + result.successful = false; + result.reason = e.what(); + RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str()); + break; + } + } + + return result; +} + +} // namespace controller_interface diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp index 98ca67226f..b91f620bba 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp @@ -15,6 +15,8 @@ #ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_ #define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_ +#include + namespace hardware_interface { /// Constant defining position interface @@ -25,6 +27,11 @@ constexpr char HW_IF_VELOCITY[] = "velocity"; constexpr char HW_IF_ACCELERATION[] = "acceleration"; /// Constant defining effort interface constexpr char HW_IF_EFFORT[] = "effort"; + +// TODO(destogl): use "inline static const"-type when switched to C++17 +/// Definition of standard names for movement interfaces +const std::vector MOVEMENT_INTERFACES = { + HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_ACCELERATION, HW_IF_EFFORT}; } // namespace hardware_interface #endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_ From aa004a3bec65cd1afc42132097e38c7dd726db03 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 18 Sep 2021 02:08:29 +0200 Subject: [PATCH 02/37] Added joint limiter plugins. - Added initial structures for joint-limit plugins. - Correct ruckig name and make tests to work. - Rename the joint_limits package - Comment and author cleanup - Base class does not require libary. - Delete extra layer of abstraction since not all plugins require a vector of smoothing objects. - Restore simple_joint_limiter to a working state - Implement init() and enforce() - Return of joint_limits package. - Move Ruckig limiter to package joint_limits_enforcement_plugins and make it working. --- joint_limits/CMakeLists.txt | 47 ++++- .../joint_limits/joint_limiter_interface.hpp | 92 ++++++++++ .../joint_limits/simple_joint_limiter.hpp | 41 +++++ .../include/joint_limits/visibility_control.h | 49 +++++ joint_limits/joint_limiters.xml | 9 + joint_limits/package.xml | 9 +- joint_limits/src/joint_limiter_interface.cpp | 72 ++++++++ joint_limits/src/simple_joint_limiter.cpp | 170 ++++++++++++++++++ .../test/test_simple_joint_limiter.cpp | 42 +++++ .../CMakeLists.txt | 66 +++++++ .../visibility_control.h | 49 +++++ .../ruckig_joint_limiter.hpp | 64 +++++++ .../joint_limits_enforcement_plugins.xml | 9 + joint_limits_enforcement_plugins/package.xml | 31 ++++ .../src/ruckig_joint_limiter.cpp | 123 +++++++++++++ .../test/test_ruckig_joint_limiter.cpp | 44 +++++ 16 files changed, 913 insertions(+), 4 deletions(-) create mode 100644 joint_limits/include/joint_limits/joint_limiter_interface.hpp create mode 100644 joint_limits/include/joint_limits/simple_joint_limiter.hpp create mode 100644 joint_limits/include/joint_limits/visibility_control.h create mode 100644 joint_limits/joint_limiters.xml create mode 100644 joint_limits/src/joint_limiter_interface.cpp create mode 100644 joint_limits/src/simple_joint_limiter.cpp create mode 100644 joint_limits/test/test_simple_joint_limiter.cpp create mode 100644 joint_limits_enforcement_plugins/CMakeLists.txt create mode 100644 joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h create mode 100644 joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp create mode 100644 joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml create mode 100644 joint_limits_enforcement_plugins/package.xml create mode 100644 joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp create mode 100644 joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 9ec9221220..b88f677e5e 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -6,8 +6,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") endif() set(THIS_PACKAGE_INCLUDE_DEPENDS + pluginlib rclcpp rclcpp_lifecycle + trajectory_msgs ) find_package(ament_cmake REQUIRED) @@ -23,10 +25,41 @@ target_include_directories(joint_limits INTERFACE ) ament_target_dependencies(joint_limits INTERFACE ${THIS_PACKAGE_INCLUDE_DEPENDS}) +add_library(joint_limiter_interface SHARED + src/joint_limiter_interface.cpp +) +target_compile_features(joint_limiter_interface PUBLIC cxx_std_17) +target_include_directories(joint_limiter_interface PUBLIC + $ + $ +) +ament_target_dependencies(joint_limiter_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(joint_limiter_interface PRIVATE "JOINT_LIMITS_BUILDING_DLL") + + +add_library(simple_joint_limiter SHARED + src/simple_joint_limiter.cpp +) +target_compile_features(simple_joint_limiter PUBLIC cxx_std_17) +target_include_directories(simple_joint_limiter PUBLIC + $ + $ +) +ament_target_dependencies(simple_joint_limiter PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(simple_joint_limiter PRIVATE "JOINT_LIMITS_BUILDING_DLL") + +pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) + find_package(pluginlib REQUIRED) + find_package(rclcpp REQUIRED) ament_add_gtest_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) target_link_libraries(joint_limits_rosparam_test joint_limits) @@ -40,13 +73,25 @@ if(BUILD_TESTING) FILES test/joint_limits_rosparam.yaml DESTINATION share/joint_limits/test ) + + ament_add_gmock(test_simple_joint_limiter test/test_simple_joint_limiter.cpp) + target_include_directories(test_simple_joint_limiter PRIVATE include) + target_link_libraries(test_simple_joint_limiter joint_limiter_interface) + ament_target_dependencies( + test_simple_joint_limiter + pluginlib + rclcpp + ) endif() install( DIRECTORY include/ DESTINATION include/joint_limits ) -install(TARGETS joint_limits +install(TARGETS + joint_limits + joint_limiter_interface + simple_joint_limiter EXPORT export_joint_limits ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp new file mode 100644 index 0000000000..f1207eaf30 --- /dev/null +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -0,0 +1,92 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#ifndef JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ +#define JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ + +#include +#include + +#include "joint_limits/joint_limits.hpp" +#include "joint_limits/visibility_control.h" +#include "rclcpp/node.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace joint_limits +{ +template +class JointLimiterInterface +{ +public: + JOINT_LIMITS_PUBLIC JointLimiterInterface() = default; + + JOINT_LIMITS_PUBLIC virtual ~JointLimiterInterface() = default; + + /// Initialization of every JointLimiter. + /** + * Initialization of JointLimiter for defined joints with their names. + * Robot description topic provides a topic name where URDF of the robot can be found. + * This is needed to use joint limits from URDF (not implemented yet!). + * Override this method only if Initialization and reading joint limits should be adapted. + * Otherwise, initialize your custom limiter in `on_limit` method. + * + * \param[in] joint_names names of joints where limits should be applied. + * \param[in] node shared pointer to the node where joint limit parameters defined. + * \param[in] robot_description_topic string of a topic where robot description is accessible. + * + */ + JOINT_LIMITS_PUBLIC virtual bool init( + const std::vector joint_names, const rclcpp::Node::SharedPtr & node, + const std::string & robot_description_topic = "/robot_description"); + + JOINT_LIMITS_PUBLIC virtual bool configure( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) + { + return on_configure(current_joint_states); + } + + JOINT_LIMITS_PUBLIC virtual bool enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) + { + // TODO(destogl): add checks if sizes of vectors and number of limits correspond. + return on_enforce(current_joint_states, desired_joint_states, dt); + } + + // TODO(destogl): Make those protected? + // Methods that each limiter implementation has to implement + JOINT_LIMITS_PUBLIC virtual bool on_init() { return true; } + + JOINT_LIMITS_PUBLIC virtual bool on_configure( + const trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/) + { + return true; + } + + JOINT_LIMITS_PUBLIC virtual bool on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, + const rclcpp::Duration & dt) = 0; + +protected: + size_t number_of_joints_; + std::vector joint_limits_; + rclcpp::Node::SharedPtr node_; +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ diff --git a/joint_limits/include/joint_limits/simple_joint_limiter.hpp b/joint_limits/include/joint_limits/simple_joint_limiter.hpp new file mode 100644 index 0000000000..2011e0d48e --- /dev/null +++ b/joint_limits/include/joint_limits/simple_joint_limiter.hpp @@ -0,0 +1,41 @@ +// Copyright (c) 2021, PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#ifndef JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ +#define JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ + +#include + +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" + +namespace joint_limits +{ +template +class SimpleJointLimiter : public JointLimiterInterface +{ +public: + JOINT_LIMITS_PUBLIC SimpleJointLimiter(); + + JOINT_LIMITS_PUBLIC bool on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, + const rclcpp::Duration & dt) override; +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ diff --git a/joint_limits/include/joint_limits/visibility_control.h b/joint_limits/include/joint_limits/visibility_control.h new file mode 100644 index 0000000000..0dcc0193a1 --- /dev/null +++ b/joint_limits/include/joint_limits/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_LIMITS__VISIBILITY_CONTROL_H_ +#define JOINT_LIMITS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define JOINT_LIMITS_EXPORT __attribute__((dllexport)) +#define JOINT_LIMITS_IMPORT __attribute__((dllimport)) +#else +#define JOINT_LIMITS_EXPORT __declspec(dllexport) +#define JOINT_LIMITS_IMPORT __declspec(dllimport) +#endif +#ifdef JOINT_LIMITS_BUILDING_DLL +#define JOINT_LIMITS_PUBLIC JOINT_LIMITS_EXPORT +#else +#define JOINT_LIMITS_PUBLIC JOINT_LIMITS_IMPORT +#endif +#define JOINT_LIMITS_PUBLIC_TYPE JOINT_LIMITS_PUBLIC +#define JOINT_LIMITS_LOCAL +#else +#define JOINT_LIMITS_EXPORT __attribute__((visibility("default"))) +#define JOINT_LIMITS_IMPORT +#if __GNUC__ >= 4 +#define JOINT_LIMITS_PUBLIC __attribute__((visibility("default"))) +#define JOINT_LIMITS_LOCAL __attribute__((visibility("hidden"))) +#else +#define JOINT_LIMITS_PUBLIC +#define JOINT_LIMITS_LOCAL +#endif +#define JOINT_LIMITS_PUBLIC_TYPE +#endif + +#endif // JOINT_LIMITS__VISIBILITY_CONTROL_H_ diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml new file mode 100644 index 0000000000..ff45611198 --- /dev/null +++ b/joint_limits/joint_limiters.xml @@ -0,0 +1,9 @@ + + + + Simple joint limiter using clamping approach. + + + diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 3718e1d035..8ab62e458b 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,10 +1,10 @@ joint_limits 3.9.0 - Interfaces for handling of joint limits for controllers or hardware. + Package for with interfaces for handling of joint limits for use in controllers or in hardware. The package also implements a simple, default joint limit strategy by clamping the values. Bence Magyar - Denis Štogl + Denis Štogl Apache License 2.0 @@ -14,11 +14,14 @@ ament_cmake + pluginlib rclcpp rclcpp_lifecycle + trajectory_msgs - launch_testing_ament_cmake + ament_cmake_gmock ament_cmake_gtest + launch_testing_ament_cmake ament_cmake diff --git a/joint_limits/src/joint_limiter_interface.cpp b/joint_limits/src/joint_limiter_interface.cpp new file mode 100644 index 0000000000..b0d109b0cd --- /dev/null +++ b/joint_limits/src/joint_limiter_interface.cpp @@ -0,0 +1,72 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#include "joint_limits/joint_limiter_interface.hpp" + +#include +#include + +#include "joint_limits/joint_limits_rosparam.hpp" + +// TODO(anyone): Add handing of SoftLimits +namespace joint_limits +{ +template <> +bool JointLimiterInterface::init( + const std::vector joint_names, const rclcpp::Node::SharedPtr & node, + const std::string & /*robot_description_topic*/) +{ + number_of_joints_ = joint_names.size(); + joint_limits_.resize(number_of_joints_); + node_ = node; + + bool result = true; + + // TODO(destogl): get limits from URDF + + // Initialize and get joint limits from parameter server + for (auto i = 0ul; i < number_of_joints_; ++i) + { + if (!declare_parameters(joint_names[i], node)) + { + RCLCPP_ERROR( + node->get_logger(), "JointLimiter: Joint '%s': parameter declaration has failed", + joint_names[i].c_str()); + result = false; + break; + } + if (!joint_limits::get_joint_limits(joint_names[i], node, joint_limits_[i])) + { + RCLCPP_ERROR( + node->get_logger(), "JointLimiter: Joint '%s': getting parameters has failed", + joint_names[i].c_str()); + result = false; + break; + } + RCLCPP_INFO( + node->get_logger(), "Joint '%s':\n%s", joint_names[i].c_str(), + joint_limits_[i].to_string().c_str()); + } + + if (result) + { + result = on_init(); + } + + return result; +} + +} // namespace joint_limits diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp new file mode 100644 index 0000000000..5106615ea8 --- /dev/null +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -0,0 +1,170 @@ +// Copyright (c) 2021, PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \authors Nathan Brooks, Denis Stogl + +#include "joint_limits/simple_joint_limiter.hpp" + +#include + +#include "rclcpp/duration.hpp" +#include "rcutils/logging_macros.h" + +constexpr size_t ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops + +namespace joint_limits +{ +template <> +SimpleJointLimiter::SimpleJointLimiter() +: joint_limits::JointLimiterInterface() +{ +} + +template <> +bool SimpleJointLimiter::on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) +{ + auto num_joints = joint_limits_.size(); + auto dt_seconds = dt.seconds(); + + if (current_joint_states.velocities.empty()) + { + // First update() after activating does not have velocity available, assume 0 + current_joint_states.velocities.resize(num_joints, 0.0); + } + + // Clamp velocities to limits + for (auto index = 0u; index < num_joints; ++index) + { + if (joint_limits_[index].has_velocity_limits) + { + if (std::abs(desired_joint_states.velocities[index]) > joint_limits_[index].max_velocity) + { + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, + "Joint(s) would exceed velocity limits, limiting"); + desired_joint_states.velocities[index] = + copysign(joint_limits_[index].max_velocity, desired_joint_states.velocities[index]); + double accel = + (desired_joint_states.velocities[index] - current_joint_states.velocities[index]) / + dt_seconds; + // Recompute position + desired_joint_states.positions[index] = + current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * accel * dt_seconds * dt_seconds; + } + } + } + + // Clamp acclerations to limits + for (auto index = 0u; index < num_joints; ++index) + { + if (joint_limits_[index].has_acceleration_limits) + { + double accel = + (desired_joint_states.velocities[index] - current_joint_states.velocities[index]) / + dt_seconds; + if (std::abs(accel) > joint_limits_[index].max_acceleration) + { + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, + "Joint(s) would exceed acceleration limits, limiting"); + desired_joint_states.velocities[index] = + current_joint_states.velocities[index] + + copysign(joint_limits_[index].max_acceleration, accel) * dt_seconds; + // Recompute position + desired_joint_states.positions[index] = + current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * copysign(joint_limits_[index].max_acceleration, accel) * dt_seconds * dt_seconds; + } + } + } + + // Check that stopping distance is within joint limits + // - In joint mode, slow down only joints whose stopping distance isn't inside joint limits, + // at maximum decel + // - In Cartesian mode, slow down all joints at maximum decel if any don't have stopping distance + // within joint limits + bool position_limit_triggered = false; + for (auto index = 0u; index < num_joints; ++index) + { + if (joint_limits_[index].has_acceleration_limits) + { + // delta_x = (v2*v2 - v1*v1) / (2*a) + // stopping_distance = (- v1*v1) / (2*max_acceleration) + // Here we assume we will not trigger velocity limits while maximally decelerating. + // This is a valid assumption if we are not currently at a velocity limit since we are just + // coming to a rest. + double stopping_distance = std::abs( + (-desired_joint_states.velocities[index] * desired_joint_states.velocities[index]) / + (2 * joint_limits_[index].max_acceleration)); + // Check that joint limits are beyond stopping_distance and desired_velocity is towards + // that limit + // TODO(anyone): Should we consider sign on acceleration here? + if ( + (desired_joint_states.velocities[index] < 0 && + (current_joint_states.positions[index] - joint_limits_[index].min_position < + stopping_distance)) || + (desired_joint_states.velocities[index] > 0 && + (joint_limits_[index].max_position - current_joint_states.positions[index] < + stopping_distance))) + { + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, + "Joint(s) would exceed position limits, limiting"); + position_limit_triggered = true; + + // We will limit all joints + break; + } + } + } + + if (position_limit_triggered) + { + // In Cartesian admittance mode, stop all joints if one would exceed limit + for (auto index = 0u; index < num_joints; ++index) + { + if (joint_limits_[index].has_acceleration_limits) + { + // Compute accel to stop + // Here we aren't explicitly maximally decelerating, but for joints near their limits this + // should still result in max decel being used + double accel_to_stop = -current_joint_states.velocities[index] / dt_seconds; + double limited_accel = copysign( + std::min(std::abs(accel_to_stop), joint_limits_[index].max_acceleration), accel_to_stop); + + desired_joint_states.velocities[index] = + current_joint_states.velocities[index] + limited_accel * dt_seconds; + // Recompute position + desired_joint_states.positions[index] = + current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * limited_accel * dt_seconds * dt_seconds; + } + } + } + return true; +} + +} // namespace joint_limits + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + joint_limits::SimpleJointLimiter, + joint_limits::JointLimiterInterface) diff --git a/joint_limits/test/test_simple_joint_limiter.cpp b/joint_limits/test/test_simple_joint_limiter.cpp new file mode 100644 index 0000000000..e025ac0b9f --- /dev/null +++ b/joint_limits/test/test_simple_joint_limiter.cpp @@ -0,0 +1,42 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#include + +#include +#include + +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/executor.hpp" + +TEST(TestLoadSimpleJointLimiter, load_limiter) +{ + rclcpp::init(0, nullptr); + + using JointLimiter = joint_limits::JointLimiterInterface; + pluginlib::ClassLoader joint_limiter_loader( + "joint_limits", "joint_limits::JointLimiterInterface"); + + std::unique_ptr joint_limiter; + std::string joint_limiter_type = "joint_limits/SimpleJointLimiter"; + + ASSERT_NO_THROW( + joint_limiter = std::unique_ptr( + joint_limiter_loader.createUnmanagedInstance(joint_limiter_type))); + ASSERT_NE(joint_limiter, nullptr); +} diff --git a/joint_limits_enforcement_plugins/CMakeLists.txt b/joint_limits_enforcement_plugins/CMakeLists.txt new file mode 100644 index 0000000000..d66a41bc8b --- /dev/null +++ b/joint_limits_enforcement_plugins/CMakeLists.txt @@ -0,0 +1,66 @@ +cmake_minimum_required(VERSION 3.16) +project(joint_limits_enforcement_plugins LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + joint_limits + pluginlib + rclcpp + rcutils + ruckig +) + +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +add_library(ruckig_joint_limiter SHARED + src/ruckig_joint_limiter.cpp +) +target_compile_features(ruckig_joint_limiter PUBLIC cxx_std_17) +target_include_directories(ruckig_joint_limiter PUBLIC + $ + $ +) +ament_target_dependencies(ruckig_joint_limiter PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(ruckig_joint_limiter PRIVATE "JOINT_LIMITS_ENFORCEMENT_PLUGINS_BUILDING_DLL") + +pluginlib_export_plugin_description_file(joint_limits joint_limits_enforcement_plugins.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(joint_limits REQUIRED) + find_package(pluginlib REQUIRED) + find_package(rclcpp REQUIRED) + + ament_add_gmock(test_ruckig_joint_limiter test/test_ruckig_joint_limiter.cpp) + target_include_directories(test_ruckig_joint_limiter PRIVATE include) + ament_target_dependencies( + test_ruckig_joint_limiter + joint_limits + pluginlib + rclcpp + ) +endif() + +install(DIRECTORY include/ + DESTINATION include/joint_limits_enforcement_plugins +) +install( + TARGETS + ruckig_joint_limiter + EXPORT export_joint_limits_enforcement_plugins + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_targets(export_joint_limits_enforcement_plugins HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h b/joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h new file mode 100644 index 0000000000..abd0019cf6 --- /dev/null +++ b/joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_LIMITS_ENFORCEMENT_PLUGINS__VISIBILITY_CONTROL_H_ +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT __attribute__((dllexport)) +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT __attribute__((dllimport)) +#else +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT __declspec(dllexport) +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT __declspec(dllimport) +#endif +#ifdef JOINT_LIMITS_ENFORCEMENT_PLUGINS_BUILDING_DLL +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT +#else +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT +#endif +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC_TYPE JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_LOCAL +#else +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT __attribute__((visibility("default"))) +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT +#if __GNUC__ >= 4 +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC __attribute__((visibility("default"))) +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_LOCAL __attribute__((visibility("hidden"))) +#else +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_LOCAL +#endif +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC_TYPE +#endif + +#endif // JOINT_LIMITS_ENFORCEMENT_PLUGINS__VISIBILITY_CONTROL_H_ diff --git a/joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp b/joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp new file mode 100644 index 0000000000..fd577c32b3 --- /dev/null +++ b/joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp @@ -0,0 +1,64 @@ +// Copyright (c) 2021, PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Andy Zelenak, Denis Stogl + +#ifndef RUCKIG_JOINT_LIMITER__RUCKIG_JOINT_LIMITER_HPP_ +#define RUCKIG_JOINT_LIMITER__RUCKIG_JOINT_LIMITER_HPP_ + +#include + +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "joint_limits_enforcement_plugins/visibility_control.h" +#include "ruckig/input_parameter.hpp" +#include "ruckig/output_parameter.hpp" +#include "ruckig/ruckig.hpp" + +namespace ruckig_joint_limiter +{ +namespace // utility namespace +{ +constexpr double DEFAULT_MAX_VELOCITY = 5; // rad/s +constexpr double DEFAULT_MAX_ACCELERATION = 10; // rad/s^2 +constexpr double DEFAULT_MAX_JERK = 20; // rad/s^3 +} // namespace + +template +class RuckigJointLimiter : public joint_limits::JointLimiterInterface +{ +public: + JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC RuckigJointLimiter(); + + JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC bool on_init() override; + + JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC bool on_configure( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) override; + + JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC bool on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, + const rclcpp::Duration & dt) override; + +private: + bool received_initial_state_ = false; + // Ruckig algorithm + std::shared_ptr> ruckig_; + std::shared_ptr> ruckig_input_; + std::shared_ptr> ruckig_output_; +}; + +} // namespace ruckig_joint_limiter + +#endif // RUCKIG_JOINT_LIMITER__RUCKIG_JOINT_LIMITER_HPP_ diff --git a/joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml b/joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml new file mode 100644 index 0000000000..aa2996282f --- /dev/null +++ b/joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml @@ -0,0 +1,9 @@ + + + + Jerk-limited smoothing with the Ruckig library. + + + diff --git a/joint_limits_enforcement_plugins/package.xml b/joint_limits_enforcement_plugins/package.xml new file mode 100644 index 0000000000..16a241767a --- /dev/null +++ b/joint_limits_enforcement_plugins/package.xml @@ -0,0 +1,31 @@ + + joint_limits_enforcement_plugins + 0.0.0 + Package for handling of joint limits using external libraries for use in controllers or in hardware. + + Bence Magyar + Denis Štogl + + Denis Štogl + Andy Zelenak + + Apache License 2.0 + + https://github.com/ros-controls/ros2_control/wiki + https://github.com/ros-controls/ros2_control/issues + https://github.com/ros-controls/ros2_control + + ament_cmake + + joint_limits + rclcpp + pluginlib + rcutils + ruckig + + ament_cmake_gmock + + + ament_cmake + + diff --git a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp new file mode 100644 index 0000000000..1d6dc33d73 --- /dev/null +++ b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp @@ -0,0 +1,123 @@ +// Copyright (c) 2021, PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \authors Andy Zelenak, Denis Stogl + +#include "ruckig_joint_limiter/ruckig_joint_limiter.hpp" + +#include +#include + +#include "joint_limits/joint_limits_rosparam.hpp" +#include "rcutils/logging_macros.h" +#include "ruckig/input_parameter.hpp" +#include "ruckig/output_parameter.hpp" +#include "ruckig/ruckig.hpp" + +namespace ruckig_joint_limiter +{ +template <> +RuckigJointLimiter::RuckigJointLimiter() +: joint_limits::JointLimiterInterface() +{ +} + +template <> +bool RuckigJointLimiter::on_init() +{ + // Initialize Ruckig + ruckig_ = std::make_shared>(number_of_joints_, 0.01 /*timestep*/); + ruckig_input_ = std::make_shared>(number_of_joints_); + ruckig_output_ = std::make_shared>(number_of_joints_); + + // Velocity mode works best for smoothing one waypoint at a time + ruckig_input_->control_interface = ruckig::ControlInterface::Velocity; + + for (auto joint = 0ul; joint < number_of_joints_; ++joint) + { + if (joint_limits_[joint].has_jerk_limits) + { + ruckig_input_->max_jerk.at(joint) = joint_limits_[joint].max_acceleration; + } + if (joint_limits_[joint].has_acceleration_limits) + { + ruckig_input_->max_acceleration.at(joint) = joint_limits_[joint].max_acceleration; + } + if (joint_limits_[joint].has_velocity_limits) + { + ruckig_input_->max_velocity.at(joint) = joint_limits_[joint].max_velocity; + } + } + + RCUTILS_LOG_INFO_NAMED("ruckig_joint_limiter", "Successfully initialized."); + + return true; +} + +template <> +bool RuckigJointLimiter::on_configure( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) +{ + // Initialize Ruckig with current_joint_states + std::copy_n( + current_joint_states.positions.begin(), number_of_joints_, + ruckig_input_->current_position.begin()); + std::copy_n( + current_joint_states.velocities.begin(), number_of_joints_, + ruckig_input_->current_velocity.begin()); + std::copy_n( + current_joint_states.accelerations.begin(), number_of_joints_, + ruckig_input_->current_acceleration.begin()); + + // Initialize output data + ruckig_output_->new_position = ruckig_input_->current_position; + ruckig_output_->new_velocity = ruckig_input_->current_velocity; + ruckig_output_->new_acceleration = ruckig_input_->current_acceleration; + + return true; +} + +template <> +bool RuckigJointLimiter::on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, + const rclcpp::Duration & /*dt*/) +{ + // We don't use current_joint_states. For stability, it is recommended to feed previous Ruckig + // output back in as input for the next iteration. This assumes the robot tracks the command well. + + // Feed output from the previous timestep back as input + for (auto joint = 0ul; joint < number_of_joints_; ++joint) + { + ruckig_input_->current_position.at(joint) = ruckig_output_->new_position.at(joint); + ruckig_input_->current_velocity.at(joint) = ruckig_output_->new_velocity.at(joint); + ruckig_input_->current_acceleration.at(joint) = ruckig_output_->new_acceleration.at(joint); + + // Target state is the next waypoint + ruckig_input_->target_velocity.at(joint) = desired_joint_states.velocities.at(joint); + ruckig_input_->target_acceleration.at(joint) = desired_joint_states.accelerations.at(joint); + } + + ruckig::Result result = ruckig_->update(*ruckig_input_, *ruckig_output_); + + return result == ruckig::Result::Finished; +} + +} // namespace ruckig_joint_limiter + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ruckig_joint_limiter::RuckigJointLimiter, + joint_limits::JointLimiterInterface) diff --git a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp new file mode 100644 index 0000000000..b1b19d0587 --- /dev/null +++ b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp @@ -0,0 +1,44 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#include + +#include +#include + +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/executor.hpp" + +TEST(TestLoadSimpleJointLimiter, load_limiter) +{ + rclcpp::init(0, nullptr); + + using JointLimiter = joint_limits::JointLimiterInterface; + pluginlib::ClassLoader joint_limiter_loader( + "joint_limits", "joint_limits::JointLimiterInterface"); + + std::unique_ptr joint_limiter; + std::string joint_limiter_type = "ruckig_joint_limiter/RuckigJointLimiter"; + + joint_limiter = + std::unique_ptr(joint_limiter_loader.createUnmanagedInstance(joint_limiter_type)); + ASSERT_NO_THROW( + joint_limiter = std::unique_ptr( + joint_limiter_loader.createUnmanagedInstance(joint_limiter_type))); + ASSERT_NE(joint_limiter, nullptr); +} From bbd9d59c855e9499fbb1d16083ee62dcab2b9c44 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 24 Sep 2021 19:44:52 +0200 Subject: [PATCH 03/37] Debug Ruckig JointLimiter. Debug and optimize Rucking JointLimiter. --- .../joint_limits/joint_limiter_interface.hpp | 1 + .../src/ruckig_joint_limiter.cpp | 101 ++++++++++++++---- 2 files changed, 81 insertions(+), 21 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index f1207eaf30..0aaf3ee57e 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -55,6 +55,7 @@ class JointLimiterInterface JOINT_LIMITS_PUBLIC virtual bool configure( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) { + // TODO(destogl): add checks for position return on_configure(current_joint_states); } diff --git a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp index 1d6dc33d73..ad264aed67 100644 --- a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp +++ b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp @@ -18,9 +18,11 @@ #include #include +#include #include "joint_limits/joint_limits_rosparam.hpp" #include "rcutils/logging_macros.h" +#include "ruckig/brake.hpp" #include "ruckig/input_parameter.hpp" #include "ruckig/output_parameter.hpp" #include "ruckig/ruckig.hpp" @@ -34,15 +36,19 @@ RuckigJointLimiter::RuckigJointLimiter() } template <> -bool RuckigJointLimiter::on_init() +bool RuckigJointLimiter::on_init(/*const rclcpp::Duration & dt*/) { + // TODO(destogl): This should be used from parameter + const rclcpp::Duration dt = rclcpp::Duration::from_seconds(0.005); + // Initialize Ruckig - ruckig_ = std::make_shared>(number_of_joints_, 0.01 /*timestep*/); + ruckig_ = std::make_shared>(number_of_joints_, dt.seconds()); ruckig_input_ = std::make_shared>(number_of_joints_); ruckig_output_ = std::make_shared>(number_of_joints_); // Velocity mode works best for smoothing one waypoint at a time ruckig_input_->control_interface = ruckig::ControlInterface::Velocity; + ruckig_input_->synchronization = ruckig::Synchronization::Time; for (auto joint = 0ul; joint < number_of_joints_; ++joint) { @@ -50,18 +56,28 @@ bool RuckigJointLimiter::on_init() { ruckig_input_->max_jerk.at(joint) = joint_limits_[joint].max_acceleration; } + else + { + ruckig_input_->max_jerk.at(joint) = DEFAULT_MAX_JERK; + } if (joint_limits_[joint].has_acceleration_limits) { ruckig_input_->max_acceleration.at(joint) = joint_limits_[joint].max_acceleration; } + else + { + ruckig_input_->max_acceleration.at(joint) = DEFAULT_MAX_ACCELERATION; + } if (joint_limits_[joint].has_velocity_limits) { ruckig_input_->max_velocity.at(joint) = joint_limits_[joint].max_velocity; } + else + { + ruckig_input_->max_velocity.at(joint) = DEFAULT_MAX_VELOCITY; + } } - RCUTILS_LOG_INFO_NAMED("ruckig_joint_limiter", "Successfully initialized."); - return true; } @@ -70,15 +86,24 @@ bool RuckigJointLimiter::on_configure( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) { // Initialize Ruckig with current_joint_states - std::copy_n( - current_joint_states.positions.begin(), number_of_joints_, - ruckig_input_->current_position.begin()); - std::copy_n( - current_joint_states.velocities.begin(), number_of_joints_, - ruckig_input_->current_velocity.begin()); - std::copy_n( - current_joint_states.accelerations.begin(), number_of_joints_, - ruckig_input_->current_acceleration.begin()); + ruckig_input_->current_position = current_joint_states.positions; + + if (current_joint_states.velocities.size() == number_of_joints_) + { + ruckig_input_->current_velocity = current_joint_states.velocities; + } + else + { + ruckig_input_->current_velocity = std::vector(number_of_joints_, 0.0); + } + if (current_joint_states.accelerations.size() == number_of_joints_) + { + ruckig_input_->current_acceleration = current_joint_states.accelerations; + } + else + { + ruckig_input_->current_acceleration = std::vector(number_of_joints_, 0.0); + } // Initialize output data ruckig_output_->new_position = ruckig_input_->current_position; @@ -96,20 +121,54 @@ bool RuckigJointLimiter::on_enforce( { // We don't use current_joint_states. For stability, it is recommended to feed previous Ruckig // output back in as input for the next iteration. This assumes the robot tracks the command well. + ruckig_input_->current_position = ruckig_output_->new_position; + ruckig_input_->current_velocity = ruckig_output_->new_velocity; + ruckig_input_->current_acceleration = ruckig_output_->new_acceleration; + + // Target state is the next waypoint + ruckig_input_->target_position = desired_joint_states.positions; + // TODO(destogl): in current use-case we use only velocity + if (desired_joint_states.velocities.size() == number_of_joints_) + { + ruckig_input_->target_velocity = desired_joint_states.velocities; + } + else + { + ruckig_input_->target_velocity = std::vector(number_of_joints_, 0.0); + } + if (desired_joint_states.accelerations.size() == number_of_joints_) + { + ruckig_input_->target_acceleration = desired_joint_states.accelerations; + } + else + { + ruckig_input_->target_acceleration = std::vector(number_of_joints_, 0.0); + } + + ruckig::Result result = ruckig_->update(*ruckig_input_, *ruckig_output_); + RCUTILS_LOG_DEBUG_NAMED("ruckig_joint_limiter", "Rucking result %d", result); + + desired_joint_states.positions = ruckig_output_->new_position; + desired_joint_states.velocities = ruckig_output_->new_velocity; + desired_joint_states.accelerations = ruckig_output_->new_acceleration; // Feed output from the previous timestep back as input for (auto joint = 0ul; joint < number_of_joints_; ++joint) { - ruckig_input_->current_position.at(joint) = ruckig_output_->new_position.at(joint); - ruckig_input_->current_velocity.at(joint) = ruckig_output_->new_velocity.at(joint); - ruckig_input_->current_acceleration.at(joint) = ruckig_output_->new_acceleration.at(joint); - - // Target state is the next waypoint - ruckig_input_->target_velocity.at(joint) = desired_joint_states.velocities.at(joint); - ruckig_input_->target_acceleration.at(joint) = desired_joint_states.accelerations.at(joint); + RCUTILS_LOG_DEBUG_NAMED( + "ruckig_joint_limiter", + "Desired position: %e \nDesired velocity: %e\n Desired acceleration: %e.", + ruckig_input_->target_position.at(joint), ruckig_input_->target_velocity.at(joint), + ruckig_input_->target_acceleration.at(joint)); } - ruckig::Result result = ruckig_->update(*ruckig_input_, *ruckig_output_); + for (auto joint = 0ul; joint < number_of_joints_; ++joint) + { + RCUTILS_LOG_DEBUG_NAMED( + "ruckig_joint_limiter", "New position: %e \nNew velocity: %e\nNew acceleration: %e.", + ruckig_output_->new_position.at(joint), ruckig_output_->new_velocity.at(joint), + ruckig_output_->new_acceleration.at(joint)); + } return result == ruckig::Result::Finished; } From ee7faf209427ec9758c7fae554ab04c36856b042 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 9 Feb 2023 18:42:44 +0100 Subject: [PATCH 04/37] Modify simple joint limiting plugin (same as changes to moveit2 filter) (#6) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Merge error handling possilibity on read and write. * Ros2 control extensions rolling joint limits plugins (#5) * Added initial structures for joint-limit plugins. * Move Ruckig limiter to package joint_limits_enforcement_plugins and make it working. Co-authored-by: AndyZe * Add option to automatically update parameters after getting them from parameter server. * Modify simple joint limiting plugin (same as changes to moveit2 filter) * Add backward_ros dependency for crash stack trace * Check for required inputs in simple joint limiter * Change services history QOS to 'keep all' so client req are not dropped * Add missing on 'pluginlib' dependency explicitly. * Update ControllerParameters structure to support custom prefix and use in filters. * Update messge. * Change controller param changed msg log level to info instead of error --------- Co-authored-by: Denis Štogl Co-authored-by: AndyZe Co-authored-by: bijoua Co-authored-by: bijoua29 <73511637+bijoua29@users.noreply.github.com> --- controller_interface/CMakeLists.txt | 1 + .../controller_parameters.hpp | 212 ------------------ controller_interface/package.xml | 1 + .../src/controller_parameters.cpp | 119 ---------- hardware_interface/CMakeLists.txt | 1 + hardware_interface/package.xml | 1 + joint_limits/CMakeLists.txt | 1 + .../joint_limits/joint_limiter_interface.hpp | 1 + joint_limits/package.xml | 1 + joint_limits/src/joint_limiter_interface.cpp | 1 + joint_limits/src/simple_joint_limiter.cpp | 187 +++++++++------ .../CMakeLists.txt | 1 + joint_limits_enforcement_plugins/package.xml | 1 + joint_limits_interface/CMakeLists.txt | 1 + joint_limits_interface/package.xml | 1 + 15 files changed, 128 insertions(+), 402 deletions(-) delete mode 100644 controller_interface/include/controller_interface/controller_parameters.hpp delete mode 100644 controller_interface/src/controller_parameters.cpp diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 034556d19f..698c66b97a 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -11,6 +11,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/controller_interface/include/controller_interface/controller_parameters.hpp b/controller_interface/include/controller_interface/controller_parameters.hpp deleted file mode 100644 index 93368688c5..0000000000 --- a/controller_interface/include/controller_interface/controller_parameters.hpp +++ /dev/null @@ -1,212 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -/// \author: Denis Stogl - -#ifndef CONTROLLER_INTERFACE__CONTROLLER_PARAMETERS_HPP_ -#define CONTROLLER_INTERFACE__CONTROLLER_PARAMETERS_HPP_ - -#include -#include -#include -#include - -#include "rclcpp/node.hpp" -#include "rcutils/logging_macros.h" - -namespace controller_interface -{ -struct Parameter -{ - Parameter() = default; - - explicit Parameter(const std::string & name, bool configurable = false) - : name(name), configurable(configurable) - { - } - - std::string name; - bool configurable; -}; - -class ControllerParameters -{ -public: - ControllerParameters( - int nr_bool_params = 0, int nr_double_params = 0, int nr_string_params = 0, - int nr_string_list_params = 0); - - virtual ~ControllerParameters() = default; - - virtual void declare_parameters(rclcpp::Node::SharedPtr node); - - /** - * Gets all defined parameters from. - * - * \param[node] shared pointer to the node where parameters should be read. - * \return true if all parameters are read Successfully, false if a parameter is not provided or - * parameter configuration is wrong. - */ - virtual bool get_parameters(rclcpp::Node::SharedPtr node, bool check_validity = true); - - virtual rcl_interfaces::msg::SetParametersResult set_parameter_callback( - const std::vector & parameters); - - /** - * Default implementation of parameter check. No check is done. Always returns true. - * - * \return true - */ - virtual bool check_if_parameters_are_valid() { return true; } - - virtual void update() = 0; - - // TODO(destogl): Test this: "const rclcpp::Node::SharedPtr & node" - -protected: - void add_bool_parameter( - const std::string & name, bool configurable = false, bool default_value = false) - { - bool_parameters_.push_back({Parameter(name, configurable), default_value}); - } - - void add_double_parameter( - const std::string & name, bool configurable = false, - double default_value = std::numeric_limits::quiet_NaN()) - { - double_parameters_.push_back({Parameter(name, configurable), default_value}); - } - - void add_string_parameter( - const std::string & name, bool configurable = false, const std::string & default_value = "") - { - string_parameters_.push_back({Parameter(name, configurable), default_value}); - } - - void add_string_list_parameter( - const std::string & name, bool configurable = false, - const std::vector & default_value = {}) - { - string_list_parameters_.push_back({Parameter(name, configurable), default_value}); - } - - template - bool get_parameters_from_list( - const rclcpp::Node::SharedPtr node, std::vector> & parameters) - { - bool ret = true; - for (auto & parameter : parameters) - { - try - { - rclcpp::Parameter input_parameter; // TODO(destogl): will this be allocated each time? - ret = node->get_parameter(parameter.first.name, input_parameter); - parameter.second = input_parameter.get_value(); - } - catch (rclcpp::ParameterTypeException & e) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str()); - ret = false; - break; - } - } - return ret; - } - - template - bool empty_parameter_in_list(const std::vector> & parameters) - { - bool ret = false; - for (const auto & parameter : parameters) - { - if (parameter.second.empty()) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter is empty", parameter.first.name.c_str()); - ret = true; - } - } - return ret; - } - - bool empty_parameter_in_list(const std::vector> & parameters) - { - bool ret = false; - for (const auto & parameter : parameters) - { - if (std::isnan(parameter.second)) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter is not set", parameter.first.name.c_str()); - ret = true; - } - } - return ret; - } - - template - bool find_and_assign_parameter_value( - std::vector> & parameter_list, - const rclcpp::Parameter & input_parameter) - { - auto is_in_list = [&](const auto & parameter) - { return parameter.first.name == input_parameter.get_name(); }; - - auto it = std::find_if(parameter_list.begin(), parameter_list.end(), is_in_list); - - if (it != parameter_list.end()) - { - if (!it->first.configurable) - { - throw std::runtime_error( - "Parameter " + input_parameter.get_name() + " is not configurable."); - } - else - { - it->second = input_parameter.get_value(); - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter is updated to value: %s", it->first.name.c_str(), - input_parameter.value_to_string().c_str()); - return true; - } - } - else - { - return false; - } - } - - std::vector> bool_parameters_; - std::vector> double_parameters_; - std::vector> string_parameters_; - std::vector>> string_list_parameters_; - - std::string logger_name_; - -private: - template - void declare_parameters_from_list( - rclcpp::Node::SharedPtr node, const std::vector> & parameters) - { - for (const auto & parameter : parameters) - { - node->declare_parameter(parameter.first.name, parameter.second); - } - } -}; - -} // namespace controller_interface - -#endif // CONTROLLER_INTERFACE__CONTROLLER_PARAMETERS_HPP_ diff --git a/controller_interface/package.xml b/controller_interface/package.xml index b49dead034..be660f7f61 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -10,6 +10,7 @@ ament_cmake + backward_ros hardware_interface rclcpp_lifecycle sensor_msgs diff --git a/controller_interface/src/controller_parameters.cpp b/controller_interface/src/controller_parameters.cpp deleted file mode 100644 index efb87c2dba..0000000000 --- a/controller_interface/src/controller_parameters.cpp +++ /dev/null @@ -1,119 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -/// \author: Denis Stogl - -#include "controller_interface/controller_parameters.hpp" - -#include -#include - -#include "rcutils/logging_macros.h" - -namespace controller_interface -{ -ControllerParameters::ControllerParameters( - int nr_bool_params, int nr_double_params, int nr_string_params, int nr_string_list_params) -{ - bool_parameters_.reserve(nr_bool_params); - double_parameters_.reserve(nr_double_params); - string_parameters_.reserve(nr_string_params); - string_list_parameters_.reserve(nr_string_list_params); -} - -void ControllerParameters::declare_parameters(rclcpp::Node::SharedPtr node) -{ - logger_name_ = std::string(node->get_name()) + "::parameters"; - - declare_parameters_from_list(node, bool_parameters_); - declare_parameters_from_list(node, double_parameters_); - declare_parameters_from_list(node, string_parameters_); - declare_parameters_from_list(node, string_list_parameters_); -} - -/** - * Gets all defined parameters from. - * - * \param[node] shared pointer to the node where parameters should be read. - * \return true if all parameters are read Successfully, false if a parameter is not provided or - * parameter configuration is wrong. - */ -bool ControllerParameters::get_parameters(rclcpp::Node::SharedPtr node, bool check_validity) -{ - bool ret = false; - - ret = get_parameters_from_list(node, bool_parameters_) && - get_parameters_from_list(node, double_parameters_) && - get_parameters_from_list(node, string_parameters_) && - get_parameters_from_list(node, string_list_parameters_); - - if (ret && check_validity) - { - ret = check_if_parameters_are_valid(); - } - - return ret; -} - -rcl_interfaces::msg::SetParametersResult ControllerParameters::set_parameter_callback( - const std::vector & parameters) -{ - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - - for (const auto & input_parameter : parameters) - { - bool found = false; - - try - { - found = find_and_assign_parameter_value(bool_parameters_, input_parameter); - if (!found) - { - found = find_and_assign_parameter_value(double_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(string_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(string_list_parameters_, input_parameter); - } - - RCUTILS_LOG_INFO_EXPRESSION_NAMED( - found, logger_name_.c_str(), - "Dynamic parameters got changed! To update the parameters internally please " - "restart the controller."); - } - catch (const rclcpp::exceptions::InvalidParameterTypeException & e) - { - result.successful = false; - result.reason = e.what(); - RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str()); - break; - } - catch (const std::runtime_error & e) - { - result.successful = false; - result.reason = e.what(); - RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str()); - break; - } - } - - return result; -} - -} // namespace controller_interface diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 77eec3ec86..9945f1b541 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -17,6 +17,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index e8ade083e0..8950dfaf81 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -9,6 +9,7 @@ ament_cmake + backward_ros control_msgs lifecycle_msgs pluginlib diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index b88f677e5e..f81f15c21a 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -13,6 +13,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 0aaf3ee57e..85425ff8a1 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -84,6 +84,7 @@ class JointLimiterInterface protected: size_t number_of_joints_; + std::vector joint_names_; std::vector joint_limits_; rclcpp::Node::SharedPtr node_; }; diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 8ab62e458b..ec13b44445 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -14,6 +14,7 @@ ament_cmake + backward_ros pluginlib rclcpp rclcpp_lifecycle diff --git a/joint_limits/src/joint_limiter_interface.cpp b/joint_limits/src/joint_limiter_interface.cpp index b0d109b0cd..b3b4d65bb6 100644 --- a/joint_limits/src/joint_limiter_interface.cpp +++ b/joint_limits/src/joint_limiter_interface.cpp @@ -30,6 +30,7 @@ bool JointLimiterInterface::init( const std::string & /*robot_description_topic*/) { number_of_joints_ = joint_names.size(); + joint_names_ = joint_names; joint_limits_.resize(number_of_joints_); node_ = node; diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 5106615ea8..8ebcfde77b 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -45,119 +45,164 @@ bool SimpleJointLimiter::on_enforce( current_joint_states.velocities.resize(num_joints, 0.0); } - // Clamp velocities to limits + // check for required inputs + if ( + (desired_joint_states.positions.size() < num_joints) || + (desired_joint_states.velocities.size() < num_joints) || + (current_joint_states.positions.size() < num_joints)) + { + return false; + } + + std::vector desired_accel(num_joints); + std::vector desired_vel(num_joints); + std::vector desired_pos(num_joints); + std::vector pos_limit_trig_jnts(num_joints, false); + std::vector limited_jnts_vel, limited_jnts_acc; + + bool position_limit_triggered = false; + for (auto index = 0u; index < num_joints; ++index) { + desired_pos[index] = desired_joint_states.positions[index]; + + // limit position + if (joint_limits_[index].has_position_limits) + { + auto pos = std::max( + std::min(joint_limits_[index].max_position, desired_pos[index]), + joint_limits_[index].min_position); + if (pos != desired_pos[index]) + { + pos_limit_trig_jnts[index] = true; + desired_pos[index] = pos; + } + } + + desired_vel[index] = desired_joint_states.velocities[index]; + + // limit velocity if (joint_limits_[index].has_velocity_limits) { - if (std::abs(desired_joint_states.velocities[index]) > joint_limits_[index].max_velocity) + if (std::abs(desired_vel[index]) > joint_limits_[index].max_velocity) { - RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, - "Joint(s) would exceed velocity limits, limiting"); - desired_joint_states.velocities[index] = - copysign(joint_limits_[index].max_velocity, desired_joint_states.velocities[index]); - double accel = - (desired_joint_states.velocities[index] - current_joint_states.velocities[index]) / - dt_seconds; - // Recompute position - desired_joint_states.positions[index] = - current_joint_states.positions[index] + - current_joint_states.velocities[index] * dt_seconds + - 0.5 * accel * dt_seconds * dt_seconds; + desired_vel[index] = std::copysign(joint_limits_[index].max_velocity, desired_vel[index]); + limited_jnts_vel.emplace_back(joint_names_[index]); } } - } - // Clamp acclerations to limits - for (auto index = 0u; index < num_joints; ++index) - { + desired_accel[index] = + (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; + + // limit acceleration if (joint_limits_[index].has_acceleration_limits) { - double accel = - (desired_joint_states.velocities[index] - current_joint_states.velocities[index]) / - dt_seconds; - if (std::abs(accel) > joint_limits_[index].max_acceleration) + if (std::abs(desired_accel[index]) > joint_limits_[index].max_acceleration) { - RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, - "Joint(s) would exceed acceleration limits, limiting"); - desired_joint_states.velocities[index] = - current_joint_states.velocities[index] + - copysign(joint_limits_[index].max_acceleration, accel) * dt_seconds; - // Recompute position - desired_joint_states.positions[index] = - current_joint_states.positions[index] + - current_joint_states.velocities[index] * dt_seconds + - 0.5 * copysign(joint_limits_[index].max_acceleration, accel) * dt_seconds * dt_seconds; + desired_accel[index] = + std::copysign(joint_limits_[index].max_acceleration, desired_accel[index]); + desired_vel[index] = + current_joint_states.velocities[index] + desired_accel[index] * dt_seconds; + // recalc desired position after acceleration limiting + desired_pos[index] = current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * desired_accel[index] * dt_seconds * dt_seconds; + limited_jnts_acc.emplace_back(joint_names_[index]); } } - } - // Check that stopping distance is within joint limits - // - In joint mode, slow down only joints whose stopping distance isn't inside joint limits, - // at maximum decel - // - In Cartesian mode, slow down all joints at maximum decel if any don't have stopping distance - // within joint limits - bool position_limit_triggered = false; - for (auto index = 0u; index < num_joints; ++index) - { - if (joint_limits_[index].has_acceleration_limits) + // Check that stopping distance is within joint limits + // Slow down all joints at maximum decel if any don't have stopping distance within joint limits + if (joint_limits_[index].has_position_limits) { // delta_x = (v2*v2 - v1*v1) / (2*a) // stopping_distance = (- v1*v1) / (2*max_acceleration) // Here we assume we will not trigger velocity limits while maximally decelerating. // This is a valid assumption if we are not currently at a velocity limit since we are just // coming to a rest. - double stopping_distance = std::abs( - (-desired_joint_states.velocities[index] * desired_joint_states.velocities[index]) / - (2 * joint_limits_[index].max_acceleration)); + double stopping_accel = joint_limits_[index].has_acceleration_limits + ? joint_limits_[index].max_acceleration + : std::abs(desired_vel[index] / dt_seconds); + double stopping_distance = + std::abs((-desired_vel[index] * desired_vel[index]) / (2 * stopping_accel)); // Check that joint limits are beyond stopping_distance and desired_velocity is towards // that limit - // TODO(anyone): Should we consider sign on acceleration here? if ( - (desired_joint_states.velocities[index] < 0 && + (desired_vel[index] < 0 && (current_joint_states.positions[index] - joint_limits_[index].min_position < stopping_distance)) || - (desired_joint_states.velocities[index] > 0 && + (desired_vel[index] > 0 && (joint_limits_[index].max_position - current_joint_states.positions[index] < stopping_distance))) { - RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, - "Joint(s) would exceed position limits, limiting"); + pos_limit_trig_jnts[index] = true; position_limit_triggered = true; - - // We will limit all joints - break; } } } if (position_limit_triggered) { - // In Cartesian admittance mode, stop all joints if one would exceed limit + std::ostringstream ostr; for (auto index = 0u; index < num_joints; ++index) { + // Compute accel to stop + // Here we aren't explicitly maximally decelerating, but for joints near their limits this + // should still result in max decel being used + desired_accel[index] = -current_joint_states.velocities[index] / dt_seconds; if (joint_limits_[index].has_acceleration_limits) { - // Compute accel to stop - // Here we aren't explicitly maximally decelerating, but for joints near their limits this - // should still result in max decel being used - double accel_to_stop = -current_joint_states.velocities[index] / dt_seconds; - double limited_accel = copysign( - std::min(std::abs(accel_to_stop), joint_limits_[index].max_acceleration), accel_to_stop); - - desired_joint_states.velocities[index] = - current_joint_states.velocities[index] + limited_accel * dt_seconds; - // Recompute position - desired_joint_states.positions[index] = - current_joint_states.positions[index] + - current_joint_states.velocities[index] * dt_seconds + - 0.5 * limited_accel * dt_seconds * dt_seconds; + desired_accel[index] = std::copysign( + std::min(std::abs(desired_accel[index]), joint_limits_[index].max_acceleration), + desired_accel[index]); } + + // Recompute velocity and position + desired_vel[index] = + current_joint_states.velocities[index] + desired_accel[index] * dt_seconds; + desired_pos[index] = current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * desired_accel[index] * dt_seconds * dt_seconds; + } + } + + if ( + std::count_if( + pos_limit_trig_jnts.begin(), pos_limit_trig_jnts.end(), [](bool trig) { return trig; }) > 0) + { + std::ostringstream ostr; + for (auto index = 0u; index < num_joints; ++index) + { + if (pos_limit_trig_jnts[index]) ostr << joint_names_[index] << " "; } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed position limits, limiting"); + } + + if (limited_jnts_vel.size() > 0) + { + std::ostringstream ostr; + for (auto jnt : limited_jnts_vel) ostr << jnt << " "; + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed velocity limits, limiting"); } + + if (limited_jnts_acc.size() > 0) + { + std::ostringstream ostr; + for (auto jnt : limited_jnts_acc) ostr << jnt << " "; + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed acceleration limits, limiting"); + } + + desired_joint_states.positions = desired_pos; + desired_joint_states.velocities = desired_vel; return true; } diff --git a/joint_limits_enforcement_plugins/CMakeLists.txt b/joint_limits_enforcement_plugins/CMakeLists.txt index d66a41bc8b..9580e35c2e 100644 --- a/joint_limits_enforcement_plugins/CMakeLists.txt +++ b/joint_limits_enforcement_plugins/CMakeLists.txt @@ -14,6 +14,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/joint_limits_enforcement_plugins/package.xml b/joint_limits_enforcement_plugins/package.xml index 16a241767a..26e6820598 100644 --- a/joint_limits_enforcement_plugins/package.xml +++ b/joint_limits_enforcement_plugins/package.xml @@ -17,6 +17,7 @@ ament_cmake + backward_ros joint_limits rclcpp pluginlib diff --git a/joint_limits_interface/CMakeLists.txt b/joint_limits_interface/CMakeLists.txt index a5d4577343..d48085177c 100644 --- a/joint_limits_interface/CMakeLists.txt +++ b/joint_limits_interface/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(ament_cmake REQUIRED) find_package(hardware_interface REQUIRED) find_package(rclcpp REQUIRED) find_package(urdf REQUIRED) +find_package(backward_ros REQUIRED) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) diff --git a/joint_limits_interface/package.xml b/joint_limits_interface/package.xml index badbb51773..199d53120b 100644 --- a/joint_limits_interface/package.xml +++ b/joint_limits_interface/package.xml @@ -18,6 +18,7 @@ rclcpp urdf + backward_ros hardware_interface From a07c7ac24f18cc23c8b40f1c1374f3edff76f1a8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 16 Mar 2023 15:51:17 +0100 Subject: [PATCH 05/37] Remove all Ruckig-limiter files. --- .../CMakeLists.txt | 67 ------- .../visibility_control.h | 49 ----- .../ruckig_joint_limiter.hpp | 64 ------ .../joint_limits_enforcement_plugins.xml | 9 - joint_limits_enforcement_plugins/package.xml | 32 --- .../src/ruckig_joint_limiter.cpp | 182 ------------------ .../test/test_ruckig_joint_limiter.cpp | 44 ----- 7 files changed, 447 deletions(-) delete mode 100644 joint_limits_enforcement_plugins/CMakeLists.txt delete mode 100644 joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h delete mode 100644 joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp delete mode 100644 joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml delete mode 100644 joint_limits_enforcement_plugins/package.xml delete mode 100644 joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp delete mode 100644 joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp diff --git a/joint_limits_enforcement_plugins/CMakeLists.txt b/joint_limits_enforcement_plugins/CMakeLists.txt deleted file mode 100644 index 9580e35c2e..0000000000 --- a/joint_limits_enforcement_plugins/CMakeLists.txt +++ /dev/null @@ -1,67 +0,0 @@ -cmake_minimum_required(VERSION 3.16) -project(joint_limits_enforcement_plugins LANGUAGES CXX) - -if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra) -endif() - -set(THIS_PACKAGE_INCLUDE_DEPENDS - joint_limits - pluginlib - rclcpp - rcutils - ruckig -) - -find_package(ament_cmake REQUIRED) -find_package(backward_ros REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -add_library(ruckig_joint_limiter SHARED - src/ruckig_joint_limiter.cpp -) -target_compile_features(ruckig_joint_limiter PUBLIC cxx_std_17) -target_include_directories(ruckig_joint_limiter PUBLIC - $ - $ -) -ament_target_dependencies(ruckig_joint_limiter PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(ruckig_joint_limiter PRIVATE "JOINT_LIMITS_ENFORCEMENT_PLUGINS_BUILDING_DLL") - -pluginlib_export_plugin_description_file(joint_limits joint_limits_enforcement_plugins.xml) - -if(BUILD_TESTING) - find_package(ament_cmake_gmock REQUIRED) - find_package(joint_limits REQUIRED) - find_package(pluginlib REQUIRED) - find_package(rclcpp REQUIRED) - - ament_add_gmock(test_ruckig_joint_limiter test/test_ruckig_joint_limiter.cpp) - target_include_directories(test_ruckig_joint_limiter PRIVATE include) - ament_target_dependencies( - test_ruckig_joint_limiter - joint_limits - pluginlib - rclcpp - ) -endif() - -install(DIRECTORY include/ - DESTINATION include/joint_limits_enforcement_plugins -) -install( - TARGETS - ruckig_joint_limiter - EXPORT export_joint_limits_enforcement_plugins - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -ament_export_targets(export_joint_limits_enforcement_plugins HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_package() diff --git a/joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h b/joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h deleted file mode 100644 index abd0019cf6..0000000000 --- a/joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef JOINT_LIMITS_ENFORCEMENT_PLUGINS__VISIBILITY_CONTROL_H_ -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT __attribute__((dllexport)) -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT __attribute__((dllimport)) -#else -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT __declspec(dllexport) -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT __declspec(dllimport) -#endif -#ifdef JOINT_LIMITS_ENFORCEMENT_PLUGINS_BUILDING_DLL -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT -#else -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT -#endif -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC_TYPE JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_LOCAL -#else -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT __attribute__((visibility("default"))) -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT -#if __GNUC__ >= 4 -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC __attribute__((visibility("default"))) -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_LOCAL __attribute__((visibility("hidden"))) -#else -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_LOCAL -#endif -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC_TYPE -#endif - -#endif // JOINT_LIMITS_ENFORCEMENT_PLUGINS__VISIBILITY_CONTROL_H_ diff --git a/joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp b/joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp deleted file mode 100644 index fd577c32b3..0000000000 --- a/joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp +++ /dev/null @@ -1,64 +0,0 @@ -// Copyright (c) 2021, PickNik Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Andy Zelenak, Denis Stogl - -#ifndef RUCKIG_JOINT_LIMITER__RUCKIG_JOINT_LIMITER_HPP_ -#define RUCKIG_JOINT_LIMITER__RUCKIG_JOINT_LIMITER_HPP_ - -#include - -#include "joint_limits/joint_limiter_interface.hpp" -#include "joint_limits/joint_limits.hpp" -#include "joint_limits_enforcement_plugins/visibility_control.h" -#include "ruckig/input_parameter.hpp" -#include "ruckig/output_parameter.hpp" -#include "ruckig/ruckig.hpp" - -namespace ruckig_joint_limiter -{ -namespace // utility namespace -{ -constexpr double DEFAULT_MAX_VELOCITY = 5; // rad/s -constexpr double DEFAULT_MAX_ACCELERATION = 10; // rad/s^2 -constexpr double DEFAULT_MAX_JERK = 20; // rad/s^3 -} // namespace - -template -class RuckigJointLimiter : public joint_limits::JointLimiterInterface -{ -public: - JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC RuckigJointLimiter(); - - JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC bool on_init() override; - - JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC bool on_configure( - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) override; - - JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC bool on_enforce( - trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, - trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, - const rclcpp::Duration & dt) override; - -private: - bool received_initial_state_ = false; - // Ruckig algorithm - std::shared_ptr> ruckig_; - std::shared_ptr> ruckig_input_; - std::shared_ptr> ruckig_output_; -}; - -} // namespace ruckig_joint_limiter - -#endif // RUCKIG_JOINT_LIMITER__RUCKIG_JOINT_LIMITER_HPP_ diff --git a/joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml b/joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml deleted file mode 100644 index aa2996282f..0000000000 --- a/joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - Jerk-limited smoothing with the Ruckig library. - - - diff --git a/joint_limits_enforcement_plugins/package.xml b/joint_limits_enforcement_plugins/package.xml deleted file mode 100644 index 26e6820598..0000000000 --- a/joint_limits_enforcement_plugins/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - joint_limits_enforcement_plugins - 0.0.0 - Package for handling of joint limits using external libraries for use in controllers or in hardware. - - Bence Magyar - Denis Štogl - - Denis Štogl - Andy Zelenak - - Apache License 2.0 - - https://github.com/ros-controls/ros2_control/wiki - https://github.com/ros-controls/ros2_control/issues - https://github.com/ros-controls/ros2_control - - ament_cmake - - backward_ros - joint_limits - rclcpp - pluginlib - rcutils - ruckig - - ament_cmake_gmock - - - ament_cmake - - diff --git a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp deleted file mode 100644 index ad264aed67..0000000000 --- a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp +++ /dev/null @@ -1,182 +0,0 @@ -// Copyright (c) 2021, PickNik Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \authors Andy Zelenak, Denis Stogl - -#include "ruckig_joint_limiter/ruckig_joint_limiter.hpp" - -#include -#include -#include - -#include "joint_limits/joint_limits_rosparam.hpp" -#include "rcutils/logging_macros.h" -#include "ruckig/brake.hpp" -#include "ruckig/input_parameter.hpp" -#include "ruckig/output_parameter.hpp" -#include "ruckig/ruckig.hpp" - -namespace ruckig_joint_limiter -{ -template <> -RuckigJointLimiter::RuckigJointLimiter() -: joint_limits::JointLimiterInterface() -{ -} - -template <> -bool RuckigJointLimiter::on_init(/*const rclcpp::Duration & dt*/) -{ - // TODO(destogl): This should be used from parameter - const rclcpp::Duration dt = rclcpp::Duration::from_seconds(0.005); - - // Initialize Ruckig - ruckig_ = std::make_shared>(number_of_joints_, dt.seconds()); - ruckig_input_ = std::make_shared>(number_of_joints_); - ruckig_output_ = std::make_shared>(number_of_joints_); - - // Velocity mode works best for smoothing one waypoint at a time - ruckig_input_->control_interface = ruckig::ControlInterface::Velocity; - ruckig_input_->synchronization = ruckig::Synchronization::Time; - - for (auto joint = 0ul; joint < number_of_joints_; ++joint) - { - if (joint_limits_[joint].has_jerk_limits) - { - ruckig_input_->max_jerk.at(joint) = joint_limits_[joint].max_acceleration; - } - else - { - ruckig_input_->max_jerk.at(joint) = DEFAULT_MAX_JERK; - } - if (joint_limits_[joint].has_acceleration_limits) - { - ruckig_input_->max_acceleration.at(joint) = joint_limits_[joint].max_acceleration; - } - else - { - ruckig_input_->max_acceleration.at(joint) = DEFAULT_MAX_ACCELERATION; - } - if (joint_limits_[joint].has_velocity_limits) - { - ruckig_input_->max_velocity.at(joint) = joint_limits_[joint].max_velocity; - } - else - { - ruckig_input_->max_velocity.at(joint) = DEFAULT_MAX_VELOCITY; - } - } - - return true; -} - -template <> -bool RuckigJointLimiter::on_configure( - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) -{ - // Initialize Ruckig with current_joint_states - ruckig_input_->current_position = current_joint_states.positions; - - if (current_joint_states.velocities.size() == number_of_joints_) - { - ruckig_input_->current_velocity = current_joint_states.velocities; - } - else - { - ruckig_input_->current_velocity = std::vector(number_of_joints_, 0.0); - } - if (current_joint_states.accelerations.size() == number_of_joints_) - { - ruckig_input_->current_acceleration = current_joint_states.accelerations; - } - else - { - ruckig_input_->current_acceleration = std::vector(number_of_joints_, 0.0); - } - - // Initialize output data - ruckig_output_->new_position = ruckig_input_->current_position; - ruckig_output_->new_velocity = ruckig_input_->current_velocity; - ruckig_output_->new_acceleration = ruckig_input_->current_acceleration; - - return true; -} - -template <> -bool RuckigJointLimiter::on_enforce( - trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/, - trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, - const rclcpp::Duration & /*dt*/) -{ - // We don't use current_joint_states. For stability, it is recommended to feed previous Ruckig - // output back in as input for the next iteration. This assumes the robot tracks the command well. - ruckig_input_->current_position = ruckig_output_->new_position; - ruckig_input_->current_velocity = ruckig_output_->new_velocity; - ruckig_input_->current_acceleration = ruckig_output_->new_acceleration; - - // Target state is the next waypoint - ruckig_input_->target_position = desired_joint_states.positions; - // TODO(destogl): in current use-case we use only velocity - if (desired_joint_states.velocities.size() == number_of_joints_) - { - ruckig_input_->target_velocity = desired_joint_states.velocities; - } - else - { - ruckig_input_->target_velocity = std::vector(number_of_joints_, 0.0); - } - if (desired_joint_states.accelerations.size() == number_of_joints_) - { - ruckig_input_->target_acceleration = desired_joint_states.accelerations; - } - else - { - ruckig_input_->target_acceleration = std::vector(number_of_joints_, 0.0); - } - - ruckig::Result result = ruckig_->update(*ruckig_input_, *ruckig_output_); - RCUTILS_LOG_DEBUG_NAMED("ruckig_joint_limiter", "Rucking result %d", result); - - desired_joint_states.positions = ruckig_output_->new_position; - desired_joint_states.velocities = ruckig_output_->new_velocity; - desired_joint_states.accelerations = ruckig_output_->new_acceleration; - - // Feed output from the previous timestep back as input - for (auto joint = 0ul; joint < number_of_joints_; ++joint) - { - RCUTILS_LOG_DEBUG_NAMED( - "ruckig_joint_limiter", - "Desired position: %e \nDesired velocity: %e\n Desired acceleration: %e.", - ruckig_input_->target_position.at(joint), ruckig_input_->target_velocity.at(joint), - ruckig_input_->target_acceleration.at(joint)); - } - - for (auto joint = 0ul; joint < number_of_joints_; ++joint) - { - RCUTILS_LOG_DEBUG_NAMED( - "ruckig_joint_limiter", "New position: %e \nNew velocity: %e\nNew acceleration: %e.", - ruckig_output_->new_position.at(joint), ruckig_output_->new_velocity.at(joint), - ruckig_output_->new_acceleration.at(joint)); - } - - return result == ruckig::Result::Finished; -} - -} // namespace ruckig_joint_limiter - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - ruckig_joint_limiter::RuckigJointLimiter, - joint_limits::JointLimiterInterface) diff --git a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp deleted file mode 100644 index b1b19d0587..0000000000 --- a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Denis Stogl - -#include - -#include -#include - -#include "joint_limits/joint_limiter_interface.hpp" -#include "joint_limits/joint_limits.hpp" -#include "pluginlib/class_loader.hpp" -#include "rclcpp/executor.hpp" - -TEST(TestLoadSimpleJointLimiter, load_limiter) -{ - rclcpp::init(0, nullptr); - - using JointLimiter = joint_limits::JointLimiterInterface; - pluginlib::ClassLoader joint_limiter_loader( - "joint_limits", "joint_limits::JointLimiterInterface"); - - std::unique_ptr joint_limiter; - std::string joint_limiter_type = "ruckig_joint_limiter/RuckigJointLimiter"; - - joint_limiter = - std::unique_ptr(joint_limiter_loader.createUnmanagedInstance(joint_limiter_type)); - ASSERT_NO_THROW( - joint_limiter = std::unique_ptr( - joint_limiter_loader.createUnmanagedInstance(joint_limiter_type))); - ASSERT_NE(joint_limiter, nullptr); -} From f939193f86b4428968f7fed74b5d711f2d50d4bd Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 13 Apr 2023 15:00:20 +0200 Subject: [PATCH 06/37] Apply suggestions from code review --- controller_interface/CMakeLists.txt | 1 - controller_interface/package.xml | 1 - hardware_interface/CMakeLists.txt | 1 - .../hardware_interface/types/hardware_interface_type_values.hpp | 2 +- 4 files changed, 1 insertion(+), 4 deletions(-) diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 698c66b97a..034556d19f 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -11,7 +11,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) -find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 69436ddcb2..9d2109f975 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -10,7 +10,6 @@ ament_cmake - backward_ros hardware_interface rclcpp_lifecycle sensor_msgs diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 9945f1b541..77eec3ec86 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -17,7 +17,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) -find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp index b91f620bba..ce13e855a7 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp @@ -19,7 +19,7 @@ namespace hardware_interface { -/// Constant defining position interface +/// Constant defining position interface name constexpr char HW_IF_POSITION[] = "position"; /// Constant defining velocity interface constexpr char HW_IF_VELOCITY[] = "velocity"; From d7db07029994c8f6b886c7bcdc76f5e2c1416a10 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 13 Apr 2023 15:01:25 +0200 Subject: [PATCH 07/37] Remove definition of movement interface because the concept is not used yet. Concept of m --- .../types/hardware_interface_type_values.hpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp index ce13e855a7..d9652a28f9 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp @@ -21,17 +21,13 @@ namespace hardware_interface { /// Constant defining position interface name constexpr char HW_IF_POSITION[] = "position"; -/// Constant defining velocity interface +/// Constant defining velocity interface name constexpr char HW_IF_VELOCITY[] = "velocity"; -/// Constant defining acceleration interface +/// Constant defining acceleration interface name constexpr char HW_IF_ACCELERATION[] = "acceleration"; -/// Constant defining effort interface +/// Constant defining effort interface name constexpr char HW_IF_EFFORT[] = "effort"; -// TODO(destogl): use "inline static const"-type when switched to C++17 -/// Definition of standard names for movement interfaces -const std::vector MOVEMENT_INTERFACES = { - HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_ACCELERATION, HW_IF_EFFORT}; } // namespace hardware_interface #endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_ From 4054b048859451d3a35f39f18221f399004570b7 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 13 Apr 2023 15:32:23 +0200 Subject: [PATCH 08/37] Polish out some stuff using "GH Programming" Co-authored-by: AndyZe --- .../types/hardware_interface_type_values.hpp | 1 - hardware_interface/package.xml | 1 - .../include/joint_limits/joint_limiter_interface.hpp | 9 ++++----- .../include/joint_limits/simple_joint_limiter.hpp | 6 +++--- joint_limits/include/joint_limits/visibility_control.h | 2 +- joint_limits/joint_limiters.xml | 2 +- joint_limits/package.xml | 2 +- joint_limits/src/joint_limiter_interface.cpp | 6 +++--- joint_limits/src/simple_joint_limiter.cpp | 9 ++++----- joint_limits/test/test_simple_joint_limiter.cpp | 4 ++-- joint_limits_interface/CMakeLists.txt | 1 - joint_limits_interface/package.xml | 1 - 12 files changed, 19 insertions(+), 25 deletions(-) diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp index d9652a28f9..c49925b701 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp @@ -27,7 +27,6 @@ constexpr char HW_IF_VELOCITY[] = "velocity"; constexpr char HW_IF_ACCELERATION[] = "acceleration"; /// Constant defining effort interface name constexpr char HW_IF_EFFORT[] = "effort"; - } // namespace hardware_interface #endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_ diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 75589df5a3..0d3ffdbe04 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -9,7 +9,6 @@ ament_cmake - backward_ros control_msgs lifecycle_msgs pluginlib diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 85425ff8a1..79fd40a047 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -49,7 +49,7 @@ class JointLimiterInterface * */ JOINT_LIMITS_PUBLIC virtual bool init( - const std::vector joint_names, const rclcpp::Node::SharedPtr & node, + const std::vector & joint_names, const rclcpp::Node::SharedPtr & node, const std::string & robot_description_topic = "/robot_description"); JOINT_LIMITS_PUBLIC virtual bool configure( @@ -67,7 +67,7 @@ class JointLimiterInterface return on_enforce(current_joint_states, desired_joint_states, dt); } - // TODO(destogl): Make those protected? +protected: // Methods that each limiter implementation has to implement JOINT_LIMITS_PUBLIC virtual bool on_init() { return true; } @@ -78,11 +78,10 @@ class JointLimiterInterface } JOINT_LIMITS_PUBLIC virtual bool on_enforce( - trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) = 0; -protected: size_t number_of_joints_; std::vector joint_names_; std::vector joint_limits_; diff --git a/joint_limits/include/joint_limits/simple_joint_limiter.hpp b/joint_limits/include/joint_limits/simple_joint_limiter.hpp index 2011e0d48e..aa2e0a8d6c 100644 --- a/joint_limits/include/joint_limits/simple_joint_limiter.hpp +++ b/joint_limits/include/joint_limits/simple_joint_limiter.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, PickNik Inc. +// Copyright (c) 2023, PickNik Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -/// \author Denis Stogl +/// \author Dr. Denis Stogl #ifndef JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ #define JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ @@ -31,7 +31,7 @@ class SimpleJointLimiter : public JointLimiterInterface JOINT_LIMITS_PUBLIC SimpleJointLimiter(); JOINT_LIMITS_PUBLIC bool on_enforce( - trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) override; }; diff --git a/joint_limits/include/joint_limits/visibility_control.h b/joint_limits/include/joint_limits/visibility_control.h index 0dcc0193a1..9c957a3cf9 100644 --- a/joint_limits/include/joint_limits/visibility_control.h +++ b/joint_limits/include/joint_limits/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml index ff45611198..a25667d2c1 100644 --- a/joint_limits/joint_limiters.xml +++ b/joint_limits/joint_limiters.xml @@ -3,7 +3,7 @@ type="joint_limits::SimpleJointLimiter<joint_limits::JointLimits>" base_class_type="joint_limits::JointLimiterInterface<joint_limits::JointLimits>"> - Simple joint limiter using clamping approach. + Simple joint limiter using clamping approach. Warning: clamping can cause discontinuities. diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 9a8996a84c..0e0b401814 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,7 +1,7 @@ joint_limits 3.12.0 - Package for with interfaces for handling of joint limits for use in controllers or in hardware. The package also implements a simple, default joint limit strategy by clamping the values. + Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements a simple, default joint limit strategy by clamping the values. Bence Magyar Denis Štogl diff --git a/joint_limits/src/joint_limiter_interface.cpp b/joint_limits/src/joint_limiter_interface.cpp index b3b4d65bb6..290bee9842 100644 --- a/joint_limits/src/joint_limiter_interface.cpp +++ b/joint_limits/src/joint_limiter_interface.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -/// \author Denis Stogl +/// \author Dr. Denis Stogl #include "joint_limits/joint_limiter_interface.hpp" @@ -39,7 +39,7 @@ bool JointLimiterInterface::init( // TODO(destogl): get limits from URDF // Initialize and get joint limits from parameter server - for (auto i = 0ul; i < number_of_joints_; ++i) + for (size_t i = 0; i < number_of_joints_; ++i) { if (!declare_parameters(joint_names[i], node)) { diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 8ebcfde77b..9ebbefbea8 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, PickNik Inc. +// Copyright (c) 2023, PickNik Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -62,7 +62,7 @@ bool SimpleJointLimiter::on_enforce( bool position_limit_triggered = false; - for (auto index = 0u; index < num_joints; ++index) + for (size_t index = 0; index < num_joints; ++index) { desired_pos[index] = desired_joint_states.positions[index]; @@ -143,8 +143,7 @@ bool SimpleJointLimiter::on_enforce( if (position_limit_triggered) { - std::ostringstream ostr; - for (auto index = 0u; index < num_joints; ++index) + for (size_t index = 0; index < num_joints; ++index) { // Compute accel to stop // Here we aren't explicitly maximally decelerating, but for joints near their limits this @@ -171,7 +170,7 @@ bool SimpleJointLimiter::on_enforce( pos_limit_trig_jnts.begin(), pos_limit_trig_jnts.end(), [](bool trig) { return trig; }) > 0) { std::ostringstream ostr; - for (auto index = 0u; index < num_joints; ++index) + for (size_t index = 0; index < num_joints; ++index) { if (pos_limit_trig_jnts[index]) ostr << joint_names_[index] << " "; } diff --git a/joint_limits/test/test_simple_joint_limiter.cpp b/joint_limits/test/test_simple_joint_limiter.cpp index e025ac0b9f..e5bd3697ed 100644 --- a/joint_limits/test/test_simple_joint_limiter.cpp +++ b/joint_limits/test/test_simple_joint_limiter.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -/// \author Denis Stogl +/// \author Dr. Denis Stogl #include diff --git a/joint_limits_interface/CMakeLists.txt b/joint_limits_interface/CMakeLists.txt index d48085177c..a5d4577343 100644 --- a/joint_limits_interface/CMakeLists.txt +++ b/joint_limits_interface/CMakeLists.txt @@ -5,7 +5,6 @@ find_package(ament_cmake REQUIRED) find_package(hardware_interface REQUIRED) find_package(rclcpp REQUIRED) find_package(urdf REQUIRED) -find_package(backward_ros REQUIRED) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) diff --git a/joint_limits_interface/package.xml b/joint_limits_interface/package.xml index 199d53120b..badbb51773 100644 --- a/joint_limits_interface/package.xml +++ b/joint_limits_interface/package.xml @@ -18,7 +18,6 @@ rclcpp urdf - backward_ros hardware_interface From 456c71528f1e50ab024d22ca5791277d4ac7ff52 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 13 Apr 2023 15:34:43 +0200 Subject: [PATCH 09/37] Polish out some stuff using "GH Programming" --- joint_limits/src/simple_joint_limiter.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 9ebbefbea8..795f122caf 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -33,11 +33,11 @@ SimpleJointLimiter::SimpleJointLimiter() template <> bool SimpleJointLimiter::on_enforce( - trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) { - auto num_joints = joint_limits_.size(); - auto dt_seconds = dt.seconds(); + const auto num_joints = joint_limits_.size(); + const auto dt_seconds = dt.seconds(); if (current_joint_states.velocities.empty()) { From 8b33153363a8b9a4cdbe23685be8e83520d03c89 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 24 Apr 2023 18:04:09 +0200 Subject: [PATCH 10/37] Add SimpleJointLimiter as plugin into controllers. --- .../joint_limits/joint_limiter_interface.hpp | 93 ++++++++++++++++++- .../joint_limits/simple_joint_limiter.hpp | 24 ++++- joint_limits/joint_limiters.xml | 20 ++-- joint_limits/src/joint_limiter_interface.cpp | 52 ----------- joint_limits/src/simple_joint_limiter.cpp | 15 +-- 5 files changed, 127 insertions(+), 77 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 79fd40a047..3c7f13e97b 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -21,8 +21,10 @@ #include #include "joint_limits/joint_limits.hpp" +#include "joint_limits/joint_limits_rosparam.hpp" #include "joint_limits/visibility_control.h" #include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" namespace joint_limits @@ -44,13 +46,86 @@ class JointLimiterInterface * Otherwise, initialize your custom limiter in `on_limit` method. * * \param[in] joint_names names of joints where limits should be applied. - * \param[in] node shared pointer to the node where joint limit parameters defined. + * \param[in] param_itf node parameters interface object to access parameters. + * \param[in] logging_itf node logging interface to log if error happens. * \param[in] robot_description_topic string of a topic where robot description is accessible. - * + */ + JOINT_LIMITS_PUBLIC virtual bool init( + const std::vector & joint_names, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, + const std::string & robot_description_topic = "/robot_description") + { + number_of_joints_ = joint_names.size(); + joint_names_ = joint_names; + joint_limits_.resize(number_of_joints_); + node_param_itf_ = param_itf; + node_logging_itf_ = logging_itf; + + bool result = true; + + // TODO(destogl): get limits from URDF + + // Initialize and get joint limits from parameter server + for (size_t i = 0; i < number_of_joints_; ++i) + { + // FIXME?(destogl): this seems to be a bit unclear because we use the same namespace for + // limiters interface and rosparam helper functions - should we use here another namespace? + if (!declare_parameters(joint_names[i], node_param_itf_, node_logging_itf_)) + { + RCLCPP_ERROR( + node_logging_itf_->get_logger(), + "JointLimiter: Joint '%s': parameter declaration has failed", joint_names[i].c_str()); + result = false; + break; + } + if (!get_joint_limits(joint_names[i], node_param_itf_, node_logging_itf_, joint_limits_[i])) + { + RCLCPP_ERROR( + node_logging_itf_->get_logger(), + "JointLimiter: Joint '%s': getting parameters has failed", joint_names[i].c_str()); + result = false; + break; + } + RCLCPP_INFO( + node_logging_itf_->get_logger(), "Limits for joint %zu (%s) are:\n%s", i, + joint_names[i].c_str(), joint_limits_[i].to_string().c_str()); + } + + if (result) + { + result = on_init(); + } + + return result; + } + + /** + * Wrapper init method that accepts pointer to the Node. + * For details see other init method. */ JOINT_LIMITS_PUBLIC virtual bool init( const std::vector & joint_names, const rclcpp::Node::SharedPtr & node, - const std::string & robot_description_topic = "/robot_description"); + const std::string & robot_description_topic = "/robot_description") + { + return init( + joint_names, node->get_node_parameters_interface(), node->get_node_logging_interface(), + robot_description_topic); + } + + /** + * Wrapper init method that accepts pointer to the LifecycleNode. + * For details see other init method. + */ + JOINT_LIMITS_PUBLIC virtual bool init( + const std::vector & joint_names, + const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node, + const std::string & robot_description_topic = "/robot_description") + { + return init( + joint_names, lifecycle_node->get_node_parameters_interface(), + lifecycle_node->get_node_logging_interface(), robot_description_topic); + } JOINT_LIMITS_PUBLIC virtual bool configure( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) @@ -77,8 +152,16 @@ class JointLimiterInterface return true; } + /** \brief Enforce joint limits to desired joint state. + * + * Filter-specific implementation of the joint limits enforce algorithm. + * + * \param[in] current_joint_states current joint states a robot is in. + * \param[out] desired_joint_states joint state that should be adjusted to obey the limits. + * \param[in] dt time delta to calculate missing integrals and derivation in joint limits. + */ JOINT_LIMITS_PUBLIC virtual bool on_enforce( - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) = 0; @@ -86,6 +169,8 @@ class JointLimiterInterface std::vector joint_names_; std::vector joint_limits_; rclcpp::Node::SharedPtr node_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_param_itf_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_itf_; }; } // namespace joint_limits diff --git a/joint_limits/include/joint_limits/simple_joint_limiter.hpp b/joint_limits/include/joint_limits/simple_joint_limiter.hpp index aa2e0a8d6c..6002a0b4f7 100644 --- a/joint_limits/include/joint_limits/simple_joint_limiter.hpp +++ b/joint_limits/include/joint_limits/simple_joint_limiter.hpp @@ -17,25 +17,45 @@ #ifndef JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ #define JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ +#include #include #include "joint_limits/joint_limiter_interface.hpp" #include "joint_limits/joint_limits.hpp" +#include "rclcpp/rclcpp.hpp" namespace joint_limits { template -class SimpleJointLimiter : public JointLimiterInterface +class SimpleJointLimiter : public JointLimiterInterface { public: + /** \brief Constructor */ JOINT_LIMITS_PUBLIC SimpleJointLimiter(); + /** \brief Destructor */ + JOINT_LIMITS_PUBLIC ~SimpleJointLimiter(); + JOINT_LIMITS_PUBLIC bool on_enforce( - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) override; + +private: + rclcpp::Clock::SharedPtr clock_; }; +template +SimpleJointLimiter::SimpleJointLimiter() : JointLimiterInterface() +{ + clock_ = std::make_shared(rclcpp::Clock(RCL_ROS_TIME)); +} + +template +SimpleJointLimiter::~SimpleJointLimiter() +{ +} + } // namespace joint_limits #endif // JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml index a25667d2c1..036b39859a 100644 --- a/joint_limits/joint_limiters.xml +++ b/joint_limits/joint_limiters.xml @@ -1,9 +1,11 @@ - - - - Simple joint limiter using clamping approach. Warning: clamping can cause discontinuities. - - - + + + + + Simple joint limiter using clamping approach. Warning: clamping can cause discontinuities. + + + + diff --git a/joint_limits/src/joint_limiter_interface.cpp b/joint_limits/src/joint_limiter_interface.cpp index 290bee9842..843337259d 100644 --- a/joint_limits/src/joint_limiter_interface.cpp +++ b/joint_limits/src/joint_limiter_interface.cpp @@ -16,58 +16,6 @@ #include "joint_limits/joint_limiter_interface.hpp" -#include -#include - -#include "joint_limits/joint_limits_rosparam.hpp" - -// TODO(anyone): Add handing of SoftLimits namespace joint_limits { -template <> -bool JointLimiterInterface::init( - const std::vector joint_names, const rclcpp::Node::SharedPtr & node, - const std::string & /*robot_description_topic*/) -{ - number_of_joints_ = joint_names.size(); - joint_names_ = joint_names; - joint_limits_.resize(number_of_joints_); - node_ = node; - - bool result = true; - - // TODO(destogl): get limits from URDF - - // Initialize and get joint limits from parameter server - for (size_t i = 0; i < number_of_joints_; ++i) - { - if (!declare_parameters(joint_names[i], node)) - { - RCLCPP_ERROR( - node->get_logger(), "JointLimiter: Joint '%s': parameter declaration has failed", - joint_names[i].c_str()); - result = false; - break; - } - if (!joint_limits::get_joint_limits(joint_names[i], node, joint_limits_[i])) - { - RCLCPP_ERROR( - node->get_logger(), "JointLimiter: Joint '%s': getting parameters has failed", - joint_names[i].c_str()); - result = false; - break; - } - RCLCPP_INFO( - node->get_logger(), "Joint '%s':\n%s", joint_names[i].c_str(), - joint_limits_[i].to_string().c_str()); - } - - if (result) - { - result = on_init(); - } - - return result; -} - } // namespace joint_limits diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 795f122caf..797862f4a5 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -25,17 +25,12 @@ constexpr size_t ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttl namespace joint_limits { -template <> -SimpleJointLimiter::SimpleJointLimiter() -: joint_limits::JointLimiterInterface() -{ -} - template <> bool SimpleJointLimiter::on_enforce( - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) { + // TODO(destogl): replace `num_joints` with `number_of_joints_` const auto num_joints = joint_limits_.size(); const auto dt_seconds = dt.seconds(); @@ -176,7 +171,7 @@ bool SimpleJointLimiter::on_enforce( } ostr << "\b \b"; // erase last character RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, "Joint(s) [" << ostr.str().c_str() << "] would exceed position limits, limiting"); } @@ -186,7 +181,7 @@ bool SimpleJointLimiter::on_enforce( for (auto jnt : limited_jnts_vel) ostr << jnt << " "; ostr << "\b \b"; // erase last character RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, "Joint(s) [" << ostr.str().c_str() << "] would exceed velocity limits, limiting"); } @@ -196,7 +191,7 @@ bool SimpleJointLimiter::on_enforce( for (auto jnt : limited_jnts_acc) ostr << jnt << " "; ostr << "\b \b"; // erase last character RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, "Joint(s) [" << ostr.str().c_str() << "] would exceed acceleration limits, limiting"); } From cbfc06a58d54811da356f4cb22b8c6823feebc73 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 4 May 2023 17:45:30 +0200 Subject: [PATCH 11/37] Version with deceleration for simple joint limiter. --- joint_limits/src/simple_joint_limiter.cpp | 83 ++++++++++++++++------- 1 file changed, 59 insertions(+), 24 deletions(-) diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 797862f4a5..ba02fbb60f 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -30,34 +30,32 @@ bool SimpleJointLimiter::on_enforce( trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) { - // TODO(destogl): replace `num_joints` with `number_of_joints_` - const auto num_joints = joint_limits_.size(); const auto dt_seconds = dt.seconds(); if (current_joint_states.velocities.empty()) { // First update() after activating does not have velocity available, assume 0 - current_joint_states.velocities.resize(num_joints, 0.0); + current_joint_states.velocities.resize(number_of_joints_, 0.0); } // check for required inputs if ( - (desired_joint_states.positions.size() < num_joints) || - (desired_joint_states.velocities.size() < num_joints) || - (current_joint_states.positions.size() < num_joints)) + (desired_joint_states.positions.size() < number_of_joints_) || + (desired_joint_states.velocities.size() < number_of_joints_) || + (current_joint_states.positions.size() < number_of_joints_)) { return false; } - std::vector desired_accel(num_joints); - std::vector desired_vel(num_joints); - std::vector desired_pos(num_joints); - std::vector pos_limit_trig_jnts(num_joints, false); - std::vector limited_jnts_vel, limited_jnts_acc; + std::vector desired_accel(number_of_joints_); + std::vector desired_vel(number_of_joints_); + std::vector desired_pos(number_of_joints_); + std::vector pos_limit_trig_jnts(number_of_joints_, false); + std::vector limited_jnts_vel, limited_jnts_acc, limited_jnts_dec; bool position_limit_triggered = false; - for (size_t index = 0; index < num_joints; ++index) + for (size_t index = 0; index < number_of_joints_; ++index) { desired_pos[index] = desired_joint_states.positions[index]; @@ -89,21 +87,40 @@ bool SimpleJointLimiter::on_enforce( desired_accel[index] = (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; - // limit acceleration - if (joint_limits_[index].has_acceleration_limits) + auto apply_acc_or_dec_limit = + [&](const double max_acc_or_dec, std::vector & limited_jnts) { - if (std::abs(desired_accel[index]) > joint_limits_[index].max_acceleration) + if (std::abs(desired_accel[index]) > max_acc_or_dec) { - desired_accel[index] = - std::copysign(joint_limits_[index].max_acceleration, desired_accel[index]); + desired_accel[index] = std::copysign(max_acc_or_dec, desired_accel[index]); desired_vel[index] = current_joint_states.velocities[index] + desired_accel[index] * dt_seconds; // recalc desired position after acceleration limiting desired_pos[index] = current_joint_states.positions[index] + current_joint_states.velocities[index] * dt_seconds + 0.5 * desired_accel[index] * dt_seconds * dt_seconds; - limited_jnts_acc.emplace_back(joint_names_[index]); + limited_jnts.emplace_back(joint_names_[index]); } + }; + + // check if decelerating - if velocity is changing toward 0 + bool deceleration_limit_applied = false; + if ( + desired_accel[index] < 0 && current_joint_states.velocities[index] > 0 || + desired_accel[index] > 0 && current_joint_states.velocities[index] < 0) + { + // limit deceleration + if (joint_limits_[index].has_deceleration_limits) + { + apply_acc_or_dec_limit(joint_limits_[index].max_deceleration, limited_jnts_dec); + deceleration_limit_applied = true; + } + } + + // limit acceleration (fallback to acceleration if no deceleration limits) + if (joint_limits_[index].has_acceleration_limits && !deceleration_limit_applied) + { + apply_acc_or_dec_limit(joint_limits_[index].max_acceleration, limited_jnts_acc); } // Check that stopping distance is within joint limits @@ -115,11 +132,18 @@ bool SimpleJointLimiter::on_enforce( // Here we assume we will not trigger velocity limits while maximally decelerating. // This is a valid assumption if we are not currently at a velocity limit since we are just // coming to a rest. - double stopping_accel = joint_limits_[index].has_acceleration_limits - ? joint_limits_[index].max_acceleration - : std::abs(desired_vel[index] / dt_seconds); + double stopping_deccel = std::abs(desired_vel[index] / dt_seconds); + if (joint_limits_[index].has_deceleration_limits) + { + stopping_deccel = joint_limits_[index].max_deceleration; + } + else if (joint_limits_[index].has_acceleration_limits) + { + stopping_deccel = joint_limits_[index].max_acceleration; + } + double stopping_distance = - std::abs((-desired_vel[index] * desired_vel[index]) / (2 * stopping_accel)); + std::abs((-desired_vel[index] * desired_vel[index]) / (2 * stopping_deccel)); // Check that joint limits are beyond stopping_distance and desired_velocity is towards // that limit if ( @@ -138,7 +162,7 @@ bool SimpleJointLimiter::on_enforce( if (position_limit_triggered) { - for (size_t index = 0; index < num_joints; ++index) + for (size_t index = 0; index < number_of_joints_; ++index) { // Compute accel to stop // Here we aren't explicitly maximally decelerating, but for joints near their limits this @@ -165,7 +189,7 @@ bool SimpleJointLimiter::on_enforce( pos_limit_trig_jnts.begin(), pos_limit_trig_jnts.end(), [](bool trig) { return trig; }) > 0) { std::ostringstream ostr; - for (size_t index = 0; index < num_joints; ++index) + for (size_t index = 0; index < number_of_joints_; ++index) { if (pos_limit_trig_jnts[index]) ostr << joint_names_[index] << " "; } @@ -195,8 +219,19 @@ bool SimpleJointLimiter::on_enforce( "Joint(s) [" << ostr.str().c_str() << "] would exceed acceleration limits, limiting"); } + if (limited_jnts_dec.size() > 0) + { + std::ostringstream ostr; + for (auto jnt : limited_jnts_dec) ostr << jnt << " "; + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed deceleration limits, limiting"); + } + desired_joint_states.positions = desired_pos; desired_joint_states.velocities = desired_vel; + desired_joint_states.accelerations = desired_accel; return true; } From b4011691b0eb2cecf66772265967be5ab3c38c93 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 12 May 2023 14:34:57 +0200 Subject: [PATCH 12/37] Formatting and comment to check. --- .../include/joint_limits/joint_limiter_interface.hpp | 3 +++ joint_limits/src/simple_joint_limiter.cpp | 6 ++++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 3c7f13e97b..82a042ccba 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -97,6 +97,9 @@ class JointLimiterInterface result = on_init(); } + // avoid linters output + (void)robot_description_topic; + return result; } diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index ba02fbb60f..91d8ff2333 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -47,6 +47,8 @@ bool SimpleJointLimiter::on_enforce( return false; } + // TODO(destogl): please check if we get too much malloc from this initialization, + // if so then we should use members instead local variables and initialize them in other method std::vector desired_accel(number_of_joints_); std::vector desired_vel(number_of_joints_); std::vector desired_pos(number_of_joints_); @@ -106,8 +108,8 @@ bool SimpleJointLimiter::on_enforce( // check if decelerating - if velocity is changing toward 0 bool deceleration_limit_applied = false; if ( - desired_accel[index] < 0 && current_joint_states.velocities[index] > 0 || - desired_accel[index] > 0 && current_joint_states.velocities[index] < 0) + (desired_accel[index] < 0 && current_joint_states.velocities[index] > 0) || + (desired_accel[index] > 0 && current_joint_states.velocities[index] < 0)) { // limit deceleration if (joint_limits_[index].has_deceleration_limits) From e6f52d756516faa0d95621e27f35a8a6054a9f25 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Wed, 7 Jun 2023 22:28:41 +0200 Subject: [PATCH 13/37] Added test of joint_limiter --- joint_limits/CMakeLists.txt | 8 +- joint_limits/package.xml | 1 + .../test/simple_joint_limiter_param.yaml | 24 +++ .../test/test_simple_joint_limiter.cpp | 173 ++++++++++++++++-- .../test/test_simple_joint_limiter.hpp | 72 ++++++++ 5 files changed, 257 insertions(+), 21 deletions(-) create mode 100644 joint_limits/test/simple_joint_limiter_param.yaml create mode 100644 joint_limits/test/test_simple_joint_limiter.hpp diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index f81f15c21a..0dff5d3c20 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -58,6 +58,7 @@ pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_gmock REQUIRED) + find_package(generate_parameter_library REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) @@ -71,11 +72,13 @@ if(BUILD_TESTING) DESTINATION lib/joint_limits ) install( - FILES test/joint_limits_rosparam.yaml + FILES test/joint_limits_rosparam.yaml test/simple_joint_limiter_param.yaml DESTINATION share/joint_limits/test ) - ament_add_gmock(test_simple_joint_limiter test/test_simple_joint_limiter.cpp) + add_rostest_with_parameters_gmock(test_simple_joint_limiter test/test_simple_joint_limiter.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/simple_joint_limiter_param.yaml + ) target_include_directories(test_simple_joint_limiter PRIVATE include) target_link_libraries(test_simple_joint_limiter joint_limiter_interface) ament_target_dependencies( @@ -83,6 +86,7 @@ if(BUILD_TESTING) pluginlib rclcpp ) + endif() install( diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 5fcd2fc855..ade5cc1e39 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -22,6 +22,7 @@ ament_cmake_gmock ament_cmake_gtest + generate_parameter_library launch_testing_ament_cmake diff --git a/joint_limits/test/simple_joint_limiter_param.yaml b/joint_limits/test/simple_joint_limiter_param.yaml new file mode 100644 index 0000000000..7421793625 --- /dev/null +++ b/joint_limits/test/simple_joint_limiter_param.yaml @@ -0,0 +1,24 @@ +validate_limitation: + ros__parameters: + joint_limits: + # Get full specification from parameter server + foo_joint: + has_position_limits: true + min_position: -5.0 + max_position: 5.0 + has_velocity_limits: true + max_velocity: 2.0 + has_acceleration_limits: true + max_acceleration: 5.0 + has_deceleration_limits: true + max_deceleration: 7.5 + has_jerk_limits: true + max_jerk: 100.0 + has_effort_limits: true + max_effort: 20.0 + angle_wraparound: true # should be ignored, has position limits + has_soft_limits: true + k_position: 10.0 + k_velocity: 20.0 + soft_lower_limit: 0.1 + soft_upper_limit: 0.9 diff --git a/joint_limits/test/test_simple_joint_limiter.cpp b/joint_limits/test/test_simple_joint_limiter.cpp index e5bd3697ed..514f0bcd61 100644 --- a/joint_limits/test/test_simple_joint_limiter.cpp +++ b/joint_limits/test/test_simple_joint_limiter.cpp @@ -14,29 +14,164 @@ /// \author Dr. Denis Stogl -#include +#include "test_simple_joint_limiter.hpp" -#include -#include - -#include "joint_limits/joint_limiter_interface.hpp" -#include "joint_limits/joint_limits.hpp" -#include "pluginlib/class_loader.hpp" -#include "rclcpp/executor.hpp" +TEST_F(SimpleJointLimiterTest, load_limiter) +{ + ASSERT_NO_THROW( + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_))); + ASSERT_NE(joint_limiter_, nullptr); +} -TEST(TestLoadSimpleJointLimiter, load_limiter) +TEST_F(SimpleJointLimiterTest, validate_limitation) { - rclcpp::init(0, nullptr); + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_)); - using JointLimiter = joint_limits::JointLimiterInterface; - pluginlib::ClassLoader joint_limiter_loader( - "joint_limits", "joint_limits::JointLimiterInterface"); + if (joint_limiter_) + { + rclcpp::Duration period(1.0, 0.0); // 1 second - std::unique_ptr joint_limiter; - std::string joint_limiter_type = "joint_limits/SimpleJointLimiter"; + // initialize the limiter + std::vector joint_names = {"foo_joint"}; + auto num_joints = joint_names.size(); + ASSERT_TRUE(joint_limiter_->init(joint_names, node_)); - ASSERT_NO_THROW( - joint_limiter = std::unique_ptr( - joint_limiter_loader.createUnmanagedInstance(joint_limiter_type))); - ASSERT_NE(joint_limiter, nullptr); + last_commanded_state_.positions.resize(num_joints); + last_commanded_state_.velocities.resize(num_joints, 0.0); + last_commanded_state_.accelerations.resize(num_joints, 0.0); + + // no size check occurs (yet) so expect true + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + desired_joint_states_ = last_commanded_state_; + current_joint_states_ = last_commanded_state_; + + // pos, vel, acc, dec = 1.0, 2.0, 5.0, 7.5 + + desired_joint_states_.velocities.clear(); + // trigger size check with one wrong size + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + // fix size + desired_joint_states_.velocities.resize(num_joints, 0.0); + + // within limits + desired_joint_states_.positions[0] = 1.0; + desired_joint_states_.velocities[0] = 1.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if no limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + desired_joint_states_.positions[0], // pos unchanged + desired_joint_states_.velocities[0], // vel unchanged + desired_joint_states_.velocities[0] // acc = vel / 1.0 + ); + + // desired velocity exceeds + desired_joint_states_.velocities[0] = 3.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + desired_joint_states_.positions[0], // pos unchanged + 2.0, // vel limited to max_vel + 2.0 / 1.0 // acc set to vel change/DT + ); + + // check opposite velocity direction (sign copy) + desired_joint_states_.velocities[0] = -3.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + desired_joint_states_.positions[0], // pos unchanged + -2.0, // vel limited to -max_vel + -2.0 / 1.0 // acc set to vel change/DT + ); + + // desired pos exceeds + desired_joint_states_.positions[0] = 20.0; + desired_joint_states_.velocities[0] = 0.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 5.0, // pos unchanged + desired_joint_states_.velocities[0], // vel unchanged + desired_joint_states_.accelerations[0] // acc unchanged + ); + + // close to pos limit should reduce velocity and stop + current_joint_states_.positions[0] = 4.851; + current_joint_states_.velocities[0] = 1.5; + desired_joint_states_.positions[0] = 4.926; + desired_joint_states_.velocities[0] = 1.5; + + // changing to 0.05 because 1.0 sec invalidates the "small dt integration" + period = rclcpp::Duration::from_seconds(0.05); + + // this setup requires 0.15 distance to stop, and 0.2 seconds (so 4 cycles at 0.05) + for (auto i = 0u; i < 4; ++i) + { + auto previous_vel_request = desired_joint_states_.velocities[0]; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + ASSERT_LE( + desired_joint_states_.velocities[0], + previous_vel_request); // vel adapted to reach end-stop should be decreasing + // NOTE: after the first cycle, vel is reduced and does not trigger stopping position limit + // hence no max deceleration anymore... + ASSERT_LE( + desired_joint_states_.positions[0], + 5.0 + + 10.0 * + COMMON_THRESHOLD); // is not decelerating at max all the time, hence err in last cycle + ASSERT_LE(desired_joint_states_.accelerations[0], 0.0); // should decelerate + + INTEGRATE(current_joint_states_, desired_joint_states_, period.seconds()); + + ASSERT_LE(current_joint_states_.positions[0], 5.0); // below joint limit + } + + // desired acceleration exceeds + current_joint_states_.positions[0] = 0.0; + current_joint_states_.velocities[0] = 0.0; + desired_joint_states_.velocities[0] = 0.5; // leads to acc > max acc + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + desired_joint_states_.positions[0], // pos unchanged + 0.25, // vel limited max acc * dt + 5.0 // acc max acc + ); + + // desired deceleration exceeds + current_joint_states_.positions[0] = 0.0; + current_joint_states_.velocities[0] = 1.0; + desired_joint_states_.velocities[0] = 0.5; // leads to acc > -max dec + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + desired_joint_states_.positions[0], // pos unchanged + 0.625, // vel limited by max dec * dt + -7.5 // acc max dec + ); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; } diff --git a/joint_limits/test/test_simple_joint_limiter.hpp b/joint_limits/test/test_simple_joint_limiter.hpp new file mode 100644 index 0000000000..516752c4aa --- /dev/null +++ b/joint_limits/test/test_simple_joint_limiter.hpp @@ -0,0 +1,72 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_SIMPLE_JOINT_LIMITER_HPP_ +#define TEST_SIMPLE_JOINT_LIMITER_HPP_ + +#include + +#include +#include +#include +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/node.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +const double COMMON_THRESHOLD = 0.0011; + +#define CHECK_STATE_SINGLE_JOINT(tested_traj_point, idx, expected_pos, expected_vel, expected_acc) \ + EXPECT_NEAR(tested_traj_point.positions[idx], expected_pos, COMMON_THRESHOLD); \ + EXPECT_NEAR(tested_traj_point.velocities[idx], expected_vel, COMMON_THRESHOLD); \ + EXPECT_NEAR(tested_traj_point.accelerations[idx], expected_acc, COMMON_THRESHOLD); + +#define INTEGRATE(cur, des, dt) \ + cur.positions[0] += des.velocities[0] * dt + 0.5 * des.accelerations[0] * dt * dt; \ + cur.velocities[0] += des.accelerations[0] * dt; + +using JointLimiter = joint_limits::JointLimiterInterface; + +class SimpleJointLimiterTest : public ::testing::Test +{ +public: + void SetUp() override + { + auto testname = ::testing::UnitTest::GetInstance()->current_test_info()->name(); + node_ = std::make_shared(testname); + } + + SimpleJointLimiterTest() + : joint_limiter_loader_( + "joint_limits", "joint_limits::JointLimiterInterface") + { + joint_limiter_type_ = "joint_limits/SimpleJointLimiter"; + } + + void TearDown() override { node_.reset(); } + +protected: + rclcpp::Node::SharedPtr node_; + std::unique_ptr joint_limiter_; + std::string joint_limiter_type_; + pluginlib::ClassLoader joint_limiter_loader_; + + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; + trajectory_msgs::msg::JointTrajectoryPoint desired_joint_states_; + trajectory_msgs::msg::JointTrajectoryPoint current_joint_states_; +}; + +#endif // TEST_SIMPLE_JOINT_LIMITER_HPP_ From 7bfc94e26a829bdeb8de94270a23c26334928e97 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Wed, 7 Jun 2023 22:29:54 +0200 Subject: [PATCH 14/37] Fixed deceleration limit application --- joint_limits/src/simple_joint_limiter.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 91d8ff2333..4b5afae05c 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -170,7 +170,13 @@ bool SimpleJointLimiter::on_enforce( // Here we aren't explicitly maximally decelerating, but for joints near their limits this // should still result in max decel being used desired_accel[index] = -current_joint_states.velocities[index] / dt_seconds; - if (joint_limits_[index].has_acceleration_limits) + if (joint_limits_[index].has_deceleration_limits) + { + desired_accel[index] = std::copysign( + std::min(std::abs(desired_accel[index]), joint_limits_[index].max_deceleration), + desired_accel[index]); + } + else if (joint_limits_[index].has_acceleration_limits) { desired_accel[index] = std::copysign( std::min(std::abs(desired_accel[index]), joint_limits_[index].max_acceleration), From c338b8cfefb10d3dd481a8ea78c978ff1b14c580 Mon Sep 17 00:00:00 2001 From: gwalck Date: Fri, 9 Jun 2023 10:44:34 +0200 Subject: [PATCH 15/37] Updated authorship Co-authored-by: Dr. Denis --- joint_limits/test/test_simple_joint_limiter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_limits/test/test_simple_joint_limiter.cpp b/joint_limits/test/test_simple_joint_limiter.cpp index 514f0bcd61..151abb501a 100644 --- a/joint_limits/test/test_simple_joint_limiter.cpp +++ b/joint_limits/test/test_simple_joint_limiter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -/// \author Dr. Denis Stogl +/// \author Dr. Denis Stogl, Guillaume Walck #include "test_simple_joint_limiter.hpp" From 6e0e6c5d6159613d0f5e3fdf017bec298942d028 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Fri, 9 Jun 2023 13:10:41 +0200 Subject: [PATCH 16/37] Split test, apply review changes, add future tests --- .../test/simple_joint_limiter_param.yaml | 2 +- .../test/test_simple_joint_limiter.cpp | 144 +++++++++++++++--- .../test/test_simple_joint_limiter.hpp | 47 +++++- 3 files changed, 162 insertions(+), 31 deletions(-) diff --git a/joint_limits/test/simple_joint_limiter_param.yaml b/joint_limits/test/simple_joint_limiter_param.yaml index 7421793625..02db074b14 100644 --- a/joint_limits/test/simple_joint_limiter_param.yaml +++ b/joint_limits/test/simple_joint_limiter_param.yaml @@ -1,4 +1,4 @@ -validate_limitation: +simple_joint_limiter: ros__parameters: joint_limits: # Get full specification from parameter server diff --git a/joint_limits/test/test_simple_joint_limiter.cpp b/joint_limits/test/test_simple_joint_limiter.cpp index 151abb501a..9ed78bb688 100644 --- a/joint_limits/test/test_simple_joint_limiter.cpp +++ b/joint_limits/test/test_simple_joint_limiter.cpp @@ -16,44 +16,76 @@ #include "test_simple_joint_limiter.hpp" -TEST_F(SimpleJointLimiterTest, load_limiter) +TEST_F(SimpleJointLimiterTest, when_loading_limiter_plugin_expect_loaded) { + // Test SimpleJointLimiter loading ASSERT_NO_THROW( joint_limiter_ = std::unique_ptr( joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_))); ASSERT_NE(joint_limiter_, nullptr); } -TEST_F(SimpleJointLimiterTest, validate_limitation) +/* FIXME: currently init does not check if limit parameters exist for the requested joint +TEST_F(SimpleJointLimiterTest, when_joint_not_found_expect_init_fail) { - joint_limiter_ = std::unique_ptr( - joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_)); + SetupNode("simple_joint_limiter"); + Load(); if (joint_limiter_) { - rclcpp::Duration period(1.0, 0.0); // 1 second - // initialize the limiter - std::vector joint_names = {"foo_joint"}; - auto num_joints = joint_names.size(); - ASSERT_TRUE(joint_limiter_->init(joint_names, node_)); + std::vector joint_names = {"bar_joint"}; + ASSERT_FALSE(joint_limiter_->init(joint_names, node_)); + } +} +*/ + +/*FIXME: currently dt is not tested and leads to nan which lets all other tests pass +TEST_F(SimpleJointLimiterTest, when_invalid_dt_expect_enforce_fail) +{ + SetupNode("simple_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + rclcpp::Duration period(0, 0); // 0 second + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + } +} +*/ - last_commanded_state_.positions.resize(num_joints); - last_commanded_state_.velocities.resize(num_joints, 0.0); - last_commanded_state_.accelerations.resize(num_joints, 0.0); +TEST_F(SimpleJointLimiterTest, when_wrong_input_expect_enforce_fail) +{ + SetupNode("simple_joint_limiter"); + Load(); + if (joint_limiter_) + { + Init(); // no size check occurs (yet) so expect true ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); - desired_joint_states_ = last_commanded_state_; - current_joint_states_ = last_commanded_state_; - - // pos, vel, acc, dec = 1.0, 2.0, 5.0, 7.5 + rclcpp::Duration period(1, 0); // 1 second desired_joint_states_.velocities.clear(); // trigger size check with one wrong size ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - // fix size - desired_joint_states_.velocities.resize(num_joints, 0.0); + } +} + +TEST_F(SimpleJointLimiterTest, when_within_limits_expect_no_limits_applied) +{ + SetupNode("simple_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + // pos, vel, acc, dec = 1.0, 2.0, 5.0, 7.5 // within limits desired_joint_states_.positions[0] = 1.0; @@ -67,6 +99,20 @@ TEST_F(SimpleJointLimiterTest, validate_limitation) desired_joint_states_.velocities[0], // vel unchanged desired_joint_states_.velocities[0] // acc = vel / 1.0 ); + } +} + +TEST_F(SimpleJointLimiterTest, when_velocity_exceeded_expect_vel_and_acc_enforced) +{ + SetupNode("simple_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second // desired velocity exceeds desired_joint_states_.velocities[0] = 3.0; @@ -91,6 +137,20 @@ TEST_F(SimpleJointLimiterTest, validate_limitation) -2.0, // vel limited to -max_vel -2.0 / 1.0 // acc set to vel change/DT ); + } +} + +TEST_F(SimpleJointLimiterTest, when_position_exceeded_expect_pos_enforced) +{ + SetupNode("simple_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second // desired pos exceeds desired_joint_states_.positions[0] = 20.0; @@ -104,6 +164,21 @@ TEST_F(SimpleJointLimiterTest, validate_limitation) desired_joint_states_.velocities[0], // vel unchanged desired_joint_states_.accelerations[0] // acc unchanged ); + } +} + +TEST_F(SimpleJointLimiterTest, when_position_close_to_pos_limit_expect_deceleration_enforced) +{ + SetupNode("simple_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + // using 0.05 because 1.0 sec invalidates the "small dt integration" + rclcpp::Duration period(0, 50000000); // 0.05 second // close to pos limit should reduce velocity and stop current_joint_states_.positions[0] = 4.851; @@ -111,9 +186,6 @@ TEST_F(SimpleJointLimiterTest, validate_limitation) desired_joint_states_.positions[0] = 4.926; desired_joint_states_.velocities[0] = 1.5; - // changing to 0.05 because 1.0 sec invalidates the "small dt integration" - period = rclcpp::Duration::from_seconds(0.05); - // this setup requires 0.15 distance to stop, and 0.2 seconds (so 4 cycles at 0.05) for (auto i = 0u; i < 4; ++i) { @@ -132,10 +204,24 @@ TEST_F(SimpleJointLimiterTest, validate_limitation) COMMON_THRESHOLD); // is not decelerating at max all the time, hence err in last cycle ASSERT_LE(desired_joint_states_.accelerations[0], 0.0); // should decelerate - INTEGRATE(current_joint_states_, desired_joint_states_, period.seconds()); + Integrate(period.seconds()); ASSERT_LE(current_joint_states_.positions[0], 5.0); // below joint limit } + } +} + +TEST_F(SimpleJointLimiterTest, when_acceleration_exceeded_expect_acc_enforced) +{ + SetupNode("simple_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); // desired acceleration exceeds current_joint_states_.positions[0] = 0.0; @@ -150,6 +236,20 @@ TEST_F(SimpleJointLimiterTest, validate_limitation) 0.25, // vel limited max acc * dt 5.0 // acc max acc ); + } +} + +TEST_F(SimpleJointLimiterTest, when_deceleration_exceeded_expect_acc_enforced) +{ + SetupNode("simple_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); // desired deceleration exceeds current_joint_states_.positions[0] = 0.0; diff --git a/joint_limits/test/test_simple_joint_limiter.hpp b/joint_limits/test/test_simple_joint_limiter.hpp index 516752c4aa..27597eb628 100644 --- a/joint_limits/test/test_simple_joint_limiter.hpp +++ b/joint_limits/test/test_simple_joint_limiter.hpp @@ -34,10 +34,6 @@ const double COMMON_THRESHOLD = 0.0011; EXPECT_NEAR(tested_traj_point.velocities[idx], expected_vel, COMMON_THRESHOLD); \ EXPECT_NEAR(tested_traj_point.accelerations[idx], expected_acc, COMMON_THRESHOLD); -#define INTEGRATE(cur, des, dt) \ - cur.positions[0] += des.velocities[0] * dt + 0.5 * des.accelerations[0] * dt * dt; \ - cur.velocities[0] += des.accelerations[0] * dt; - using JointLimiter = joint_limits::JointLimiterInterface; class SimpleJointLimiterTest : public ::testing::Test @@ -45,21 +41,56 @@ class SimpleJointLimiterTest : public ::testing::Test public: void SetUp() override { - auto testname = ::testing::UnitTest::GetInstance()->current_test_info()->name(); - node_ = std::make_shared(testname); + node_name_ = ::testing::UnitTest::GetInstance()->current_test_info()->name(); + } + + void SetupNode(const std::string node_name = "") + { + if (!node_name.empty()) node_name_ = node_name; + node_ = std::make_shared(node_name_); + } + + void Load() + { + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_)); + } + + void Init() + { + joint_names_ = {"foo_joint"}; + joint_limiter_->init(joint_names_, node_); + num_joints_ = joint_names_.size(); + last_commanded_state_.positions.resize(num_joints_, 0.0); + last_commanded_state_.velocities.resize(num_joints_, 0.0); + last_commanded_state_.accelerations.resize(num_joints_, 0.0); + desired_joint_states_ = last_commanded_state_; + current_joint_states_ = last_commanded_state_; + } + + void Configure() { joint_limiter_->configure(last_commanded_state_); } + + void Integrate(double dt) + { + current_joint_states_.positions[0] += desired_joint_states_.velocities[0] * dt + + 0.5 * desired_joint_states_.accelerations[0] * dt * dt; + current_joint_states_.velocities[0] += desired_joint_states_.accelerations[0] * dt; } SimpleJointLimiterTest() - : joint_limiter_loader_( + : joint_limiter_type_("joint_limits/SimpleJointLimiter"), + joint_limiter_loader_( "joint_limits", "joint_limits::JointLimiterInterface") { - joint_limiter_type_ = "joint_limits/SimpleJointLimiter"; } void TearDown() override { node_.reset(); } protected: + std::string node_name_; rclcpp::Node::SharedPtr node_; + std::vector joint_names_; + size_t num_joints_; std::unique_ptr joint_limiter_; std::string joint_limiter_type_; pluginlib::ClassLoader joint_limiter_loader_; From cfc8fe5556cb80d1e3eee3b9d9238e213a3e0319 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 12 Jun 2023 12:57:11 +0200 Subject: [PATCH 17/37] Applied review comment, added 2 tests & fixed typo --- joint_limits/src/simple_joint_limiter.cpp | 2 ++ .../test/simple_joint_limiter_param.yaml | 12 +++++++ .../test/test_simple_joint_limiter.cpp | 36 ++++++++++++++++--- 3 files changed, 45 insertions(+), 5 deletions(-) diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 4b5afae05c..0fa9a98f23 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -31,6 +31,8 @@ bool SimpleJointLimiter::on_enforce( trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) { const auto dt_seconds = dt.seconds(); + // negative or null is not allowed + if (dt_seconds <= 0.0) return false; if (current_joint_states.velocities.empty()) { diff --git a/joint_limits/test/simple_joint_limiter_param.yaml b/joint_limits/test/simple_joint_limiter_param.yaml index 02db074b14..4ca1ffdf07 100644 --- a/joint_limits/test/simple_joint_limiter_param.yaml +++ b/joint_limits/test/simple_joint_limiter_param.yaml @@ -22,3 +22,15 @@ simple_joint_limiter: k_velocity: 20.0 soft_lower_limit: 0.1 soft_upper_limit: 0.9 + +simple_joint_limiter_nodeclimit: + ros__parameters: + joint_limits: + foo_joint: + has_position_limits: true + min_position: -5.0 + max_position: 5.0 + has_velocity_limits: true + max_velocity: 2.0 + has_acceleration_limits: true + max_acceleration: 5.0 diff --git a/joint_limits/test/test_simple_joint_limiter.cpp b/joint_limits/test/test_simple_joint_limiter.cpp index 9ed78bb688..95da121712 100644 --- a/joint_limits/test/test_simple_joint_limiter.cpp +++ b/joint_limits/test/test_simple_joint_limiter.cpp @@ -40,7 +40,6 @@ TEST_F(SimpleJointLimiterTest, when_joint_not_found_expect_init_fail) } */ -/*FIXME: currently dt is not tested and leads to nan which lets all other tests pass TEST_F(SimpleJointLimiterTest, when_invalid_dt_expect_enforce_fail) { SetupNode("simple_joint_limiter"); @@ -54,7 +53,6 @@ TEST_F(SimpleJointLimiterTest, when_invalid_dt_expect_enforce_fail) ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); } } -*/ TEST_F(SimpleJointLimiterTest, when_wrong_input_expect_enforce_fail) { @@ -239,7 +237,7 @@ TEST_F(SimpleJointLimiterTest, when_acceleration_exceeded_expect_acc_enforced) } } -TEST_F(SimpleJointLimiterTest, when_deceleration_exceeded_expect_acc_enforced) +TEST_F(SimpleJointLimiterTest, when_deceleration_exceeded_expect_dec_enforced) { SetupNode("simple_joint_limiter"); Load(); @@ -261,8 +259,36 @@ TEST_F(SimpleJointLimiterTest, when_deceleration_exceeded_expect_acc_enforced) CHECK_STATE_SINGLE_JOINT( desired_joint_states_, 0, desired_joint_states_.positions[0], // pos unchanged - 0.625, // vel limited by max dec * dt - -7.5 // acc max dec + 0.625, // vel limited by vel-max dec * dt + -7.5 // acc limited by -max dec + ); + } +} + +TEST_F(SimpleJointLimiterTest, when_deceleration_exceeded_with_no_maxdec_expect_acc_enforced) +{ + SetupNode("simple_joint_limiter_nodeclimit"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + // desired deceleration exceeds + current_joint_states_.positions[0] = 0.0; + current_joint_states_.velocities[0] = 1.0; + desired_joint_states_.velocities[0] = 0.5; // leads to acc > -max acc + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + desired_joint_states_.positions[0], // pos unchanged + 0.75, // vel limited by vel-max acc * dt + -5.0 // acc limited to -max acc ); } } From 186c66e18fdf52c4c9309ae77a1e283d30a4342c Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 12 Jun 2023 21:15:23 +0200 Subject: [PATCH 18/37] Improved close to limit trigger --- joint_limits/src/simple_joint_limiter.cpp | 21 +++++++++++++++++++ .../test/test_simple_joint_limiter.cpp | 8 +++---- 2 files changed, 24 insertions(+), 5 deletions(-) diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 0fa9a98f23..46b7015026 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -148,6 +148,9 @@ bool SimpleJointLimiter::on_enforce( double stopping_distance = std::abs((-desired_vel[index] * desired_vel[index]) / (2 * stopping_deccel)); + // compute stopping duration at stopping_deccel + double stopping_duration = std::abs((desired_vel[index]) / (stopping_deccel)); + // Check that joint limits are beyond stopping_distance and desired_velocity is towards // that limit if ( @@ -161,6 +164,24 @@ bool SimpleJointLimiter::on_enforce( pos_limit_trig_jnts[index] = true; position_limit_triggered = true; } + else + { + // compute the travel_distance at new desired velocity, in best case duration stopping_duration + double motion_after_stopping_duration = desired_vel[index] * stopping_duration; + // re-check what happens if we don't slow down + if ( + (desired_vel[index] < 0 && + (current_joint_states.positions[index] - joint_limits_[index].min_position < + motion_after_stopping_duration)) || + (desired_vel[index] > 0 && + (joint_limits_[index].max_position - current_joint_states.positions[index] < + motion_after_stopping_duration))) + { + pos_limit_trig_jnts[index] = true; + position_limit_triggered = true; + } + // else no need to slow down. in worse case we won't hit the limit at current velocity + } } } diff --git a/joint_limits/test/test_simple_joint_limiter.cpp b/joint_limits/test/test_simple_joint_limiter.cpp index 95da121712..2de646e8dd 100644 --- a/joint_limits/test/test_simple_joint_limiter.cpp +++ b/joint_limits/test/test_simple_joint_limiter.cpp @@ -195,16 +195,14 @@ TEST_F(SimpleJointLimiterTest, when_position_close_to_pos_limit_expect_decelerat previous_vel_request); // vel adapted to reach end-stop should be decreasing // NOTE: after the first cycle, vel is reduced and does not trigger stopping position limit // hence no max deceleration anymore... - ASSERT_LE( + ASSERT_LT( desired_joint_states_.positions[0], - 5.0 + - 10.0 * - COMMON_THRESHOLD); // is not decelerating at max all the time, hence err in last cycle + 5.0 + COMMON_THRESHOLD); // should decelerate at each cycle and stay below limits ASSERT_LE(desired_joint_states_.accelerations[0], 0.0); // should decelerate Integrate(period.seconds()); - ASSERT_LE(current_joint_states_.positions[0], 5.0); // below joint limit + ASSERT_LT(current_joint_states_.positions[0], 5.0); // below joint limit } } } From 1749f6ddfacf36b9a6c04d256b1e208d1080bb7e Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 27 Jun 2023 16:53:05 +0200 Subject: [PATCH 19/37] Update joint_limits/src/simple_joint_limiter.cpp --- joint_limits/src/simple_joint_limiter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 46b7015026..e7cb96ae6b 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -/// \authors Nathan Brooks, Denis Stogl +/// \authors Nathan Brooks, Dr. Denis Stogl #include "joint_limits/simple_joint_limiter.hpp" From e25429494d844092dc037558eeaad4b18b06ae98 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 29 Jun 2023 15:19:32 +0200 Subject: [PATCH 20/37] Fix formatting. --- joint_limits/src/simple_joint_limiter.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index e7cb96ae6b..8f3de96f38 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -166,7 +166,8 @@ bool SimpleJointLimiter::on_enforce( } else { - // compute the travel_distance at new desired velocity, in best case duration stopping_duration + // compute the travel_distance at new desired velocity, in best case duration + // stopping_duration double motion_after_stopping_duration = desired_vel[index] * stopping_duration; // re-check what happens if we don't slow down if ( From 7186e9e17bb2305c16bab418505ea84d1013ed9a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 29 Jun 2023 15:39:41 +0200 Subject: [PATCH 21/37] Restore Rucking Limiter. --- .../CMakeLists.txt | 67 +++++++ .../visibility_control.h | 49 +++++ .../ruckig_joint_limiter.hpp | 64 ++++++ .../joint_limits_enforcement_plugins.xml | 9 + joint_limits_enforcement_plugins/package.xml | 32 +++ .../src/ruckig_joint_limiter.cpp | 182 ++++++++++++++++++ .../test/test_ruckig_joint_limiter.cpp | 44 +++++ 7 files changed, 447 insertions(+) create mode 100644 joint_limits_enforcement_plugins/CMakeLists.txt create mode 100644 joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h create mode 100644 joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp create mode 100644 joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml create mode 100644 joint_limits_enforcement_plugins/package.xml create mode 100644 joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp create mode 100644 joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp diff --git a/joint_limits_enforcement_plugins/CMakeLists.txt b/joint_limits_enforcement_plugins/CMakeLists.txt new file mode 100644 index 0000000000..9580e35c2e --- /dev/null +++ b/joint_limits_enforcement_plugins/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 3.16) +project(joint_limits_enforcement_plugins LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + joint_limits + pluginlib + rclcpp + rcutils + ruckig +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +add_library(ruckig_joint_limiter SHARED + src/ruckig_joint_limiter.cpp +) +target_compile_features(ruckig_joint_limiter PUBLIC cxx_std_17) +target_include_directories(ruckig_joint_limiter PUBLIC + $ + $ +) +ament_target_dependencies(ruckig_joint_limiter PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(ruckig_joint_limiter PRIVATE "JOINT_LIMITS_ENFORCEMENT_PLUGINS_BUILDING_DLL") + +pluginlib_export_plugin_description_file(joint_limits joint_limits_enforcement_plugins.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(joint_limits REQUIRED) + find_package(pluginlib REQUIRED) + find_package(rclcpp REQUIRED) + + ament_add_gmock(test_ruckig_joint_limiter test/test_ruckig_joint_limiter.cpp) + target_include_directories(test_ruckig_joint_limiter PRIVATE include) + ament_target_dependencies( + test_ruckig_joint_limiter + joint_limits + pluginlib + rclcpp + ) +endif() + +install(DIRECTORY include/ + DESTINATION include/joint_limits_enforcement_plugins +) +install( + TARGETS + ruckig_joint_limiter + EXPORT export_joint_limits_enforcement_plugins + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_targets(export_joint_limits_enforcement_plugins HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h b/joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h new file mode 100644 index 0000000000..abd0019cf6 --- /dev/null +++ b/joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_LIMITS_ENFORCEMENT_PLUGINS__VISIBILITY_CONTROL_H_ +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT __attribute__((dllexport)) +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT __attribute__((dllimport)) +#else +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT __declspec(dllexport) +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT __declspec(dllimport) +#endif +#ifdef JOINT_LIMITS_ENFORCEMENT_PLUGINS_BUILDING_DLL +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT +#else +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT +#endif +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC_TYPE JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_LOCAL +#else +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT __attribute__((visibility("default"))) +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT +#if __GNUC__ >= 4 +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC __attribute__((visibility("default"))) +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_LOCAL __attribute__((visibility("hidden"))) +#else +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_LOCAL +#endif +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC_TYPE +#endif + +#endif // JOINT_LIMITS_ENFORCEMENT_PLUGINS__VISIBILITY_CONTROL_H_ diff --git a/joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp b/joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp new file mode 100644 index 0000000000..fd577c32b3 --- /dev/null +++ b/joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp @@ -0,0 +1,64 @@ +// Copyright (c) 2021, PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Andy Zelenak, Denis Stogl + +#ifndef RUCKIG_JOINT_LIMITER__RUCKIG_JOINT_LIMITER_HPP_ +#define RUCKIG_JOINT_LIMITER__RUCKIG_JOINT_LIMITER_HPP_ + +#include + +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "joint_limits_enforcement_plugins/visibility_control.h" +#include "ruckig/input_parameter.hpp" +#include "ruckig/output_parameter.hpp" +#include "ruckig/ruckig.hpp" + +namespace ruckig_joint_limiter +{ +namespace // utility namespace +{ +constexpr double DEFAULT_MAX_VELOCITY = 5; // rad/s +constexpr double DEFAULT_MAX_ACCELERATION = 10; // rad/s^2 +constexpr double DEFAULT_MAX_JERK = 20; // rad/s^3 +} // namespace + +template +class RuckigJointLimiter : public joint_limits::JointLimiterInterface +{ +public: + JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC RuckigJointLimiter(); + + JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC bool on_init() override; + + JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC bool on_configure( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) override; + + JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC bool on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, + const rclcpp::Duration & dt) override; + +private: + bool received_initial_state_ = false; + // Ruckig algorithm + std::shared_ptr> ruckig_; + std::shared_ptr> ruckig_input_; + std::shared_ptr> ruckig_output_; +}; + +} // namespace ruckig_joint_limiter + +#endif // RUCKIG_JOINT_LIMITER__RUCKIG_JOINT_LIMITER_HPP_ diff --git a/joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml b/joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml new file mode 100644 index 0000000000..aa2996282f --- /dev/null +++ b/joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml @@ -0,0 +1,9 @@ + + + + Jerk-limited smoothing with the Ruckig library. + + + diff --git a/joint_limits_enforcement_plugins/package.xml b/joint_limits_enforcement_plugins/package.xml new file mode 100644 index 0000000000..26e6820598 --- /dev/null +++ b/joint_limits_enforcement_plugins/package.xml @@ -0,0 +1,32 @@ + + joint_limits_enforcement_plugins + 0.0.0 + Package for handling of joint limits using external libraries for use in controllers or in hardware. + + Bence Magyar + Denis Štogl + + Denis Štogl + Andy Zelenak + + Apache License 2.0 + + https://github.com/ros-controls/ros2_control/wiki + https://github.com/ros-controls/ros2_control/issues + https://github.com/ros-controls/ros2_control + + ament_cmake + + backward_ros + joint_limits + rclcpp + pluginlib + rcutils + ruckig + + ament_cmake_gmock + + + ament_cmake + + diff --git a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp new file mode 100644 index 0000000000..ad264aed67 --- /dev/null +++ b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp @@ -0,0 +1,182 @@ +// Copyright (c) 2021, PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \authors Andy Zelenak, Denis Stogl + +#include "ruckig_joint_limiter/ruckig_joint_limiter.hpp" + +#include +#include +#include + +#include "joint_limits/joint_limits_rosparam.hpp" +#include "rcutils/logging_macros.h" +#include "ruckig/brake.hpp" +#include "ruckig/input_parameter.hpp" +#include "ruckig/output_parameter.hpp" +#include "ruckig/ruckig.hpp" + +namespace ruckig_joint_limiter +{ +template <> +RuckigJointLimiter::RuckigJointLimiter() +: joint_limits::JointLimiterInterface() +{ +} + +template <> +bool RuckigJointLimiter::on_init(/*const rclcpp::Duration & dt*/) +{ + // TODO(destogl): This should be used from parameter + const rclcpp::Duration dt = rclcpp::Duration::from_seconds(0.005); + + // Initialize Ruckig + ruckig_ = std::make_shared>(number_of_joints_, dt.seconds()); + ruckig_input_ = std::make_shared>(number_of_joints_); + ruckig_output_ = std::make_shared>(number_of_joints_); + + // Velocity mode works best for smoothing one waypoint at a time + ruckig_input_->control_interface = ruckig::ControlInterface::Velocity; + ruckig_input_->synchronization = ruckig::Synchronization::Time; + + for (auto joint = 0ul; joint < number_of_joints_; ++joint) + { + if (joint_limits_[joint].has_jerk_limits) + { + ruckig_input_->max_jerk.at(joint) = joint_limits_[joint].max_acceleration; + } + else + { + ruckig_input_->max_jerk.at(joint) = DEFAULT_MAX_JERK; + } + if (joint_limits_[joint].has_acceleration_limits) + { + ruckig_input_->max_acceleration.at(joint) = joint_limits_[joint].max_acceleration; + } + else + { + ruckig_input_->max_acceleration.at(joint) = DEFAULT_MAX_ACCELERATION; + } + if (joint_limits_[joint].has_velocity_limits) + { + ruckig_input_->max_velocity.at(joint) = joint_limits_[joint].max_velocity; + } + else + { + ruckig_input_->max_velocity.at(joint) = DEFAULT_MAX_VELOCITY; + } + } + + return true; +} + +template <> +bool RuckigJointLimiter::on_configure( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) +{ + // Initialize Ruckig with current_joint_states + ruckig_input_->current_position = current_joint_states.positions; + + if (current_joint_states.velocities.size() == number_of_joints_) + { + ruckig_input_->current_velocity = current_joint_states.velocities; + } + else + { + ruckig_input_->current_velocity = std::vector(number_of_joints_, 0.0); + } + if (current_joint_states.accelerations.size() == number_of_joints_) + { + ruckig_input_->current_acceleration = current_joint_states.accelerations; + } + else + { + ruckig_input_->current_acceleration = std::vector(number_of_joints_, 0.0); + } + + // Initialize output data + ruckig_output_->new_position = ruckig_input_->current_position; + ruckig_output_->new_velocity = ruckig_input_->current_velocity; + ruckig_output_->new_acceleration = ruckig_input_->current_acceleration; + + return true; +} + +template <> +bool RuckigJointLimiter::on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, + const rclcpp::Duration & /*dt*/) +{ + // We don't use current_joint_states. For stability, it is recommended to feed previous Ruckig + // output back in as input for the next iteration. This assumes the robot tracks the command well. + ruckig_input_->current_position = ruckig_output_->new_position; + ruckig_input_->current_velocity = ruckig_output_->new_velocity; + ruckig_input_->current_acceleration = ruckig_output_->new_acceleration; + + // Target state is the next waypoint + ruckig_input_->target_position = desired_joint_states.positions; + // TODO(destogl): in current use-case we use only velocity + if (desired_joint_states.velocities.size() == number_of_joints_) + { + ruckig_input_->target_velocity = desired_joint_states.velocities; + } + else + { + ruckig_input_->target_velocity = std::vector(number_of_joints_, 0.0); + } + if (desired_joint_states.accelerations.size() == number_of_joints_) + { + ruckig_input_->target_acceleration = desired_joint_states.accelerations; + } + else + { + ruckig_input_->target_acceleration = std::vector(number_of_joints_, 0.0); + } + + ruckig::Result result = ruckig_->update(*ruckig_input_, *ruckig_output_); + RCUTILS_LOG_DEBUG_NAMED("ruckig_joint_limiter", "Rucking result %d", result); + + desired_joint_states.positions = ruckig_output_->new_position; + desired_joint_states.velocities = ruckig_output_->new_velocity; + desired_joint_states.accelerations = ruckig_output_->new_acceleration; + + // Feed output from the previous timestep back as input + for (auto joint = 0ul; joint < number_of_joints_; ++joint) + { + RCUTILS_LOG_DEBUG_NAMED( + "ruckig_joint_limiter", + "Desired position: %e \nDesired velocity: %e\n Desired acceleration: %e.", + ruckig_input_->target_position.at(joint), ruckig_input_->target_velocity.at(joint), + ruckig_input_->target_acceleration.at(joint)); + } + + for (auto joint = 0ul; joint < number_of_joints_; ++joint) + { + RCUTILS_LOG_DEBUG_NAMED( + "ruckig_joint_limiter", "New position: %e \nNew velocity: %e\nNew acceleration: %e.", + ruckig_output_->new_position.at(joint), ruckig_output_->new_velocity.at(joint), + ruckig_output_->new_acceleration.at(joint)); + } + + return result == ruckig::Result::Finished; +} + +} // namespace ruckig_joint_limiter + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ruckig_joint_limiter::RuckigJointLimiter, + joint_limits::JointLimiterInterface) diff --git a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp new file mode 100644 index 0000000000..b1b19d0587 --- /dev/null +++ b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp @@ -0,0 +1,44 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#include + +#include +#include + +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/executor.hpp" + +TEST(TestLoadSimpleJointLimiter, load_limiter) +{ + rclcpp::init(0, nullptr); + + using JointLimiter = joint_limits::JointLimiterInterface; + pluginlib::ClassLoader joint_limiter_loader( + "joint_limits", "joint_limits::JointLimiterInterface"); + + std::unique_ptr joint_limiter; + std::string joint_limiter_type = "ruckig_joint_limiter/RuckigJointLimiter"; + + joint_limiter = + std::unique_ptr(joint_limiter_loader.createUnmanagedInstance(joint_limiter_type)); + ASSERT_NO_THROW( + joint_limiter = std::unique_ptr( + joint_limiter_loader.createUnmanagedInstance(joint_limiter_type))); + ASSERT_NE(joint_limiter, nullptr); +} From 2cb1cdf733ddfe7e96b34cd33a243c46ce101fb4 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 3 Jul 2023 12:09:25 +0200 Subject: [PATCH 22/37] Fixed ruckig linking --- joint_limits_enforcement_plugins/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/joint_limits_enforcement_plugins/CMakeLists.txt b/joint_limits_enforcement_plugins/CMakeLists.txt index 9580e35c2e..902b0f49dc 100644 --- a/joint_limits_enforcement_plugins/CMakeLists.txt +++ b/joint_limits_enforcement_plugins/CMakeLists.txt @@ -27,6 +27,9 @@ target_include_directories(ruckig_joint_limiter PUBLIC $ $ ) +target_link_libraries(ruckig_joint_limiter PRIVATE + ruckig::ruckig +) ament_target_dependencies(ruckig_joint_limiter PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. From 8a46d4cba3cb2dd769ec0f7f93ef7bcd05ae0466 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 3 Jul 2023 12:10:08 +0200 Subject: [PATCH 23/37] Fixed max_jerk initialization --- joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp index ad264aed67..ee3b577b6d 100644 --- a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp +++ b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp @@ -54,7 +54,7 @@ bool RuckigJointLimiter::on_init(/*const rclcpp::Dura { if (joint_limits_[joint].has_jerk_limits) { - ruckig_input_->max_jerk.at(joint) = joint_limits_[joint].max_acceleration; + ruckig_input_->max_jerk.at(joint) = joint_limits_[joint].max_jerk; } else { From 79b75790bd2bd0da4d246fa31188af868a23bb2e Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 3 Jul 2023 12:12:53 +0200 Subject: [PATCH 24/37] Added protection from invalid desired_positions --- .../src/ruckig_joint_limiter.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp index ee3b577b6d..24ad158d38 100644 --- a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp +++ b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp @@ -126,7 +126,18 @@ bool RuckigJointLimiter::on_enforce( ruckig_input_->current_acceleration = ruckig_output_->new_acceleration; // Target state is the next waypoint - ruckig_input_->target_position = desired_joint_states.positions; + if (desired_joint_states.positions.size() == number_of_joints_) + { + ruckig_input_->target_position = desired_joint_states.positions; + } + else + { + RCUTILS_LOG_WARN_NAMED( + "ruckig_joint_limiter", + "Size of desired positions (%zu) does not match number of joint (%zu).", + desired_joint_states.positions.size(), number_of_joints_); + return false; + } // TODO(destogl): in current use-case we use only velocity if (desired_joint_states.velocities.size() == number_of_joints_) { From 1352e3068616f14e0c8138e69f07f5a76b2e612b Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 3 Jul 2023 17:08:16 +0200 Subject: [PATCH 25/37] Removed misleading comment --- joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp index 24ad158d38..f851ca8f62 100644 --- a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp +++ b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp @@ -163,7 +163,6 @@ bool RuckigJointLimiter::on_enforce( desired_joint_states.velocities = ruckig_output_->new_velocity; desired_joint_states.accelerations = ruckig_output_->new_acceleration; - // Feed output from the previous timestep back as input for (auto joint = 0ul; joint < number_of_joints_; ++joint) { RCUTILS_LOG_DEBUG_NAMED( From 1ff2bfe4658c59deeac383ee202b6de857631f43 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 3 Jul 2023 17:09:20 +0200 Subject: [PATCH 26/37] Fixed return value to allow working results --- joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp index f851ca8f62..c5ade3c8b6 100644 --- a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp +++ b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp @@ -180,7 +180,7 @@ bool RuckigJointLimiter::on_enforce( ruckig_output_->new_acceleration.at(joint)); } - return result == ruckig::Result::Finished; + return (result == ruckig::Result::Working || result == ruckig::Result::Finished); } } // namespace ruckig_joint_limiter From e3085af936bf3ffcbb40558101b8f6778b28b382 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 3 Jul 2023 17:24:52 +0200 Subject: [PATCH 27/37] Added tests (some fail for unknown reason) --- .../CMakeLists.txt | 13 +- joint_limits_enforcement_plugins/package.xml | 2 + .../test/ruckig_joint_limiter_param.yaml | 23 ++ .../test/test_ruckig_joint_limiter.cpp | 287 ++++++++++++++++-- .../test/test_ruckig_joint_limiter.hpp | 104 +++++++ 5 files changed, 406 insertions(+), 23 deletions(-) create mode 100644 joint_limits_enforcement_plugins/test/ruckig_joint_limiter_param.yaml create mode 100644 joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.hpp diff --git a/joint_limits_enforcement_plugins/CMakeLists.txt b/joint_limits_enforcement_plugins/CMakeLists.txt index 902b0f49dc..613b790199 100644 --- a/joint_limits_enforcement_plugins/CMakeLists.txt +++ b/joint_limits_enforcement_plugins/CMakeLists.txt @@ -39,12 +39,23 @@ pluginlib_export_plugin_description_file(joint_limits joint_limits_enforcement_p if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + find_package(generate_parameter_library REQUIRED) find_package(joint_limits REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) - ament_add_gmock(test_ruckig_joint_limiter test/test_ruckig_joint_limiter.cpp) + + install( + FILES test/ruckig_joint_limiter_param.yaml + DESTINATION share/joint_limits_enforcement_plugins/test + ) + + add_rostest_with_parameters_gmock(test_ruckig_joint_limiter test/test_ruckig_joint_limiter.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/ruckig_joint_limiter_param.yaml + ) target_include_directories(test_ruckig_joint_limiter PRIVATE include) + target_link_libraries(test_ruckig_joint_limiter ruckig_joint_limiter) ament_target_dependencies( test_ruckig_joint_limiter joint_limits diff --git a/joint_limits_enforcement_plugins/package.xml b/joint_limits_enforcement_plugins/package.xml index 26e6820598..2aed1330ba 100644 --- a/joint_limits_enforcement_plugins/package.xml +++ b/joint_limits_enforcement_plugins/package.xml @@ -25,6 +25,8 @@ ruckig ament_cmake_gmock + ament_cmake_gtest + generate_parameter_library ament_cmake diff --git a/joint_limits_enforcement_plugins/test/ruckig_joint_limiter_param.yaml b/joint_limits_enforcement_plugins/test/ruckig_joint_limiter_param.yaml new file mode 100644 index 0000000000..0a88f0a759 --- /dev/null +++ b/joint_limits_enforcement_plugins/test/ruckig_joint_limiter_param.yaml @@ -0,0 +1,23 @@ +ruckig_joint_limiter: + ros__parameters: + joint_limits: + foo_joint: + has_position_limits: true + min_position: -5.0 + max_position: 5.0 + has_velocity_limits: true + max_velocity: 2.0 + has_acceleration_limits: true + max_acceleration: 5.0 + has_deceleration_limits: true + max_deceleration: 7.5 + has_jerk_limits: true + max_jerk: 100.0 + has_effort_limits: true + max_effort: 20.0 + angle_wraparound: true # should be ignored, has position limits + has_soft_limits: true + k_position: 10.0 + k_velocity: 20.0 + soft_lower_limit: 0.1 + soft_upper_limit: 0.9 diff --git a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp index b1b19d0587..2a96cd7bdc 100644 --- a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp +++ b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,33 +12,276 @@ // See the License for the specific language governing permissions and // limitations under the License. -/// \author Denis Stogl +/// \author Dr. Denis Stogl, Guillaume Walck -#include +#include "test_ruckig_joint_limiter.hpp" -#include -#include +TEST_F(RuckigJointLimiterTest, when_loading_limiter_plugin_expect_loaded) +{ + // Test RuckigJointLimiterTest loading + ASSERT_NO_THROW( + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_))); + ASSERT_NE(joint_limiter_, nullptr); +} + +TEST_F(RuckigJointLimiterTest, when_wrong_input_expect_enforce_fail) +{ + SetupNode("ruckig_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + // no size check occurs (yet) so expect true + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); -#include "joint_limits/joint_limiter_interface.hpp" -#include "joint_limits/joint_limits.hpp" -#include "pluginlib/class_loader.hpp" -#include "rclcpp/executor.hpp" + rclcpp::Duration period(1, 0); // 1 second + desired_joint_states_.positions.clear(); + // trigger size check with one wrong size + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + } +} -TEST(TestLoadSimpleJointLimiter, load_limiter) +TEST_F(RuckigJointLimiterTest, when_within_limits_expect_no_limits_applied) { - rclcpp::init(0, nullptr); + SetupNode("ruckig_joint_limiter"); + Load(); - using JointLimiter = joint_limits::JointLimiterInterface; - pluginlib::ClassLoader joint_limiter_loader( - "joint_limits", "joint_limits::JointLimiterInterface"); + if (joint_limiter_) + { + Init(); + Configure(); - std::unique_ptr joint_limiter; - std::string joint_limiter_type = "ruckig_joint_limiter/RuckigJointLimiter"; + rclcpp::Duration period(0, 5000000); // 0.005 second AS HARDCODED in the lib + // pos, vel, acc, dec = 1.0, 2.0, 5.0, 7.5 - joint_limiter = - std::unique_ptr(joint_limiter_loader.createUnmanagedInstance(joint_limiter_type)); - ASSERT_NO_THROW( - joint_limiter = std::unique_ptr( - joint_limiter_loader.createUnmanagedInstance(joint_limiter_type))); - ASSERT_NE(joint_limiter, nullptr); + // within limits + desired_joint_states_.positions[0] = 1.0; + desired_joint_states_.velocities[0] = 0.01; + desired_joint_states_.accelerations[0] = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if max jerk applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + desired_joint_states_.positions[0], // pos unchanged + desired_joint_states_.velocities[0], // vel unchanged + desired_joint_states_.accelerations[0] // acc unchanged + ); + } +} + +TEST_F(RuckigJointLimiterTest, when_velocity_exceeded_expect_vel_and_acc_enforced) +{ + SetupNode("ruckig_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 5000000); // 0.005 second AS HARDCODED in the lib + + // desired velocity exceeds + desired_joint_states_.positions[0] = 1.0; + desired_joint_states_.velocities[0] = 1.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + desired_joint_states_.positions[0], // pos unchanged + // TODO(gwalck) why is the result divided by 2.0 ? + desired_joint_states_.accelerations[0] * period.seconds() / + 2.0, // vel limited to max_acc * dt / 2.0 + 100.0 * period.seconds() // acc set to max_jerk * dt + ); + + // check opposite velocity direction (sign copy) + desired_joint_states_.positions[0] = -1.0; + desired_joint_states_.velocities[0] = -1.0; + desired_joint_states_.accelerations[0] = -1.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + desired_joint_states_.positions[0], // pos unchanged + // TODO(gwalck) why is the result divided by 2.0 ? + desired_joint_states_.accelerations[0] * period.seconds() / + 2.0, // vel limited to max_acc * dt / 2.0 + -100.0 * period.seconds() // acc set to max_jerk * dt + ); + } +} + +/* no pos enforcing available in ruckig +TEST_F(RuckigJointLimiterTest, when_position_exceeded_expect_pos_enforced) +{ + SetupNode("ruckig_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + + // desired pos exceeds + desired_joint_states_.positions[0] = 20.0; + desired_joint_states_.velocities[0] = 0.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 5.0, // pos unchanged + desired_joint_states_.velocities[0], // vel unchanged + desired_joint_states_.accelerations[0] // acc unchanged + ); + } +} +*/ + +TEST_F(RuckigJointLimiterTest, when_position_close_to_pos_limit_expect_deceleration_enforced) +{ + SetupNode("ruckig_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + // using 0.05 because 1.0 sec invalidates the "small dt integration" + rclcpp::Duration period(0, 5000000); // 0.005 second + + // close to vel limit should trigger braking + current_joint_states_.positions[0] = 1.0; + current_joint_states_.velocities[0] = 1.9; + desired_joint_states_.positions[0] = 1.5; + desired_joint_states_.velocities[0] = 1.98; + desired_joint_states_.accelerations[0] = 5.0; + + for (auto i = 0u; i < 4; ++i) + { + auto previous_vel_request = desired_joint_states_.velocities[0]; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + ASSERT_LE( + desired_joint_states_.velocities[0], + previous_vel_request); // vel adapted to reach end-stop should be decreasing + // NOTE: after the first cycle, vel is reduced and does not trigger stopping position limit + // hence no max deceleration anymore... + ASSERT_LT( + desired_joint_states_.positions[0], + 5.0 + COMMON_THRESHOLD); // should decelerate at each cycle and stay below limits + ASSERT_LE(desired_joint_states_.accelerations[0], 0.0); // should decelerate + + Integrate(period.seconds()); + + ASSERT_LT(current_joint_states_.positions[0], 5.0); // below joint limit + } + } +} + +TEST_F(RuckigJointLimiterTest, when_acceleration_exceeded_expect_acc_enforced) +{ + SetupNode("ruckig_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 5000000); // 0.005 second + + // desired acceleration exceeds + current_joint_states_.positions[0] = 0.0; + current_joint_states_.velocities[0] = 0.0; + desired_joint_states_.velocities[0] = 0.5; // leads to acc > max acc + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + desired_joint_states_.positions[0], // pos unchanged + // TODO(gwalck) why is the result divided by 2.0 ? + desired_joint_states_.accelerations[0] * period.seconds() / + 2.0, // vel limited to max_acc * dt / 2.0 + 100.0 * period.seconds() // acc is max_jerk * dt + ); + } +} + +TEST_F(RuckigJointLimiterTest, when_deceleration_exceeded_expect_dec_enforced) +{ + SetupNode("ruckig_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 5000000); // 0.005 second + + // desired deceleration exceeds + current_joint_states_.positions[0] = 0.0; + current_joint_states_.velocities[0] = 1.0; + desired_joint_states_.velocities[0] = 0.5; // leads to acc > -max dec + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + desired_joint_states_.positions[0], // pos unchanged + // TODO(gwalck) why is the result divided by 2.0 ? + desired_joint_states_.accelerations[0] * period.seconds() / + 2.0, // vel limited to max_acc * dt / 2.0 + -100.0 * period.seconds() // acc is max_jerk * dt + ); + } +} +/* ruckig does not use max_dec anyway +TEST_F(RuckigJointLimiterTest, when_deceleration_exceeded_with_no_maxdec_expect_acc_enforced) +{ + SetupNode("ruckig_joint_limiter_nodeclimit"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + // desired deceleration exceeds + current_joint_states_.positions[0] = 0.0; + current_joint_states_.velocities[0] = 1.0; + desired_joint_states_.velocities[0] = 0.5; // leads to acc > -max acc + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + desired_joint_states_.positions[0], // pos unchanged + 0.75, // vel limited by vel-max acc * dt + -5.0 // acc limited to -max acc + ); + } +} +*/ + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; } diff --git a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.hpp b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.hpp new file mode 100644 index 0000000000..673e604c51 --- /dev/null +++ b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.hpp @@ -0,0 +1,104 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_RUCKIG_JOINT_LIMITER_HPP_ +#define TEST_RUCKIG_JOINT_LIMITER_HPP_ + +#include + +#include +#include +#include +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/node.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +#include "rclcpp/executor.hpp" + +const double COMMON_THRESHOLD = 0.0011; + +#define CHECK_STATE_SINGLE_JOINT(tested_traj_point, idx, expected_pos, expected_vel, expected_acc) \ + EXPECT_NEAR(tested_traj_point.positions[idx], expected_pos, COMMON_THRESHOLD); \ + EXPECT_NEAR(tested_traj_point.velocities[idx], expected_vel, COMMON_THRESHOLD); \ + EXPECT_NEAR(tested_traj_point.accelerations[idx], expected_acc, COMMON_THRESHOLD); + +using JointLimiter = joint_limits::JointLimiterInterface; + +class RuckigJointLimiterTest : public ::testing::Test +{ +public: + void SetUp() override + { + node_name_ = ::testing::UnitTest::GetInstance()->current_test_info()->name(); + } + + void SetupNode(const std::string node_name = "") + { + if (!node_name.empty()) node_name_ = node_name; + node_ = std::make_shared(node_name_); + } + + void Load() + { + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_)); + } + + void Init() + { + joint_names_ = {"foo_joint"}; + joint_limiter_->init(joint_names_, node_); + num_joints_ = joint_names_.size(); + last_commanded_state_.positions.resize(num_joints_, 0.0); + last_commanded_state_.velocities.resize(num_joints_, 0.0); + last_commanded_state_.accelerations.resize(num_joints_, 0.0); + desired_joint_states_ = last_commanded_state_; + current_joint_states_ = last_commanded_state_; + } + + void Configure() { joint_limiter_->configure(last_commanded_state_); } + + void Integrate(double dt) + { + current_joint_states_.positions[0] += desired_joint_states_.velocities[0] * dt + + 0.5 * desired_joint_states_.accelerations[0] * dt * dt; + current_joint_states_.velocities[0] += desired_joint_states_.accelerations[0] * dt; + } + + RuckigJointLimiterTest() + : joint_limiter_type_("ruckig_joint_limiter/RuckigJointLimiter"), + joint_limiter_loader_( + "joint_limits", "joint_limits::JointLimiterInterface") + { + } + void TearDown() override { node_.reset(); } + +protected: + std::string node_name_; + rclcpp::Node::SharedPtr node_; + std::vector joint_names_; + size_t num_joints_; + std::unique_ptr joint_limiter_; + std::string joint_limiter_type_; + pluginlib::ClassLoader joint_limiter_loader_; + + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; + trajectory_msgs::msg::JointTrajectoryPoint desired_joint_states_; + trajectory_msgs::msg::JointTrajectoryPoint current_joint_states_; +}; + +#endif // TEST_RUCKIG_JOINT_LIMITER_HPP_ From 817a379718f161f720636859e14f7e4c905bbb40 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 3 Jul 2023 17:25:43 +0200 Subject: [PATCH 28/37] Added limitation for max vel and max acc --- .../src/ruckig_joint_limiter.cpp | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp index c5ade3c8b6..a5495b1890 100644 --- a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp +++ b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp @@ -156,6 +156,38 @@ bool RuckigJointLimiter::on_enforce( ruckig_input_->target_acceleration = std::vector(number_of_joints_, 0.0); } + // Ruckig fails if provided values are out of limit, + // therefore we first limit things by scaling to the maximum ruckig velocity + double max_vel_ratio = 1.0; + for (auto joint = 0ul; joint < number_of_joints_; ++joint) + { + if (std::fabs(ruckig_input_->target_velocity.at(joint)) > ruckig_input_->max_velocity.at(joint)) + { + const double ratio = + std::fabs(ruckig_input_->target_velocity.at(joint) / ruckig_input_->max_velocity.at(joint)); + if (ratio > max_vel_ratio) + { + max_vel_ratio = ratio; + } + } + } + for (auto joint = 0ul; joint < number_of_joints_; ++joint) + { + // Set the target velocities to follow the ruckig joint limits + if (max_vel_ratio != 1.0) + { + ruckig_input_->target_velocity.at(joint) = + ruckig_input_->target_velocity.at(joint) / max_vel_ratio; + RCUTILS_LOG_WARN_ONCE_NAMED( + "ruckig_joint_limiter", "Limiting target velocity by a factor : %lf.", max_vel_ratio); + } + + // Set the target accelerations to follow the ruckig joint limits + ruckig_input_->target_acceleration.at(joint) = std::clamp( + ruckig_input_->target_acceleration.at(joint), + -1.0 * ruckig_input_->max_acceleration.at(joint), ruckig_input_->max_acceleration.at(joint)); + } + // apply ruckig ruckig::Result result = ruckig_->update(*ruckig_input_, *ruckig_output_); RCUTILS_LOG_DEBUG_NAMED("ruckig_joint_limiter", "Rucking result %d", result); From 3d273ef90ed931b472bb5da6c5587fcf3c4e5e9c Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 3 Jul 2023 21:29:01 +0200 Subject: [PATCH 29/37] Added a warning for deceleration limits --- .../src/ruckig_joint_limiter.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp index a5495b1890..47680f8001 100644 --- a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp +++ b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp @@ -68,6 +68,12 @@ bool RuckigJointLimiter::on_init(/*const rclcpp::Dura { ruckig_input_->max_acceleration.at(joint) = DEFAULT_MAX_ACCELERATION; } + if (joint_limits_[joint].has_deceleration_limits) + { + RCUTILS_LOG_WARN_NAMED( + "ruckig_joint_limiter", + "Deceleration limits not supported in community version of Ruckig."); + } if (joint_limits_[joint].has_velocity_limits) { ruckig_input_->max_velocity.at(joint) = joint_limits_[joint].max_velocity; From e64256907c709394a0b698e8672cf2930481811f Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 3 Jul 2023 21:29:56 +0200 Subject: [PATCH 30/37] Activated pos test which expect no limits --- .../test/test_ruckig_joint_limiter.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp index 2a96cd7bdc..8262a4c905 100644 --- a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp +++ b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp @@ -117,8 +117,7 @@ TEST_F(RuckigJointLimiterTest, when_velocity_exceeded_expect_vel_and_acc_enforce } } -/* no pos enforcing available in ruckig -TEST_F(RuckigJointLimiterTest, when_position_exceeded_expect_pos_enforced) +TEST_F(RuckigJointLimiterTest, when_position_exceeded_expect_nochange) { SetupNode("ruckig_joint_limiter"); Load(); @@ -138,13 +137,12 @@ TEST_F(RuckigJointLimiterTest, when_position_exceeded_expect_pos_enforced) // check if pos limits applied CHECK_STATE_SINGLE_JOINT( desired_joint_states_, 0, - 5.0, // pos unchanged + desired_joint_states_.positions[0], // pos unchanged desired_joint_states_.velocities[0], // vel unchanged desired_joint_states_.accelerations[0] // acc unchanged ); } } -*/ TEST_F(RuckigJointLimiterTest, when_position_close_to_pos_limit_expect_deceleration_enforced) { From 1239e14499e54be55e86aaa40f7fcba44830700f Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 3 Jul 2023 21:30:24 +0200 Subject: [PATCH 31/37] Removed deceleration test (not supported) --- .../test/test_ruckig_joint_limiter.cpp | 29 ------------------- 1 file changed, 29 deletions(-) diff --git a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp index 8262a4c905..c98f2e6a81 100644 --- a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp +++ b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp @@ -245,35 +245,6 @@ TEST_F(RuckigJointLimiterTest, when_deceleration_exceeded_expect_dec_enforced) ); } } -/* ruckig does not use max_dec anyway -TEST_F(RuckigJointLimiterTest, when_deceleration_exceeded_with_no_maxdec_expect_acc_enforced) -{ - SetupNode("ruckig_joint_limiter_nodeclimit"); - Load(); - - if (joint_limiter_) - { - Init(); - Configure(); - - rclcpp::Duration period(0, 50000000); - - // desired deceleration exceeds - current_joint_states_.positions[0] = 0.0; - current_joint_states_.velocities[0] = 1.0; - desired_joint_states_.velocities[0] = 0.5; // leads to acc > -max acc - ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - - // check if vel and acc limits applied - CHECK_STATE_SINGLE_JOINT( - desired_joint_states_, 0, - desired_joint_states_.positions[0], // pos unchanged - 0.75, // vel limited by vel-max acc * dt - -5.0 // acc limited to -max acc - ); - } -} -*/ int main(int argc, char ** argv) { From 156d59074504d075a786af14d54d50c10b4f2671 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Tue, 4 Jul 2023 10:48:48 +0200 Subject: [PATCH 32/37] Fixed negative velocity test Split test for positive and negative vel to have same initial conditions --- .../test/test_ruckig_joint_limiter.cpp | 21 +++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp index c98f2e6a81..243237b29d 100644 --- a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp +++ b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp @@ -72,7 +72,7 @@ TEST_F(RuckigJointLimiterTest, when_within_limits_expect_no_limits_applied) } } -TEST_F(RuckigJointLimiterTest, when_velocity_exceeded_expect_vel_and_acc_enforced) +TEST_F(RuckigJointLimiterTest, when_pos_vel_leads_to_acc_exceeded_expect_vel_and_acc_enforced) { SetupNode("ruckig_joint_limiter"); Load(); @@ -84,7 +84,7 @@ TEST_F(RuckigJointLimiterTest, when_velocity_exceeded_expect_vel_and_acc_enforce rclcpp::Duration period(0, 5000000); // 0.005 second AS HARDCODED in the lib - // desired velocity exceeds + // desired vel makes acceleration exceed desired_joint_states_.positions[0] = 1.0; desired_joint_states_.velocities[0] = 1.0; ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); @@ -98,11 +98,24 @@ TEST_F(RuckigJointLimiterTest, when_velocity_exceeded_expect_vel_and_acc_enforce 2.0, // vel limited to max_acc * dt / 2.0 100.0 * period.seconds() // acc set to max_jerk * dt ); + } +} + +TEST_F(RuckigJointLimiterTest, when_neg_vel_leads_to_acc_exceeded_expect_vel_and_acc_enforced) +{ + SetupNode("ruckig_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 5000000); // 0.005 second AS HARDCODED in the lib - // check opposite velocity direction (sign copy) + // desired vel makes acceleration exceed desired_joint_states_.positions[0] = -1.0; desired_joint_states_.velocities[0] = -1.0; - desired_joint_states_.accelerations[0] = -1.0; ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); // check if vel and acc limits applied From bd5d01c6eed83665e68cc6e33356098e66396f61 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Tue, 4 Jul 2023 10:49:41 +0200 Subject: [PATCH 33/37] Removed brake test which cannot be triggered --- .../test/test_ruckig_joint_limiter.cpp | 42 ------------------- 1 file changed, 42 deletions(-) diff --git a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp index 243237b29d..b73c9e4c8c 100644 --- a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp +++ b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp @@ -157,48 +157,6 @@ TEST_F(RuckigJointLimiterTest, when_position_exceeded_expect_nochange) } } -TEST_F(RuckigJointLimiterTest, when_position_close_to_pos_limit_expect_deceleration_enforced) -{ - SetupNode("ruckig_joint_limiter"); - Load(); - - if (joint_limiter_) - { - Init(); - Configure(); - - // using 0.05 because 1.0 sec invalidates the "small dt integration" - rclcpp::Duration period(0, 5000000); // 0.005 second - - // close to vel limit should trigger braking - current_joint_states_.positions[0] = 1.0; - current_joint_states_.velocities[0] = 1.9; - desired_joint_states_.positions[0] = 1.5; - desired_joint_states_.velocities[0] = 1.98; - desired_joint_states_.accelerations[0] = 5.0; - - for (auto i = 0u; i < 4; ++i) - { - auto previous_vel_request = desired_joint_states_.velocities[0]; - ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - - ASSERT_LE( - desired_joint_states_.velocities[0], - previous_vel_request); // vel adapted to reach end-stop should be decreasing - // NOTE: after the first cycle, vel is reduced and does not trigger stopping position limit - // hence no max deceleration anymore... - ASSERT_LT( - desired_joint_states_.positions[0], - 5.0 + COMMON_THRESHOLD); // should decelerate at each cycle and stay below limits - ASSERT_LE(desired_joint_states_.accelerations[0], 0.0); // should decelerate - - Integrate(period.seconds()); - - ASSERT_LT(current_joint_states_.positions[0], 5.0); // below joint limit - } - } -} - TEST_F(RuckigJointLimiterTest, when_acceleration_exceeded_expect_acc_enforced) { SetupNode("ruckig_joint_limiter"); From 6f47cf963f56e5aa6c5e1fed079261fd6b866323 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Tue, 4 Jul 2023 10:52:45 +0200 Subject: [PATCH 34/37] Fixed acceleration test Requires velocity to be set --- .../test/test_ruckig_joint_limiter.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp index b73c9e4c8c..9029b0d731 100644 --- a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp +++ b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp @@ -170,9 +170,8 @@ TEST_F(RuckigJointLimiterTest, when_acceleration_exceeded_expect_acc_enforced) rclcpp::Duration period(0, 5000000); // 0.005 second // desired acceleration exceeds - current_joint_states_.positions[0] = 0.0; - current_joint_states_.velocities[0] = 0.0; - desired_joint_states_.velocities[0] = 0.5; // leads to acc > max acc + desired_joint_states_.velocities[0] = 0.002; // if no velocity set, acc is computed negative ! + desired_joint_states_.accelerations[0] = 0.6; // leads to jerk > max_jerk ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); // check if vel and acc limits applied @@ -200,9 +199,8 @@ TEST_F(RuckigJointLimiterTest, when_deceleration_exceeded_expect_dec_enforced) rclcpp::Duration period(0, 5000000); // 0.005 second // desired deceleration exceeds - current_joint_states_.positions[0] = 0.0; - current_joint_states_.velocities[0] = 1.0; - desired_joint_states_.velocities[0] = 0.5; // leads to acc > -max dec + desired_joint_states_.velocities[0] = -0.002; // if no velocity set, acc is computed positve ! + desired_joint_states_.accelerations[0] = -0.6; // leads to jerk > max_jerk ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); // check if vel and acc limits applied From 37143439bba7e45a7de62887b4c45155ed87f9a4 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Tue, 4 Jul 2023 11:48:58 +0200 Subject: [PATCH 35/37] Avoid failures due to double precision errors --- joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp index 47680f8001..8cd463b9e6 100644 --- a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp +++ b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp @@ -191,7 +191,8 @@ bool RuckigJointLimiter::on_enforce( // Set the target accelerations to follow the ruckig joint limits ruckig_input_->target_acceleration.at(joint) = std::clamp( ruckig_input_->target_acceleration.at(joint), - -1.0 * ruckig_input_->max_acceleration.at(joint), ruckig_input_->max_acceleration.at(joint)); + -0.999 * ruckig_input_->max_acceleration.at(joint), + 0.999 * ruckig_input_->max_acceleration.at(joint)); } // apply ruckig ruckig::Result result = ruckig_->update(*ruckig_input_, *ruckig_output_); From a04788bcb06ed12e2524476104416ee782614bf8 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Tue, 4 Jul 2023 11:50:08 +0200 Subject: [PATCH 36/37] Fixed enforce in case ruckig fails desired_joint_states should not be changed if ruckig failed --- .../src/ruckig_joint_limiter.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp index 8cd463b9e6..5b725c74bf 100644 --- a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp +++ b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp @@ -196,7 +196,13 @@ bool RuckigJointLimiter::on_enforce( } // apply ruckig ruckig::Result result = ruckig_->update(*ruckig_input_, *ruckig_output_); - RCUTILS_LOG_DEBUG_NAMED("ruckig_joint_limiter", "Rucking result %d", result); + if (result == ruckig::Result::Working || result == ruckig::Result::Finished) + RCUTILS_LOG_DEBUG_NAMED("ruckig_joint_limiter", "Rucking result %d", result); + else + { + RCUTILS_LOG_WARN_ONCE_NAMED("ruckig_joint_limiter", "Rucking result %d", result); + return false; + } desired_joint_states.positions = ruckig_output_->new_position; desired_joint_states.velocities = ruckig_output_->new_velocity; @@ -219,7 +225,7 @@ bool RuckigJointLimiter::on_enforce( ruckig_output_->new_acceleration.at(joint)); } - return (result == ruckig::Result::Working || result == ruckig::Result::Finished); + return true; } } // namespace ruckig_joint_limiter From 2a8525f3616adb39e248d880239886200d42c293 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Tue, 4 Jul 2023 11:54:41 +0200 Subject: [PATCH 37/37] Added and fixed tests when current_state differs from configured state enforce current_state is required to get correct computations --- .../test/test_ruckig_joint_limiter.cpp | 169 +++++++++++++++++- 1 file changed, 163 insertions(+), 6 deletions(-) diff --git a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp index 9029b0d731..38e7a3fe6c 100644 --- a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp +++ b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp @@ -72,7 +72,65 @@ TEST_F(RuckigJointLimiterTest, when_within_limits_expect_no_limits_applied) } } -TEST_F(RuckigJointLimiterTest, when_pos_vel_leads_to_acc_exceeded_expect_vel_and_acc_enforced) +TEST_F(RuckigJointLimiterTest, when_positive_velocity_exceeded_expect_acc_vel_enforced) +{ + SetupNode("ruckig_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 5000000); // 0.005 second AS HARDCODED in the lib + + // desired pos exceeds + desired_joint_states_.positions[0] = 1.0; + desired_joint_states_.velocities[0] = 3.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + desired_joint_states_.positions[0], // pos unchanged + // TODO(gwalck) why is the result divided by 2.0 ? + desired_joint_states_.accelerations[0] * period.seconds() / + 2.0, // vel limited to max_acc * dt / 2.0 + 100.0 * period.seconds() // acc set to max_jerk * dt + ); + } +} + +TEST_F(RuckigJointLimiterTest, when_negative_velocity_exceeded_expect_acc_vel_enforced) +{ + SetupNode("ruckig_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 5000000); // 0.005 second AS HARDCODED in the lib + + // desired pos exceeds + desired_joint_states_.positions[0] = -1.0; + desired_joint_states_.velocities[0] = -3.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + desired_joint_states_.positions[0], // pos unchanged + // TODO(gwalck) why is the result divided by 2.0 ? + desired_joint_states_.accelerations[0] * period.seconds() / + 2.0, // vel limited to max_acc * dt / 2.0 + -100.0 * period.seconds() // acc set to max_jerk * dt + ); + } +} + +TEST_F(RuckigJointLimiterTest, when_incr_vel_leads_to_acc_exceeded_expect_vel_and_acc_enforced) { SetupNode("ruckig_joint_limiter"); Load(); @@ -101,7 +159,7 @@ TEST_F(RuckigJointLimiterTest, when_pos_vel_leads_to_acc_exceeded_expect_vel_and } } -TEST_F(RuckigJointLimiterTest, when_neg_vel_leads_to_acc_exceeded_expect_vel_and_acc_enforced) +TEST_F(RuckigJointLimiterTest, when_decr_vel_leads_to_acc_exceeded_expect_vel_and_acc_enforced) { SetupNode("ruckig_joint_limiter"); Load(); @@ -157,7 +215,63 @@ TEST_F(RuckigJointLimiterTest, when_position_exceeded_expect_nochange) } } -TEST_F(RuckigJointLimiterTest, when_acceleration_exceeded_expect_acc_enforced) +TEST_F(RuckigJointLimiterTest, when_acc_within_jerk_limit_exceeded_expect_vel_and_acc_enforced) +{ + SetupNode("ruckig_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 5000000); // 0.005 second AS HARDCODED in the lib + + // desired acc exceed but within jerk limits + current_joint_states_.velocities[0] = 1.0; + current_joint_states_.accelerations[0] = 4.6; + // required to initialize the limiter correctly + joint_limiter_->configure(current_joint_states_); + desired_joint_states_.positions[0] = 1.0; + desired_joint_states_.velocities[0] = 1.023; + desired_joint_states_.accelerations[0] = 5.1; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // acc should be close to 5.0 + ASSERT_GT(desired_joint_states_.accelerations[0], 4.6); + ASSERT_LE(desired_joint_states_.accelerations[0], 5.0); + } +} + +TEST_F(RuckigJointLimiterTest, when_dec_within_jerk_limit_exceeded_expect_vel_and_acc_enforced) +{ + SetupNode("ruckig_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 5000000); // 0.005 second AS HARDCODED in the lib + + // desired acc exceed but within jerk limits + current_joint_states_.velocities[0] = -1.0; + current_joint_states_.accelerations[0] = -4.6; + // required to initialize the limiter correctly + joint_limiter_->configure(current_joint_states_); + desired_joint_states_.positions[0] = -1.0; + desired_joint_states_.velocities[0] = -1.023; + desired_joint_states_.accelerations[0] = -5.1; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // acc should be close to -5.0 + ASSERT_LT(desired_joint_states_.accelerations[0], -4.6); + ASSERT_GE(desired_joint_states_.accelerations[0], -5.0); + } +} + +TEST_F(RuckigJointLimiterTest, when_acc_leads_to_jerk_exceeded_expect_acc_enforced) { SetupNode("ruckig_joint_limiter"); Load(); @@ -170,7 +284,7 @@ TEST_F(RuckigJointLimiterTest, when_acceleration_exceeded_expect_acc_enforced) rclcpp::Duration period(0, 5000000); // 0.005 second // desired acceleration exceeds - desired_joint_states_.velocities[0] = 0.002; // if no velocity set, acc is computed negative ! + desired_joint_states_.velocities[0] = 0.002; // if no velocity set, acc is computed negative ! desired_joint_states_.accelerations[0] = 0.6; // leads to jerk > max_jerk ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); @@ -186,7 +300,7 @@ TEST_F(RuckigJointLimiterTest, when_acceleration_exceeded_expect_acc_enforced) } } -TEST_F(RuckigJointLimiterTest, when_deceleration_exceeded_expect_dec_enforced) +TEST_F(RuckigJointLimiterTest, when_dec_leads_to_jerk_exceeded_expect_dec_enforced) { SetupNode("ruckig_joint_limiter"); Load(); @@ -199,7 +313,7 @@ TEST_F(RuckigJointLimiterTest, when_deceleration_exceeded_expect_dec_enforced) rclcpp::Duration period(0, 5000000); // 0.005 second // desired deceleration exceeds - desired_joint_states_.velocities[0] = -0.002; // if no velocity set, acc is computed positve ! + desired_joint_states_.velocities[0] = -0.002; // if no velocity set, acc is computed positive ! desired_joint_states_.accelerations[0] = -0.6; // leads to jerk > max_jerk ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); @@ -215,6 +329,49 @@ TEST_F(RuckigJointLimiterTest, when_deceleration_exceeded_expect_dec_enforced) } } +TEST_F(RuckigJointLimiterTest, when_vel_close_to_vel_limit_expect_deceleration_enforced) +{ + SetupNode("ruckig_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + // using 0.05 because 1.0 sec invalidates the "small dt integration" + rclcpp::Duration period(0, 5000000); // 0.005 second + + // close to vel limit should trigger braking + current_joint_states_.positions[0] = 1.0; + current_joint_states_.velocities[0] = 1.9; + // required to initialize the limiter correctly + joint_limiter_->configure(current_joint_states_); + desired_joint_states_.positions[0] = 1.5; + desired_joint_states_.velocities[0] = 1.98; + desired_joint_states_.accelerations[0] = 5.0; + + for (auto i = 0u; i < 4; ++i) + { + auto previous_vel_request = desired_joint_states_.velocities[0]; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + ASSERT_LE( + desired_joint_states_.velocities[0], + previous_vel_request); // vel adapted to stay below or equal to previous vel + + ASSERT_LE(desired_joint_states_.velocities[0], + 2.0); // vel adapted to stay below max vel + + ASSERT_LE(desired_joint_states_.accelerations[0], 0.0); // should decelerate + + Integrate(period.seconds()); + + ASSERT_LT(current_joint_states_.positions[0], 5.0); // below joint limit + } + } +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv);