diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 37afc88778e..5f8e380b473 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -45,6 +45,7 @@ target_compile_definitions(joint_limiter_interface PRIVATE "JOINT_LIMITS_BUILDIN add_library(joint_saturation_limiter SHARED src/joint_saturation_limiter.cpp + src/joint_range_limiter.cpp ) target_compile_features(joint_saturation_limiter PUBLIC cxx_std_17) target_include_directories(joint_saturation_limiter PUBLIC @@ -56,19 +57,6 @@ ament_target_dependencies(joint_saturation_limiter PUBLIC ${THIS_PACKAGE_INCLUDE # which is appropriate when building the dll but not consuming it. target_compile_definitions(joint_saturation_limiter PRIVATE "JOINT_LIMITS_BUILDING_DLL") -add_library(joint_range_limiter SHARED - src/joint_range_limiter.cpp -) -target_compile_features(joint_range_limiter PUBLIC cxx_std_17) -target_include_directories(joint_range_limiter PUBLIC - $ - $ -) -ament_target_dependencies(joint_range_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(joint_range_limiter PRIVATE "JOINT_LIMITS_BUILDING_DLL") - pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml) if(BUILD_TESTING) @@ -116,7 +104,6 @@ install(TARGETS joint_limits joint_limiter_interface joint_saturation_limiter - joint_range_limiter EXPORT export_joint_limits ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/joint_limits/include/joint_limits/joint_range_limiter.hpp b/joint_limits/include/joint_limits/joint_range_limiter.hpp deleted file mode 100644 index 9d75ba6ad40..00000000000 --- a/joint_limits/include/joint_limits/joint_range_limiter.hpp +++ /dev/null @@ -1,96 +0,0 @@ -// Copyright 2024 PAL Robotics S.L. -// -// 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 Sai Kishor Kothakota - -#ifndef JOINT_LIMITS__JOINT_RANGE_LIMITER_HPP_ -#define JOINT_LIMITS__JOINT_RANGE_LIMITER_HPP_ - -#include -#include -#include -#include - -#include "joint_limits/joint_limiter_interface.hpp" -#include "joint_limits/joint_limits.hpp" -#include "rclcpp/clock.hpp" -#include "rclcpp/duration.hpp" - -namespace joint_limits -{ -/** - * Joint Range Limiter limits joints' position, velocity, effort, acceleration and jerk by clamping - * values to its minimal and maximal allowed values. Since the position, velocity, effort, - * acceleration and jerk 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, effort, acceleration and jerk will be reduced accordingly. - */ -template -class JointRangeLimiter : public JointLimiterInterface -{ -public: - /** \brief Constructor */ - JOINT_LIMITS_PUBLIC JointRangeLimiter(); - - /** \brief Destructor */ - JOINT_LIMITS_PUBLIC ~JointRangeLimiter(); - - JOINT_LIMITS_PUBLIC bool on_init() override; - - JOINT_LIMITS_PUBLIC bool on_configure( - const JointLimitsStateDataType & current_joint_states) override - { - prev_command_ = current_joint_states; - return true; - } - - /** \brief Enforce joint limits to desired position, velocity and acceleration using clamping. - * Class implements this method accepting following data types: - * - JointInterfacesCommandLimiterData: limiting position, velocity, effort, acceleration and - * jerk; - * - * 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( - JointLimitsStateDataType & actual, JointLimitsStateDataType & desired, - const rclcpp::Duration & dt) override; - -private: - rclcpp::Clock::SharedPtr clock_; - JointLimitsStateDataType prev_command_; -}; - -template -JointRangeLimiter::JointRangeLimiter() -: JointLimiterInterface() -{ - clock_ = std::make_shared(rclcpp::Clock(RCL_ROS_TIME)); -} - -template -JointRangeLimiter::~JointRangeLimiter() -{ -} -} // namespace joint_limits - -#endif // JOINT_LIMITS__JOINT_RANGE_LIMITER_HPP_ diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index bb1b2070bf2..9dbebb997b7 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -49,8 +49,9 @@ class JointSaturationLimiter : public JointLimiterInterface diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml index 906e19094f1..5bfab983447 100644 --- a/joint_limits/joint_limiters.xml +++ b/joint_limits/joint_limiters.xml @@ -7,10 +7,8 @@ Simple joint limiter using clamping approach. Warning: clamping can cause discontinuities. - - - Simple joint range limiter using clamping approach with the parsed limits. diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index 340afef3f6b..bb489f5bff3 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -14,7 +14,7 @@ /// \author Sai Kishor Kothakota -#include "joint_limits/joint_range_limiter.hpp" +#include "joint_limits/joint_saturation_limiter.hpp" #include #include "joint_limits/joint_limiter_struct.hpp" @@ -119,21 +119,22 @@ namespace joint_limits { template <> -bool JointRangeLimiter::on_init() +bool JointSaturationLimiter::on_init() { const bool result = (number_of_joints_ != 1); if (!result && has_logging_interface()) { RCLCPP_ERROR( node_logging_itf_->get_logger(), - "JointLimiter: The JointRangeLimiter expects the number of joints to be 1, but given : %zu", + "JointLimiter: The JointSaturationLimiter expects the number of joints to be 1, but given : " + "%zu", number_of_joints_); } return result; } template <> -bool JointRangeLimiter::on_enforce( +bool JointSaturationLimiter::on_enforce( JointControlInterfacesData & actual, JointControlInterfacesData & desired, const rclcpp::Duration & dt) { @@ -225,10 +226,10 @@ bool JointRangeLimiter::on_enforce( // typedefs are needed here to avoid issues with macro expansion. ref: // https://stackoverflow.com/a/8942986 -typedef joint_limits::JointRangeLimiter< +typedef joint_limits::JointSaturationLimiter< joint_limits::JointLimits, joint_limits::JointControlInterfacesData> - JointInterfacesRangeLimiter; + JointInterfacesSaturationLimiter; typedef joint_limits::JointLimiterInterface< joint_limits::JointLimits, joint_limits::JointControlInterfacesData> - JointInterfacesRangeLimiterBase; -PLUGINLIB_EXPORT_CLASS(JointInterfacesRangeLimiter, JointInterfacesRangeLimiterBase) + JointInterfacesSaturationLLimiterBase; +PLUGINLIB_EXPORT_CLASS(JointInterfacesSaturationLimiter, JointInterfacesSaturationLLimiterBase)