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/83] 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/83] 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/83] 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/83] 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/83] 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/83] 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/83] 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/83] 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/83] 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/83] 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/83] 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/83] 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/83] 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/83] 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/83] 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/83] 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/83] 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/83] 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/83] 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/83] 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 21944b3f8b71ed19d4b132047440d62194bbf4b1 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Fri, 7 Jul 2023 17:35:42 +0200 Subject: [PATCH 21/83] Added temporary pseudo-code --- joint_limits/src/simple_joint_limiter.cpp | 150 ++++++++++++++++++++++ 1 file changed, 150 insertions(+) diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 8f3de96f38..97acc195af 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -30,6 +30,156 @@ bool SimpleJointLimiter::on_enforce( trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) { + + + /* + if has_pos_cmd only + *if has_pos_limit + * clamp pos_cmd + + *compute expected_vel with pos_cmd and pos_state + *if has_vel_limit + * if expected_vel over limit + * clamp expected_vel + * integrate pos_cmd to be compatible with limited expected_vel + + *if has_acc_limit + * if has_vel_state + * compute expected_acc with expected expected_vel and vel_state + * or + * we cannot compute the acc, we cannot derive acc from zero velocity this would be wrong + * break +* + * if expected_acc over limit + * clamp expected_acc + * integrate expected_vel to be compatible with limited expected_acc as well + * integrate pos_cmd to be compatible with newly limited expected_vel and limited expected_acc as well + + *if has_pos_limit + * using only expected_vel and expected_acc check if expected_pos might be reached at max braking // check for future steps + * recompute pos_cmd in such case + + WE HAVE pos_cmd + + if has_vel_cmd only + *if has_vel_limit + * clamp vel_cmd + + *if has_acc_limit + * if has_vel_state + * compute expected_acc with limited vel_cmd and vel_state + * or + * we cannot compute the acc, we cannot derive acc from zero velocity this would be wrong + * break + * if expected_acc over limit + * clamp expected_acc + * integrate vel_cmd to be compatible with limited expected_acc as well + + *if has_pos_limit + * compute expected_pos by integrating vel_cmd // check for next step + * if expected_pos over limit + * consider braking and compute ideal vel_cmd that would permit to slow down in time at full deceleration + * in any case limit pos to max and recompute vel_cmd that would lead to pos_max (not ideal as velocity would not be zero) + + * using only vel_cmd and expected_acc if available check if expected_pos might be reached at max braking // check for future steps + * recompute vel_cmd in such case + + WE HAVE vel_cmd + + if has_pos_cmd AND has_vel_cmd + *if has_pos_limit + * clamp pos_cmd + + *compute expected_vel with pos_cmd with pos_state to be able to limit pos_cmd increment later + + *if has_vel_limit + *if expected_vel over limit + * clamp expected_vel and use that one for pos_cmd integration + * integrate pos_cmd to be compatible with limited expected_vel + * if vel_cmd over limit + * clamp vel_cmd independently from expected_vel + + *if has_acc_limit + *if has_vel_state + *compute expected_acc with expected_vel and vel_state + *compute expected_acc2 with limited vel_cmd and vel_state + * or + * we cannot compute the acc, we cannot derive acc from zero velocity this would be wrong + * break + + * if expected_acc over limit + * clamp expected_acc + * integrate expected_vel to be compatible with limited expected_acc as well + * integrate pos_cmd to be compatible with newly limited expected_vel and limited expected_acc as well + + * if expected_acc2 over limit + * clamp expected_acc2 + * integrate vel_cmd to be compatible with limited expected_acc2 as well + + *if has_pos_limit + * using only vel_cmd and expected_acc2, check if expected_pos might be reached at max braking + * recompute pos_cmd and vel_cmd in such case + + WE HAVE pos_cmd + WE HAVE vel_cmd independently limited so that pos_cmd != pos_state + vel_cmd * dt (unless braking) + + + +Now a factorized version + + if has_pos_limit + if has_pos_cmd + clamp pos_cmd + compute expected_vel with pos_cmd and pos_state + else + //nothing to do yet + + if has_vel_limit + clamp vel_cmd + + if has_pos_cmd + if expected_vel over limit + clamp expected_vel + integrate pos_cmd to be compatible with limited expected_vel + + if has_acc_limit + if has_vel_state + if has_vel_cmd + compute expected_acc2 with limited vel_cmd and vel_state + if has_pos_cmd + compute expected_acc with expected_vel and vel_state + else + or + we cannot compute the acc, we cannot derive acc from zero velocity this would be wrong + break + + if has_pos_cmd + if expected_acc over limit + clamp expected_acc + integrate expected_vel to be compatible with limited expected_acc as well + integrate pos_cmd to be compatible with newly limited expected_vel and limited expected_acc as well + + if has_vel_cmd + if expected_acc2 over limit + clamp expected_acc2 + integrate vel_cmd to be compatible with limited expected_acc2 as well + + if has_pos_limit + if has_vel_cmd + compute expected_pos by integrating vel_cmd // check for next step + if expected_pos over limit + consider braking and compute ideal vel_cmd that would permit to slow down in time at full deceleration + in any case limit pos to max and recompute vel_cmd that would lead to pos_max (not ideal as velocity would not be zero) + + des_vel = vel_cmd or expected_vel + check if expected_pos might be reached at max braking // check for future steps + recompute vel_cmd or pos_cmd or both + + +*/ + + + const auto dt_seconds = dt.seconds(); // negative or null is not allowed if (dt_seconds <= 0.0) return false; From 7530ba81281e3539f04ce5206e3cd3e885fd8304 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Fri, 7 Jul 2023 22:47:18 +0200 Subject: [PATCH 22/83] Updated pseudo-code --- joint_limits/src/simple_joint_limiter.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 97acc195af..99da1ba1df 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -127,20 +127,23 @@ bool SimpleJointLimiter::on_enforce( Now a factorized version + if has_pos_cmd + compute expected_vel with pos_cmd and pos_state + if has_pos_limit if has_pos_cmd clamp pos_cmd - compute expected_vel with pos_cmd and pos_state + else //nothing to do yet if has_vel_limit - clamp vel_cmd - - if has_pos_cmd - if expected_vel over limit - clamp expected_vel - integrate pos_cmd to be compatible with limited expected_vel + if has_pos_cmd // priority of pos_cmd + vel_cmd = expected_vel + + if vel_cmd over limit + clamp vel_cmd + if clamped pos_cmd = integrate vel_cmd if has_acc_limit if has_vel_state From 27e3304b94266c9a13f9a2ee7b5eb3fe08f24c9a Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Fri, 7 Jul 2023 22:47:57 +0200 Subject: [PATCH 23/83] Refactored to match pseudo-code --- joint_limits/src/simple_joint_limiter.cpp | 393 ++++++++++++---------- 1 file changed, 219 insertions(+), 174 deletions(-) diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 99da1ba1df..11777fee46 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -31,100 +31,7 @@ bool SimpleJointLimiter::on_enforce( trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) { - - /* - if has_pos_cmd only - *if has_pos_limit - * clamp pos_cmd - - *compute expected_vel with pos_cmd and pos_state - *if has_vel_limit - * if expected_vel over limit - * clamp expected_vel - * integrate pos_cmd to be compatible with limited expected_vel - - *if has_acc_limit - * if has_vel_state - * compute expected_acc with expected expected_vel and vel_state - * or - * we cannot compute the acc, we cannot derive acc from zero velocity this would be wrong - * break -* - * if expected_acc over limit - * clamp expected_acc - * integrate expected_vel to be compatible with limited expected_acc as well - * integrate pos_cmd to be compatible with newly limited expected_vel and limited expected_acc as well - - *if has_pos_limit - * using only expected_vel and expected_acc check if expected_pos might be reached at max braking // check for future steps - * recompute pos_cmd in such case - - WE HAVE pos_cmd - - if has_vel_cmd only - *if has_vel_limit - * clamp vel_cmd - - *if has_acc_limit - * if has_vel_state - * compute expected_acc with limited vel_cmd and vel_state - * or - * we cannot compute the acc, we cannot derive acc from zero velocity this would be wrong - * break - * if expected_acc over limit - * clamp expected_acc - * integrate vel_cmd to be compatible with limited expected_acc as well - - *if has_pos_limit - * compute expected_pos by integrating vel_cmd // check for next step - * if expected_pos over limit - * consider braking and compute ideal vel_cmd that would permit to slow down in time at full deceleration - * in any case limit pos to max and recompute vel_cmd that would lead to pos_max (not ideal as velocity would not be zero) - - * using only vel_cmd and expected_acc if available check if expected_pos might be reached at max braking // check for future steps - * recompute vel_cmd in such case - - WE HAVE vel_cmd - - if has_pos_cmd AND has_vel_cmd - *if has_pos_limit - * clamp pos_cmd - - *compute expected_vel with pos_cmd with pos_state to be able to limit pos_cmd increment later - - *if has_vel_limit - *if expected_vel over limit - * clamp expected_vel and use that one for pos_cmd integration - * integrate pos_cmd to be compatible with limited expected_vel - * if vel_cmd over limit - * clamp vel_cmd independently from expected_vel - - *if has_acc_limit - *if has_vel_state - *compute expected_acc with expected_vel and vel_state - *compute expected_acc2 with limited vel_cmd and vel_state - * or - * we cannot compute the acc, we cannot derive acc from zero velocity this would be wrong - * break - - * if expected_acc over limit - * clamp expected_acc - * integrate expected_vel to be compatible with limited expected_acc as well - * integrate pos_cmd to be compatible with newly limited expected_vel and limited expected_acc as well - - * if expected_acc2 over limit - * clamp expected_acc2 - * integrate vel_cmd to be compatible with limited expected_acc2 as well - - *if has_pos_limit - * using only vel_cmd and expected_acc2, check if expected_pos might be reached at max braking - * recompute pos_cmd and vel_cmd in such case - - WE HAVE pos_cmd - WE HAVE vel_cmd independently limited so that pos_cmd != pos_state + vel_cmd * dt (unless braking) - - - +/* Now a factorized version if has_pos_cmd @@ -187,103 +94,220 @@ Now a factorized version // negative or null is not allowed if (dt_seconds <= 0.0) return false; - if (current_joint_states.velocities.empty()) + // TODO(gwalck) compute if the max are not implicitly violated with the given dt + // eg for max vel 2.0 and max acc 5.0, with dt >0.4 + // velocity max is implicitly already violated due to max_acc * dt > 2.0 + + // check for required inputs combination + bool has_pos_cmd = (desired_joint_states.positions.size() == number_of_joints_); + bool has_vel_cmd = (desired_joint_states.velocities.size() == number_of_joints_); + bool has_acc_cmd = (desired_joint_states.accelerations.size() == number_of_joints_); + bool has_pos_state = (current_joint_states.positions.size() == number_of_joints_); + bool has_vel_state = (current_joint_states.velocities.size() == number_of_joints_); + + // pos state and vel or pos cmd is required, vel state is optional + if (!(has_pos_state && (has_pos_cmd || has_vel_cmd))) { - // First update() after activating does not have velocity available, assume 0 - current_joint_states.velocities.resize(number_of_joints_, 0.0); + return false; } - // check for required inputs - if ( - (desired_joint_states.positions.size() < number_of_joints_) || - (desired_joint_states.velocities.size() < number_of_joints_) || - (current_joint_states.positions.size() < number_of_joints_)) + if (!has_vel_state) { - return false; + // First update() after activating does not have velocity available, assume 0 + current_joint_states.velocities.resize(number_of_joints_, 0.0); } + // 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_acc(number_of_joints_); std::vector desired_vel(number_of_joints_); std::vector desired_pos(number_of_joints_); + std::vector expected_vel(number_of_joints_); + std::vector expected_pos(number_of_joints_); + + // limits triggered std::vector pos_limit_trig_jnts(number_of_joints_, false); + // vel_limit_trig_jnts(number_of_joints_, false); + //std::vector limited_jnts_vel_idx; std::vector limited_jnts_vel, limited_jnts_acc, limited_jnts_dec; - bool position_limit_triggered = false; + bool braking_near_position_limit_triggered = false; for (size_t index = 0; index < number_of_joints_; ++index) { - desired_pos[index] = desired_joint_states.positions[index]; - + /* + if (index == 0) + RCLCPP_INFO(node_logging_itf_->get_logger(), "vel input %f", desired_vel[0]); + }*/ + + if (has_pos_cmd) + { + 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]) + if (has_pos_cmd) { - pos_limit_trig_jnts[index] = true; - desired_pos[index] = pos; + if (index == 0) + RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_pos input %f", desired_pos[0]); + // clamp input pos_cmd + auto pos = std::max( + std::min(joint_limits_[index].max_position, desired_pos[index]), + joint_limits_[index].min_position); + if (pos != desired_pos[index]) + { + desired_pos[index] = pos; + pos_limit_trig_jnts[index] = true; + if (index == 0) + RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_pos clamped %f", desired_pos[0]); + } + } } - desired_vel[index] = desired_joint_states.velocities[index]; - + // prepare velocity + if (has_pos_cmd) + { + // priority to pos_cmd derivative over cmd_vel because we always have a pos_state so recomputing vel_cmd is fine + // compute expected_vel with already clamped pos_cmd and pos_state + // TODO(gwalck) handle the case of continuous joints with angle_wraparound to compute vel correctly + desired_vel[index] = + (desired_pos[index] - current_joint_states.positions[index]) / dt_seconds; + if (index == 0) + RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_vel input from pos derivative %f", desired_vel[0]); + } + else + { + if (has_vel_cmd) + { + desired_vel[index] = desired_joint_states.velocities[index]; + if (index == 0) + RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_vel input %f", desired_vel[0]); + } + + } + // limit velocity if (joint_limits_[index].has_velocity_limits) { + //clamp input vel_cmd if (std::abs(desired_vel[index]) > joint_limits_[index].max_velocity) { desired_vel[index] = std::copysign(joint_limits_[index].max_velocity, desired_vel[index]); limited_jnts_vel.emplace_back(joint_names_[index]); + + // recompute pos_cmd if needed + if (has_pos_cmd) + { + desired_pos[index] = current_joint_states.positions[index] + + desired_vel[index] * dt_seconds; + pos_limit_trig_jnts[index] = true; + if (index == 0) + RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_pos limited by expected vel limited %f", desired_pos[0]); + } + if (index == 0) + RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_vel clamped %f", desired_vel[0]); } } - desired_accel[index] = - (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; - - auto apply_acc_or_dec_limit = - [&](const double max_acc_or_dec, std::vector & limited_jnts) + // limit acceleration + if (joint_limits_[index].has_acceleration_limits || joint_limits_[index].has_deceleration_limits) { - if (std::abs(desired_accel[index]) > max_acc_or_dec) + //if(has_vel_state) + if(1) // for now use a zero velocity if not provided { - 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.emplace_back(joint_names_[index]); - } - }; + // limiting acc or dec function + auto apply_acc_or_dec_limit = + [&](const double max_acc_or_dec, std::vector &acc, std::vector & limited_jnts) -> bool + { + if (std::abs(acc[index]) > max_acc_or_dec) + { + acc[index] = std::copysign(max_acc_or_dec, acc[index]); + limited_jnts.emplace_back(joint_names_[index]); + return true; + } + else + return false; + }; + + // limit acc for pos_cmd and/or vel_cmd + + // compute desired_acc with desired_vel and vel_state + desired_acc[index] = + (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; + + // check if decelerating - if velocity is changing toward 0 + bool deceleration_limit_applied = false; + bool limit_applied = false; + if ( + (desired_acc[index] < 0 && current_joint_states.velocities[index] > 0) || + (desired_acc[index] > 0 && current_joint_states.velocities[index] < 0)) + { + // limit deceleration + if (joint_limits_[index].has_deceleration_limits) + { + limit_applied = apply_acc_or_dec_limit(joint_limits_[index].max_deceleration, desired_acc, limited_jnts_dec); + deceleration_limit_applied = true; + } + } - // 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) + { + limit_applied = apply_acc_or_dec_limit(joint_limits_[index].max_acceleration, desired_acc, limited_jnts_acc); + } - // 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); + if (limit_applied) + { + // vel_cmd from integration of desired_acc, needed even if no vel output + desired_vel[index] = + current_joint_states.velocities[index] + desired_acc[index] * dt_seconds; + if (has_pos_cmd) + { + // pos_cmd from from double integration of desired_acc + desired_pos[index] = current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * desired_acc[index] * dt_seconds * dt_seconds; + } + if (index == 0) + RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_pos limited by expected acc limited %f", desired_pos[0]); + } + } + //else we cannot compute acc and so not limiting it } - // 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 + // plan ahead for position limits if (joint_limits_[index].has_position_limits) { + if (has_vel_cmd && !has_pos_cmd) + { + // Check immediate next step when using vel_cmd only, other cases already handled + // integrate pos + expected_pos[index] = current_joint_states.positions[index] + + desired_vel[index] * dt_seconds; + // if expected_pos over limit + auto pos = std::max( + std::min(joint_limits_[index].max_position, expected_pos[index]), + joint_limits_[index].min_position); + if (pos != expected_pos[index]) + { + // TODO(gwalck) compute vel_cmd that would permit to slow down in time at full deceleration + // in any case limit pos to max + expected_pos[index] = pos; + // and recompute vel_cmd that would lead to pos_max (not ideal as velocity would not be zero) + desired_vel[index] = (expected_pos[index] - current_joint_states.positions[index]) / dt_seconds; + pos_limit_trig_jnts[index] = true; + if (index == 0) + RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_vel would trigger a pos limit violation at %f, limiting to %f", expected_pos[0], desired_vel[0]); + } + } + + // 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 + // 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. @@ -308,67 +332,85 @@ Now a factorized version // that limit if ( (desired_vel[index] < 0 && - (current_joint_states.positions[index] - joint_limits_[index].min_position < + (current_joint_states.positions[index] - joint_limits_[index].min_position < stopping_distance)) || (desired_vel[index] > 0 && - (joint_limits_[index].max_position - current_joint_states.positions[index] < + (joint_limits_[index].max_position - current_joint_states.positions[index] < stopping_distance))) { pos_limit_trig_jnts[index] = true; - position_limit_triggered = true; + braking_near_position_limit_triggered = true; } else { - // compute the travel_distance at new desired velocity, in best case duration + // compute the travel_distance at new desired velocity, with 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 < + (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] < + (joint_limits_[index].max_position - current_joint_states.positions[index] < motion_after_stopping_duration))) { pos_limit_trig_jnts[index] = true; - position_limit_triggered = true; + braking_near_position_limit_triggered = true; } // else no need to slow down. in worse case we won't hit the limit at current velocity } } } - if (position_limit_triggered) + // update variables according to triggers + if (braking_near_position_limit_triggered) { + // this limit applies to all joints even if a single one is triggered + std::ostringstream ostr; 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 // should still result in max decel being used - desired_accel[index] = -current_joint_states.velocities[index] / dt_seconds; + desired_acc[index] = -current_joint_states.velocities[index] / dt_seconds; 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]); + desired_acc[index] = std::copysign( + std::min(std::abs(desired_acc[index]), joint_limits_[index].max_deceleration), + desired_acc[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), - desired_accel[index]); + desired_acc[index] = std::copysign( + std::min(std::abs(desired_acc[index]), joint_limits_[index].max_acceleration), + desired_acc[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 (has_vel_cmd) + { + desired_vel[index] = + current_joint_states.velocities[index] + desired_acc[index] * dt_seconds; + } + if (has_pos_cmd) + desired_pos[index] = current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * desired_acc[index] * dt_seconds * dt_seconds; + + if (pos_limit_trig_jnts[index]) + ostr << joint_names_[index] << " "; } + 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 position limits" + " if continuing at current state, limiting all joints"); } + // display limitations + + // if position limiting if ( std::count_if( pos_limit_trig_jnts.begin(), pos_limit_trig_jnts.end(), [](bool trig) { return trig; }) > 0) @@ -413,10 +455,13 @@ Now a factorized version 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; + + if (has_pos_cmd) + desired_joint_states.positions = desired_pos; + if (has_vel_cmd) + desired_joint_states.velocities = desired_vel; + if (has_acc_cmd) + desired_joint_states.accelerations = desired_acc; return true; } From d5892e8888092487f336a84e680a6b4bbffb435d Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Fri, 7 Jul 2023 22:50:55 +0200 Subject: [PATCH 24/83] Adapted tests Expectation changed, output is now always a valid state including between each cmds (derivatives are valid) --- .../test/test_simple_joint_limiter.cpp | 248 +++++++++++++++--- 1 file changed, 216 insertions(+), 32 deletions(-) diff --git a/joint_limits/test/test_simple_joint_limiter.cpp b/joint_limits/test/test_simple_joint_limiter.cpp index 2de646e8dd..d6154e0c13 100644 --- a/joint_limits/test/test_simple_joint_limiter.cpp +++ b/joint_limits/test/test_simple_joint_limiter.cpp @@ -54,7 +54,7 @@ TEST_F(SimpleJointLimiterTest, when_invalid_dt_expect_enforce_fail) } } -TEST_F(SimpleJointLimiterTest, when_wrong_input_expect_enforce_fail) +TEST_F(SimpleJointLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce_fail) { SetupNode("simple_joint_limiter"); Load(); @@ -66,12 +66,48 @@ TEST_F(SimpleJointLimiterTest, when_wrong_input_expect_enforce_fail) ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); rclcpp::Duration period(1, 0); // 1 second + // test no desired interface + desired_joint_states_.positions.clear(); desired_joint_states_.velocities.clear(); - // trigger size check with one wrong size + // currently not handled desired_joint_states_.accelerations.clear(); ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); } } +TEST_F(SimpleJointLimiterTest, when_no_posstate_expect_enforce_fail) +{ + SetupNode("simple_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + joint_limiter_->configure(last_commanded_state_); + + rclcpp::Duration period(1, 0); // 1 second + // test no position interface + current_joint_states_.positions.clear(); + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + } +} + +TEST_F(SimpleJointLimiterTest, when_no_velstate_expect_enforce_succeed) +{ + SetupNode("simple_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + joint_limiter_->configure(last_commanded_state_); + + rclcpp::Duration period(1, 0); // 1 second + // test no vel interface + current_joint_states_.velocities.clear(); + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + } +} + TEST_F(SimpleJointLimiterTest, when_within_limits_expect_no_limits_applied) { SetupNode("simple_joint_limiter"); @@ -87,20 +123,20 @@ TEST_F(SimpleJointLimiterTest, when_within_limits_expect_no_limits_applied) // within limits desired_joint_states_.positions[0] = 1.0; - desired_joint_states_.velocities[0] = 1.0; + desired_joint_states_.velocities[0] = 1.0; // valid pos derivatite as well 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 + 1.0, // pos unchanged + 1.0, // vel unchanged + 1.0 // acc = vel / 1.0 ); } } -TEST_F(SimpleJointLimiterTest, when_velocity_exceeded_expect_vel_and_acc_enforced) +TEST_F(SimpleJointLimiterTest, when_posvel_leads_to_vel_exceeded_expect_limits_enforced) { SetupNode("simple_joint_limiter"); Load(); @@ -112,33 +148,123 @@ TEST_F(SimpleJointLimiterTest, when_velocity_exceeded_expect_vel_and_acc_enforce rclcpp::Duration period(1.0, 0.0); // 1 second - // desired velocity exceeds + // pos/vel cmd ifs + current_joint_states_.positions[0] = -2.0; + desired_joint_states_.positions[0] = 1.0; + // desired velocity exceeds although correctly computed from pos derivative 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 if limits applied CHECK_STATE_SINGLE_JOINT( desired_joint_states_, 0, - desired_joint_states_.positions[0], // pos unchanged + 0.0, // pos = pos + max_vel * dt 2.0, // vel limited to max_vel 2.0 / 1.0 // acc set to vel change/DT ); // check opposite velocity direction (sign copy) + current_joint_states_.positions[0] = 2.0; + desired_joint_states_.positions[0] = -1.0; + // desired velocity exceeds although correctly computed from pos derivative 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 + 0.0, // pos = pos - max_vel * dt -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) +TEST_F(SimpleJointLimiterTest, when_posonly_leads_to_vel_exceeded_expect_pos_acc_enforced) +{ + SetupNode("simple_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + + // set current velocity close to limits to not be blocked by max acc to reach max + current_joint_states_.velocities[0] = 1.9; + // desired pos leads to vel exceeded (4.0 / sec instead of 2.0) + desired_joint_states_.positions[0] = 4.0; + // no vel cmd (will lead to internal computation of velocity) + desired_joint_states_.velocities.clear(); + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos and acc limits applied + ASSERT_EQ(desired_joint_states_.positions[0], 2.0); // pos limited to max_vel * DT + // no vel cmd ifs + ASSERT_EQ(desired_joint_states_.accelerations[0], (2.0-1.9) / 1.0); // acc set to vel change/DT + + // check opposite position and direction + current_joint_states_.positions[0] = 0.0; + current_joint_states_.velocities[0] = -1.9; + desired_joint_states_.positions[0] = -4.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos and acc limits applied + ASSERT_EQ(desired_joint_states_.positions[0], -2.0); // pos limited to -max_vel * DT + // no vel cmd ifs + ASSERT_EQ(desired_joint_states_.accelerations[0], (-2.0+1.9) / 1.0); // acc set to vel change/DT + } +} + + +TEST_F(SimpleJointLimiterTest, when_velonly_leads_to_vel_exceeded_expect_vel_acc_enforced) +{ + SetupNode("simple_joint_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + + + // vel cmd ifs + current_joint_states_.positions[0] = -2.0; + // set current velocity close to limits to not be blocked by max acc to reach max + current_joint_states_.velocities[0] = 1.9; + // no pos cmd + desired_joint_states_.positions.clear(); + // 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 + ASSERT_EQ(desired_joint_states_.velocities[0], 2.0); // vel limited to max_vel + // no vel cmd ifs + ASSERT_EQ(desired_joint_states_.accelerations[0], (2.0-1.9) / 1.0); // acc set to vel change/DT + + // check opposite velocity direction + current_joint_states_.positions[0] = 2.0; + // set current velocity close to limits to not be blocked by max acc to reach max + current_joint_states_.velocities[0] = -1.9; + // 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 + ASSERT_EQ(desired_joint_states_.velocities[0], -2.0); // vel limited to -max_vel + // no vel cmd ifs + ASSERT_EQ(desired_joint_states_.accelerations[0], (-2.0+1.9) / 1.0); // acc set to vel change/DT + } +} + +TEST_F(SimpleJointLimiterTest, when_posonly_exceeded_expect_pos_enforced) { SetupNode("simple_joint_limiter"); Load(); @@ -151,17 +277,17 @@ TEST_F(SimpleJointLimiterTest, when_position_exceeded_expect_pos_enforced) rclcpp::Duration period(1.0, 0.0); // 1 second // desired pos exceeds + current_joint_states_.positions[0] = 5.0; desired_joint_states_.positions[0] = 20.0; - desired_joint_states_.velocities[0] = 0.0; + // no velocity interface + desired_joint_states_.velocities.clear(); 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 - ); + ASSERT_EQ(desired_joint_states_.positions[0], 5.0); // pos limited clamped (was already at limit) + // no vel cmd ifs + ASSERT_EQ(desired_joint_states_.accelerations[0], 0.0); // acc set to vel change/DT + } } @@ -207,7 +333,7 @@ TEST_F(SimpleJointLimiterTest, when_position_close_to_pos_limit_expect_decelerat } } -TEST_F(SimpleJointLimiterTest, when_acceleration_exceeded_expect_acc_enforced) +TEST_F(SimpleJointLimiterTest, when_posvel_leads_to_acc_exceeded_expect_limits_enforced) { SetupNode("simple_joint_limiter"); Load(); @@ -220,21 +346,76 @@ TEST_F(SimpleJointLimiterTest, when_acceleration_exceeded_expect_acc_enforced) rclcpp::Duration period(0, 50000000); // desired acceleration exceeds - current_joint_states_.positions[0] = 0.0; - current_joint_states_.velocities[0] = 0.0; + current_joint_states_.positions[0] = 0.1; + current_joint_states_.velocities[0] = 0.1; + desired_joint_states_.positions[0] = 0.125; // valid pos cmd for the desired velocity 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 if limits applied CHECK_STATE_SINGLE_JOINT( desired_joint_states_, 0, - desired_joint_states_.positions[0], // pos unchanged - 0.25, // vel limited max acc * dt + 0.11125, // pos = double integration from max acc with current state + 0.35, // vel limited to vel + max acc * dt 5.0 // acc max acc ); } } + +TEST_F(SimpleJointLimiterTest, when_posonly_leads_to_acc_exceeded_expect_limits_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.1; + current_joint_states_.velocities[0] = 0.1; + desired_joint_states_.positions[0] = 0.125; // pos cmd leads to vel 0.5 that leads to acc > max acc + desired_joint_states_.velocities.clear(); + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos and acc limits applied + ASSERT_NEAR(desired_joint_states_.positions[0], 0.111250, COMMON_THRESHOLD); // pos = double integration from max acc with current state + // no vel cmd ifs + ASSERT_EQ(desired_joint_states_.accelerations[0], 5.0); // acc max acc + } +} + +TEST_F(SimpleJointLimiterTest, when_velonly_leads_to_acc_exceeded_expect_limits_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.1; + current_joint_states_.velocities[0] = 0.1; + desired_joint_states_.positions.clear(); // = 0.125; + 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 pos and acc limits applied + // no pos cmd ifs + ASSERT_EQ(desired_joint_states_.velocities[0], 0.35); // vel limited to vel + max acc * dt + ASSERT_EQ(desired_joint_states_.accelerations[0], 5.0); // acc max acc + } +} + + TEST_F(SimpleJointLimiterTest, when_deceleration_exceeded_expect_dec_enforced) { SetupNode("simple_joint_limiter"); @@ -248,16 +429,18 @@ TEST_F(SimpleJointLimiterTest, when_deceleration_exceeded_expect_dec_enforced) 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 dec + current_joint_states_.positions[0] = 0.3; + current_joint_states_.velocities[0] = 0.5; + desired_joint_states_.positions[0] = 0.305; // valid pos cmd for the desired velocity + desired_joint_states_.velocities[0] = 0.1; // 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 vel-max dec * dt + 0.315625, // pos = double integration from max dec with current state + 0.125, // vel limited by vel - max dec * dt -7.5 // acc limited by -max dec ); } @@ -276,15 +459,16 @@ TEST_F(SimpleJointLimiterTest, when_deceleration_exceeded_with_no_maxdec_expect_ rclcpp::Duration period(0, 50000000); // desired deceleration exceeds - current_joint_states_.positions[0] = 0.0; + current_joint_states_.positions[0] = 1.0; current_joint_states_.velocities[0] = 1.0; + desired_joint_states_.positions[0] = 1.025; // valid pos cmd for the desired decreased velocity 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 + 1.04375, // pos = double integration from max acc with current state 0.75, // vel limited by vel-max acc * dt -5.0 // acc limited to -max acc ); From d848a7d576bb81d3931ff9f374de051ce9f95054 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Fri, 7 Jul 2023 22:53:33 +0200 Subject: [PATCH 25/83] lint --- joint_limits/src/simple_joint_limiter.cpp | 222 +++++++++--------- .../test/test_simple_joint_limiter.cpp | 85 +++---- 2 files changed, 160 insertions(+), 147 deletions(-) diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 11777fee46..4146b75dc7 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -30,72 +30,71 @@ bool SimpleJointLimiter::on_enforce( trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) { + /* + Now a factorized version -/* -Now a factorized version - - if has_pos_cmd - compute expected_vel with pos_cmd and pos_state - - if has_pos_limit if has_pos_cmd - clamp pos_cmd - - else - //nothing to do yet - - if has_vel_limit - if has_pos_cmd // priority of pos_cmd - vel_cmd = expected_vel - - if vel_cmd over limit - clamp vel_cmd - if clamped pos_cmd = integrate vel_cmd - - if has_acc_limit - if has_vel_state - if has_vel_cmd - compute expected_acc2 with limited vel_cmd and vel_state + compute expected_vel with pos_cmd and pos_state + + if has_pos_limit if has_pos_cmd - compute expected_acc with expected_vel and vel_state - else - or - we cannot compute the acc, we cannot derive acc from zero velocity this would be wrong - break + clamp pos_cmd - if has_pos_cmd - if expected_acc over limit - clamp expected_acc - integrate expected_vel to be compatible with limited expected_acc as well - integrate pos_cmd to be compatible with newly limited expected_vel and limited expected_acc as well + else + //nothing to do yet + + if has_vel_limit + if has_pos_cmd // priority of pos_cmd + vel_cmd = expected_vel + + if vel_cmd over limit + clamp vel_cmd + if clamped pos_cmd = integrate vel_cmd + + if has_acc_limit + if has_vel_state + if has_vel_cmd + compute expected_acc2 with limited vel_cmd and vel_state + if has_pos_cmd + compute expected_acc with expected_vel and vel_state + else + or + we cannot compute the acc, we cannot derive acc from zero velocity this would be wrong + break + + if has_pos_cmd + if expected_acc over limit + clamp expected_acc + integrate expected_vel to be compatible with limited expected_acc as well + integrate pos_cmd to be compatible with newly limited expected_vel and limited + expected_acc as well - if has_vel_cmd - if expected_acc2 over limit - clamp expected_acc2 - integrate vel_cmd to be compatible with limited expected_acc2 as well + if has_vel_cmd + if expected_acc2 over limit + clamp expected_acc2 + integrate vel_cmd to be compatible with limited expected_acc2 as well - if has_pos_limit - if has_vel_cmd - compute expected_pos by integrating vel_cmd // check for next step - if expected_pos over limit - consider braking and compute ideal vel_cmd that would permit to slow down in time at full deceleration - in any case limit pos to max and recompute vel_cmd that would lead to pos_max (not ideal as velocity would not be zero) - - des_vel = vel_cmd or expected_vel - check if expected_pos might be reached at max braking // check for future steps - recompute vel_cmd or pos_cmd or both - + if has_pos_limit + if has_vel_cmd + compute expected_pos by integrating vel_cmd // check for next step + if expected_pos over limit + consider braking and compute ideal vel_cmd that would permit to slow down in time at full + deceleration in any case limit pos to max and recompute vel_cmd that would lead to pos_max (not + ideal as velocity would not be zero) -*/ + des_vel = vel_cmd or expected_vel + check if expected_pos might be reached at max braking // check for future steps + recompute vel_cmd or pos_cmd or both + */ const auto dt_seconds = dt.seconds(); // negative or null is not allowed if (dt_seconds <= 0.0) return false; // TODO(gwalck) compute if the max are not implicitly violated with the given dt - // eg for max vel 2.0 and max acc 5.0, with dt >0.4 + // eg for max vel 2.0 and max acc 5.0, with dt >0.4 // velocity max is implicitly already violated due to max_acc * dt > 2.0 // check for required inputs combination @@ -117,7 +116,6 @@ Now a factorized version current_joint_states.velocities.resize(number_of_joints_, 0.0); } - // 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_acc(number_of_joints_); @@ -129,7 +127,7 @@ Now a factorized version // limits triggered std::vector pos_limit_trig_jnts(number_of_joints_, false); // vel_limit_trig_jnts(number_of_joints_, false); - //std::vector limited_jnts_vel_idx; + // std::vector limited_jnts_vel_idx; std::vector limited_jnts_vel, limited_jnts_acc, limited_jnts_dec; bool braking_near_position_limit_triggered = false; @@ -140,12 +138,12 @@ Now a factorized version if (index == 0) RCLCPP_INFO(node_logging_itf_->get_logger(), "vel input %f", desired_vel[0]); }*/ - + if (has_pos_cmd) { desired_pos[index] = desired_joint_states.positions[index]; } - + // limit position if (joint_limits_[index].has_position_limits) { @@ -164,21 +162,23 @@ Now a factorized version if (index == 0) RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_pos clamped %f", desired_pos[0]); } - } } // prepare velocity if (has_pos_cmd) { - // priority to pos_cmd derivative over cmd_vel because we always have a pos_state so recomputing vel_cmd is fine - // compute expected_vel with already clamped pos_cmd and pos_state - // TODO(gwalck) handle the case of continuous joints with angle_wraparound to compute vel correctly + // priority to pos_cmd derivative over cmd_vel because we always have a pos_state so + // recomputing vel_cmd is fine compute expected_vel with already clamped pos_cmd and pos_state + // TODO(gwalck) handle the case of continuous joints with angle_wraparound to compute vel + // correctly desired_vel[index] = (desired_pos[index] - current_joint_states.positions[index]) / dt_seconds; if (index == 0) - RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_vel input from pos derivative %f", desired_vel[0]); - } + RCLCPP_INFO( + node_logging_itf_->get_logger(), "desired_vel input from pos derivative %f", + desired_vel[0]); + } else { if (has_vel_cmd) @@ -187,13 +187,12 @@ Now a factorized version if (index == 0) RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_vel input %f", desired_vel[0]); } - - } - + } + // limit velocity if (joint_limits_[index].has_velocity_limits) { - //clamp input vel_cmd + // clamp input vel_cmd if (std::abs(desired_vel[index]) > joint_limits_[index].max_velocity) { desired_vel[index] = std::copysign(joint_limits_[index].max_velocity, desired_vel[index]); @@ -202,26 +201,30 @@ Now a factorized version // recompute pos_cmd if needed if (has_pos_cmd) { - desired_pos[index] = current_joint_states.positions[index] + - desired_vel[index] * dt_seconds; + desired_pos[index] = + current_joint_states.positions[index] + desired_vel[index] * dt_seconds; pos_limit_trig_jnts[index] = true; if (index == 0) - RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_pos limited by expected vel limited %f", desired_pos[0]); + RCLCPP_INFO( + node_logging_itf_->get_logger(), "desired_pos limited by expected vel limited %f", + desired_pos[0]); } if (index == 0) - RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_vel clamped %f", desired_vel[0]); + RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_vel clamped %f", desired_vel[0]); } } // limit acceleration - if (joint_limits_[index].has_acceleration_limits || joint_limits_[index].has_deceleration_limits) + if ( + joint_limits_[index].has_acceleration_limits || joint_limits_[index].has_deceleration_limits) { - //if(has_vel_state) - if(1) // for now use a zero velocity if not provided + // if(has_vel_state) + if (1) // for now use a zero velocity if not provided { - // limiting acc or dec function - auto apply_acc_or_dec_limit = - [&](const double max_acc_or_dec, std::vector &acc, std::vector & limited_jnts) -> bool + // limiting acc or dec function + auto apply_acc_or_dec_limit = [&]( + const double max_acc_or_dec, std::vector & acc, + std::vector & limited_jnts) -> bool { if (std::abs(acc[index]) > max_acc_or_dec) { @@ -249,7 +252,8 @@ Now a factorized version // limit deceleration if (joint_limits_[index].has_deceleration_limits) { - limit_applied = apply_acc_or_dec_limit(joint_limits_[index].max_deceleration, desired_acc, limited_jnts_dec); + limit_applied = apply_acc_or_dec_limit( + joint_limits_[index].max_deceleration, desired_acc, limited_jnts_dec); deceleration_limit_applied = true; } } @@ -257,26 +261,29 @@ Now a factorized version // limit acceleration (fallback to acceleration if no deceleration limits) if (joint_limits_[index].has_acceleration_limits && !deceleration_limit_applied) { - limit_applied = apply_acc_or_dec_limit(joint_limits_[index].max_acceleration, desired_acc, limited_jnts_acc); + limit_applied = apply_acc_or_dec_limit( + joint_limits_[index].max_acceleration, desired_acc, limited_jnts_acc); } if (limit_applied) { // vel_cmd from integration of desired_acc, needed even if no vel output - desired_vel[index] = + desired_vel[index] = current_joint_states.velocities[index] + desired_acc[index] * dt_seconds; if (has_pos_cmd) { // pos_cmd from from double integration of desired_acc desired_pos[index] = current_joint_states.positions[index] + - current_joint_states.velocities[index] * dt_seconds + - 0.5 * desired_acc[index] * dt_seconds * dt_seconds; + current_joint_states.velocities[index] * dt_seconds + + 0.5 * desired_acc[index] * dt_seconds * dt_seconds; } if (index == 0) - RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_pos limited by expected acc limited %f", desired_pos[0]); + RCLCPP_INFO( + node_logging_itf_->get_logger(), "desired_pos limited by expected acc limited %f", + desired_pos[0]); } } - //else we cannot compute acc and so not limiting it + // else we cannot compute acc and so not limiting it } // plan ahead for position limits @@ -286,28 +293,34 @@ Now a factorized version { // Check immediate next step when using vel_cmd only, other cases already handled // integrate pos - expected_pos[index] = current_joint_states.positions[index] - + desired_vel[index] * dt_seconds; + expected_pos[index] = + current_joint_states.positions[index] + desired_vel[index] * dt_seconds; // if expected_pos over limit auto pos = std::max( std::min(joint_limits_[index].max_position, expected_pos[index]), joint_limits_[index].min_position); if (pos != expected_pos[index]) { - // TODO(gwalck) compute vel_cmd that would permit to slow down in time at full deceleration - // in any case limit pos to max + // TODO(gwalck) compute vel_cmd that would permit to slow down in time at full + // deceleration in any case limit pos to max expected_pos[index] = pos; - // and recompute vel_cmd that would lead to pos_max (not ideal as velocity would not be zero) - desired_vel[index] = (expected_pos[index] - current_joint_states.positions[index]) / dt_seconds; + // and recompute vel_cmd that would lead to pos_max (not ideal as velocity would not be + // zero) + desired_vel[index] = + (expected_pos[index] - current_joint_states.positions[index]) / dt_seconds; pos_limit_trig_jnts[index] = true; if (index == 0) - RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_vel would trigger a pos limit violation at %f, limiting to %f", expected_pos[0], desired_vel[0]); + RCLCPP_INFO( + node_logging_itf_->get_logger(), + "desired_vel would trigger a pos limit violation at %f, limiting to %f", + expected_pos[0], desired_vel[0]); } } // 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 - + // Slow down all joints at maximum decel if any don't have stopping distance within joint + // 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. @@ -332,10 +345,10 @@ Now a factorized version // that limit if ( (desired_vel[index] < 0 && - (current_joint_states.positions[index] - joint_limits_[index].min_position < + (current_joint_states.positions[index] - joint_limits_[index].min_position < stopping_distance)) || (desired_vel[index] > 0 && - (joint_limits_[index].max_position - current_joint_states.positions[index] < + (joint_limits_[index].max_position - current_joint_states.positions[index] < stopping_distance))) { pos_limit_trig_jnts[index] = true; @@ -349,10 +362,10 @@ Now a factorized version // 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 < + (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] < + (joint_limits_[index].max_position - current_joint_states.positions[index] < motion_after_stopping_duration))) { pos_limit_trig_jnts[index] = true; @@ -398,14 +411,14 @@ Now a factorized version current_joint_states.velocities[index] * dt_seconds + 0.5 * desired_acc[index] * dt_seconds * dt_seconds; - if (pos_limit_trig_jnts[index]) - ostr << joint_names_[index] << " "; + if (pos_limit_trig_jnts[index]) ostr << joint_names_[index] << " "; } 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 position limits" - " if continuing at current state, limiting all joints"); + "Joint(s) [" << ostr.str().c_str() + << "] would exceed position limits" + " if continuing at current state, limiting all joints"); } // display limitations @@ -455,13 +468,10 @@ Now a factorized version node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, "Joint(s) [" << ostr.str().c_str() << "] would exceed deceleration limits, limiting"); } - - if (has_pos_cmd) - desired_joint_states.positions = desired_pos; - if (has_vel_cmd) - desired_joint_states.velocities = desired_vel; - if (has_acc_cmd) - desired_joint_states.accelerations = desired_acc; + + if (has_pos_cmd) desired_joint_states.positions = desired_pos; + if (has_vel_cmd) desired_joint_states.velocities = desired_vel; + if (has_acc_cmd) desired_joint_states.accelerations = desired_acc; return true; } diff --git a/joint_limits/test/test_simple_joint_limiter.cpp b/joint_limits/test/test_simple_joint_limiter.cpp index d6154e0c13..f448c03562 100644 --- a/joint_limits/test/test_simple_joint_limiter.cpp +++ b/joint_limits/test/test_simple_joint_limiter.cpp @@ -123,13 +123,13 @@ TEST_F(SimpleJointLimiterTest, when_within_limits_expect_no_limits_applied) // within limits desired_joint_states_.positions[0] = 1.0; - desired_joint_states_.velocities[0] = 1.0; // valid pos derivatite as well + desired_joint_states_.velocities[0] = 1.0; // valid pos derivatite as well 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, - 1.0, // pos unchanged + 1.0, // pos unchanged 1.0, // vel unchanged 1.0 // acc = vel / 1.0 ); @@ -158,9 +158,9 @@ TEST_F(SimpleJointLimiterTest, when_posvel_leads_to_vel_exceeded_expect_limits_e // check if limits applied CHECK_STATE_SINGLE_JOINT( desired_joint_states_, 0, - 0.0, // pos = pos + max_vel * dt - 2.0, // vel limited to max_vel - 2.0 / 1.0 // acc set to vel change/DT + 0.0, // pos = pos + max_vel * dt + 2.0, // vel limited to max_vel + 2.0 / 1.0 // acc set to vel change/DT ); // check opposite velocity direction (sign copy) @@ -173,9 +173,9 @@ TEST_F(SimpleJointLimiterTest, when_posvel_leads_to_vel_exceeded_expect_limits_e // check if vel and acc limits applied CHECK_STATE_SINGLE_JOINT( desired_joint_states_, 0, - 0.0, // pos = pos - max_vel * dt - -2.0, // vel limited to -max_vel - -2.0 / 1.0 // acc set to vel change/DT + 0.0, // pos = pos - max_vel * dt + -2.0, // vel limited to -max_vel + -2.0 / 1.0 // acc set to vel change/DT ); } } @@ -203,7 +203,8 @@ TEST_F(SimpleJointLimiterTest, when_posonly_leads_to_vel_exceeded_expect_pos_acc // check if pos and acc limits applied ASSERT_EQ(desired_joint_states_.positions[0], 2.0); // pos limited to max_vel * DT // no vel cmd ifs - ASSERT_EQ(desired_joint_states_.accelerations[0], (2.0-1.9) / 1.0); // acc set to vel change/DT + ASSERT_EQ( + desired_joint_states_.accelerations[0], (2.0 - 1.9) / 1.0); // acc set to vel change/DT // check opposite position and direction current_joint_states_.positions[0] = 0.0; @@ -214,11 +215,11 @@ TEST_F(SimpleJointLimiterTest, when_posonly_leads_to_vel_exceeded_expect_pos_acc // check if pos and acc limits applied ASSERT_EQ(desired_joint_states_.positions[0], -2.0); // pos limited to -max_vel * DT // no vel cmd ifs - ASSERT_EQ(desired_joint_states_.accelerations[0], (-2.0+1.9) / 1.0); // acc set to vel change/DT + ASSERT_EQ( + desired_joint_states_.accelerations[0], (-2.0 + 1.9) / 1.0); // acc set to vel change/DT } } - TEST_F(SimpleJointLimiterTest, when_velonly_leads_to_vel_exceeded_expect_vel_acc_enforced) { SetupNode("simple_joint_limiter"); @@ -231,7 +232,6 @@ TEST_F(SimpleJointLimiterTest, when_velonly_leads_to_vel_exceeded_expect_vel_acc rclcpp::Duration period(1.0, 0.0); // 1 second - // vel cmd ifs current_joint_states_.positions[0] = -2.0; // set current velocity close to limits to not be blocked by max acc to reach max @@ -246,13 +246,14 @@ TEST_F(SimpleJointLimiterTest, when_velonly_leads_to_vel_exceeded_expect_vel_acc // check if vel and acc limits applied ASSERT_EQ(desired_joint_states_.velocities[0], 2.0); // vel limited to max_vel // no vel cmd ifs - ASSERT_EQ(desired_joint_states_.accelerations[0], (2.0-1.9) / 1.0); // acc set to vel change/DT + ASSERT_EQ( + desired_joint_states_.accelerations[0], (2.0 - 1.9) / 1.0); // acc set to vel change/DT // check opposite velocity direction current_joint_states_.positions[0] = 2.0; // set current velocity close to limits to not be blocked by max acc to reach max current_joint_states_.velocities[0] = -1.9; - // desired velocity exceeds + // desired velocity exceeds desired_joint_states_.velocities[0] = -3.0; ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); @@ -260,7 +261,8 @@ TEST_F(SimpleJointLimiterTest, when_velonly_leads_to_vel_exceeded_expect_vel_acc // check if vel and acc limits applied ASSERT_EQ(desired_joint_states_.velocities[0], -2.0); // vel limited to -max_vel // no vel cmd ifs - ASSERT_EQ(desired_joint_states_.accelerations[0], (-2.0+1.9) / 1.0); // acc set to vel change/DT + ASSERT_EQ( + desired_joint_states_.accelerations[0], (-2.0 + 1.9) / 1.0); // acc set to vel change/DT } } @@ -284,10 +286,10 @@ TEST_F(SimpleJointLimiterTest, when_posonly_exceeded_expect_pos_enforced) ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); // check if pos limits applied - ASSERT_EQ(desired_joint_states_.positions[0], 5.0); // pos limited clamped (was already at limit) + ASSERT_EQ( + desired_joint_states_.positions[0], 5.0); // pos limited clamped (was already at limit) // no vel cmd ifs - ASSERT_EQ(desired_joint_states_.accelerations[0], 0.0); // acc set to vel change/DT - + ASSERT_EQ(desired_joint_states_.accelerations[0], 0.0); // acc set to vel change/DT } } @@ -348,21 +350,20 @@ TEST_F(SimpleJointLimiterTest, when_posvel_leads_to_acc_exceeded_expect_limits_e // desired acceleration exceeds current_joint_states_.positions[0] = 0.1; current_joint_states_.velocities[0] = 0.1; - desired_joint_states_.positions[0] = 0.125; // valid pos cmd for the desired velocity - desired_joint_states_.velocities[0] = 0.5; // leads to acc > max acc + desired_joint_states_.positions[0] = 0.125; // valid pos cmd for the desired velocity + 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 limits applied CHECK_STATE_SINGLE_JOINT( desired_joint_states_, 0, - 0.11125, // pos = double integration from max acc with current state - 0.35, // vel limited to vel + max acc * dt - 5.0 // acc max acc + 0.11125, // pos = double integration from max acc with current state + 0.35, // vel limited to vel + max acc * dt + 5.0 // acc max acc ); } } - TEST_F(SimpleJointLimiterTest, when_posonly_leads_to_acc_exceeded_expect_limits_enforced) { SetupNode("simple_joint_limiter"); @@ -378,14 +379,17 @@ TEST_F(SimpleJointLimiterTest, when_posonly_leads_to_acc_exceeded_expect_limits_ // desired acceleration exceeds current_joint_states_.positions[0] = 0.1; current_joint_states_.velocities[0] = 0.1; - desired_joint_states_.positions[0] = 0.125; // pos cmd leads to vel 0.5 that leads to acc > max acc + desired_joint_states_.positions[0] = + 0.125; // pos cmd leads to vel 0.5 that leads to acc > max acc desired_joint_states_.velocities.clear(); ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); // check if pos and acc limits applied - ASSERT_NEAR(desired_joint_states_.positions[0], 0.111250, COMMON_THRESHOLD); // pos = double integration from max acc with current state + ASSERT_NEAR( + desired_joint_states_.positions[0], 0.111250, + COMMON_THRESHOLD); // pos = double integration from max acc with current state // no vel cmd ifs - ASSERT_EQ(desired_joint_states_.accelerations[0], 5.0); // acc max acc + ASSERT_EQ(desired_joint_states_.accelerations[0], 5.0); // acc max acc } } @@ -404,18 +408,17 @@ TEST_F(SimpleJointLimiterTest, when_velonly_leads_to_acc_exceeded_expect_limits_ // desired acceleration exceeds current_joint_states_.positions[0] = 0.1; current_joint_states_.velocities[0] = 0.1; - desired_joint_states_.positions.clear(); // = 0.125; + desired_joint_states_.positions.clear(); // = 0.125; 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 pos and acc limits applied // no pos cmd ifs - ASSERT_EQ(desired_joint_states_.velocities[0], 0.35); // vel limited to vel + max acc * dt - ASSERT_EQ(desired_joint_states_.accelerations[0], 5.0); // acc max acc + ASSERT_EQ(desired_joint_states_.velocities[0], 0.35); // vel limited to vel + max acc * dt + ASSERT_EQ(desired_joint_states_.accelerations[0], 5.0); // acc max acc } } - TEST_F(SimpleJointLimiterTest, when_deceleration_exceeded_expect_dec_enforced) { SetupNode("simple_joint_limiter"); @@ -431,17 +434,17 @@ TEST_F(SimpleJointLimiterTest, when_deceleration_exceeded_expect_dec_enforced) // desired deceleration exceeds current_joint_states_.positions[0] = 0.3; current_joint_states_.velocities[0] = 0.5; - desired_joint_states_.positions[0] = 0.305; // valid pos cmd for the desired velocity - desired_joint_states_.velocities[0] = 0.1; // leads to acc < -max dec + desired_joint_states_.positions[0] = 0.305; // valid pos cmd for the desired velocity + desired_joint_states_.velocities[0] = 0.1; // 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, - 0.315625, // pos = double integration from max dec with current state - 0.125, // vel limited by vel - max dec * dt - -7.5 // acc limited by -max dec + 0.315625, // pos = double integration from max dec with current state + 0.125, // vel limited by vel - max dec * dt + -7.5 // acc limited by -max dec ); } } @@ -461,16 +464,16 @@ TEST_F(SimpleJointLimiterTest, when_deceleration_exceeded_with_no_maxdec_expect_ // desired deceleration exceeds current_joint_states_.positions[0] = 1.0; current_joint_states_.velocities[0] = 1.0; - desired_joint_states_.positions[0] = 1.025; // valid pos cmd for the desired decreased velocity - desired_joint_states_.velocities[0] = 0.5; // leads to acc > -max acc + desired_joint_states_.positions[0] = 1.025; // valid pos cmd for the desired decreased velocity + 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, - 1.04375, // pos = double integration from max acc with current state - 0.75, // vel limited by vel-max acc * dt - -5.0 // acc limited to -max acc + 1.04375, // pos = double integration from max acc with current state + 0.75, // vel limited by vel-max acc * dt + -5.0 // acc limited to -max acc ); } } From 8c771cfc73221b866edfc369cc6f72602f473f20 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Fri, 7 Jul 2023 23:03:36 +0200 Subject: [PATCH 26/83] Cleanup debug helpers --- joint_limits/src/simple_joint_limiter.cpp | 38 ++--------------------- 1 file changed, 3 insertions(+), 35 deletions(-) diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 4146b75dc7..0e61beb682 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, Dr. Denis Stogl +/// \authors Nathan Brooks, Dr. Denis Stogl, Guillaume Walck #include "joint_limits/simple_joint_limiter.hpp" @@ -94,7 +94,7 @@ bool SimpleJointLimiter::on_enforce( if (dt_seconds <= 0.0) return false; // TODO(gwalck) compute if the max are not implicitly violated with the given dt - // eg for max vel 2.0 and max acc 5.0, with dt >0.4 + // e.g. for max vel 2.0 and max acc 5.0, with dt >0.4 // velocity max is implicitly already violated due to max_acc * dt > 2.0 // check for required inputs combination @@ -126,19 +126,12 @@ bool SimpleJointLimiter::on_enforce( // limits triggered std::vector pos_limit_trig_jnts(number_of_joints_, false); - // vel_limit_trig_jnts(number_of_joints_, false); - // std::vector limited_jnts_vel_idx; std::vector limited_jnts_vel, limited_jnts_acc, limited_jnts_dec; bool braking_near_position_limit_triggered = false; for (size_t index = 0; index < number_of_joints_; ++index) { - /* - if (index == 0) - RCLCPP_INFO(node_logging_itf_->get_logger(), "vel input %f", desired_vel[0]); - }*/ - if (has_pos_cmd) { desired_pos[index] = desired_joint_states.positions[index]; @@ -149,8 +142,6 @@ bool SimpleJointLimiter::on_enforce( { if (has_pos_cmd) { - if (index == 0) - RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_pos input %f", desired_pos[0]); // clamp input pos_cmd auto pos = std::max( std::min(joint_limits_[index].max_position, desired_pos[index]), @@ -159,8 +150,6 @@ bool SimpleJointLimiter::on_enforce( { desired_pos[index] = pos; pos_limit_trig_jnts[index] = true; - if (index == 0) - RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_pos clamped %f", desired_pos[0]); } } } @@ -174,18 +163,12 @@ bool SimpleJointLimiter::on_enforce( // correctly desired_vel[index] = (desired_pos[index] - current_joint_states.positions[index]) / dt_seconds; - if (index == 0) - RCLCPP_INFO( - node_logging_itf_->get_logger(), "desired_vel input from pos derivative %f", - desired_vel[0]); } else { if (has_vel_cmd) { desired_vel[index] = desired_joint_states.velocities[index]; - if (index == 0) - RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_vel input %f", desired_vel[0]); } } @@ -204,13 +187,7 @@ bool SimpleJointLimiter::on_enforce( desired_pos[index] = current_joint_states.positions[index] + desired_vel[index] * dt_seconds; pos_limit_trig_jnts[index] = true; - if (index == 0) - RCLCPP_INFO( - node_logging_itf_->get_logger(), "desired_pos limited by expected vel limited %f", - desired_pos[0]); } - if (index == 0) - RCLCPP_INFO(node_logging_itf_->get_logger(), "desired_vel clamped %f", desired_vel[0]); } } @@ -277,13 +254,9 @@ bool SimpleJointLimiter::on_enforce( current_joint_states.velocities[index] * dt_seconds + 0.5 * desired_acc[index] * dt_seconds * dt_seconds; } - if (index == 0) - RCLCPP_INFO( - node_logging_itf_->get_logger(), "desired_pos limited by expected acc limited %f", - desired_pos[0]); } } - // else we cannot compute acc and so not limiting it + // else we cannot compute acc, so not limiting it } // plan ahead for position limits @@ -309,11 +282,6 @@ bool SimpleJointLimiter::on_enforce( desired_vel[index] = (expected_pos[index] - current_joint_states.positions[index]) / dt_seconds; pos_limit_trig_jnts[index] = true; - if (index == 0) - RCLCPP_INFO( - node_logging_itf_->get_logger(), - "desired_vel would trigger a pos limit violation at %f, limiting to %f", - expected_pos[0], desired_vel[0]); } } From 7e8e4cdbff4a25324ac6e690ea25bf58fab5e3ef Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Fri, 7 Jul 2023 23:03:50 +0200 Subject: [PATCH 27/83] Removed pseudo-code --- joint_limits/src/simple_joint_limiter.cpp | 59 ----------------------- 1 file changed, 59 deletions(-) diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 0e61beb682..09715ddecd 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -30,65 +30,6 @@ bool SimpleJointLimiter::on_enforce( trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) { - /* - Now a factorized version - - if has_pos_cmd - compute expected_vel with pos_cmd and pos_state - - if has_pos_limit - if has_pos_cmd - clamp pos_cmd - - else - //nothing to do yet - - if has_vel_limit - if has_pos_cmd // priority of pos_cmd - vel_cmd = expected_vel - - if vel_cmd over limit - clamp vel_cmd - if clamped pos_cmd = integrate vel_cmd - - if has_acc_limit - if has_vel_state - if has_vel_cmd - compute expected_acc2 with limited vel_cmd and vel_state - if has_pos_cmd - compute expected_acc with expected_vel and vel_state - else - or - we cannot compute the acc, we cannot derive acc from zero velocity this would be wrong - break - - if has_pos_cmd - if expected_acc over limit - clamp expected_acc - integrate expected_vel to be compatible with limited expected_acc as well - integrate pos_cmd to be compatible with newly limited expected_vel and limited - expected_acc as well - - if has_vel_cmd - if expected_acc2 over limit - clamp expected_acc2 - integrate vel_cmd to be compatible with limited expected_acc2 as well - - if has_pos_limit - if has_vel_cmd - compute expected_pos by integrating vel_cmd // check for next step - if expected_pos over limit - consider braking and compute ideal vel_cmd that would permit to slow down in time at full - deceleration in any case limit pos to max and recompute vel_cmd that would lead to pos_max (not - ideal as velocity would not be zero) - - des_vel = vel_cmd or expected_vel - check if expected_pos might be reached at max braking // check for future steps - recompute vel_cmd or pos_cmd or both - - - */ - const auto dt_seconds = dt.seconds(); // negative or null is not allowed if (dt_seconds <= 0.0) return false; From 345370a302881df75f98a03a8f3cb0244fa6da54 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 4 Sep 2023 17:52:21 +0200 Subject: [PATCH 28/83] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich Co-authored-by: Sai Kishor Kothakota --- .../joint_limits/joint_limiter_interface.hpp | 2 +- joint_limits/src/simple_joint_limiter.cpp | 25 +++++++++---------- 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 82a042ccba..a6869c2ca4 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -42,7 +42,7 @@ class JointLimiterInterface * 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. + * 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. diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 09715ddecd..5e8b39dccf 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -84,9 +84,9 @@ bool SimpleJointLimiter::on_enforce( if (has_pos_cmd) { // clamp input pos_cmd - auto pos = std::max( - std::min(joint_limits_[index].max_position, desired_pos[index]), - joint_limits_[index].min_position); + auto pos = std::clamp( + desired_pos[index], + joint_limits_[index].min_position, joint_limits_[index].max_position); if (pos != desired_pos[index]) { desired_pos[index] = pos; @@ -117,7 +117,7 @@ bool SimpleJointLimiter::on_enforce( if (joint_limits_[index].has_velocity_limits) { // clamp input vel_cmd - if (std::abs(desired_vel[index]) > joint_limits_[index].max_velocity) + if (std::fabs(desired_vel[index]) > joint_limits_[index].max_velocity) { desired_vel[index] = std::copysign(joint_limits_[index].max_velocity, desired_vel[index]); limited_jnts_vel.emplace_back(joint_names_[index]); @@ -144,7 +144,7 @@ bool SimpleJointLimiter::on_enforce( const double max_acc_or_dec, std::vector & acc, std::vector & limited_jnts) -> bool { - if (std::abs(acc[index]) > max_acc_or_dec) + if (std::fabs(acc[index]) > max_acc_or_dec) { acc[index] = std::copysign(max_acc_or_dec, acc[index]); limited_jnts.emplace_back(joint_names_[index]); @@ -210,9 +210,8 @@ bool SimpleJointLimiter::on_enforce( expected_pos[index] = current_joint_states.positions[index] + desired_vel[index] * dt_seconds; // if expected_pos over limit - auto pos = std::max( - std::min(joint_limits_[index].max_position, expected_pos[index]), - joint_limits_[index].min_position); + auto pos = std::clamp(expected_pos[index], + joint_limits_[index].min_position, joint_limits_[index].max_position); if (pos != expected_pos[index]) { // TODO(gwalck) compute vel_cmd that would permit to slow down in time at full @@ -235,7 +234,7 @@ 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_deccel = std::abs(desired_vel[index] / dt_seconds); + double stopping_deccel = std::fabs(desired_vel[index] / dt_seconds); if (joint_limits_[index].has_deceleration_limits) { stopping_deccel = joint_limits_[index].max_deceleration; @@ -246,9 +245,9 @@ bool SimpleJointLimiter::on_enforce( } double stopping_distance = - std::abs((-desired_vel[index] * desired_vel[index]) / (2 * stopping_deccel)); + std::fabs((-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)); + double stopping_duration = std::fabs((desired_vel[index]) / (stopping_deccel)); // Check that joint limits are beyond stopping_distance and desired_velocity is towards // that limit @@ -299,13 +298,13 @@ bool SimpleJointLimiter::on_enforce( if (joint_limits_[index].has_deceleration_limits) { desired_acc[index] = std::copysign( - std::min(std::abs(desired_acc[index]), joint_limits_[index].max_deceleration), + std::min(std::fabs(desired_acc[index]), joint_limits_[index].max_deceleration), desired_acc[index]); } else if (joint_limits_[index].has_acceleration_limits) { desired_acc[index] = std::copysign( - std::min(std::abs(desired_acc[index]), joint_limits_[index].max_acceleration), + std::min(std::fabs(desired_acc[index]), joint_limits_[index].max_acceleration), desired_acc[index]); } From c99eb86b4f6513378513d1eab09accbbc7d216dc Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 4 Sep 2023 19:22:55 +0200 Subject: [PATCH 29/83] Apply suggestions from code review --- joint_limits/include/joint_limits/joint_limiter_interface.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index a6869c2ca4..8a76b133bc 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -69,8 +69,6 @@ class JointLimiterInterface // 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( @@ -146,7 +144,6 @@ class JointLimiterInterface } 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( From 5a9985c2d8198051d6293905ea506f2ef5ecd161 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 4 Sep 2023 19:24:15 +0200 Subject: [PATCH 30/83] Unify how joints were limits are reached are stored. --- joint_limits/src/simple_joint_limiter.cpp | 26 +++++++++-------------- 1 file changed, 10 insertions(+), 16 deletions(-) diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 5e8b39dccf..17aaf4547d 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -66,8 +66,7 @@ bool SimpleJointLimiter::on_enforce( std::vector expected_pos(number_of_joints_); // limits triggered - std::vector pos_limit_trig_jnts(number_of_joints_, false); - std::vector limited_jnts_vel, limited_jnts_acc, limited_jnts_dec; + std::vector limited_jnts_pos, limited_jnts_vel, limited_jnts_acc, limited_jnts_dec; bool braking_near_position_limit_triggered = false; @@ -90,7 +89,7 @@ bool SimpleJointLimiter::on_enforce( if (pos != desired_pos[index]) { desired_pos[index] = pos; - pos_limit_trig_jnts[index] = true; + limited_jnts_pos.emplace_back(joint_names_[index]); } } } @@ -127,7 +126,7 @@ bool SimpleJointLimiter::on_enforce( { desired_pos[index] = current_joint_states.positions[index] + desired_vel[index] * dt_seconds; - pos_limit_trig_jnts[index] = true; + limited_jnts_pos.emplace_back(joint_names_[index]); } } } @@ -221,7 +220,7 @@ bool SimpleJointLimiter::on_enforce( // zero) desired_vel[index] = (expected_pos[index] - current_joint_states.positions[index]) / dt_seconds; - pos_limit_trig_jnts[index] = true; + limited_jnts_pos.emplace_back(joint_names_[index]); } } @@ -259,7 +258,7 @@ bool SimpleJointLimiter::on_enforce( (joint_limits_[index].max_position - current_joint_states.positions[index] < stopping_distance))) { - pos_limit_trig_jnts[index] = true; + limited_jnts_pos.emplace_back(joint_names_[index]); braking_near_position_limit_triggered = true; } else @@ -276,7 +275,7 @@ bool SimpleJointLimiter::on_enforce( (joint_limits_[index].max_position - current_joint_states.positions[index] < motion_after_stopping_duration))) { - pos_limit_trig_jnts[index] = true; + limited_jnts_pos.emplace_back(joint_names_[index]); braking_near_position_limit_triggered = true; } // else no need to slow down. in worse case we won't hit the limit at current velocity @@ -288,7 +287,6 @@ bool SimpleJointLimiter::on_enforce( if (braking_near_position_limit_triggered) { // this limit applies to all joints even if a single one is triggered - std::ostringstream ostr; for (size_t index = 0; index < number_of_joints_; ++index) { // Compute accel to stop @@ -319,8 +317,9 @@ bool SimpleJointLimiter::on_enforce( current_joint_states.velocities[index] * dt_seconds + 0.5 * desired_acc[index] * dt_seconds * dt_seconds; - if (pos_limit_trig_jnts[index]) ostr << joint_names_[index] << " "; } + std::ostringstream ostr; + for (auto jnt : limited_jnts_pos) ostr << jnt << " "; ostr << "\b \b"; // erase last character RCLCPP_WARN_STREAM_THROTTLE( node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, @@ -332,15 +331,10 @@ bool SimpleJointLimiter::on_enforce( // display limitations // if position limiting - if ( - std::count_if( - pos_limit_trig_jnts.begin(), pos_limit_trig_jnts.end(), [](bool trig) { return trig; }) > 0) + if (limited_jnts_pos.size() > 0) { std::ostringstream ostr; - for (size_t index = 0; index < number_of_joints_; ++index) - { - if (pos_limit_trig_jnts[index]) ostr << joint_names_[index] << " "; - } + for (auto jnt : limited_jnts_pos) ostr << jnt << " "; ostr << "\b \b"; // erase last character RCLCPP_WARN_STREAM_THROTTLE( node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, From 9c2bba7cb0316d616dba92bdb39c09835987236c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 4 Sep 2023 19:28:15 +0200 Subject: [PATCH 31/83] Fix formatting. --- joint_limits/src/simple_joint_limiter.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 17aaf4547d..c11c6ac9f1 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -84,8 +84,7 @@ bool SimpleJointLimiter::on_enforce( { // clamp input pos_cmd auto pos = std::clamp( - desired_pos[index], - joint_limits_[index].min_position, joint_limits_[index].max_position); + desired_pos[index], joint_limits_[index].min_position, joint_limits_[index].max_position); if (pos != desired_pos[index]) { desired_pos[index] = pos; @@ -209,8 +208,9 @@ bool SimpleJointLimiter::on_enforce( expected_pos[index] = current_joint_states.positions[index] + desired_vel[index] * dt_seconds; // if expected_pos over limit - auto pos = std::clamp(expected_pos[index], - joint_limits_[index].min_position, joint_limits_[index].max_position); + auto pos = std::clamp( + expected_pos[index], joint_limits_[index].min_position, + joint_limits_[index].max_position); if (pos != expected_pos[index]) { // TODO(gwalck) compute vel_cmd that would permit to slow down in time at full @@ -316,7 +316,6 @@ bool SimpleJointLimiter::on_enforce( desired_pos[index] = current_joint_states.positions[index] + current_joint_states.velocities[index] * dt_seconds + 0.5 * desired_acc[index] * dt_seconds * dt_seconds; - } std::ostringstream ostr; for (auto jnt : limited_jnts_pos) ostr << jnt << " "; From 6ca6594b3690a82997fd13abfcc78269c7043740 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 12 Sep 2023 18:43:00 +0200 Subject: [PATCH 32/83] Fix unstable limiter because velocity is caculated from position when position limit it not used. --- joint_limits/src/simple_joint_limiter.cpp | 28 ++++++++--------------- 1 file changed, 10 insertions(+), 18 deletions(-) diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index c11c6ac9f1..ff0cbec8bc 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -76,6 +76,10 @@ bool SimpleJointLimiter::on_enforce( { desired_pos[index] = desired_joint_states.positions[index]; } + if (has_vel_cmd) + { + desired_vel[index] = desired_joint_states.velocities[index]; + } // limit position if (joint_limits_[index].has_position_limits) @@ -90,24 +94,12 @@ bool SimpleJointLimiter::on_enforce( desired_pos[index] = pos; limited_jnts_pos.emplace_back(joint_names_[index]); } - } - } - - // prepare velocity - if (has_pos_cmd) - { - // priority to pos_cmd derivative over cmd_vel because we always have a pos_state so - // recomputing vel_cmd is fine compute expected_vel with already clamped pos_cmd and pos_state - // TODO(gwalck) handle the case of continuous joints with angle_wraparound to compute vel - // correctly - desired_vel[index] = - (desired_pos[index] - current_joint_states.positions[index]) / dt_seconds; - } - else - { - if (has_vel_cmd) - { - desired_vel[index] = desired_joint_states.velocities[index]; + // priority to pos_cmd derivative over cmd_vel because we always have a pos_state so + // recomputing vel_cmd is fine compute expected_vel with already clamped pos_cmd and pos_state + // TODO(gwalck) handle the case of continuous joints with angle_wraparound to compute vel + // correctly + desired_vel[index] = + (desired_pos[index] - current_joint_states.positions[index]) / dt_seconds; } } From 40228d9a8f073b764d9da4d8955bbf040a4a4a30 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 12 Sep 2023 19:01:24 +0200 Subject: [PATCH 33/83] 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 ff0cbec8bc..e0468394c0 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -95,7 +95,8 @@ bool SimpleJointLimiter::on_enforce( limited_jnts_pos.emplace_back(joint_names_[index]); } // priority to pos_cmd derivative over cmd_vel because we always have a pos_state so - // recomputing vel_cmd is fine compute expected_vel with already clamped pos_cmd and pos_state + // recomputing vel_cmd is fine compute expected_vel with already clamped pos_cmd and + // pos_state // TODO(gwalck) handle the case of continuous joints with angle_wraparound to compute vel // correctly desired_vel[index] = From 5d164b1f4e671a57d69b9d2e9ff1c9dd776a2a2f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 12 Sep 2023 18:43:00 +0200 Subject: [PATCH 34/83] Fix unstable limiter because velocity is caculated from position when position limit it not used. --- 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 e0468394c0..d177844d1c 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -97,7 +97,7 @@ bool SimpleJointLimiter::on_enforce( // priority to pos_cmd derivative over cmd_vel because we always have a pos_state so // recomputing vel_cmd is fine compute expected_vel with already clamped pos_cmd and // pos_state - // TODO(gwalck) handle the case of continuous joints with angle_wraparound to compute vel + // TODO(destogl) handle the case of continuous joints with angle_wraparound to compute vel // correctly desired_vel[index] = (desired_pos[index] - current_joint_states.positions[index]) / dt_seconds; From e99fca231df249f5a3c297c4c906dfdd52cbb529 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 14 Sep 2023 20:32:51 +0200 Subject: [PATCH 35/83] Make acceleration more stable in limiter. --- joint_limits/src/simple_joint_limiter.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index d177844d1c..e458fa9dda 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -59,9 +59,9 @@ bool SimpleJointLimiter::on_enforce( // 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_acc(number_of_joints_); - std::vector desired_vel(number_of_joints_); std::vector desired_pos(number_of_joints_); + std::vector desired_vel(number_of_joints_); + std::vector desired_acc(number_of_joints_); std::vector expected_vel(number_of_joints_); std::vector expected_pos(number_of_joints_); @@ -80,6 +80,10 @@ bool SimpleJointLimiter::on_enforce( { desired_vel[index] = desired_joint_states.velocities[index]; } + if (has_acc_cmd) + { + desired_acc[index] = desired_joint_states.accelerations[index]; + } // limit position if (joint_limits_[index].has_position_limits) @@ -120,6 +124,10 @@ bool SimpleJointLimiter::on_enforce( current_joint_states.positions[index] + desired_vel[index] * dt_seconds; limited_jnts_pos.emplace_back(joint_names_[index]); } + + // compute desired_acc when velocity is limited + desired_acc[index] = + (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; } } From 96bed7c77e6df7917ec711f4ffb784be43552bb9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Wed, 1 Nov 2023 16:21:34 +0100 Subject: [PATCH 36/83] Enable dynamic update of limits from parameters. --- joint_limits/CMakeLists.txt | 1 + .../joint_limits/joint_limiter_interface.hpp | 35 ++- .../joint_limits/joint_limits_rosparam.hpp | 243 +++++++++++++++++- joint_limits/package.xml | 1 + 4 files changed, 269 insertions(+), 11 deletions(-) diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 09afc4dfd4..2714b6e012 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -7,6 +7,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS pluginlib + realtime_tools rclcpp rclcpp_lifecycle trajectory_msgs diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 8a76b133bc..d9b97f082e 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -23,6 +23,7 @@ #include "joint_limits/joint_limits.hpp" #include "joint_limits/joint_limits_rosparam.hpp" #include "joint_limits/visibility_control.h" +#include "realtime_tools/realtime_buffer.h" #include "rclcpp/node.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" @@ -89,14 +90,39 @@ class JointLimiterInterface 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()); } + updated_limits_.writeFromNonRT(joint_limits_); + + auto on_parameter_event_callback = [this](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + std::vector updated_joint_limits = joint_limits_; + bool changed = false; + + for (size_t i = 0; i < number_of_joints_; ++i) + { + changed |= joint_limits::check_for_limits_update( + joint_names_[i], parameters, node_logging_itf_, updated_joint_limits[i]); + } + + if (changed) + { + updated_limits_.writeFromNonRT(updated_joint_limits); + RCLCPP_INFO(node_logging_itf_->get_logger(), "Limits are dynamically updated!"); + } + + return result; + }; + + parameter_callback_ = node_param_itf_->add_on_set_parameters_callback(on_parameter_event_callback); + if (result) { result = on_init(); } - // avoid linters output - (void)robot_description_topic; + (void)robot_description_topic; // avoid linters output return result; } @@ -140,6 +166,7 @@ class JointLimiterInterface trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) { // TODO(destogl): add checks if sizes of vectors and number of limits correspond. + joint_limits_ = *(updated_limits_.readFromRT()); return on_enforce(current_joint_states, desired_joint_states, dt); } @@ -171,6 +198,10 @@ class JointLimiterInterface rclcpp::Node::SharedPtr node_; rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_param_itf_; rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_itf_; + +private: + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_; + realtime_tools::RealtimeBuffer> updated_limits_; }; } // namespace joint_limits diff --git a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp index 2f32d49271..683f76bfa1 100644 --- a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp +++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp @@ -96,8 +96,6 @@ inline bool declare_parameters( auto_declare( param_itf, param_base_name + ".max_position", std::numeric_limits::quiet_NaN()); auto_declare(param_itf, param_base_name + ".has_velocity_limits", false); - auto_declare( - param_itf, param_base_name + ".min_velocity", std::numeric_limits::quiet_NaN()); auto_declare( param_itf, param_base_name + ".max_velocity", std::numeric_limits::quiet_NaN()); auto_declare(param_itf, param_base_name + ".has_acceleration_limits", false); @@ -237,7 +235,6 @@ inline bool get_joint_limits( !param_itf->has_parameter(param_base_name + ".min_position") && !param_itf->has_parameter(param_base_name + ".max_position") && !param_itf->has_parameter(param_base_name + ".has_velocity_limits") && - !param_itf->has_parameter(param_base_name + ".min_velocity") && !param_itf->has_parameter(param_base_name + ".max_velocity") && !param_itf->has_parameter(param_base_name + ".has_acceleration_limits") && !param_itf->has_parameter(param_base_name + ".max_acceleration") && @@ -247,12 +244,8 @@ inline bool get_joint_limits( !param_itf->has_parameter(param_base_name + ".max_jerk") && !param_itf->has_parameter(param_base_name + ".has_effort_limits") && !param_itf->has_parameter(param_base_name + ".max_effort") && - !param_itf->has_parameter(param_base_name + ".angle_wraparound") && - !param_itf->has_parameter(param_base_name + ".has_soft_limits") && - !param_itf->has_parameter(param_base_name + ".k_position") && - !param_itf->has_parameter(param_base_name + ".k_velocity") && - !param_itf->has_parameter(param_base_name + ".soft_lower_limit") && - !param_itf->has_parameter(param_base_name + ".soft_upper_limit")) + !param_itf->has_parameter(param_base_name + ".angle_wraparound") + ) { RCLCPP_ERROR( logging_itf->get_logger(), @@ -421,6 +414,172 @@ inline bool get_joint_limits( lifecycle_node->get_node_logging_interface(), limits); } +/** + * Check if any of updated parameters are related to JointLimits. + * + * This method is intended to be used in the parameters update callback. + * It is recommended that it's result is temporarily stored and synchronized with the JointLimits + * structure in the main loop. + * + * \param[in] joint_name Name of the joint whose limits should be checked. + * \param[in] parameters List of the parameters that should be checked if they belong to this + * structure and that are used for updating the internal values. + * \param[in] logging_itf node logging interface to provide log errors. + * \param[out] updated_limits structure with updated JointLimits. It is recommended that the + * currently used limits are stored into this structure before calling this method. + */ +inline bool check_for_limits_update(const std::string & joint_name, const std::vector & parameters, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, JointLimits & updated_limits) +{ + const std::string param_base_name = "joint_limits." + joint_name; + bool changed = false; + + // update first numerical values to make later checks for "has" limits members + for (auto & parameter : parameters) { + const std::string param_name = parameter.get_name(); + try { + if (param_name == param_base_name + ".min_position") + { + changed = updated_limits.min_position != parameter.get_value(); + updated_limits.min_position = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_position") + { + changed = updated_limits.max_position != parameter.get_value(); + updated_limits.max_position = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_velocity") + { + changed = updated_limits.max_velocity != parameter.get_value(); + updated_limits.max_velocity = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_acceleration") + { + changed = updated_limits.max_acceleration != parameter.get_value(); + updated_limits.max_acceleration = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_deceleration") + { + changed = updated_limits.max_deceleration != parameter.get_value(); + updated_limits.max_deceleration = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_jerk") + { + changed = updated_limits.max_jerk != parameter.get_value(); + updated_limits.max_jerk = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_effort") + { + changed = updated_limits.max_effort != parameter.get_value(); + updated_limits.max_effort = parameter.get_value(); + } + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + RCLCPP_WARN(logging_itf->get_logger(), "Please use the right type: %s", e.what()); + } + } + + for (auto & parameter : parameters) { + const std::string param_name = parameter.get_name(); + try { + if (param_name == param_base_name + ".has_position_limits") + { + updated_limits.has_position_limits = parameter.get_value(); + if (updated_limits.has_position_limits && (std::isnan(updated_limits.min_position) || std::isnan(updated_limits.max_position))) + { + RCLCPP_WARN(logging_itf->get_logger(), "PARAMETER NOT UPDATED: 'has_position_limits' flag can not be set if 'min_position' and 'max_position' are not set or not have valid double values."); + updated_limits.has_position_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".has_velocity_limits") + { + updated_limits.has_velocity_limits = parameter.get_value(); + if (updated_limits.has_velocity_limits && std::isnan(updated_limits.max_velocity)) + { + RCLCPP_WARN(logging_itf->get_logger(), "PARAMETER NOT UPDATED: 'has_velocity_limits' flag can not be set if 'min_velocity' and 'max_velocity' are not set or not have valid double values."); + updated_limits.has_velocity_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".has_acceleration_limits") + { + updated_limits.has_acceleration_limits = parameter.get_value(); + if (updated_limits.has_acceleration_limits && std::isnan(updated_limits.max_acceleration)) + { + RCLCPP_WARN(logging_itf->get_logger(), "PARAMETER NOT UPDATED: 'has_acceleration_limits' flag can not be set if 'max_acceleration' is not set or not have valid double values."); + updated_limits.has_acceleration_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".has_deceleration_limits") + { + updated_limits.has_deceleration_limits = parameter.get_value(); + if (updated_limits.has_deceleration_limits && std::isnan(updated_limits.max_deceleration)) + { + RCLCPP_WARN(logging_itf->get_logger(), "PARAMETER NOT UPDATED: 'has_deceleration_limits' flag can not be set if 'max_deceleration' is not set or not have valid double values."); + updated_limits.has_deceleration_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".has_jerk_limits") + { + updated_limits.has_jerk_limits = parameter.get_value(); + if (updated_limits.has_jerk_limits && std::isnan(updated_limits.max_jerk)) + { + RCLCPP_WARN(logging_itf->get_logger(), "PARAMETER NOT UPDATED: 'has_jerk_limits' flag can not be set if 'max_jerk' is not set or not have valid double values."); + updated_limits.has_jerk_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".has_effort_limits") + { + updated_limits.has_effort_limits = parameter.get_value(); + if (updated_limits.has_effort_limits && std::isnan(updated_limits.max_effort)) + { + RCLCPP_WARN(logging_itf->get_logger(), "PARAMETER NOT UPDATED: 'has_effort_limits' flag can not be set if 'max_effort' is not set or not have valid double values."); + updated_limits.has_effort_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".angle_wraparound") + { + updated_limits.angle_wraparound = parameter.get_value(); + if (updated_limits.angle_wraparound && updated_limits.has_position_limits) + { + RCLCPP_WARN(logging_itf->get_logger(), "PARAMETER NOT UPDATED: 'angle_wraparound' flag can not be set if 'has_position_limits' flag is set."); + updated_limits.angle_wraparound = false; + } + else + { + changed = true; + } + } + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + RCLCPP_WARN(logging_itf->get_logger(), "PARAMETER NOT UPDATED: Please use the right type: %s", e.what()); + } + } + + return changed; +} + + /// Populate a SoftJointLimits instance from the ROS parameter server. /** * It is assumed that the parameter structure is the following: @@ -550,6 +709,72 @@ inline bool get_joint_limits( lifecycle_node->get_node_logging_interface(), soft_limits); } +/** + * Check if any of updated parameters are related to SoftJointLimits. + * + * This method is intended to be used in the parameters update callback. + * It is recommended that it's result is temporarily stored and synchronized with the SoftJointLimits + * structure in the main loop. + * + * \param[in] joint_name Name of the joint whose limits should be checked. + * \param[in] parameters List of the parameters that should be checked if they belong to this + * structure and that are used for updating the internal values. + * \param[in] logging_itf node logging interface to provide log errors. + * \param[out] updated_limits structure with updated SoftJointLimits. It is recommended that the + * currently used limits are stored into this structure before calling this method. + */ +inline bool check_for_limits_update(const std::string & joint_name, const std::vector & parameters, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, SoftJointLimits & updated_limits) +{ + const std::string param_base_name = "joint_limits." + joint_name; + bool changed = false; + + for (auto & parameter : parameters) { + const std::string param_name = parameter.get_name(); + try { + if (param_name == param_base_name + ".has_soft_limits") + { + if (!parameter.get_value()) + { + RCLCPP_WARN(logging_itf->get_logger(), "Parameter 'has_soft_limits' is not set, therefore the limits will not be updated!"); + return false; + } + } + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + RCLCPP_INFO(logging_itf->get_logger(), "Please use the right type: %s", e.what()); + } + } + + for (auto & parameter : parameters) { + const std::string param_name = parameter.get_name(); + try { + if (param_name == param_base_name + ".k_position") + { + changed = updated_limits.k_position != parameter.get_value(); + updated_limits.k_position = parameter.get_value(); + } + else if (param_name == param_base_name + ".k_velocity") + { + changed = updated_limits.k_velocity != parameter.get_value(); + updated_limits.k_velocity = parameter.get_value(); + } + else if (param_name == param_base_name + ".soft_lower_limit") + { + changed = updated_limits.min_position != parameter.get_value(); + updated_limits.min_position = parameter.get_value(); + } + else if (param_name == param_base_name + ".soft_upper_limit") + { + changed = updated_limits.max_position != parameter.get_value(); + updated_limits.max_position = parameter.get_value(); + } + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + RCLCPP_INFO(logging_itf->get_logger(), "Please use the right type: %s", e.what()); + } + } + + return changed; +} + } // namespace joint_limits #endif // JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ diff --git a/joint_limits/package.xml b/joint_limits/package.xml index f224cd374c..178752f74a 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -16,6 +16,7 @@ backward_ros pluginlib + realtime_tools rclcpp rclcpp_lifecycle trajectory_msgs From 47389fd08f1c9de86c5f27133f8b8fe3adfe2bdd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Wed, 1 Nov 2023 17:33:42 +0100 Subject: [PATCH 37/83] Fix formatting. --- .../joint_limits/joint_limiter_interface.hpp | 9 +- .../joint_limits/joint_limits_rosparam.hpp | 105 +++++++++++++----- joint_limits/src/simple_joint_limiter.cpp | 2 +- 3 files changed, 82 insertions(+), 34 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index d9b97f082e..961101ba30 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -23,9 +23,9 @@ #include "joint_limits/joint_limits.hpp" #include "joint_limits/joint_limits_rosparam.hpp" #include "joint_limits/visibility_control.h" -#include "realtime_tools/realtime_buffer.h" #include "rclcpp/node.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "realtime_tools/realtime_buffer.h" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" namespace joint_limits @@ -92,7 +92,8 @@ class JointLimiterInterface } updated_limits_.writeFromNonRT(joint_limits_); - auto on_parameter_event_callback = [this](const std::vector & parameters) { + auto on_parameter_event_callback = [this](const std::vector & parameters) + { rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -114,8 +115,8 @@ class JointLimiterInterface return result; }; - parameter_callback_ = node_param_itf_->add_on_set_parameters_callback(on_parameter_event_callback); - + parameter_callback_ = + node_param_itf_->add_on_set_parameters_callback(on_parameter_event_callback); if (result) { diff --git a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp index 683f76bfa1..67e8660c62 100644 --- a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp +++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp @@ -244,8 +244,7 @@ inline bool get_joint_limits( !param_itf->has_parameter(param_base_name + ".max_jerk") && !param_itf->has_parameter(param_base_name + ".has_effort_limits") && !param_itf->has_parameter(param_base_name + ".max_effort") && - !param_itf->has_parameter(param_base_name + ".angle_wraparound") - ) + !param_itf->has_parameter(param_base_name + ".angle_wraparound")) { RCLCPP_ERROR( logging_itf->get_logger(), @@ -428,15 +427,20 @@ inline bool get_joint_limits( * \param[out] updated_limits structure with updated JointLimits. It is recommended that the * currently used limits are stored into this structure before calling this method. */ -inline bool check_for_limits_update(const std::string & joint_name, const std::vector & parameters, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, JointLimits & updated_limits) +inline bool check_for_limits_update( + const std::string & joint_name, const std::vector & parameters, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, + JointLimits & updated_limits) { const std::string param_base_name = "joint_limits." + joint_name; bool changed = false; // update first numerical values to make later checks for "has" limits members - for (auto & parameter : parameters) { + for (auto & parameter : parameters) + { const std::string param_name = parameter.get_name(); - try { + try + { if (param_name == param_base_name + ".min_position") { changed = updated_limits.min_position != parameter.get_value(); @@ -472,20 +476,29 @@ inline bool check_for_limits_update(const std::string & joint_name, const std::v changed = updated_limits.max_effort != parameter.get_value(); updated_limits.max_effort = parameter.get_value(); } - } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { RCLCPP_WARN(logging_itf->get_logger(), "Please use the right type: %s", e.what()); } } - for (auto & parameter : parameters) { + for (auto & parameter : parameters) + { const std::string param_name = parameter.get_name(); - try { + try + { if (param_name == param_base_name + ".has_position_limits") { updated_limits.has_position_limits = parameter.get_value(); - if (updated_limits.has_position_limits && (std::isnan(updated_limits.min_position) || std::isnan(updated_limits.max_position))) + if ( + updated_limits.has_position_limits && + (std::isnan(updated_limits.min_position) || std::isnan(updated_limits.max_position))) { - RCLCPP_WARN(logging_itf->get_logger(), "PARAMETER NOT UPDATED: 'has_position_limits' flag can not be set if 'min_position' and 'max_position' are not set or not have valid double values."); + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_position_limits' flag can not be set if 'min_position' " + "and 'max_position' are not set or not have valid double values."); updated_limits.has_position_limits = false; } else @@ -498,7 +511,10 @@ inline bool check_for_limits_update(const std::string & joint_name, const std::v updated_limits.has_velocity_limits = parameter.get_value(); if (updated_limits.has_velocity_limits && std::isnan(updated_limits.max_velocity)) { - RCLCPP_WARN(logging_itf->get_logger(), "PARAMETER NOT UPDATED: 'has_velocity_limits' flag can not be set if 'min_velocity' and 'max_velocity' are not set or not have valid double values."); + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_velocity_limits' flag can not be set if 'min_velocity' " + "and 'max_velocity' are not set or not have valid double values."); updated_limits.has_velocity_limits = false; } else @@ -511,7 +527,10 @@ inline bool check_for_limits_update(const std::string & joint_name, const std::v updated_limits.has_acceleration_limits = parameter.get_value(); if (updated_limits.has_acceleration_limits && std::isnan(updated_limits.max_acceleration)) { - RCLCPP_WARN(logging_itf->get_logger(), "PARAMETER NOT UPDATED: 'has_acceleration_limits' flag can not be set if 'max_acceleration' is not set or not have valid double values."); + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_acceleration_limits' flag can not be set if " + "'max_acceleration' is not set or not have valid double values."); updated_limits.has_acceleration_limits = false; } else @@ -524,7 +543,10 @@ inline bool check_for_limits_update(const std::string & joint_name, const std::v updated_limits.has_deceleration_limits = parameter.get_value(); if (updated_limits.has_deceleration_limits && std::isnan(updated_limits.max_deceleration)) { - RCLCPP_WARN(logging_itf->get_logger(), "PARAMETER NOT UPDATED: 'has_deceleration_limits' flag can not be set if 'max_deceleration' is not set or not have valid double values."); + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_deceleration_limits' flag can not be set if " + "'max_deceleration' is not set or not have valid double values."); updated_limits.has_deceleration_limits = false; } else @@ -537,7 +559,10 @@ inline bool check_for_limits_update(const std::string & joint_name, const std::v updated_limits.has_jerk_limits = parameter.get_value(); if (updated_limits.has_jerk_limits && std::isnan(updated_limits.max_jerk)) { - RCLCPP_WARN(logging_itf->get_logger(), "PARAMETER NOT UPDATED: 'has_jerk_limits' flag can not be set if 'max_jerk' is not set or not have valid double values."); + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_jerk_limits' flag can not be set if 'max_jerk' is not set " + "or not have valid double values."); updated_limits.has_jerk_limits = false; } else @@ -550,7 +575,10 @@ inline bool check_for_limits_update(const std::string & joint_name, const std::v updated_limits.has_effort_limits = parameter.get_value(); if (updated_limits.has_effort_limits && std::isnan(updated_limits.max_effort)) { - RCLCPP_WARN(logging_itf->get_logger(), "PARAMETER NOT UPDATED: 'has_effort_limits' flag can not be set if 'max_effort' is not set or not have valid double values."); + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_effort_limits' flag can not be set if 'max_effort' is not " + "set or not have valid double values."); updated_limits.has_effort_limits = false; } else @@ -563,7 +591,10 @@ inline bool check_for_limits_update(const std::string & joint_name, const std::v updated_limits.angle_wraparound = parameter.get_value(); if (updated_limits.angle_wraparound && updated_limits.has_position_limits) { - RCLCPP_WARN(logging_itf->get_logger(), "PARAMETER NOT UPDATED: 'angle_wraparound' flag can not be set if 'has_position_limits' flag is set."); + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'angle_wraparound' flag can not be set if " + "'has_position_limits' flag is set."); updated_limits.angle_wraparound = false; } else @@ -571,15 +602,18 @@ inline bool check_for_limits_update(const std::string & joint_name, const std::v changed = true; } } - } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { - RCLCPP_WARN(logging_itf->get_logger(), "PARAMETER NOT UPDATED: Please use the right type: %s", e.what()); + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { + RCLCPP_WARN( + logging_itf->get_logger(), "PARAMETER NOT UPDATED: Please use the right type: %s", + e.what()); } } return changed; } - /// Populate a SoftJointLimits instance from the ROS parameter server. /** * It is assumed that the parameter structure is the following: @@ -713,8 +747,8 @@ inline bool get_joint_limits( * Check if any of updated parameters are related to SoftJointLimits. * * This method is intended to be used in the parameters update callback. - * It is recommended that it's result is temporarily stored and synchronized with the SoftJointLimits - * structure in the main loop. + * It is recommended that it's result is temporarily stored and synchronized with the + * SoftJointLimits structure in the main loop. * * \param[in] joint_name Name of the joint whose limits should be checked. * \param[in] parameters List of the parameters that should be checked if they belong to this @@ -723,30 +757,41 @@ inline bool get_joint_limits( * \param[out] updated_limits structure with updated SoftJointLimits. It is recommended that the * currently used limits are stored into this structure before calling this method. */ -inline bool check_for_limits_update(const std::string & joint_name, const std::vector & parameters, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, SoftJointLimits & updated_limits) +inline bool check_for_limits_update( + const std::string & joint_name, const std::vector & parameters, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, + SoftJointLimits & updated_limits) { const std::string param_base_name = "joint_limits." + joint_name; bool changed = false; - for (auto & parameter : parameters) { + for (auto & parameter : parameters) + { const std::string param_name = parameter.get_name(); - try { + try + { if (param_name == param_base_name + ".has_soft_limits") { if (!parameter.get_value()) { - RCLCPP_WARN(logging_itf->get_logger(), "Parameter 'has_soft_limits' is not set, therefore the limits will not be updated!"); + RCLCPP_WARN( + logging_itf->get_logger(), + "Parameter 'has_soft_limits' is not set, therefore the limits will not be updated!"); return false; } } - } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { RCLCPP_INFO(logging_itf->get_logger(), "Please use the right type: %s", e.what()); } } - for (auto & parameter : parameters) { + for (auto & parameter : parameters) + { const std::string param_name = parameter.get_name(); - try { + try + { if (param_name == param_base_name + ".k_position") { changed = updated_limits.k_position != parameter.get_value(); @@ -767,7 +812,9 @@ inline bool check_for_limits_update(const std::string & joint_name, const std::v changed = updated_limits.max_position != parameter.get_value(); updated_limits.max_position = parameter.get_value(); } - } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { RCLCPP_INFO(logging_itf->get_logger(), "Please use the right type: %s", e.what()); } } diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index e458fa9dda..32dbcfa1d0 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -127,7 +127,7 @@ bool SimpleJointLimiter::on_enforce( // compute desired_acc when velocity is limited desired_acc[index] = - (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; + (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; } } From af5e00417248a55fc4b1770ddafe7adf1d58af1f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 16 Nov 2023 19:52:00 +0100 Subject: [PATCH 38/83] Debug position limiting in the simiple joint limiter. --- .../joint_limits/joint_limiter_interface.hpp | 1 + joint_limits/src/simple_joint_limiter.cpp | 38 +++++++++++++------ 2 files changed, 27 insertions(+), 12 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 961101ba30..0cabaaa9cf 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -187,6 +187,7 @@ class JointLimiterInterface * \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. + * \returns true if limits are enforced, otherwise false. */ JOINT_LIMITS_PUBLIC virtual bool on_enforce( trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 32dbcfa1d0..efb90092f2 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -22,6 +22,7 @@ #include "rcutils/logging_macros.h" constexpr size_t ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops +constexpr double VALUE_CONSIDERED_ZERO = 1e-10; namespace joint_limits { @@ -30,6 +31,8 @@ bool SimpleJointLimiter::on_enforce( trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) { + bool limits_enforced = false; + const auto dt_seconds = dt.seconds(); // negative or null is not allowed if (dt_seconds <= 0.0) return false; @@ -85,10 +88,10 @@ bool SimpleJointLimiter::on_enforce( desired_acc[index] = desired_joint_states.accelerations[index]; } - // limit position - if (joint_limits_[index].has_position_limits) + if (has_pos_cmd) { - if (has_pos_cmd) + // limit position + if (joint_limits_[index].has_position_limits) { // clamp input pos_cmd auto pos = std::clamp( @@ -97,14 +100,20 @@ bool SimpleJointLimiter::on_enforce( { desired_pos[index] = pos; limited_jnts_pos.emplace_back(joint_names_[index]); + limits_enforced = true; } - // priority to pos_cmd derivative over cmd_vel because we always have a pos_state so - // recomputing vel_cmd is fine compute expected_vel with already clamped pos_cmd and - // pos_state - // TODO(destogl) handle the case of continuous joints with angle_wraparound to compute vel - // correctly - desired_vel[index] = - (desired_pos[index] - current_joint_states.positions[index]) / dt_seconds; + } + // priority to pos_cmd derivative over cmd_vel when not defined. If done always then we might + // get jumps in the velocity based on the system's dynamics. Position limit clamping is done + // below once again. + // TODO(destogl) handle the case of continuous joints with angle_wraparound to compute vel + // correctly + const double position_difference = desired_pos[index] - current_joint_states.positions[index]; + if ( + std::fabs(position_difference) > VALUE_CONSIDERED_ZERO && + std::fabs(desired_vel[index]) <= VALUE_CONSIDERED_ZERO) + { + desired_vel[index] = position_difference / dt_seconds; } } @@ -116,13 +125,13 @@ bool SimpleJointLimiter::on_enforce( { desired_vel[index] = std::copysign(joint_limits_[index].max_velocity, desired_vel[index]); limited_jnts_vel.emplace_back(joint_names_[index]); + limits_enforced = true; // recompute pos_cmd if needed if (has_pos_cmd) { desired_pos[index] = current_joint_states.positions[index] + desired_vel[index] * dt_seconds; - limited_jnts_pos.emplace_back(joint_names_[index]); } // compute desired_acc when velocity is limited @@ -147,6 +156,7 @@ bool SimpleJointLimiter::on_enforce( { acc[index] = std::copysign(max_acc_or_dec, acc[index]); limited_jnts.emplace_back(joint_names_[index]); + limits_enforced = true; return true; } else @@ -222,6 +232,7 @@ bool SimpleJointLimiter::on_enforce( desired_vel[index] = (expected_pos[index] - current_joint_states.positions[index]) / dt_seconds; limited_jnts_pos.emplace_back(joint_names_[index]); + limits_enforced = true; } } @@ -261,6 +272,7 @@ bool SimpleJointLimiter::on_enforce( { limited_jnts_pos.emplace_back(joint_names_[index]); braking_near_position_limit_triggered = true; + limits_enforced = true; } else { @@ -278,6 +290,7 @@ bool SimpleJointLimiter::on_enforce( { limited_jnts_pos.emplace_back(joint_names_[index]); braking_near_position_limit_triggered = true; + limits_enforced = true; } // else no need to slow down. in worse case we won't hit the limit at current velocity } @@ -374,7 +387,8 @@ bool SimpleJointLimiter::on_enforce( if (has_pos_cmd) desired_joint_states.positions = desired_pos; if (has_vel_cmd) desired_joint_states.velocities = desired_vel; if (has_acc_cmd) desired_joint_states.accelerations = desired_acc; - return true; + + return limits_enforced; } } // namespace joint_limits From 4f868b1f3aad3e81389621018fb742652799cf9c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Tue, 21 Nov 2023 18:02:31 +0100 Subject: [PATCH 39/83] Update tests after changes. --- joint_limits/test/test_simple_joint_limiter.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/joint_limits/test/test_simple_joint_limiter.cpp b/joint_limits/test/test_simple_joint_limiter.cpp index f448c03562..9ce2e63480 100644 --- a/joint_limits/test/test_simple_joint_limiter.cpp +++ b/joint_limits/test/test_simple_joint_limiter.cpp @@ -74,7 +74,7 @@ TEST_F(SimpleJointLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce_fai } } -TEST_F(SimpleJointLimiterTest, when_no_posstate_expect_enforce_fail) +TEST_F(SimpleJointLimiterTest, when_no_posstate_expect_enforce_false) { SetupNode("simple_joint_limiter"); Load(); @@ -88,10 +88,14 @@ TEST_F(SimpleJointLimiterTest, when_no_posstate_expect_enforce_fail) // test no position interface current_joint_states_.positions.clear(); ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // also fail if out fo limits + desired_joint_states_.positions[0] = 20.0; + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); } } -TEST_F(SimpleJointLimiterTest, when_no_velstate_expect_enforce_succeed) +TEST_F(SimpleJointLimiterTest, when_no_velstate_expect_limiting) { SetupNode("simple_joint_limiter"); Load(); @@ -104,6 +108,9 @@ TEST_F(SimpleJointLimiterTest, when_no_velstate_expect_enforce_succeed) rclcpp::Duration period(1, 0); // 1 second // test no vel interface current_joint_states_.velocities.clear(); + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + // also fail if out fo limits + desired_joint_states_.positions[0] = 20.0; ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); } } @@ -124,7 +131,7 @@ TEST_F(SimpleJointLimiterTest, when_within_limits_expect_no_limits_applied) // within limits desired_joint_states_.positions[0] = 1.0; desired_joint_states_.velocities[0] = 1.0; // valid pos derivatite as well - ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); // check if no limits applied CHECK_STATE_SINGLE_JOINT( @@ -313,10 +320,12 @@ TEST_F(SimpleJointLimiterTest, when_position_close_to_pos_limit_expect_decelerat desired_joint_states_.velocities[0] = 1.5; // this setup requires 0.15 distance to stop, and 0.2 seconds (so 4 cycles at 0.05) + std::vector expected_ret = {true, true, true, false}; 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)); + // expect limits applied until the end stop + ASSERT_EQ(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period), expected_ret[i]); ASSERT_LE( desired_joint_states_.velocities[0], From 94da9975f29d79bd553b2f0c34099d81781e027a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 7 Dec 2023 16:41:49 +0100 Subject: [PATCH 40/83] Fixup shadowing variable. --- .../include/joint_limits/joint_limiter_interface.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 0cabaaa9cf..370ebf1f9b 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -94,8 +94,8 @@ class JointLimiterInterface auto on_parameter_event_callback = [this](const std::vector & parameters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; + rcl_interfaces::msg::SetParametersResult set_parameters_result; + set_parameters_result.successful = true; std::vector updated_joint_limits = joint_limits_; bool changed = false; @@ -112,7 +112,7 @@ class JointLimiterInterface RCLCPP_INFO(node_logging_itf_->get_logger(), "Limits are dynamically updated!"); } - return result; + return set_parameters_result; }; parameter_callback_ = From bcf42aa1e833b3b9a3d40f2acb147d5f487c6279 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 8 Dec 2023 13:56:27 +0100 Subject: [PATCH 41/83] rename to JointSaturationLimiter --- .../types/hardware_interface_type_values.hpp | 2 - .../joint_limits/simple_joint_limiter.hpp | 10 ++--- joint_limits/joint_limiters.xml | 4 +- joint_limits/src/simple_joint_limiter.cpp | 4 +- .../test/test_simple_joint_limiter.cpp | 44 ++++++++++--------- .../test/test_simple_joint_limiter.hpp | 6 +-- 6 files changed, 35 insertions(+), 35 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 c49925b701..27b5207a0f 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,8 +15,6 @@ #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 name diff --git a/joint_limits/include/joint_limits/simple_joint_limiter.hpp b/joint_limits/include/joint_limits/simple_joint_limiter.hpp index 6002a0b4f7..1b9bfd8cad 100644 --- a/joint_limits/include/joint_limits/simple_joint_limiter.hpp +++ b/joint_limits/include/joint_limits/simple_joint_limiter.hpp @@ -27,14 +27,14 @@ namespace joint_limits { template -class SimpleJointLimiter : public JointLimiterInterface +class JointSaturationLimiter : public JointLimiterInterface { public: /** \brief Constructor */ - JOINT_LIMITS_PUBLIC SimpleJointLimiter(); + JOINT_LIMITS_PUBLIC JointSaturationLimiter(); /** \brief Destructor */ - JOINT_LIMITS_PUBLIC ~SimpleJointLimiter(); + JOINT_LIMITS_PUBLIC ~JointSaturationLimiter(); JOINT_LIMITS_PUBLIC bool on_enforce( trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, @@ -46,13 +46,13 @@ class SimpleJointLimiter : public JointLimiterInterface }; template -SimpleJointLimiter::SimpleJointLimiter() : JointLimiterInterface() +JointSaturationLimiter::JointSaturationLimiter() : JointLimiterInterface() { clock_ = std::make_shared(rclcpp::Clock(RCL_ROS_TIME)); } template -SimpleJointLimiter::~SimpleJointLimiter() +JointSaturationLimiter::~JointSaturationLimiter() { } diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml index 036b39859a..e4b41f878d 100644 --- a/joint_limits/joint_limiters.xml +++ b/joint_limits/joint_limiters.xml @@ -1,7 +1,7 @@ - Simple joint limiter using clamping approach. Warning: clamping can cause discontinuities. diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index efb90092f2..a5e2e70926 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -27,7 +27,7 @@ constexpr double VALUE_CONSIDERED_ZERO = 1e-10; namespace joint_limits { template <> -bool SimpleJointLimiter::on_enforce( +bool JointSaturationLimiter::on_enforce( trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) { @@ -396,5 +396,5 @@ bool SimpleJointLimiter::on_enforce( #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - joint_limits::SimpleJointLimiter, + joint_limits::JointSaturationLimiter, joint_limits::JointLimiterInterface) diff --git a/joint_limits/test/test_simple_joint_limiter.cpp b/joint_limits/test/test_simple_joint_limiter.cpp index 9ce2e63480..363506b027 100644 --- a/joint_limits/test/test_simple_joint_limiter.cpp +++ b/joint_limits/test/test_simple_joint_limiter.cpp @@ -16,9 +16,9 @@ #include "test_simple_joint_limiter.hpp" -TEST_F(SimpleJointLimiterTest, when_loading_limiter_plugin_expect_loaded) +TEST_F(JointSaturationLimiterTest, when_loading_limiter_plugin_expect_loaded) { - // Test SimpleJointLimiter loading + // Test JointSaturationLimiter loading ASSERT_NO_THROW( joint_limiter_ = std::unique_ptr( joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_))); @@ -26,7 +26,7 @@ TEST_F(SimpleJointLimiterTest, when_loading_limiter_plugin_expect_loaded) } /* FIXME: currently init does not check if limit parameters exist for the requested joint -TEST_F(SimpleJointLimiterTest, when_joint_not_found_expect_init_fail) +TEST_F(JointSaturationLimiterTest, when_joint_not_found_expect_init_fail) { SetupNode("simple_joint_limiter"); Load(); @@ -40,7 +40,7 @@ TEST_F(SimpleJointLimiterTest, when_joint_not_found_expect_init_fail) } */ -TEST_F(SimpleJointLimiterTest, when_invalid_dt_expect_enforce_fail) +TEST_F(JointSaturationLimiterTest, when_invalid_dt_expect_enforce_fail) { SetupNode("simple_joint_limiter"); Load(); @@ -54,7 +54,7 @@ TEST_F(SimpleJointLimiterTest, when_invalid_dt_expect_enforce_fail) } } -TEST_F(SimpleJointLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce_fail) +TEST_F(JointSaturationLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce_fail) { SetupNode("simple_joint_limiter"); Load(); @@ -74,7 +74,7 @@ TEST_F(SimpleJointLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce_fai } } -TEST_F(SimpleJointLimiterTest, when_no_posstate_expect_enforce_false) +TEST_F(JointSaturationLimiterTest, when_no_posstate_expect_enforce_false) { SetupNode("simple_joint_limiter"); Load(); @@ -89,13 +89,13 @@ TEST_F(SimpleJointLimiterTest, when_no_posstate_expect_enforce_false) current_joint_states_.positions.clear(); ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - // also fail if out fo limits + // also fail if out of limits desired_joint_states_.positions[0] = 20.0; ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); } } -TEST_F(SimpleJointLimiterTest, when_no_velstate_expect_limiting) +TEST_F(JointSaturationLimiterTest, when_no_velstate_expect_limiting) { SetupNode("simple_joint_limiter"); Load(); @@ -109,13 +109,13 @@ TEST_F(SimpleJointLimiterTest, when_no_velstate_expect_limiting) // test no vel interface current_joint_states_.velocities.clear(); ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - // also fail if out fo limits + // also fail if out of limits desired_joint_states_.positions[0] = 20.0; ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); } } -TEST_F(SimpleJointLimiterTest, when_within_limits_expect_no_limits_applied) +TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied) { SetupNode("simple_joint_limiter"); Load(); @@ -143,7 +143,7 @@ TEST_F(SimpleJointLimiterTest, when_within_limits_expect_no_limits_applied) } } -TEST_F(SimpleJointLimiterTest, when_posvel_leads_to_vel_exceeded_expect_limits_enforced) +TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_vel_exceeded_expect_limits_enforced) { SetupNode("simple_joint_limiter"); Load(); @@ -187,7 +187,7 @@ TEST_F(SimpleJointLimiterTest, when_posvel_leads_to_vel_exceeded_expect_limits_e } } -TEST_F(SimpleJointLimiterTest, when_posonly_leads_to_vel_exceeded_expect_pos_acc_enforced) +TEST_F(JointSaturationLimiterTest, when_posonly_leads_to_vel_exceeded_expect_pos_acc_enforced) { SetupNode("simple_joint_limiter"); Load(); @@ -227,7 +227,7 @@ TEST_F(SimpleJointLimiterTest, when_posonly_leads_to_vel_exceeded_expect_pos_acc } } -TEST_F(SimpleJointLimiterTest, when_velonly_leads_to_vel_exceeded_expect_vel_acc_enforced) +TEST_F(JointSaturationLimiterTest, when_velonly_leads_to_vel_exceeded_expect_vel_acc_enforced) { SetupNode("simple_joint_limiter"); Load(); @@ -273,7 +273,7 @@ TEST_F(SimpleJointLimiterTest, when_velonly_leads_to_vel_exceeded_expect_vel_acc } } -TEST_F(SimpleJointLimiterTest, when_posonly_exceeded_expect_pos_enforced) +TEST_F(JointSaturationLimiterTest, when_posonly_exceeded_expect_pos_enforced) { SetupNode("simple_joint_limiter"); Load(); @@ -300,7 +300,7 @@ TEST_F(SimpleJointLimiterTest, when_posonly_exceeded_expect_pos_enforced) } } -TEST_F(SimpleJointLimiterTest, when_position_close_to_pos_limit_expect_deceleration_enforced) +TEST_F(JointSaturationLimiterTest, when_position_close_to_pos_limit_expect_deceleration_enforced) { SetupNode("simple_joint_limiter"); Load(); @@ -325,7 +325,9 @@ TEST_F(SimpleJointLimiterTest, when_position_close_to_pos_limit_expect_decelerat { auto previous_vel_request = desired_joint_states_.velocities[0]; // expect limits applied until the end stop - ASSERT_EQ(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period), expected_ret[i]); + ASSERT_EQ( + joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period), + expected_ret[i]); ASSERT_LE( desired_joint_states_.velocities[0], @@ -344,7 +346,7 @@ TEST_F(SimpleJointLimiterTest, when_position_close_to_pos_limit_expect_decelerat } } -TEST_F(SimpleJointLimiterTest, when_posvel_leads_to_acc_exceeded_expect_limits_enforced) +TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_acc_exceeded_expect_limits_enforced) { SetupNode("simple_joint_limiter"); Load(); @@ -373,7 +375,7 @@ TEST_F(SimpleJointLimiterTest, when_posvel_leads_to_acc_exceeded_expect_limits_e } } -TEST_F(SimpleJointLimiterTest, when_posonly_leads_to_acc_exceeded_expect_limits_enforced) +TEST_F(JointSaturationLimiterTest, when_posonly_leads_to_acc_exceeded_expect_limits_enforced) { SetupNode("simple_joint_limiter"); Load(); @@ -402,7 +404,7 @@ TEST_F(SimpleJointLimiterTest, when_posonly_leads_to_acc_exceeded_expect_limits_ } } -TEST_F(SimpleJointLimiterTest, when_velonly_leads_to_acc_exceeded_expect_limits_enforced) +TEST_F(JointSaturationLimiterTest, when_velonly_leads_to_acc_exceeded_expect_limits_enforced) { SetupNode("simple_joint_limiter"); Load(); @@ -428,7 +430,7 @@ TEST_F(SimpleJointLimiterTest, when_velonly_leads_to_acc_exceeded_expect_limits_ } } -TEST_F(SimpleJointLimiterTest, when_deceleration_exceeded_expect_dec_enforced) +TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_expect_dec_enforced) { SetupNode("simple_joint_limiter"); Load(); @@ -458,7 +460,7 @@ TEST_F(SimpleJointLimiterTest, when_deceleration_exceeded_expect_dec_enforced) } } -TEST_F(SimpleJointLimiterTest, when_deceleration_exceeded_with_no_maxdec_expect_acc_enforced) +TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_with_no_maxdec_expect_acc_enforced) { SetupNode("simple_joint_limiter_nodeclimit"); Load(); diff --git a/joint_limits/test/test_simple_joint_limiter.hpp b/joint_limits/test/test_simple_joint_limiter.hpp index 27597eb628..0b01cdd8e5 100644 --- a/joint_limits/test/test_simple_joint_limiter.hpp +++ b/joint_limits/test/test_simple_joint_limiter.hpp @@ -36,7 +36,7 @@ const double COMMON_THRESHOLD = 0.0011; using JointLimiter = joint_limits::JointLimiterInterface; -class SimpleJointLimiterTest : public ::testing::Test +class JointSaturationLimiterTest : public ::testing::Test { public: void SetUp() override @@ -77,8 +77,8 @@ class SimpleJointLimiterTest : public ::testing::Test current_joint_states_.velocities[0] += desired_joint_states_.accelerations[0] * dt; } - SimpleJointLimiterTest() - : joint_limiter_type_("joint_limits/SimpleJointLimiter"), + JointSaturationLimiterTest() + : joint_limiter_type_("joint_limits/JointSaturationLimiter"), joint_limiter_loader_( "joint_limits", "joint_limits::JointLimiterInterface") { From fde4814eebcf8de3b9f2fdc9e2dc5d3812403502 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 11 Dec 2023 09:29:37 +0100 Subject: [PATCH 42/83] rename files as well --- joint_limits/CMakeLists.txt | 26 +++++++------- ...miter.hpp => joint_saturation_limiter.hpp} | 6 ++-- joint_limits/joint_limiters.xml | 2 +- ...miter.cpp => joint_saturation_limiter.cpp} | 2 +- ...ml => joint_saturation_limiter_param.yaml} | 4 +-- ....cpp => test_joint_saturation_limiter.cpp} | 34 +++++++++---------- ....hpp => test_joint_saturation_limiter.hpp} | 6 ++-- 7 files changed, 40 insertions(+), 40 deletions(-) rename joint_limits/include/joint_limits/{simple_joint_limiter.hpp => joint_saturation_limiter.hpp} (91%) rename joint_limits/src/{simple_joint_limiter.cpp => joint_saturation_limiter.cpp} (99%) rename joint_limits/test/{simple_joint_limiter_param.yaml => joint_saturation_limiter_param.yaml} (93%) rename joint_limits/test/{test_simple_joint_limiter.cpp => test_joint_saturation_limiter.cpp} (95%) rename joint_limits/test/{test_simple_joint_limiter.hpp => test_joint_saturation_limiter.hpp} (96%) diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 6cb25dac8a..f1177db2a9 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -41,18 +41,18 @@ ament_target_dependencies(joint_limiter_interface PUBLIC ${THIS_PACKAGE_INCLUDE_ target_compile_definitions(joint_limiter_interface PRIVATE "JOINT_LIMITS_BUILDING_DLL") -add_library(simple_joint_limiter SHARED - src/simple_joint_limiter.cpp +add_library(joint_saturation_limiter SHARED + src/joint_saturation_limiter.cpp ) -target_compile_features(simple_joint_limiter PUBLIC cxx_std_17) -target_include_directories(simple_joint_limiter PUBLIC +target_compile_features(joint_saturation_limiter PUBLIC cxx_std_17) +target_include_directories(joint_saturation_limiter PUBLIC $ $ ) -ament_target_dependencies(simple_joint_limiter PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(joint_saturation_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") +target_compile_definitions(joint_saturation_limiter PRIVATE "JOINT_LIMITS_BUILDING_DLL") pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml) @@ -73,17 +73,17 @@ if(BUILD_TESTING) DESTINATION lib/joint_limits ) install( - FILES test/joint_limits_rosparam.yaml test/simple_joint_limiter_param.yaml + FILES test/joint_limits_rosparam.yaml test/joint_saturation_limiter_param.yaml DESTINATION share/joint_limits/test ) - 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 + add_rostest_with_parameters_gmock(test_joint_saturation_limiter test/test_joint_saturation_limiter.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/joint_saturation_limiter_param.yaml ) - target_include_directories(test_simple_joint_limiter PRIVATE include) - target_link_libraries(test_simple_joint_limiter joint_limiter_interface) + target_include_directories(test_joint_saturation_limiter PRIVATE include) + target_link_libraries(test_joint_saturation_limiter joint_limiter_interface) ament_target_dependencies( - test_simple_joint_limiter + test_joint_saturation_limiter pluginlib rclcpp ) @@ -97,7 +97,7 @@ install( install(TARGETS joint_limits joint_limiter_interface - simple_joint_limiter + joint_saturation_limiter EXPORT export_joint_limits ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/joint_limits/include/joint_limits/simple_joint_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp similarity index 91% rename from joint_limits/include/joint_limits/simple_joint_limiter.hpp rename to joint_limits/include/joint_limits/joint_saturation_limiter.hpp index 1b9bfd8cad..ea3dca93e5 100644 --- a/joint_limits/include/joint_limits/simple_joint_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -14,8 +14,8 @@ /// \author Dr. Denis Stogl -#ifndef JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ -#define JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ +#ifndef JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_ +#define JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_ #include #include @@ -58,4 +58,4 @@ JointSaturationLimiter::~JointSaturationLimiter() } // namespace joint_limits -#endif // JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ +#endif // JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_ diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml index e4b41f878d..a49d88fbc9 100644 --- a/joint_limits/joint_limiters.xml +++ b/joint_limits/joint_limiters.xml @@ -1,5 +1,5 @@ - + diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp similarity index 99% rename from joint_limits/src/simple_joint_limiter.cpp rename to joint_limits/src/joint_saturation_limiter.cpp index a5e2e70926..f531f4bc18 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -14,7 +14,7 @@ /// \authors Nathan Brooks, Dr. Denis Stogl, Guillaume Walck -#include "joint_limits/simple_joint_limiter.hpp" +#include "joint_limits/joint_saturation_limiter.hpp" #include diff --git a/joint_limits/test/simple_joint_limiter_param.yaml b/joint_limits/test/joint_saturation_limiter_param.yaml similarity index 93% rename from joint_limits/test/simple_joint_limiter_param.yaml rename to joint_limits/test/joint_saturation_limiter_param.yaml index 4ca1ffdf07..b7010abb86 100644 --- a/joint_limits/test/simple_joint_limiter_param.yaml +++ b/joint_limits/test/joint_saturation_limiter_param.yaml @@ -1,4 +1,4 @@ -simple_joint_limiter: +joint_saturation_limiter: ros__parameters: joint_limits: # Get full specification from parameter server @@ -23,7 +23,7 @@ simple_joint_limiter: soft_lower_limit: 0.1 soft_upper_limit: 0.9 -simple_joint_limiter_nodeclimit: +joint_saturation_limiter_nodeclimit: ros__parameters: joint_limits: foo_joint: diff --git a/joint_limits/test/test_simple_joint_limiter.cpp b/joint_limits/test/test_joint_saturation_limiter.cpp similarity index 95% rename from joint_limits/test/test_simple_joint_limiter.cpp rename to joint_limits/test/test_joint_saturation_limiter.cpp index 363506b027..f778e81507 100644 --- a/joint_limits/test/test_simple_joint_limiter.cpp +++ b/joint_limits/test/test_joint_saturation_limiter.cpp @@ -14,7 +14,7 @@ /// \author Dr. Denis Stogl, Guillaume Walck -#include "test_simple_joint_limiter.hpp" +#include "test_joint_saturation_limiter.hpp" TEST_F(JointSaturationLimiterTest, when_loading_limiter_plugin_expect_loaded) { @@ -28,7 +28,7 @@ TEST_F(JointSaturationLimiterTest, when_loading_limiter_plugin_expect_loaded) /* FIXME: currently init does not check if limit parameters exist for the requested joint TEST_F(JointSaturationLimiterTest, when_joint_not_found_expect_init_fail) { - SetupNode("simple_joint_limiter"); + SetupNode("joint_saturation_limiter"); Load(); if (joint_limiter_) @@ -42,7 +42,7 @@ TEST_F(JointSaturationLimiterTest, when_joint_not_found_expect_init_fail) TEST_F(JointSaturationLimiterTest, when_invalid_dt_expect_enforce_fail) { - SetupNode("simple_joint_limiter"); + SetupNode("joint_saturation_limiter"); Load(); if (joint_limiter_) @@ -56,7 +56,7 @@ TEST_F(JointSaturationLimiterTest, when_invalid_dt_expect_enforce_fail) TEST_F(JointSaturationLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce_fail) { - SetupNode("simple_joint_limiter"); + SetupNode("joint_saturation_limiter"); Load(); if (joint_limiter_) @@ -76,7 +76,7 @@ TEST_F(JointSaturationLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce TEST_F(JointSaturationLimiterTest, when_no_posstate_expect_enforce_false) { - SetupNode("simple_joint_limiter"); + SetupNode("joint_saturation_limiter"); Load(); if (joint_limiter_) @@ -97,7 +97,7 @@ TEST_F(JointSaturationLimiterTest, when_no_posstate_expect_enforce_false) TEST_F(JointSaturationLimiterTest, when_no_velstate_expect_limiting) { - SetupNode("simple_joint_limiter"); + SetupNode("joint_saturation_limiter"); Load(); if (joint_limiter_) @@ -117,7 +117,7 @@ TEST_F(JointSaturationLimiterTest, when_no_velstate_expect_limiting) TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied) { - SetupNode("simple_joint_limiter"); + SetupNode("joint_saturation_limiter"); Load(); if (joint_limiter_) @@ -145,7 +145,7 @@ TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied) TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_vel_exceeded_expect_limits_enforced) { - SetupNode("simple_joint_limiter"); + SetupNode("joint_saturation_limiter"); Load(); if (joint_limiter_) @@ -189,7 +189,7 @@ TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_vel_exceeded_expect_limi TEST_F(JointSaturationLimiterTest, when_posonly_leads_to_vel_exceeded_expect_pos_acc_enforced) { - SetupNode("simple_joint_limiter"); + SetupNode("joint_saturation_limiter"); Load(); if (joint_limiter_) @@ -229,7 +229,7 @@ TEST_F(JointSaturationLimiterTest, when_posonly_leads_to_vel_exceeded_expect_pos TEST_F(JointSaturationLimiterTest, when_velonly_leads_to_vel_exceeded_expect_vel_acc_enforced) { - SetupNode("simple_joint_limiter"); + SetupNode("joint_saturation_limiter"); Load(); if (joint_limiter_) @@ -275,7 +275,7 @@ TEST_F(JointSaturationLimiterTest, when_velonly_leads_to_vel_exceeded_expect_vel TEST_F(JointSaturationLimiterTest, when_posonly_exceeded_expect_pos_enforced) { - SetupNode("simple_joint_limiter"); + SetupNode("joint_saturation_limiter"); Load(); if (joint_limiter_) @@ -302,7 +302,7 @@ TEST_F(JointSaturationLimiterTest, when_posonly_exceeded_expect_pos_enforced) TEST_F(JointSaturationLimiterTest, when_position_close_to_pos_limit_expect_deceleration_enforced) { - SetupNode("simple_joint_limiter"); + SetupNode("joint_saturation_limiter"); Load(); if (joint_limiter_) @@ -348,7 +348,7 @@ TEST_F(JointSaturationLimiterTest, when_position_close_to_pos_limit_expect_decel TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_acc_exceeded_expect_limits_enforced) { - SetupNode("simple_joint_limiter"); + SetupNode("joint_saturation_limiter"); Load(); if (joint_limiter_) @@ -377,7 +377,7 @@ TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_acc_exceeded_expect_limi TEST_F(JointSaturationLimiterTest, when_posonly_leads_to_acc_exceeded_expect_limits_enforced) { - SetupNode("simple_joint_limiter"); + SetupNode("joint_saturation_limiter"); Load(); if (joint_limiter_) @@ -406,7 +406,7 @@ TEST_F(JointSaturationLimiterTest, when_posonly_leads_to_acc_exceeded_expect_lim TEST_F(JointSaturationLimiterTest, when_velonly_leads_to_acc_exceeded_expect_limits_enforced) { - SetupNode("simple_joint_limiter"); + SetupNode("joint_saturation_limiter"); Load(); if (joint_limiter_) @@ -432,7 +432,7 @@ TEST_F(JointSaturationLimiterTest, when_velonly_leads_to_acc_exceeded_expect_lim TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_expect_dec_enforced) { - SetupNode("simple_joint_limiter"); + SetupNode("joint_saturation_limiter"); Load(); if (joint_limiter_) @@ -462,7 +462,7 @@ TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_expect_dec_enforce TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_with_no_maxdec_expect_acc_enforced) { - SetupNode("simple_joint_limiter_nodeclimit"); + SetupNode("joint_saturation_limiter_nodeclimit"); Load(); if (joint_limiter_) diff --git a/joint_limits/test/test_simple_joint_limiter.hpp b/joint_limits/test/test_joint_saturation_limiter.hpp similarity index 96% rename from joint_limits/test/test_simple_joint_limiter.hpp rename to joint_limits/test/test_joint_saturation_limiter.hpp index 0b01cdd8e5..89a4693ba1 100644 --- a/joint_limits/test/test_simple_joint_limiter.hpp +++ b/joint_limits/test/test_joint_saturation_limiter.hpp @@ -12,8 +12,8 @@ // 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_ +#ifndef TEST_JOINT_SATURATION_LIMITER_HPP_ +#define TEST_JOINT_SATURATION_LIMITER_HPP_ #include @@ -100,4 +100,4 @@ class JointSaturationLimiterTest : public ::testing::Test trajectory_msgs::msg::JointTrajectoryPoint current_joint_states_; }; -#endif // TEST_SIMPLE_JOINT_LIMITER_HPP_ +#endif // TEST_JOINT_SATURATION_LIMITER_HPP_ From 15b92553e8920f95b14e270f42dd29796f0db866 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 13 Dec 2023 09:55:12 +0100 Subject: [PATCH 43/83] add effort limits --- joint_limits/src/joint_saturation_limiter.cpp | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp index f531f4bc18..067fa3167d 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -45,6 +45,7 @@ bool JointSaturationLimiter::on_enforce( bool has_pos_cmd = (desired_joint_states.positions.size() == number_of_joints_); bool has_vel_cmd = (desired_joint_states.velocities.size() == number_of_joints_); bool has_acc_cmd = (desired_joint_states.accelerations.size() == number_of_joints_); + bool has_effort_cmd = (desired_joint_states.effort.size() == number_of_joints_); bool has_pos_state = (current_joint_states.positions.size() == number_of_joints_); bool has_vel_state = (current_joint_states.velocities.size() == number_of_joints_); @@ -65,11 +66,13 @@ bool JointSaturationLimiter::on_enforce( std::vector desired_pos(number_of_joints_); std::vector desired_vel(number_of_joints_); std::vector desired_acc(number_of_joints_); + std::vector desired_effort(number_of_joints_); std::vector expected_vel(number_of_joints_); std::vector expected_pos(number_of_joints_); // limits triggered std::vector limited_jnts_pos, limited_jnts_vel, limited_jnts_acc, limited_jnts_dec; + std::vector limited_joints_effort; bool braking_near_position_limit_triggered = false; @@ -87,6 +90,10 @@ bool JointSaturationLimiter::on_enforce( { desired_acc[index] = desired_joint_states.accelerations[index]; } + if (has_effort_cmd) + { + desired_effort[index] = desired_joint_states.effort[index]; + } if (has_pos_cmd) { @@ -295,6 +302,20 @@ bool JointSaturationLimiter::on_enforce( // else no need to slow down. in worse case we won't hit the limit at current velocity } } + + if (joint_limits_[index].has_effort_limits) + { + double max_effort = joint_limits_[index].max_effort; + if (std::fabs(desired_effort[index]) > max_effort) + { + desired_effort[index] = std::copysign(max_effort, desired_effort[index]); + limited_joints_effort.emplace_back(joint_names_[index]); + limits_enforced = true; + return true; + } + else + return false; + } } // update variables according to triggers From 8e032f4e6638713a5692b55dd64e5f01a3c7c7e8 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 14 Dec 2023 11:29:46 +0100 Subject: [PATCH 44/83] if move limit efforts to seperate function --- .../joint_limits/joint_saturation_limiter.hpp | 4 ++ joint_limits/src/joint_saturation_limiter.cpp | 72 +++++++++++++------ 2 files changed, 55 insertions(+), 21 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index ea3dca93e5..7142c85d54 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -41,6 +41,10 @@ class JointSaturationLimiter : public JointLimiterInterface trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) override; + JOINT_LIMITS_PUBLIC bool on_enforce_effort( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt); + private: rclcpp::Clock::SharedPtr clock_; }; diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp index 067fa3167d..52c0d77a51 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -45,7 +45,6 @@ bool JointSaturationLimiter::on_enforce( bool has_pos_cmd = (desired_joint_states.positions.size() == number_of_joints_); bool has_vel_cmd = (desired_joint_states.velocities.size() == number_of_joints_); bool has_acc_cmd = (desired_joint_states.accelerations.size() == number_of_joints_); - bool has_effort_cmd = (desired_joint_states.effort.size() == number_of_joints_); bool has_pos_state = (current_joint_states.positions.size() == number_of_joints_); bool has_vel_state = (current_joint_states.velocities.size() == number_of_joints_); @@ -66,13 +65,11 @@ bool JointSaturationLimiter::on_enforce( std::vector desired_pos(number_of_joints_); std::vector desired_vel(number_of_joints_); std::vector desired_acc(number_of_joints_); - std::vector desired_effort(number_of_joints_); std::vector expected_vel(number_of_joints_); std::vector expected_pos(number_of_joints_); // limits triggered std::vector limited_jnts_pos, limited_jnts_vel, limited_jnts_acc, limited_jnts_dec; - std::vector limited_joints_effort; bool braking_near_position_limit_triggered = false; @@ -90,10 +87,6 @@ bool JointSaturationLimiter::on_enforce( { desired_acc[index] = desired_joint_states.accelerations[index]; } - if (has_effort_cmd) - { - desired_effort[index] = desired_joint_states.effort[index]; - } if (has_pos_cmd) { @@ -302,20 +295,6 @@ bool JointSaturationLimiter::on_enforce( // else no need to slow down. in worse case we won't hit the limit at current velocity } } - - if (joint_limits_[index].has_effort_limits) - { - double max_effort = joint_limits_[index].max_effort; - if (std::fabs(desired_effort[index]) > max_effort) - { - desired_effort[index] = std::copysign(max_effort, desired_effort[index]); - limited_joints_effort.emplace_back(joint_names_[index]); - limits_enforced = true; - return true; - } - else - return false; - } } // update variables according to triggers @@ -412,7 +391,58 @@ bool JointSaturationLimiter::on_enforce( return limits_enforced; } +template <> +bool JointSaturationLimiter::on_enforce_effort( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) +{ + std::vector desired_effort(number_of_joints_); + std::vector limited_joints_effort; + + bool has_effort_cmd = (desired_joint_states.effort.size() == number_of_joints_); + if (!has_effort_cmd) + { + return false; + } + + bool limits_enforced = false; + for (size_t index = 0; index < number_of_joints_; ++index) + { + desired_effort[index] = desired_joint_states.effort[index]; + if (joint_limits_[index].has_effort_limits) + { + double max_effort = joint_limits_[index].max_effort; + if (std::fabs(desired_effort[index]) > max_effort) + { + desired_effort[index] = std::copysign(max_effort, desired_effort[index]); + limited_joints_effort.emplace_back(joint_names_[index]); + limits_enforced = true; + return true; + } + else + { + return false; + } + } + } + + if (limited_joints_effort.size() > 0) + { + std::ostringstream ostr; + for (auto jnt : limited_joints_effort) 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 effort limits, limiting"); + } + // set desired values + desired_joint_states.effort = desired_effort; + + return limits_enforced; +} + } // namespace joint_limits + // namespace joint_limits #include "pluginlib/class_list_macros.hpp" From 9a40a754c3f71164918ff3092dc7769ce7c75b03 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 8 Jan 2024 16:45:29 +0100 Subject: [PATCH 45/83] add generic enforce method --- .../joint_limits/joint_saturation_limiter.hpp | 14 ++++++++--- joint_limits/src/joint_saturation_limiter.cpp | 24 +++++++------------ 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index 7142c85d54..4626d14e0b 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -19,6 +19,8 @@ #include #include +#include +#include #include "joint_limits/joint_limiter_interface.hpp" #include "joint_limits/joint_limits.hpp" @@ -41,9 +43,15 @@ class JointSaturationLimiter : public JointLimiterInterface trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) override; - JOINT_LIMITS_PUBLIC bool on_enforce_effort( - trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, - trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt); + /** + * generic function for enforcing of effort. + * + * \return std::pair>, where bool shows if the limits have been enforced + * and the std::vector contains the values + * + */ + JOINT_LIMITS_PUBLIC std::pair> on_enforce( + std::vector desired, const rclcpp::Duration & dt); private: rclcpp::Clock::SharedPtr clock_; diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp index 52c0d77a51..c318f26840 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -392,23 +392,22 @@ bool JointSaturationLimiter::on_enforce( } template <> -bool JointSaturationLimiter::on_enforce_effort( - trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, - trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) +std::pair> JointSaturationLimiter::on_enforce( + std::vector desired, const rclcpp::Duration & dt) { std::vector desired_effort(number_of_joints_); std::vector limited_joints_effort; + bool limits_enforced = false; - bool has_effort_cmd = (desired_joint_states.effort.size() == number_of_joints_); - if (!has_effort_cmd) + bool has_cmd = (desired.size() == number_of_joints_); + if (!has_cmd) { - return false; + return std::make_pair(limits_enforced, desired_effort); } - bool limits_enforced = false; for (size_t index = 0; index < number_of_joints_; ++index) { - desired_effort[index] = desired_joint_states.effort[index]; + desired_effort[index] = desired[index]; if (joint_limits_[index].has_effort_limits) { double max_effort = joint_limits_[index].max_effort; @@ -417,11 +416,6 @@ bool JointSaturationLimiter::on_enforce_effort( desired_effort[index] = std::copysign(max_effort, desired_effort[index]); limited_joints_effort.emplace_back(joint_names_[index]); limits_enforced = true; - return true; - } - else - { - return false; } } } @@ -435,10 +429,8 @@ bool JointSaturationLimiter::on_enforce_effort( node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, "Joint(s) [" << ostr.str().c_str() << "] would exceed effort limits, limiting"); } - // set desired values - desired_joint_states.effort = desired_effort; - return limits_enforced; + return std::make_pair(limits_enforced, desired_effort); } } // namespace joint_limits From 7e27edcf9fe40e23116e8dbc48265b96211c1410 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 18 Jan 2024 18:37:15 +0100 Subject: [PATCH 46/83] Interface cleanup and small optimizations from reviewers' comments. --- .../joint_limits/joint_limiter_interface.hpp | 75 ++++++++++--- .../joint_limits/joint_limits_rosparam.hpp | 36 ++++-- .../joint_limits/joint_saturation_limiter.hpp | 46 ++++++-- joint_limits/src/joint_saturation_limiter.cpp | 106 +++++++++++------- .../test/joint_saturation_limiter_param.yaml | 21 ++++ .../test/test_joint_saturation_limiter.cpp | 47 +++++++- .../test/test_joint_saturation_limiter.hpp | 10 +- 7 files changed, 259 insertions(+), 82 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 370ebf1f9b..efaf529724 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -30,6 +30,8 @@ namespace joint_limits { +using JointLimitsStateDataType = trajectory_msgs::msg::JointTrajectoryPoint; + template class JointLimiterInterface { @@ -155,49 +157,86 @@ class JointLimiterInterface lifecycle_node->get_node_logging_interface(), robot_description_topic); } - JOINT_LIMITS_PUBLIC virtual bool configure( - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) + JOINT_LIMITS_PUBLIC virtual bool configure(const JointLimitsStateDataType & current_joint_states) { - // TODO(destogl): add checks for position return on_configure(current_joint_states); } + /** \brief Enforce joint limits to desired joint state for multiple physical quantities. + * + * Generic enforce method that calls implementation-specific `on_enforce` method. + * + * \param[in] current_joint_states current joint states a robot is in. + * \param[in,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. + * \returns true if limits are enforced, otherwise false. + */ JOINT_LIMITS_PUBLIC virtual bool enforce( - trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, - trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) + JointLimitsStateDataType & current_joint_states, + JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) { - // TODO(destogl): add checks if sizes of vectors and number of limits correspond. joint_limits_ = *(updated_limits_.readFromRT()); return on_enforce(current_joint_states, desired_joint_states, dt); } + /** \brief Enforce joint limits to desired joint state for single physical quantity. + * + * Generic enforce method that calls implementation-specific `on_enforce` method. + * + * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. + * \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool enforce(std::vector & desired_joint_states) + { + joint_limits_ = *(updated_limits_.readFromRT()); + return on_enforce(desired_joint_states); + } + protected: - JOINT_LIMITS_PUBLIC virtual bool on_init() { return true; } + /** \brief Method is realized by an implementation. + * + * Implementation-specific initialization of limiter's internal states and libraries. + * \returns true if initialization was successful, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool on_init() = 0; + /** \brief Method is realized by an implementation. + * + * Implementation-specific configuration of limiter's internal states and libraries. + * \returns true if initialization was successful, otherwise false. + */ JOINT_LIMITS_PUBLIC virtual bool on_configure( - const trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/) - { - return true; - } + const JointLimitsStateDataType & current_joint_states) = 0; - /** \brief Enforce joint limits to desired joint state. + /** \brief Method is realized by an implementation. * - * Filter-specific implementation of the joint limits enforce algorithm. + * Filter-specific implementation of the joint limits enforce algorithm for multiple dependent + * physical quantities. * * \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,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. * \returns true if limits are enforced, otherwise false. */ 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; + JointLimitsStateDataType & current_joint_states, + JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) = 0; + + /** \brief Method is realized by an implementation. + * + * Filter-specific implementation of the joint limits enforce algorithm for single physical + * quantity. + * This method might use "effort" limits since they are often used as wild-card. + * Check the documentation of the exact implementation for more details. + * + * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. + * \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool on_enforce(std::vector & desired_joint_states) = 0; size_t number_of_joints_; 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_; diff --git a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp index 67e8660c62..b23607f53d 100644 --- a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp +++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp @@ -19,6 +19,7 @@ #include #include +#include #include "joint_limits/joint_limits.hpp" #include "rclcpp/rclcpp.hpp" @@ -491,19 +492,30 @@ inline bool check_for_limits_update( if (param_name == param_base_name + ".has_position_limits") { updated_limits.has_position_limits = parameter.get_value(); - if ( - updated_limits.has_position_limits && - (std::isnan(updated_limits.min_position) || std::isnan(updated_limits.max_position))) + if (updated_limits.has_position_limits) { - RCLCPP_WARN( - logging_itf->get_logger(), - "PARAMETER NOT UPDATED: 'has_position_limits' flag can not be set if 'min_position' " - "and 'max_position' are not set or not have valid double values."); - updated_limits.has_position_limits = false; - } - else - { - changed = true; + if (std::isnan(updated_limits.min_position) || std::isnan(updated_limits.max_position)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: Position limits can not be used, i.e., " + "'has_position_limits' flag can not be set, if 'min_position' " + "and 'max_position' are not set or not have valid double values."); + updated_limits.has_position_limits = false; + } + else if (updated_limits.min_position >= updated_limits.max_position) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: Position limits can not be used, i.e., " + "'has_position_limits' flag can not be set, if not " + "'min_position' < 'max_position'"); + updated_limits.has_position_limits = false; + } + else + { + changed = true; + } } } else if (param_name == param_base_name + ".has_velocity_limits") diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index 4626d14e0b..733178d695 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -24,10 +24,18 @@ #include "joint_limits/joint_limiter_interface.hpp" #include "joint_limits/joint_limits.hpp" -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" namespace joint_limits { +/** + * Joint Saturation Limiter limits joints' position, velocity and acceleration by clamping values + * to its minimal and maximal allowed values. Since the position, velocity and accelerations are + * variables in physical relation, it might be that some values are limited lower then specified + * limit. For example, if a joint is close to its position limit, velocity and acceleration will be + * reduced accordingly. + */ template class JointSaturationLimiter : public JointLimiterInterface { @@ -38,20 +46,44 @@ class JointSaturationLimiter : public JointLimiterInterface /** \brief Destructor */ JOINT_LIMITS_PUBLIC ~JointSaturationLimiter(); + JOINT_LIMITS_PUBLIC bool on_init() override { return true; } + + JOINT_LIMITS_PUBLIC bool on_configure( + const trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/) override + { + return true; + } + + /** \brief Enforce joint limits to desired position, velocity and acceleration using clamping. + * Class implements this method accepting following data types: + * - trajectory_msgs::msg::JointTrajectoryPoint: limiting position, velocity and acceleration; + * + * Implementation of saturation approach for joints with position, velocity or acceleration limits + * and values. First, position limits are checked to adjust desired velocity accordingly, then + * velocity and finally acceleration. + * The method support partial existence of limits, e.g., missing position limits for continuous + * joins. + * + * \param[in] current_joint_states current joint states a robot is in. + * \param[in,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. + * \returns true if limits are enforced, otherwise false. + */ 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; - /** - * generic function for enforcing of effort. + /** \brief Enforce joint limits to desired arbitrary physical quantity. + * Additional, commonly used method for enforcing limits by clamping desired input value. + * The limit is defined in effort limits of the `joint::limits/JointLimit` structure. * - * \return std::pair>, where bool shows if the limits have been enforced - * and the std::vector contains the values + * If `has_effort_limits` is set to false, the limits will be not enforced to a joint. * + * \param[in,out] desired_joint_states physical quantity that should be adjusted to obey the + * limits. \returns true if limits are enforced, otherwise false. */ - JOINT_LIMITS_PUBLIC std::pair> on_enforce( - std::vector desired, const rclcpp::Duration & dt); + JOINT_LIMITS_PUBLIC bool on_enforce(std::vector & desired_joint_states) override; private: rclcpp::Clock::SharedPtr clock_; diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp index c318f26840..2a0b908b0e 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -35,26 +35,29 @@ bool JointSaturationLimiter::on_enforce( const auto dt_seconds = dt.seconds(); // negative or null is not allowed - if (dt_seconds <= 0.0) return false; + if (dt_seconds <= 0.0) + { + return false; + } // TODO(gwalck) compute if the max are not implicitly violated with the given dt // e.g. for max vel 2.0 and max acc 5.0, with dt >0.4 // velocity max is implicitly already violated due to max_acc * dt > 2.0 // check for required inputs combination - bool has_pos_cmd = (desired_joint_states.positions.size() == number_of_joints_); - bool has_vel_cmd = (desired_joint_states.velocities.size() == number_of_joints_); - bool has_acc_cmd = (desired_joint_states.accelerations.size() == number_of_joints_); - bool has_pos_state = (current_joint_states.positions.size() == number_of_joints_); - bool has_vel_state = (current_joint_states.velocities.size() == number_of_joints_); + bool has_desired_position = (desired_joint_states.positions.size() == number_of_joints_); + bool has_desired_velocity = (desired_joint_states.velocities.size() == number_of_joints_); + bool has_desired_acceleration = (desired_joint_states.accelerations.size() == number_of_joints_); + bool has_current_position = (current_joint_states.positions.size() == number_of_joints_); + bool has_current_velocity = (current_joint_states.velocities.size() == number_of_joints_); // pos state and vel or pos cmd is required, vel state is optional - if (!(has_pos_state && (has_pos_cmd || has_vel_cmd))) + if (!(has_current_position && (has_desired_position || has_desired_velocity))) { return false; } - if (!has_vel_state) + if (!has_current_velocity) { // First update() after activating does not have velocity available, assume 0 current_joint_states.velocities.resize(number_of_joints_, 0.0); @@ -75,20 +78,20 @@ bool JointSaturationLimiter::on_enforce( for (size_t index = 0; index < number_of_joints_; ++index) { - if (has_pos_cmd) + if (has_desired_position) { desired_pos[index] = desired_joint_states.positions[index]; } - if (has_vel_cmd) + if (has_desired_velocity) { desired_vel[index] = desired_joint_states.velocities[index]; } - if (has_acc_cmd) + if (has_desired_acceleration) { desired_acc[index] = desired_joint_states.accelerations[index]; } - if (has_pos_cmd) + if (has_desired_position) { // limit position if (joint_limits_[index].has_position_limits) @@ -106,8 +109,6 @@ bool JointSaturationLimiter::on_enforce( // priority to pos_cmd derivative over cmd_vel when not defined. If done always then we might // get jumps in the velocity based on the system's dynamics. Position limit clamping is done // below once again. - // TODO(destogl) handle the case of continuous joints with angle_wraparound to compute vel - // correctly const double position_difference = desired_pos[index] - current_joint_states.positions[index]; if ( std::fabs(position_difference) > VALUE_CONSIDERED_ZERO && @@ -128,7 +129,7 @@ bool JointSaturationLimiter::on_enforce( limits_enforced = true; // recompute pos_cmd if needed - if (has_pos_cmd) + if (has_desired_position) { desired_pos[index] = current_joint_states.positions[index] + desired_vel[index] * dt_seconds; @@ -144,7 +145,7 @@ bool JointSaturationLimiter::on_enforce( if ( joint_limits_[index].has_acceleration_limits || joint_limits_[index].has_deceleration_limits) { - // if(has_vel_state) + // if(has_current_velocity) if (1) // for now use a zero velocity if not provided { // limiting acc or dec function @@ -160,7 +161,9 @@ bool JointSaturationLimiter::on_enforce( return true; } else + { return false; + } }; // limit acc for pos_cmd and/or vel_cmd @@ -197,7 +200,7 @@ bool JointSaturationLimiter::on_enforce( // vel_cmd from integration of desired_acc, needed even if no vel output desired_vel[index] = current_joint_states.velocities[index] + desired_acc[index] * dt_seconds; - if (has_pos_cmd) + if (has_desired_position) { // pos_cmd from from double integration of desired_acc desired_pos[index] = current_joint_states.positions[index] + @@ -212,7 +215,7 @@ bool JointSaturationLimiter::on_enforce( // plan ahead for position limits if (joint_limits_[index].has_position_limits) { - if (has_vel_cmd && !has_pos_cmd) + if (has_desired_velocity && !has_desired_position) { // Check immediate next step when using vel_cmd only, other cases already handled // integrate pos @@ -321,18 +324,23 @@ bool JointSaturationLimiter::on_enforce( } // Recompute velocity and position - if (has_vel_cmd) + if (has_desired_velocity) { desired_vel[index] = current_joint_states.velocities[index] + desired_acc[index] * dt_seconds; } - if (has_pos_cmd) + if (has_desired_position) + { desired_pos[index] = current_joint_states.positions[index] + current_joint_states.velocities[index] * dt_seconds + 0.5 * desired_acc[index] * dt_seconds * dt_seconds; + } } std::ostringstream ostr; - for (auto jnt : limited_jnts_pos) ostr << jnt << " "; + for (auto jnt : limited_jnts_pos) + { + ostr << jnt << " "; + } ostr << "\b \b"; // erase last character RCLCPP_WARN_STREAM_THROTTLE( node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, @@ -347,7 +355,10 @@ bool JointSaturationLimiter::on_enforce( if (limited_jnts_pos.size() > 0) { std::ostringstream ostr; - for (auto jnt : limited_jnts_pos) ostr << jnt << " "; + for (auto jnt : limited_jnts_pos) + { + ostr << jnt << " "; + } ostr << "\b \b"; // erase last character RCLCPP_WARN_STREAM_THROTTLE( node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, @@ -357,7 +368,10 @@ bool JointSaturationLimiter::on_enforce( if (limited_jnts_vel.size() > 0) { std::ostringstream ostr; - for (auto jnt : limited_jnts_vel) ostr << jnt << " "; + for (auto jnt : limited_jnts_vel) + { + ostr << jnt << " "; + } ostr << "\b \b"; // erase last character RCLCPP_WARN_STREAM_THROTTLE( node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, @@ -367,7 +381,10 @@ bool JointSaturationLimiter::on_enforce( if (limited_jnts_acc.size() > 0) { std::ostringstream ostr; - for (auto jnt : limited_jnts_acc) ostr << jnt << " "; + for (auto jnt : limited_jnts_acc) + { + ostr << jnt << " "; + } ostr << "\b \b"; // erase last character RCLCPP_WARN_STREAM_THROTTLE( node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, @@ -377,43 +394,52 @@ bool JointSaturationLimiter::on_enforce( if (limited_jnts_dec.size() > 0) { std::ostringstream ostr; - for (auto jnt : limited_jnts_dec) ostr << jnt << " "; + 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"); } - if (has_pos_cmd) desired_joint_states.positions = desired_pos; - if (has_vel_cmd) desired_joint_states.velocities = desired_vel; - if (has_acc_cmd) desired_joint_states.accelerations = desired_acc; + if (has_desired_position) + { + desired_joint_states.positions = desired_pos; + } + if (has_desired_velocity) + { + desired_joint_states.velocities = desired_vel; + } + if (has_desired_acceleration) + { + desired_joint_states.accelerations = desired_acc; + } return limits_enforced; } template <> -std::pair> JointSaturationLimiter::on_enforce( - std::vector desired, const rclcpp::Duration & dt) +bool JointSaturationLimiter::on_enforce(std::vector & desired_joint_states) { - std::vector desired_effort(number_of_joints_); std::vector limited_joints_effort; bool limits_enforced = false; - bool has_cmd = (desired.size() == number_of_joints_); + bool has_cmd = (desired_joint_states.size() == number_of_joints_); if (!has_cmd) { - return std::make_pair(limits_enforced, desired_effort); + return false; } for (size_t index = 0; index < number_of_joints_; ++index) { - desired_effort[index] = desired[index]; if (joint_limits_[index].has_effort_limits) { double max_effort = joint_limits_[index].max_effort; - if (std::fabs(desired_effort[index]) > max_effort) + if (std::fabs(desired_joint_states[index]) > max_effort) { - desired_effort[index] = std::copysign(max_effort, desired_effort[index]); + desired_joint_states[index] = std::copysign(max_effort, desired_joint_states[index]); limited_joints_effort.emplace_back(joint_names_[index]); limits_enforced = true; } @@ -423,18 +449,20 @@ std::pair> JointSaturationLimiter::on_enf if (limited_joints_effort.size() > 0) { std::ostringstream ostr; - for (auto jnt : limited_joints_effort) ostr << jnt << " "; + for (auto jnt : limited_joints_effort) + { + 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 effort limits, limiting"); } - return std::make_pair(limits_enforced, desired_effort); + return limits_enforced; } } // namespace joint_limits - // namespace joint_limits #include "pluginlib/class_list_macros.hpp" diff --git a/joint_limits/test/joint_saturation_limiter_param.yaml b/joint_limits/test/joint_saturation_limiter_param.yaml index b7010abb86..f025421519 100644 --- a/joint_limits/test/joint_saturation_limiter_param.yaml +++ b/joint_limits/test/joint_saturation_limiter_param.yaml @@ -23,6 +23,27 @@ joint_saturation_limiter: soft_lower_limit: 0.1 soft_upper_limit: 0.9 + foo_joint_no_effort: + 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: false + 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 + joint_saturation_limiter_nodeclimit: ros__parameters: joint_limits: diff --git a/joint_limits/test/test_joint_saturation_limiter.cpp b/joint_limits/test/test_joint_saturation_limiter.cpp index f778e81507..3c1353ecff 100644 --- a/joint_limits/test/test_joint_saturation_limiter.cpp +++ b/joint_limits/test/test_joint_saturation_limiter.cpp @@ -25,7 +25,7 @@ TEST_F(JointSaturationLimiterTest, when_loading_limiter_plugin_expect_loaded) ASSERT_NE(joint_limiter_, nullptr); } -/* FIXME: currently init does not check if limit parameters exist for the requested joint +// NOTE: We accept also if there is no limit defined for a joint. TEST_F(JointSaturationLimiterTest, when_joint_not_found_expect_init_fail) { SetupNode("joint_saturation_limiter"); @@ -35,10 +35,9 @@ TEST_F(JointSaturationLimiterTest, when_joint_not_found_expect_init_fail) { // initialize the limiter std::vector joint_names = {"bar_joint"}; - ASSERT_FALSE(joint_limiter_->init(joint_names, node_)); + ASSERT_TRUE(joint_limiter_->init(joint_names, node_)); } } -*/ TEST_F(JointSaturationLimiterTest, when_invalid_dt_expect_enforce_fail) { @@ -489,6 +488,48 @@ TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_with_no_maxdec_exp } } +TEST_F(JointSaturationLimiterTest, when_there_are_effort_limits_expect_them_to_be_applyed) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + // value not over limit + desired_joint_states_.effort[0] = 15.0; + ASSERT_FALSE(joint_limiter_->enforce(desired_joint_states_.effort)); + + // value over limit + desired_joint_states_.effort[0] = 21.0; + ASSERT_TRUE(joint_limiter_->enforce(desired_joint_states_.effort)); + ASSERT_EQ(desired_joint_states_.effort[0], 20.0); + } +} + +TEST_F(JointSaturationLimiterTest, when_there_are_no_effort_limits_expect_them_not_applyed) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init("foo_joint_no_effort"); + Configure(); + + // value not over limit + desired_joint_states_.effort[0] = 15.0; + ASSERT_FALSE(joint_limiter_->enforce(desired_joint_states_.effort)); + + // value over limit + desired_joint_states_.effort[0] = 21.0; + ASSERT_FALSE(joint_limiter_->enforce(desired_joint_states_.effort)); + ASSERT_EQ(desired_joint_states_.effort[0], 21.0); + } +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/joint_limits/test/test_joint_saturation_limiter.hpp b/joint_limits/test/test_joint_saturation_limiter.hpp index 89a4693ba1..950b611109 100644 --- a/joint_limits/test/test_joint_saturation_limiter.hpp +++ b/joint_limits/test/test_joint_saturation_limiter.hpp @@ -46,7 +46,10 @@ class JointSaturationLimiterTest : public ::testing::Test void SetupNode(const std::string node_name = "") { - if (!node_name.empty()) node_name_ = node_name; + if (!node_name.empty()) + { + node_name_ = node_name; + } node_ = std::make_shared(node_name_); } @@ -56,14 +59,15 @@ class JointSaturationLimiterTest : public ::testing::Test joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_)); } - void Init() + void Init(const std::string & joint_name = "foo_joint") { - joint_names_ = {"foo_joint"}; + joint_names_ = {joint_name}; 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); + last_commanded_state_.effort.resize(num_joints_, 0.0); desired_joint_states_ = last_commanded_state_; current_joint_states_ = last_commanded_state_; } From 56dd00f48e79109bcd152f858ee143f83cc13952 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 22 Jan 2024 17:11:12 +0100 Subject: [PATCH 47/83] Added package to CI --- .github/workflows/ci-coverage-build.yml | 1 + .github/workflows/ci-ros-lint.yml | 2 ++ 2 files changed, 3 insertions(+) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 6edb1707b1..cb53850f40 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -30,6 +30,7 @@ jobs: controller_interface controller_manager hardware_interface + joint_limits transmission_interface vcs-repo-file-url: | diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 84c68217f3..16bed93a7a 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -24,6 +24,7 @@ jobs: controller_manager controller_manager_msgs hardware_interface + joint_limits ros2controlcli ros2_control ros2_control_test_assets @@ -49,6 +50,7 @@ jobs: controller_manager controller_manager_msgs hardware_interface + joint_limits ros2controlcli ros2_control ros2_control_test_assets From 284437a56fc2831f3f0a59b055af8133df454a29 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 8 Feb 2024 19:09:55 +0100 Subject: [PATCH 48/83] Correct sorting of packages. --- .github/workflows/ci-coverage-build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 92ce384b50..84a8363d6e 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -30,8 +30,8 @@ jobs: controller_interface controller_manager hardware_interface - joint_limits hardware_interface_testing + joint_limits transmission_interface vcs-repo-file-url: | From d3838fdf928fe42ba17abcb15ad0f4c68762652d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 8 Feb 2024 18:31:32 +0100 Subject: [PATCH 49/83] Small fixes of calculation when differnet set of inputs are provided. --- joint_limits/src/joint_saturation_limiter.cpp | 19 ++++++++---- .../test/test_joint_saturation_limiter.cpp | 29 +++++++++++++++++++ 2 files changed, 42 insertions(+), 6 deletions(-) diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp index 2a0b908b0e..5020cab27b 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -121,6 +121,12 @@ bool JointSaturationLimiter::on_enforce( // limit velocity if (joint_limits_[index].has_velocity_limits) { + // if desired velocity is not defined calculate it from positions + if (std::fabs(desired_vel[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_vel[index])) + { + desired_vel[index] = + (desired_pos[index] - current_joint_states.positions[index]) / dt_seconds; + } // clamp input vel_cmd if (std::fabs(desired_vel[index]) > joint_limits_[index].max_velocity) { @@ -135,7 +141,6 @@ bool JointSaturationLimiter::on_enforce( current_joint_states.positions[index] + desired_vel[index] * dt_seconds; } - // compute desired_acc when velocity is limited desired_acc[index] = (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; } @@ -166,11 +171,13 @@ bool JointSaturationLimiter::on_enforce( } }; - // limit acc for pos_cmd and/or vel_cmd - - // compute desired_acc with desired_vel and vel_state - desired_acc[index] = - (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; + // if desired acceleration if not provided compute it from desired_vel and vel_state + if ( + std::fabs(desired_acc[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_acc[index])) + { + desired_acc[index] = + (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; + } // check if decelerating - if velocity is changing toward 0 bool deceleration_limit_applied = false; diff --git a/joint_limits/test/test_joint_saturation_limiter.cpp b/joint_limits/test/test_joint_saturation_limiter.cpp index 3c1353ecff..e78c4b8994 100644 --- a/joint_limits/test/test_joint_saturation_limiter.cpp +++ b/joint_limits/test/test_joint_saturation_limiter.cpp @@ -142,6 +142,35 @@ TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied) } } +TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied_with_acc) +{ + SetupNode("joint_saturation_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; + desired_joint_states_.velocities[0] = 1.5; // valid pos derivative as well + desired_joint_states_.accelerations[0] = 2.9; // valid pos derivative as well + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if no limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 1.0, // pos unchanged + 1.5, // vel unchanged + 2.9 // acc = vel / 1.0 + ); + } +} + TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_vel_exceeded_expect_limits_enforced) { SetupNode("joint_saturation_limiter"); From 16fbde3d7ee9e6d7ce19c789887eb4558118f59b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 21 Apr 2024 08:05:43 +0200 Subject: [PATCH 50/83] return the proper const object of the pointer in the const method (#1494) --- .../include/controller_interface/controller_interface_base.hpp | 2 +- controller_interface/src/controller_interface_base.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 182ffa7fec..1c09f71e98 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -145,7 +145,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy std::shared_ptr get_node(); CONTROLLER_INTERFACE_PUBLIC - std::shared_ptr get_node() const; + std::shared_ptr get_node() const; CONTROLLER_INTERFACE_PUBLIC const rclcpp_lifecycle::State & get_state() const; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index e48e1d21b3..6b87ba5cda 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -121,7 +121,7 @@ std::shared_ptr ControllerInterfaceBase::get_no return node_; } -std::shared_ptr ControllerInterfaceBase::get_node() const +std::shared_ptr ControllerInterfaceBase::get_node() const { if (!node_.get()) { From cca2070d04cef0f8a1def3ea19f69cabf29edb31 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 24 Apr 2024 20:17:21 +0200 Subject: [PATCH 51/83] Deactivate the controllers when they return error similar to hardware (#1499) --- controller_manager/doc/userdoc.rst | 10 +- controller_manager/src/controller_manager.cpp | 15 +++ .../test/test_controller/test_controller.cpp | 8 ++ ...roller_manager_hardware_error_handling.cpp | 100 ++++++++++++++++++ 4 files changed, 130 insertions(+), 3 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index c8aa68d774..4deb85291b 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -225,6 +225,10 @@ Note that not all controllers have to be restarted, e.g., broadcasters. Restarting hardware ^^^^^^^^^^^^^^^^^^^^^ -If hardware gets restarted then you should go through its lifecycle again. -This can be simply achieved by returning ``ERROR`` from ``write`` and ``read`` methods of interface implementation. -**NOT IMPLEMENTED YET - PLEASE STOP/RESTART ALL CONTROLLERS MANUALLY FOR NOW** The controller manager detects that and stops all the controllers that are commanding that hardware and restarts broadcasters that are listening to its states. +If hardware gets restarted then you should go through its lifecycle again in order to reconfigure and export the interfaces + +Hardware and Controller Errors +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +If the hardware during it's ``read`` or ``write`` method returns ``return_type::ERROR``, the controller manager will stop all controllers that are using the hardware's command and state interfaces. +Likewise, if a controller returns ``return_type::ERROR`` from its ``update`` method, the controller manager will deactivate the respective controller. In future, the controller manager will try to start any fallback controllers if available. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index eadd1b0d78..e3729b90f8 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2023,6 +2023,7 @@ controller_interface::return_type ControllerManager::update( ++update_loop_counter_; update_loop_counter_ %= update_rate_; + std::vector failed_controllers_list; for (const auto & loaded_controller : rt_controller_list) { // TODO(v-lopez) we could cache this information @@ -2061,11 +2062,25 @@ controller_interface::return_type ControllerManager::update( if (controller_ret != controller_interface::return_type::OK) { + failed_controllers_list.push_back(loaded_controller.info.name); ret = controller_ret; } } } } + if (!failed_controllers_list.empty()) + { + std::string failed_controllers; + for (const auto & controller : failed_controllers_list) + { + failed_controllers += "\n\t- " + controller; + } + RCLCPP_ERROR( + get_logger(), "Deactivating following controllers as their update resulted in an error :%s", + failed_controllers.c_str()); + + deactivate_controllers(rt_controller_list, failed_controllers_list); + } // there are controllers to (de)activate if (switch_params_.do_switch) diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 7585ae36e5..df41ebf34e 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -76,6 +76,14 @@ controller_interface::return_type TestController::update( { for (size_t i = 0; i < command_interfaces_.size(); ++i) { + if (!std::isfinite(external_commands_for_testing_[i])) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "External command value for command interface '%s' is not finite", + command_interfaces_[i].get_name().c_str()); + return controller_interface::return_type::ERROR; + } RCLCPP_INFO( get_node()->get_logger(), "Setting value of command interface '%s' to %f", command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]); diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp index 4c800d41c2..6e2fba23db 100644 --- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -405,6 +405,106 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er } } +TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error) +{ + auto strictness = GetParam().strictness; + SetupAndConfigureControllers(strictness); + + rclcpp_lifecycle::State state_active( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + { + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_GE(test_controller_actuator->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_controller_system->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_broadcaster_all->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_broadcaster_sensor->internal_counter, 1u) + << "Controller is started at the end of update"; + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + + // Execute first time without any errors + { + auto new_counter = test_controller_actuator->internal_counter + 1; + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) << "Execute without errors"; + } + + // Simulate error in update method of the controllers but not in hardware + test_controller_actuator->external_commands_for_testing_[0] = + std::numeric_limits::quiet_NaN(); + test_controller_system->external_commands_for_testing_[0] = + std::numeric_limits::quiet_NaN(); + { + auto new_counter = test_controller_actuator->internal_counter + 1; + EXPECT_EQ(controller_interface::return_type::ERROR, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) + << "Executes the current cycle and returns ERROR"; + EXPECT_EQ( + test_controller_actuator->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(test_controller_system->internal_counter, new_counter) + << "Executes the current cycle and returns ERROR"; + EXPECT_EQ( + test_controller_system->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + { + auto previous_counter = test_controller_actuator->internal_counter; + auto new_counter = test_controller_system->internal_counter + 1; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) + << "Cannot execute as it should be currently deactivated"; + EXPECT_EQ(test_controller_system->internal_counter, previous_counter) + << "Cannot execute as it should be currently deactivated"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) + << "Broadcaster all interfaces without errors"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + + // The states shouldn't change as there are no more controller errors + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + } +} + TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_error) { auto strictness = GetParam().strictness; From 5d1392347aa7c9af1b8ac9575ca43482926093ee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 29 Apr 2024 14:54:18 +0200 Subject: [PATCH 52/83] rosdoc2 for transmission_interface (#1496) * Adaptions for rosdoc2 * Add custom landing page --- .pre-commit-config.yaml | 1 + transmission_interface/Doxyfile | 2661 +++++++++++++++++ transmission_interface/doc/conf.py | 5 + transmission_interface/doc/index.rst | 21 + .../differential_transmission.hpp | 4 +- 5 files changed, 2691 insertions(+), 1 deletion(-) create mode 100644 transmission_interface/Doxyfile create mode 100644 transmission_interface/doc/conf.py create mode 100644 transmission_interface/doc/index.rst diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 63dc2afe69..19f07a8e77 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -104,6 +104,7 @@ repos: description: Check if copyright notice is available in all files. entry: ament_copyright language: system + exclude: .*/conf\.py$ # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 diff --git a/transmission_interface/Doxyfile b/transmission_interface/Doxyfile new file mode 100644 index 0000000000..db5b9c7e6f --- /dev/null +++ b/transmission_interface/Doxyfile @@ -0,0 +1,2661 @@ +# Doxyfile 1.9.1 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project. +# +# All text after a double hash (##) is considered a comment and is placed in +# front of the TAG it is preceding. +# +# All text after a single hash (#) is considered a comment and will be ignored. +# The format is: +# TAG = value [value, ...] +# For lists, items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (\" \"). + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the configuration +# file that follow. The default is UTF-8 which is also the encoding used for all +# text before the first occurrence of this tag. Doxygen uses libiconv (or the +# iconv built into libc) for the transcoding. See +# https://www.gnu.org/software/libiconv/ for the list of possible encodings. +# The default value is: UTF-8. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by +# double-quotes, unless you are using Doxywizard) that should identify the +# project for which the documentation is generated. This name is used in the +# title of most generated pages and in a few other places. +# The default value is: My Project. + +PROJECT_NAME = "ros2_control transmission_interface" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. This +# could be handy for archiving the generated documentation or if some version +# control system is used. + +PROJECT_NUMBER = + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer a +# quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = + +# With the PROJECT_LOGO tag one can specify a logo or an icon that is included +# in the documentation. The maximum height of the logo should not exceed 55 +# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy +# the logo to the output directory. + +PROJECT_LOGO = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path +# into which the generated documentation will be written. If a relative path is +# entered, it will be relative to the location where doxygen was started. If +# left blank the current directory will be used. + +OUTPUT_DIRECTORY = doc/_build/ + +# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub- +# directories (in 2 levels) under the output directory of each output format and +# will distribute the generated files over these directories. Enabling this +# option can be useful when feeding doxygen a huge amount of source files, where +# putting all generated files in the same directory would otherwise causes +# performance problems for the file system. +# The default value is: NO. + +CREATE_SUBDIRS = NO + +# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII +# characters to appear in the names of generated files. If set to NO, non-ASCII +# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode +# U+3044. +# The default value is: NO. + +ALLOW_UNICODE_NAMES = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese, +# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States), +# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian, +# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages), +# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian, +# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian, +# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish, +# Ukrainian and Vietnamese. +# The default value is: English. + +OUTPUT_LANGUAGE = English + +# The OUTPUT_TEXT_DIRECTION tag is used to specify the direction in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all generated output in the proper direction. +# Possible values are: None, LTR, RTL and Context. +# The default value is: None. + +OUTPUT_TEXT_DIRECTION = None + +# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member +# descriptions after the members that are listed in the file and class +# documentation (similar to Javadoc). Set to NO to disable this. +# The default value is: YES. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief +# description of a member or function before the detailed description +# +# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. +# The default value is: YES. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator that is +# used to form the text in various listings. Each string in this list, if found +# as the leading text of the brief description, will be stripped from the text +# and the result, after processing the whole list, is used as the annotated +# text. Otherwise, the brief description is used as-is. If left blank, the +# following values are used ($name is automatically replaced with the name of +# the entity):The $name class, The $name widget, The $name file, is, provides, +# specifies, contains, represents, a, an and the. + +ABBREVIATE_BRIEF = "The $name class" \ + "The $name widget" \ + "The $name file" \ + is \ + provides \ + specifies \ + contains \ + represents \ + a \ + an \ + the + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# doxygen will generate a detailed section even if there is only a brief +# description. +# The default value is: NO. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. +# The default value is: NO. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path +# before files name in the file list and in the header files. If set to NO the +# shortest path that makes the file name unique will be used +# The default value is: YES. + +FULL_PATH_NAMES = YES + +# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path. +# Stripping is only done if one of the specified strings matches the left-hand +# part of the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the path to +# strip. +# +# Note that you can specify absolute paths here, but also relative paths, which +# will be relative from the directory where doxygen is started. +# This tag requires that the tag FULL_PATH_NAMES is set to YES. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the +# path mentioned in the documentation of a class, which tells the reader which +# header file to include in order to use a class. If left blank only the name of +# the header file containing the class definition is used. Otherwise one should +# specify the list of include paths that are normally passed to the compiler +# using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but +# less readable) file names. This can be useful is your file systems doesn't +# support long names like on DOS, Mac, or CD-ROM. +# The default value is: NO. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the +# first line (until the first dot) of a Javadoc-style comment as the brief +# description. If set to NO, the Javadoc-style will behave just like regular Qt- +# style comments (thus requiring an explicit @brief command for a brief +# description.) +# The default value is: NO. + +JAVADOC_AUTOBRIEF = NO + +# If the JAVADOC_BANNER tag is set to YES then doxygen will interpret a line +# such as +# /*************** +# as being the beginning of a Javadoc-style comment "banner". If set to NO, the +# Javadoc-style will behave just like regular comments and it will not be +# interpreted by doxygen. +# The default value is: NO. + +JAVADOC_BANNER = NO + +# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first +# line (until the first dot) of a Qt-style comment as the brief description. If +# set to NO, the Qt-style will behave just like regular Qt-style comments (thus +# requiring an explicit \brief command for a brief description.) +# The default value is: NO. + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a +# multi-line C++ special comment block (i.e. a block of //! or /// comments) as +# a brief description. This used to be the default behavior. The new default is +# to treat a multi-line C++ comment block as a detailed description. Set this +# tag to YES if you prefer the old behavior instead. +# +# Note that setting this tag to YES also means that rational rose comments are +# not recognized any more. +# The default value is: NO. + +MULTILINE_CPP_IS_BRIEF = NO + +# By default Python docstrings are displayed as preformatted text and doxygen's +# special commands cannot be used. By setting PYTHON_DOCSTRING to NO the +# doxygen's special commands can be used and the contents of the docstring +# documentation blocks is shown as doxygen documentation. +# The default value is: YES. + +PYTHON_DOCSTRING = YES + +# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the +# documentation from any documented member that it re-implements. +# The default value is: YES. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new +# page for each member. If set to NO, the documentation of a member will be part +# of the file/class/namespace that contains it. +# The default value is: NO. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen +# uses this value to replace tabs by spaces in code fragments. +# Minimum value: 1, maximum value: 16, default value: 4. + +TAB_SIZE = 4 + +# This tag can be used to specify a number of aliases that act as commands in +# the documentation. An alias has the form: +# name=value +# For example adding +# "sideeffect=@par Side Effects:\n" +# will allow you to put the command \sideeffect (or @sideeffect) in the +# documentation, which will result in a user-defined paragraph with heading +# "Side Effects:". You can put \n's in the value part of an alias to insert +# newlines (in the resulting output). You can put ^^ in the value part of an +# alias to insert a newline as if a physical newline was in the original file. +# When you need a literal { or } or , in the value part of an alias you have to +# escape them by means of a backslash (\), this can lead to conflicts with the +# commands \{ and \} for these it is advised to use the version @{ and @} or use +# a double escape (\\{ and \\}) + +ALIASES = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources +# only. Doxygen will then generate output that is more tailored for C. For +# instance, some of the names that are used will be different. The list of all +# members will be omitted, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_FOR_C = NO + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or +# Python sources only. Doxygen will then generate output that is more tailored +# for that language. For instance, namespaces will be presented as packages, +# qualified scopes will look different, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources. Doxygen will then generate output that is tailored for Fortran. +# The default value is: NO. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for VHDL. +# The default value is: NO. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Set the OPTIMIZE_OUTPUT_SLICE tag to YES if your project consists of Slice +# sources only. Doxygen will then generate output that is more tailored for that +# language. For instance, namespaces will be presented as modules, types will be +# separated into more groups, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_SLICE = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given +# extension. Doxygen has a built-in mapping, but you can override or extend it +# using this tag. The format is ext=language, where ext is a file extension, and +# language is one of the parsers supported by doxygen: IDL, Java, JavaScript, +# Csharp (C#), C, C++, D, PHP, md (Markdown), Objective-C, Python, Slice, VHDL, +# Fortran (fixed format Fortran: FortranFixed, free formatted Fortran: +# FortranFree, unknown formatted Fortran: Fortran. In the later case the parser +# tries to guess whether the code is fixed or free formatted code, this is the +# default for Fortran type files). For instance to make doxygen treat .inc files +# as Fortran files (default is PHP), and .f files as C (default is Fortran), +# use: inc=Fortran f=C. +# +# Note: For files without extension you can use no_extension as a placeholder. +# +# Note that for custom extensions you also need to set FILE_PATTERNS otherwise +# the files are not read by doxygen. When specifying no_extension you should add +# * to the FILE_PATTERNS. +# +# Note see also the list of default file extension mappings. + +EXTENSION_MAPPING = + +# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments +# according to the Markdown format, which allows for more readable +# documentation. See https://daringfireball.net/projects/markdown/ for details. +# The output of markdown processing is further processed by doxygen, so you can +# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in +# case of backward compatibilities issues. +# The default value is: YES. + +MARKDOWN_SUPPORT = YES + +# When the TOC_INCLUDE_HEADINGS tag is set to a non-zero value, all headings up +# to that level are automatically included in the table of contents, even if +# they do not have an id attribute. +# Note: This feature currently applies only to Markdown headings. +# Minimum value: 0, maximum value: 99, default value: 5. +# This tag requires that the tag MARKDOWN_SUPPORT is set to YES. + +TOC_INCLUDE_HEADINGS = 5 + +# When enabled doxygen tries to link words that correspond to documented +# classes, or namespaces to their corresponding documentation. Such a link can +# be prevented in individual cases by putting a % sign in front of the word or +# globally by setting AUTOLINK_SUPPORT to NO. +# The default value is: YES. + +AUTOLINK_SUPPORT = YES + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should set this +# tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); +# versus func(std::string) {}). This also make the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. +# The default value is: NO. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. +# The default value is: NO. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip (see: +# https://www.riverbankcomputing.com/software/sip/intro) sources only. Doxygen +# will parse them like normal C++ but will assume all classes use public instead +# of private inheritance when no explicit protection keyword is present. +# The default value is: NO. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate +# getter and setter methods for a property. Setting this option to YES will make +# doxygen to replace the get and set methods by a property in the documentation. +# This will only work if the methods are indeed getting or setting a simple +# type. If this is not the case, or you want to show the methods anyway, you +# should set this option to NO. +# The default value is: YES. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. +# The default value is: NO. + +DISTRIBUTE_GROUP_DOC = NO + +# If one adds a struct or class to a group and this option is enabled, then also +# any nested class or struct is added to the same group. By default this option +# is disabled and one has to add nested compounds explicitly via \ingroup. +# The default value is: NO. + +GROUP_NESTED_COMPOUNDS = NO + +# Set the SUBGROUPING tag to YES to allow class member groups of the same type +# (for instance a group of public functions) to be put as a subgroup of that +# type (e.g. under the Public Functions section). Set it to NO to prevent +# subgrouping. Alternatively, this can be done per class using the +# \nosubgrouping command. +# The default value is: YES. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions +# are shown inside the group in which they are included (e.g. using \ingroup) +# instead of on a separate page (for HTML and Man pages) or section (for LaTeX +# and RTF). +# +# Note that this feature does not work in combination with +# SEPARATE_MEMBER_PAGES. +# The default value is: NO. + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions +# with only public data fields or simple typedef fields will be shown inline in +# the documentation of the scope in which they are defined (i.e. file, +# namespace, or group documentation), provided this scope is documented. If set +# to NO, structs, classes, and unions are shown on a separate page (for HTML and +# Man pages) or section (for LaTeX and RTF). +# The default value is: NO. + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or +# enum is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically be +# useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. +# The default value is: NO. + +TYPEDEF_HIDES_STRUCT = NO + +# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This +# cache is used to resolve symbols given their name and scope. Since this can be +# an expensive process and often the same symbol appears multiple times in the +# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small +# doxygen will become slower. If the cache is too large, memory is wasted. The +# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range +# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536 +# symbols. At the end of a run doxygen will report the cache usage and suggest +# the optimal cache size from a speed point of view. +# Minimum value: 0, maximum value: 9, default value: 0. + +LOOKUP_CACHE_SIZE = 0 + +# The NUM_PROC_THREADS specifies the number threads doxygen is allowed to use +# during processing. When set to 0 doxygen will based this on the number of +# cores available in the system. You can set it explicitly to a value larger +# than 0 to get more control over the balance between CPU load and processing +# speed. At this moment only the input processing can be done using multiple +# threads. Since this is still an experimental feature the default is set to 1, +# which efficively disables parallel processing. Please report any issues you +# encounter. Generating dot graphs in parallel is controlled by the +# DOT_NUM_THREADS setting. +# Minimum value: 0, maximum value: 32, default value: 1. + +NUM_PROC_THREADS = 1 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in +# documentation are documented, even if no documentation was available. Private +# class members and static file members will be hidden unless the +# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES. +# Note: This will also disable the warnings about undocumented members that are +# normally produced when WARNINGS is set to YES. +# The default value is: NO. + +EXTRACT_ALL = NO + +# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will +# be included in the documentation. +# The default value is: NO. + +EXTRACT_PRIVATE = NO + +# If the EXTRACT_PRIV_VIRTUAL tag is set to YES, documented private virtual +# methods of a class will be included in the documentation. +# The default value is: NO. + +EXTRACT_PRIV_VIRTUAL = NO + +# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal +# scope will be included in the documentation. +# The default value is: NO. + +EXTRACT_PACKAGE = NO + +# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be +# included in the documentation. +# The default value is: NO. + +EXTRACT_STATIC = NO + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined +# locally in source files will be included in the documentation. If set to NO, +# only classes defined in header files are included. Does not have any effect +# for Java sources. +# The default value is: YES. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. If set to YES, local methods, +# which are defined in the implementation section but not in the interface are +# included in the documentation. If set to NO, only methods in the interface are +# included. +# The default value is: NO. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base name of +# the file that contains the anonymous namespace. By default anonymous namespace +# are hidden. +# The default value is: NO. + +EXTRACT_ANON_NSPACES = NO + +# If this flag is set to YES, the name of an unnamed parameter in a declaration +# will be determined by the corresponding definition. By default unnamed +# parameters remain unnamed in the output. +# The default value is: YES. + +RESOLVE_UNNAMED_PARAMS = YES + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all +# undocumented members inside documented classes or files. If set to NO these +# members will be included in the various overviews, but no documentation +# section is generated. This option has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. If set +# to NO, these classes will be included in the various overviews. This option +# has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend +# declarations. If set to NO, these declarations will be included in the +# documentation. +# The default value is: NO. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any +# documentation blocks found inside the body of a function. If set to NO, these +# blocks will be appended to the function's detailed documentation block. +# The default value is: NO. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation that is typed after a +# \internal command is included. If the tag is set to NO then the documentation +# will be excluded. Set it to YES to include the internal documentation. +# The default value is: NO. + +INTERNAL_DOCS = NO + +# With the correct setting of option CASE_SENSE_NAMES doxygen will better be +# able to match the capabilities of the underlying filesystem. In case the +# filesystem is case sensitive (i.e. it supports files in the same directory +# whose names only differ in casing), the option must be set to YES to properly +# deal with such files in case they appear in the input. For filesystems that +# are not case sensitive the option should be be set to NO to properly deal with +# output files written for symbols that only differ in casing, such as for two +# classes, one named CLASS and the other named Class, and to also support +# references to files without having to specify the exact matching casing. On +# Windows (including Cygwin) and MacOS, users should typically set this option +# to NO, whereas on Linux or other Unix flavors it should typically be set to +# YES. +# The default value is: system dependent. + +CASE_SENSE_NAMES = YES + +# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with +# their full class and namespace scopes in the documentation. If set to YES, the +# scope will be hidden. +# The default value is: NO. + +HIDE_SCOPE_NAMES = NO + +# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will +# append additional text to a page's title, such as Class Reference. If set to +# YES the compound reference will be hidden. +# The default value is: NO. + +HIDE_COMPOUND_REFERENCE= NO + +# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of +# the files that are included by a file in the documentation of that file. +# The default value is: YES. + +SHOW_INCLUDE_FILES = YES + +# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each +# grouped member an include statement to the documentation, telling the reader +# which file to include in order to use the member. +# The default value is: NO. + +SHOW_GROUPED_MEMB_INC = NO + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include +# files with double quotes in the documentation rather than with sharp brackets. +# The default value is: NO. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the +# documentation for inline members. +# The default value is: YES. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the +# (detailed) documentation of file and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. +# The default value is: YES. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief +# descriptions of file, namespace and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. Note that +# this will also influence the order of the classes in the class list. +# The default value is: NO. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the +# (brief and detailed) documentation of class members so that constructors and +# destructors are listed first. If set to NO the constructors will appear in the +# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS. +# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief +# member documentation. +# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting +# detailed member documentation. +# The default value is: NO. + +SORT_MEMBERS_CTORS_1ST = NO + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy +# of group names into alphabetical order. If set to NO the group names will +# appear in their defined order. +# The default value is: NO. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by +# fully-qualified names, including namespaces. If set to NO, the class list will +# be sorted only by class name, not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the alphabetical +# list. +# The default value is: NO. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper +# type resolution of all parameters of a function it will reject a match between +# the prototype and the implementation of a member function even if there is +# only one candidate or it is obvious which candidate to choose by doing a +# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still +# accept a match between prototype and implementation in such cases. +# The default value is: NO. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo +# list. This list is created by putting \todo commands in the documentation. +# The default value is: YES. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test +# list. This list is created by putting \test commands in the documentation. +# The default value is: YES. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug +# list. This list is created by putting \bug commands in the documentation. +# The default value is: YES. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO) +# the deprecated list. This list is created by putting \deprecated commands in +# the documentation. +# The default value is: YES. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional documentation +# sections, marked by \if ... \endif and \cond +# ... \endcond blocks. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the +# initial value of a variable or macro / define can have for it to appear in the +# documentation. If the initializer consists of more lines than specified here +# it will be hidden. Use a value of 0 to hide initializers completely. The +# appearance of the value of individual variables and macros / defines can be +# controlled using \showinitializer or \hideinitializer command in the +# documentation regardless of this setting. +# Minimum value: 0, maximum value: 10000, default value: 30. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at +# the bottom of the documentation of classes and structs. If set to YES, the +# list will mention the files that were used to generate the documentation. +# The default value is: YES. + +SHOW_USED_FILES = YES + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This +# will remove the Files entry from the Quick Index and from the Folder Tree View +# (if specified). +# The default value is: YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces +# page. This will remove the Namespaces entry from the Quick Index and from the +# Folder Tree View (if specified). +# The default value is: YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command command input-file, where command is the value of the +# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided +# by doxygen. Whatever the program writes to standard output is used as the file +# version. For an example see the documentation. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. To create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. You can +# optionally specify a file name after the option, if omitted DoxygenLayout.xml +# will be used as the name of the layout file. +# +# Note that if you run doxygen from a directory containing a file called +# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE +# tag is left empty. + +LAYOUT_FILE = + +# The CITE_BIB_FILES tag can be used to specify one or more bib files containing +# the reference definitions. This must be a list of .bib files. The .bib +# extension is automatically appended if omitted. This requires the bibtex tool +# to be installed. See also https://en.wikipedia.org/wiki/BibTeX for more info. +# For LaTeX the style of the bibliography can be controlled using +# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the +# search path. See also \cite for info how to create references. + +CITE_BIB_FILES = + +#--------------------------------------------------------------------------- +# Configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated to +# standard output by doxygen. If QUIET is set to YES this implies that the +# messages are off. +# The default value is: NO. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES +# this implies that the warnings are on. +# +# Tip: Turn warnings on while writing the documentation. +# The default value is: YES. + +WARNINGS = YES + +# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate +# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag +# will automatically be disabled. +# The default value is: YES. + +WARN_IF_UNDOCUMENTED = YES + +# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some parameters +# in a documented function, or documenting parameters that don't exist or using +# markup commands wrongly. +# The default value is: YES. + +WARN_IF_DOC_ERROR = YES + +# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that +# are documented, but have no documentation for their parameters or return +# value. If set to NO, doxygen will only warn about wrong or incomplete +# parameter documentation, but not about the absence of documentation. If +# EXTRACT_ALL is set to YES then this flag will automatically be disabled. +# The default value is: NO. + +WARN_NO_PARAMDOC = NO + +# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when +# a warning is encountered. If the WARN_AS_ERROR tag is set to FAIL_ON_WARNINGS +# then doxygen will continue running as if WARN_AS_ERROR tag is set to NO, but +# at the end of the doxygen process doxygen will return with a non-zero status. +# Possible values are: NO, YES and FAIL_ON_WARNINGS. +# The default value is: NO. + +WARN_AS_ERROR = NO + +# The WARN_FORMAT tag determines the format of the warning messages that doxygen +# can produce. The string should contain the $file, $line, and $text tags, which +# will be replaced by the file and line number from which the warning originated +# and the warning text. Optionally the format may contain $version, which will +# be replaced by the version of the file (if it could be obtained via +# FILE_VERSION_FILTER) +# The default value is: $file:$line: $text. + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning and error +# messages should be written. If left blank the output is written to standard +# error (stderr). + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# Configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag is used to specify the files and/or directories that contain +# documented source files. You may enter file names like myfile.cpp or +# directories like /usr/src/myproject. Separate the files or directories with +# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING +# Note: If this tag is empty the current directory is searched. + +INPUT = README.md \ + . + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses +# libiconv (or the iconv built into libc) for the transcoding. See the libiconv +# documentation (see: +# https://www.gnu.org/software/libiconv/) for the list of possible encodings. +# The default value is: UTF-8. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and +# *.h) to filter out the source-files in the directories. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# read by doxygen. +# +# Note the list of default checked file patterns might differ from the list of +# default file extension mappings. +# +# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp, +# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, +# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, +# *.m, *.markdown, *.md, *.mm, *.dox (to be provided as doxygen C comment), +# *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, *.f18, *.f, *.for, *.vhd, *.vhdl, +# *.ucf, *.qsf and *.ice. + +FILE_PATTERNS = *.c \ + *.cc \ + *.cxx \ + *.cpp \ + *.c++ \ + *.java \ + *.ii \ + *.ixx \ + *.ipp \ + *.i++ \ + *.inl \ + *.idl \ + *.ddl \ + *.odl \ + *.h \ + *.hh \ + *.hxx \ + *.hpp \ + *.h++ \ + *.cs \ + *.d \ + *.php \ + *.php4 \ + *.php5 \ + *.phtml \ + *.inc \ + *.m \ + *.markdown \ + *.md \ + *.mm \ + *.dox \ + *.doc \ + *.txt \ + *.py \ + *.pyw \ + *.f90 \ + *.f95 \ + *.f03 \ + *.f08 \ + *.f \ + *.for \ + *.tcl \ + *.vhd \ + *.vhdl \ + *.ucf \ + *.qsf \ + *.ice + +# The RECURSIVE tag can be used to specify whether or not subdirectories should +# be searched for input files as well. +# The default value is: NO. + +RECURSIVE = YES + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. +# The default value is: NO. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories for example use the pattern */test/* + +EXCLUDE_PATTERNS = */test/* */doc/* + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories use the pattern */test/* + +EXCLUDE_SYMBOLS = + +# The EXAMPLE_PATH tag can be used to specify one or more files or directories +# that contain example code fragments that are included (see the \include +# command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and +# *.h) to filter out the source-files in the directories. If left blank all +# files are included. + +EXAMPLE_PATTERNS = * + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude commands +# irrespective of the value of the RECURSIVE tag. +# The default value is: NO. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or directories +# that contain images that are to be included in the documentation (see the +# \image command). + +IMAGE_PATH = images + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command: +# +# +# +# where is the value of the INPUT_FILTER tag, and is the +# name of an input file. Doxygen will then use the output that the filter +# program writes to standard output. If FILTER_PATTERNS is specified, this tag +# will be ignored. +# +# Note that the filter must not add or remove lines; it is applied before the +# code is scanned, but not when the output code is generated. If lines are added +# or removed, the anchors will not be placed correctly. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# properly processed by doxygen. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. The filters are a list of the form: pattern=filter +# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how +# filters are used. If the FILTER_PATTERNS tag is empty or if none of the +# patterns match the file name, INPUT_FILTER is applied. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# properly processed by doxygen. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will also be used to filter the input files that are used for +# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES). +# The default value is: NO. + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and +# it is also possible to disable source filtering for a specific pattern using +# *.ext= (so without naming a filter). +# This tag requires that the tag FILTER_SOURCE_FILES is set to YES. + +FILTER_SOURCE_PATTERNS = + +# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that +# is part of the input, its contents will be placed on the main page +# (index.html). This can be useful if you have a project on for instance GitHub +# and want to reuse the introduction page also for the doxygen output. + +USE_MDFILE_AS_MAINPAGE = README.md + +#--------------------------------------------------------------------------- +# Configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will be +# generated. Documented entities will be cross-referenced with these sources. +# +# Note: To get rid of all source code in the generated output, make sure that +# also VERBATIM_HEADERS is set to NO. +# The default value is: NO. + +SOURCE_BROWSER = NO + +# Setting the INLINE_SOURCES tag to YES will include the body of functions, +# classes and enums directly into the documentation. +# The default value is: NO. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any +# special comment blocks from generated source code fragments. Normal C, C++ and +# Fortran comments will always remain visible. +# The default value is: YES. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES then for each documented +# entity all documented functions referencing it will be listed. +# The default value is: NO. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES then for each documented function +# all documented entities called/used by that function will be listed. +# The default value is: NO. + +REFERENCES_RELATION = NO + +# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set +# to YES then the hyperlinks from functions in REFERENCES_RELATION and +# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will +# link to the documentation. +# The default value is: YES. + +REFERENCES_LINK_SOURCE = YES + +# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the +# source code will show a tooltip with additional information such as prototype, +# brief description and links to the definition and documentation. Since this +# will make the HTML file larger and loading of large files a bit slower, you +# can opt to disable this feature. +# The default value is: YES. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +SOURCE_TOOLTIPS = YES + +# If the USE_HTAGS tag is set to YES then the references to source code will +# point to the HTML generated by the htags(1) tool instead of doxygen built-in +# source browser. The htags tool is part of GNU's global source tagging system +# (see https://www.gnu.org/software/global/global.html). You will need version +# 4.8.6 or higher. +# +# To use it do the following: +# - Install the latest version of global +# - Enable SOURCE_BROWSER and USE_HTAGS in the configuration file +# - Make sure the INPUT points to the root of the source tree +# - Run doxygen as normal +# +# Doxygen will invoke htags (and that will in turn invoke gtags), so these +# tools must be available from the command line (i.e. in the search path). +# +# The result: instead of the source browser generated by doxygen, the links to +# source code will now point to the output of htags. +# The default value is: NO. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a +# verbatim copy of the header file for each class for which an include is +# specified. Set to NO to disable this. +# See also: Section \class. +# The default value is: YES. + +VERBATIM_HEADERS = YES + +# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the +# clang parser (see: +# http://clang.llvm.org/) for more accurate parsing at the cost of reduced +# performance. This can be particularly helpful with template rich C++ code for +# which doxygen's built-in parser lacks the necessary type information. +# Note: The availability of this option depends on whether or not doxygen was +# generated with the -Duse_libclang=ON option for CMake. +# The default value is: NO. + +CLANG_ASSISTED_PARSING = NO + +# If clang assisted parsing is enabled and the CLANG_ADD_INC_PATHS tag is set to +# YES then doxygen will add the directory of each input to the include path. +# The default value is: YES. + +CLANG_ADD_INC_PATHS = YES + +# If clang assisted parsing is enabled you can provide the compiler with command +# line options that you would normally use when invoking the compiler. Note that +# the include paths will already be set by doxygen for the files and directories +# specified with INPUT and INCLUDE_PATH. +# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES. + +CLANG_OPTIONS = + +# If clang assisted parsing is enabled you can provide the clang parser with the +# path to the directory containing a file called compile_commands.json. This +# file is the compilation database (see: +# http://clang.llvm.org/docs/HowToSetupToolingForLLVM.html) containing the +# options used when the source files were built. This is equivalent to +# specifying the -p option to a clang tool, such as clang-check. These options +# will then be passed to the parser. Any options specified with CLANG_OPTIONS +# will be added as well. +# Note: The availability of this option depends on whether or not doxygen was +# generated with the -Duse_libclang=ON option for CMake. + +CLANG_DATABASE_PATH = + +#--------------------------------------------------------------------------- +# Configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all +# compounds will be generated. Enable this if the project contains a lot of +# classes, structs, unions or interfaces. +# The default value is: YES. + +ALPHABETICAL_INDEX = YES + +# In case all classes in a project start with a common prefix, all classes will +# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag +# can be used to specify a prefix (or a list of prefixes) that should be ignored +# while generating the index headers. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output +# The default value is: YES. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. +# The default directory is: html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_OUTPUT = html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each +# generated HTML page (for example: .htm, .php, .asp). +# The default value is: .html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a user-defined HTML header file for +# each generated HTML page. If the tag is left blank doxygen will generate a +# standard header. +# +# To get valid HTML the header file that includes any scripts and style sheets +# that doxygen needs, which is dependent on the configuration options used (e.g. +# the setting GENERATE_TREEVIEW). It is highly recommended to start with a +# default header using +# doxygen -w html new_header.html new_footer.html new_stylesheet.css +# YourConfigFile +# and then modify the file new_header.html. See also section "Doxygen usage" +# for information on how to generate the default header that doxygen normally +# uses. +# Note: The header is subject to change so you typically have to regenerate the +# default header when upgrading to a newer version of doxygen. For a description +# of the possible markers and block names see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each +# generated HTML page. If the tag is left blank doxygen will generate a standard +# footer. See HTML_HEADER for more information on how to generate a default +# footer and what special commands can be used inside the footer. See also +# section "Doxygen usage" for information on how to generate the default footer +# that doxygen normally uses. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style +# sheet that is used by each HTML page. It can be used to fine-tune the look of +# the HTML output. If left blank doxygen will generate a default style sheet. +# See also section "Doxygen usage" for information on how to generate the style +# sheet that doxygen normally uses. +# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as +# it is more robust and this tag (HTML_STYLESHEET) will in the future become +# obsolete. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_STYLESHEET = + +# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined +# cascading style sheets that are included after the standard style sheets +# created by doxygen. Using this option one can overrule certain style aspects. +# This is preferred over using HTML_STYLESHEET since it does not replace the +# standard style sheet and is therefore more robust against future updates. +# Doxygen will copy the style sheet files to the output directory. +# Note: The order of the extra style sheet files is of importance (e.g. the last +# style sheet in the list overrules the setting of the previous ones in the +# list). For an example see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_STYLESHEET = + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that the +# files will be copied as-is; there are no commands or markers available. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_FILES = + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen +# will adjust the colors in the style sheet and background images according to +# this color. Hue is specified as an angle on a colorwheel, see +# https://en.wikipedia.org/wiki/Hue for more information. For instance the value +# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300 +# purple, and 360 is red again. +# Minimum value: 0, maximum value: 359, default value: 220. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors +# in the HTML output. For a value of 0 the output will use grayscales only. A +# value of 255 will produce the most vivid colors. +# Minimum value: 0, maximum value: 255, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the +# luminance component of the colors in the HTML output. Values below 100 +# gradually make the output lighter, whereas values above 100 make the output +# darker. The value divided by 100 is the actual gamma applied, so 80 represents +# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not +# change the gamma. +# Minimum value: 40, maximum value: 240, default value: 80. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting this +# to YES can help to show when doxygen was last run and thus if the +# documentation is up to date. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_TIMESTAMP = NO + +# If the HTML_DYNAMIC_MENUS tag is set to YES then the generated HTML +# documentation will contain a main index with vertical navigation menus that +# are dynamically created via JavaScript. If disabled, the navigation index will +# consists of multiple levels of tabs that are statically embedded in every HTML +# page. Disable this option to support browsers that do not have JavaScript, +# like the Qt help browser. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_DYNAMIC_MENUS = YES + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_DYNAMIC_SECTIONS = NO + +# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries +# shown in the various tree structured indices initially; the user can expand +# and collapse entries dynamically later on. Doxygen will expand the tree to +# such a level that at most the specified number of entries are visible (unless +# a fully collapsed tree already exceeds this amount). So setting the number of +# entries 1 will produce a full collapsed tree by default. 0 is a special value +# representing an infinite number of entries and will result in a full expanded +# tree by default. +# Minimum value: 0, maximum value: 9999, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_INDEX_NUM_ENTRIES = 100 + +# If the GENERATE_DOCSET tag is set to YES, additional index files will be +# generated that can be used as input for Apple's Xcode 3 integrated development +# environment (see: +# https://developer.apple.com/xcode/), introduced with OSX 10.5 (Leopard). To +# create a documentation set, doxygen will generate a Makefile in the HTML +# output directory. Running make will produce the docset in that directory and +# running make install will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at +# startup. See https://developer.apple.com/library/archive/featuredarticles/Doxy +# genXcode/_index.html for more information. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_DOCSET = NO + +# This tag determines the name of the docset feed. A documentation feed provides +# an umbrella under which multiple documentation sets from a single provider +# (such as a company or product suite) can be grouped. +# The default value is: Doxygen generated docs. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# This tag specifies a string that should uniquely identify the documentation +# set bundle. This should be a reverse domain-name style string, e.g. +# com.mycompany.MyDocSet. Doxygen will append .docset to the name. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. +# The default value is: org.doxygen.Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher. +# The default value is: Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three +# additional HTML index files: index.hhp, index.hhc, and index.hhk. The +# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop +# (see: +# https://www.microsoft.com/en-us/download/details.aspx?id=21138) on Windows. +# +# The HTML Help Workshop contains a compiler that can convert all HTML output +# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML +# files are now used as the Windows 98 help format, and will replace the old +# Windows help format (.hlp) on all Windows platforms in the future. Compressed +# HTML files also contain an index, a table of contents, and you can search for +# words in the documentation. The HTML workshop also contains a viewer for +# compressed HTML files. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_HTMLHELP = NO + +# The CHM_FILE tag can be used to specify the file name of the resulting .chm +# file. You can add a path in front of the file if the result should not be +# written to the html output directory. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_FILE = + +# The HHC_LOCATION tag can be used to specify the location (absolute path +# including file name) of the HTML help compiler (hhc.exe). If non-empty, +# doxygen will try to run the HTML help compiler on the generated index.hhp. +# The file has to be specified with full path. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +HHC_LOCATION = + +# The GENERATE_CHI flag controls if a separate .chi index file is generated +# (YES) or that it should be included in the main .chm file (NO). +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +GENERATE_CHI = NO + +# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc) +# and project file content. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_INDEX_ENCODING = + +# The BINARY_TOC flag controls whether a binary table of contents is generated +# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it +# enables the Previous and Next buttons. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members to +# the table of contents of the HTML help documentation and to the tree view. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that +# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help +# (.qch) of the generated HTML documentation. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify +# the file name of the resulting .qch file. The path specified is relative to +# the HTML output folder. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help +# Project output. For more information please see Qt Help Project / Namespace +# (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#namespace). +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt +# Help Project output. For more information please see Qt Help Project / Virtual +# Folders (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#virtual-folders). +# The default value is: doc. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_VIRTUAL_FOLDER = doc + +# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom +# filter to add. For more information please see Qt Help Project / Custom +# Filters (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see Qt Help Project / Custom +# Filters (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's filter section matches. Qt Help Project / Filter Attributes (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#filter-attributes). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_SECT_FILTER_ATTRS = + +# The QHG_LOCATION tag can be used to specify the location (absolute path +# including file name) of Qt's qhelpgenerator. If non-empty doxygen will try to +# run qhelpgenerator on the generated .qhp file. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be +# generated, together with the HTML files, they form an Eclipse help plugin. To +# install this plugin and make it available under the help contents menu in +# Eclipse, the contents of the directory containing the HTML and XML files needs +# to be copied into the plugins directory of eclipse. The name of the directory +# within the plugins directory should be the same as the ECLIPSE_DOC_ID value. +# After copying Eclipse needs to be restarted before the help appears. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the Eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have this +# name. Each documentation set should have its own identifier. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# If you want full control over the layout of the generated HTML pages it might +# be necessary to disable the index and replace it with your own. The +# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top +# of each HTML page. A value of NO enables the index and the value YES disables +# it. Since the tabs in the index contain the same information as the navigation +# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +DISABLE_INDEX = NO + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. If the tag +# value is set to YES, a side panel will be generated containing a tree-like +# index structure (just like the one that is generated for HTML Help). For this +# to work a browser that supports JavaScript, DHTML, CSS and frames is required +# (i.e. any modern browser). Windows users are probably better off using the +# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can +# further fine-tune the look of the index. As an example, the default style +# sheet generated by doxygen has an example that shows how to put an image at +# the root of the tree instead of the PROJECT_NAME. Since the tree basically has +# the same information as the tab index, you could consider setting +# DISABLE_INDEX to YES when enabling this option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_TREEVIEW = NO + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that +# doxygen will group on one line in the generated HTML documentation. +# +# Note that a value of 0 will completely suppress the enum values from appearing +# in the overview section. +# Minimum value: 0, maximum value: 20, default value: 4. +# This tag requires that the tag GENERATE_HTML is set to YES. + +ENUM_VALUES_PER_LINE = 4 + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used +# to set the initial width (in pixels) of the frame in which the tree is shown. +# Minimum value: 0, maximum value: 1500, default value: 250. +# This tag requires that the tag GENERATE_HTML is set to YES. + +TREEVIEW_WIDTH = 250 + +# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to +# external symbols imported via tag files in a separate window. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +EXT_LINKS_IN_WINDOW = NO + +# If the HTML_FORMULA_FORMAT option is set to svg, doxygen will use the pdf2svg +# tool (see https://github.com/dawbarton/pdf2svg) or inkscape (see +# https://inkscape.org) to generate formulas as SVG images instead of PNGs for +# the HTML output. These images will generally look nicer at scaled resolutions. +# Possible values are: png (the default) and svg (looks nicer but requires the +# pdf2svg or inkscape tool). +# The default value is: png. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FORMULA_FORMAT = png + +# Use this tag to change the font size of LaTeX formulas included as images in +# the HTML documentation. When you change the font size after a successful +# doxygen run you need to manually remove any form_*.png images from the HTML +# output directory to force them to be regenerated. +# Minimum value: 8, maximum value: 50, default value: 10. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_FONTSIZE = 10 + +# Use the FORMULA_TRANSPARENT tag to determine whether or not the images +# generated for formulas are transparent PNGs. Transparent PNGs are not +# supported properly for IE 6.0, but are supported on all modern browsers. +# +# Note that when changing this option you need to delete any form_*.png files in +# the HTML output directory before the changes have effect. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_TRANSPARENT = YES + +# The FORMULA_MACROFILE can contain LaTeX \newcommand and \renewcommand commands +# to create new LaTeX commands to be used in formulas as building blocks. See +# the section "Including formulas" for details. + +FORMULA_MACROFILE = + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see +# https://www.mathjax.org) which uses client side JavaScript for the rendering +# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX +# installed or if you want to formulas look prettier in the HTML output. When +# enabled you may also need to install MathJax separately and configure the path +# to it using the MATHJAX_RELPATH option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +USE_MATHJAX = NO + +# When MathJax is enabled you can set the default output format to be used for +# the MathJax output. See the MathJax site (see: +# http://docs.mathjax.org/en/v2.7-latest/output.html) for more details. +# Possible values are: HTML-CSS (which is slower, but has the best +# compatibility), NativeMML (i.e. MathML) and SVG. +# The default value is: HTML-CSS. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_FORMAT = HTML-CSS + +# When MathJax is enabled you need to specify the location relative to the HTML +# output directory using the MATHJAX_RELPATH option. The destination directory +# should contain the MathJax.js script. For instance, if the mathjax directory +# is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax +# Content Delivery Network so you can quickly see the result without installing +# MathJax. However, it is strongly recommended to install a local copy of +# MathJax from https://www.mathjax.org before deployment. +# The default value is: https://cdn.jsdelivr.net/npm/mathjax@2. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_RELPATH = https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.5/ + +# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax +# extension names that should be enabled during MathJax rendering. For example +# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_EXTENSIONS = + +# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces +# of code that will be used on startup of the MathJax code. See the MathJax site +# (see: +# http://docs.mathjax.org/en/v2.7-latest/output.html) for more details. For an +# example see the documentation. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_CODEFILE = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box for +# the HTML output. The underlying search engine uses javascript and DHTML and +# should work on any modern browser. Note that when using HTML help +# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET) +# there is already a search function so this one should typically be disabled. +# For large projects the javascript based search engine can be slow, then +# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to +# search using the keyboard; to jump to the search box use + S +# (what the is depends on the OS and browser, but it is typically +# , /