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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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/50] 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 33860f9b6f4c480ddb1fb4c8c6e41cc15784daeb Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 3 Jul 2024 20:23:10 +0200 Subject: [PATCH 50/50] Update the year in license. --- joint_limits/include/joint_limits/joint_limiter_interface.hpp | 2 +- joint_limits/include/joint_limits/joint_saturation_limiter.hpp | 2 +- joint_limits/include/joint_limits/visibility_control.h | 2 +- joint_limits/src/joint_limiter_interface.cpp | 2 +- joint_limits/src/joint_saturation_limiter.cpp | 2 +- joint_limits/test/test_joint_saturation_limiter.cpp | 2 +- joint_limits/test/test_joint_saturation_limiter.hpp | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index efaf529724..ed2f269812 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) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2024, 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/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index 733178d695..3641fc5c75 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023, PickNik Inc. +// Copyright (c) 2024, PickNik Inc. // // 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/include/joint_limits/visibility_control.h b/joint_limits/include/joint_limits/visibility_control.h index 9c957a3cf9..54edbc15f3 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) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2024, 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/src/joint_limiter_interface.cpp b/joint_limits/src/joint_limiter_interface.cpp index 843337259d..aea0e6704d 100644 --- a/joint_limits/src/joint_limiter_interface.cpp +++ b/joint_limits/src/joint_limiter_interface.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2024, 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/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp index 5020cab27b..2f8ac9636f 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023, PickNik Inc. +// Copyright (c) 2024, PickNik Inc. // // 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/test/test_joint_saturation_limiter.cpp b/joint_limits/test/test_joint_saturation_limiter.cpp index e78c4b8994..6ecbd46d71 100644 --- a/joint_limits/test/test_joint_saturation_limiter.cpp +++ b/joint_limits/test/test_joint_saturation_limiter.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2024, 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/test/test_joint_saturation_limiter.hpp b/joint_limits/test/test_joint_saturation_limiter.hpp index 950b611109..3da0706177 100644 --- a/joint_limits/test/test_joint_saturation_limiter.hpp +++ b/joint_limits/test/test_joint_saturation_limiter.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2024, 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.